rosinljubljana_objects.cpp

Go to the documentation of this file.

#include "rosinljubljana/rosinljubljana.h"
#define TIME_STEP 32
 
Obstacle::Obstacle(std::string obstacleName)
{
        name = obstacleName;
 
};
 
void Obstacle::setSegments(std::vector<Point_2> points_)
{
 
        segments.clear();
        for(int index; index < points_.size()-1; ++index) {
    segments.emplace_back(points_.at(index), points_.at(index+1));
        };
 
        points = points_;
 
 
};
 
void Obstacle::clearSegments()
{
 
        segments.clear();
        points.clear();
 
};
 
Field::Field(ros::NodeHandle n){
        map_pub = n.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
};
 
void Field::addObstacle(std::string name, std::vector<Point_2> points)
{
        obstacles.push_back(Obstacle(name));
        obstacles.back().setSegments(points);
};
 
void Field::addObstacle(std::string name) {
        obstacles.push_back(Obstacle(name));
}
 
void Field::modifyObstacle(std::string name, std::vector<Point_2> points)
{
        for(std::vector<Obstacle>::iterator it = obstacles.begin(); it != obstacles.end(); ++it) {
    if ((*it).name == name){
                        (*it).clearSegments();
                        (*it).setSegments(points);
                };
        };
};
 
void Field::deleteObstacle(std::string name)
{
        for (int index = 0; index <= obstacles.size(); ++index){
                if (obstacles.at(index).name == name){
                        obstacles.erase(obstacles.begin() + index);
                };
        };
};
 
void Field::deleteAll()
{
        obstacles.clear();
};
 
void Field::updateObstacleFromAlvar(std::string name, std::vector<int> alvar_nums)
{
        tf::StampedTransform transform;
        std::vector<Point_2> points;
 
        for(std::vector<int>::iterator it = alvar_nums.begin(); it != alvar_nums.end(); ++it) {
                try{
                frameAr_pose_markerName.str(std::string());
                frameAr_pose_markerName << "ar_marker_" << std::to_string(*it);
                listener.waitForTransform("map", frameAr_pose_markerName.str(), ros::Time::now(),
                                                                                                                                ros::Duration(10.0));
                listener.lookupTransform("map" ,frameAr_pose_markerName.str(),
                                                                                                                        ros::Time(0), transform);
                points.push_back(Point_2(transform.getOrigin().x(), transform.getOrigin().y()));
        }
        catch (tf::TransformException &ex) {
         ROS_ERROR("%s",ex.what());
       };
         };
 
         for(std::vector<Obstacle>::iterator it = obstacles.begin(); it != obstacles.end(); ++it) {
     if ((*it).name == name){
                        (*it).clearSegments();
                        (*it).setSegments(points);
                };
        };
};
 
void Field::generateOccupancyGrid()
{
        data.resize(mapWidth*mapHeight, 0);
 
        for(std::vector<Obstacle>::iterator it_obstacles = obstacles.begin(); it_obstacles != obstacles.end(); ++it_obstacles) {
                 for(std::vector<Segment_2>::iterator it_segments = (*it_obstacles).segments.begin(); it_segments != (*it_obstacles).segments.end(); ++ it_segments){
 
                         Point_2 p1 = (*it_segments).source();
                         Point_2 p2 = (*it_segments).target();
                         int x1 = (int)(CGAL::to_double(p1.x())/mapResolution + mapWidth/2);
                         int x2 = (int)(CGAL::to_double(p2.x())/mapResolution + mapWidth/2);
                         int y1 = (int)(CGAL::to_double(p1.y())/mapResolution + mapHeight/2);
                         int y2 = (int)(CGAL::to_double(p2.y())/mapResolution + mapHeight/2);
 
                         float dx = std::abs(x2 - x1);
                         float dy = std::abs(y2 - y1);
 
                         if (dx > dy) {
                                 if (x1 < x2) {
                                         float j = float(y1);
                                         float m = float((y2 - y1)/dx);
                                         for (int i = x1; i < x2; i++) {
                                                 data.at(int(j) * mapWidth + i) = 100;
                                                 data.at(int(j+1) * mapWidth + i) = 100;
                                                 j = j + m;
                                         };
                                 }
                                 else{
                                         float j = float(y2);
                                         float m = float((y1 - y2)/dx);
                                         for (int i = x2; i < x1; i++) {
                                                 data.at(int(j) * mapWidth + i) = 100;
                                                 data.at(int(j+1) * mapWidth + i) = 100;
                                                 j = j + m;
                                         };
                                 }
                         }
                         else {
                                 if (y1 < y2) {
                                         float j = float(x1);
                                         float m = float((x2 - x1)/dy);
                                         for (int i = y1; i < y2; i++) {
                                                 data.at(int(j) + i*mapWidth) = 100;
                                                 data.at(int(j) + i*mapWidth+1) = 100;
                                                 j = j + m;
                                         };
                                 }
                                 else{
                                         float j = float(x2);
                                         float m = float((x1 - x2)/dy);
                                         for (int i = y2; i < y1; i++) {
                                                 data.at(int(j) + i*mapWidth) = 100;
                                                 data.at(int(j) + i*mapWidth + 1) = 100;
                                                 j = j + m;
                                         };
                                 };
                         };
                 };
        };
}
 
void Field::publishOccupancyGrid(ros::NodeHandle n)
{
 
        nav_msgs::OccupancyGrid grid;
 
        grid.header.frame_id = "map";
 
        nav_msgs::MapMetaData mapData;
 
        mapData.map_load_time = ros::Time::now();
        mapData.resolution = mapResolution;
        mapData.width = mapWidth;
        mapData.height = mapHeight;
 
        geometry_msgs::Pose origin;
        origin.position.x = -mapResolution*(float)mapWidth/2.;
        origin.position.y = -mapResolution*(float)mapHeight/2.;
        mapData.origin = origin;
 
        grid.info = mapData;
        grid.data = data;
 
        map_pub.publish(grid);
}
 
// Constructor for robot Class
Robot::Robot(){
        name = "robot";
 
        x = 0.;
        y = 0.;
        fi = 0.;
        vx=0.0;
        vy=0.0;
        angularz=0.0;
        time = ros::Time::now();
}
 
 
void Robot::setName(std::string robotName, ros::NodeHandle nh){
        name = robotName;
        frameAr_pose_markerName<<"ar_marker_"<<name;
 
        LaserScan_publisher_name << "robot_" << name << "/LaserScan";
 
        LaserScan_publisher = nh.advertise<sensor_msgs::LaserScan>(LaserScan_publisher_name.str(), 10);
 
        odom_publisher_name << "robot_" << name << "/odom";
 
        odom_publisher = nh.advertise<nav_msgs::Odometry>(odom_publisher_name.str(), 50);
 
        pose_alvar_publisher_name << "robot_" << name << "/pose_alvar";
 
        pose_alvar_publisher = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(pose_alvar_publisher_name.str(), 50);
 
        robot_ekf_odom_subscriber_name << "robot_" << name << "/ekf_odom";
        sub_ekf_odom = nh.subscribe(robot_ekf_odom_subscriber_name.str(), 1, &Robot::EKFOdometryCallback, this);
 
 
}
 
void Robot::setFrames(){
        // Method to intialise robot frames
  transformMap.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
  tf::Quaternion qMap;
  qMap.setRPY(0, 0, 0);
  transformMap.setRotation(qMap);
 
  frameMapName << "robot_" << name << "/map";
 
  transformOdom.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
  tf::Quaternion qOdom;
  qOdom.setRPY(0, 0, 0);
  transformOdom.setRotation(qOdom);
 
  frameOdomName << "robot_" << name << "/odom";
 
  transformBase_Link.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
  tf::Quaternion qBase_link;
  qBase_link.setRPY(0, 0, 0);
  transformBase_Link.setRotation(qBase_link);
 
  frameBase_linkName << "robot_" << name << "/base_link";
}
 
void Robot::setPositionOrientation(tf::Vector3 position, tf::Quaternion orientation)
{
  // Method to set position of robot frame
  transformBase_Link.setOrigin(position);
  transformBase_Link.setRotation(orientation);
 
}
 
void Robot::transmitPoseFromAlvarPoseMsg()
{
        tf::StampedTransform transform;
 
        try{
 
                ros::Time lastCommonTime;
                std::string* error_string;
                listener.getLatestCommonTime("map", frameAr_pose_markerName.str(),
                                                                                                                                        lastCommonTime, error_string);
 
                if (lastCommonTime != time)
                {
                listener.lookupTransform("map" ,frameAr_pose_markerName.str(),
                                                                                                                        ros::Time(0), transform);
 
                ros::Duration dt = ros::Time::now() - lastCommonTime;
 
                transformBase_Link.setOrigin(tf::Vector3(transform.getOrigin().x()+vx*dt.toSec(), transform.getOrigin().y()+vy*dt.toSec(), 0.0));
                time = lastCommonTime;
 
                double roll, pitch, yaw;
                tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw);
 
                tf::Quaternion qBase_link;
                qBase_link.setRPY(0, 0, yaw+angularz*dt.toSec());
                transformBase_Link.setRotation(qBase_link);
 
                pose_alvar.header.stamp = time;
                pose_alvar.header.frame_id = frameOdomName.str();
 
                //set the position
                pose_alvar.pose.pose.position.x = transform.getOrigin().x();
                pose_alvar.pose.pose.position.y = transform.getOrigin().y();
                pose_alvar.pose.pose.position.z = 0.0;
                pose_alvar.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
 
                pose_alvar.pose.covariance[0] = 0.0001;
                pose_alvar.pose.covariance[7] = 0.0001;
                pose_alvar.pose.covariance[14] = 0.0001;
                pose_alvar.pose.covariance[21] = 0.0001;
                pose_alvar.pose.covariance[28] = 0.0001;
                pose_alvar.pose.covariance[35] = 0.0001;
 
                //publish the message
                pose_alvar_publisher.publish(pose_alvar);
        }
        }
        catch (tf::TransformException &ex) {
         //ROS_ERROR("%s",ex.what());
         //ros::Duration(1.0).sleep();
       }
}
 
void Robot::updatePositionOrientation()
{
        tf::StampedTransform transform;
 
        try{
 
                ros::Time lastCommonTime;
                std::string* error_string;
                listener.getLatestCommonTime("map", frameAr_pose_markerName.str(),
                                                                                                                                        lastCommonTime, error_string);
 
                if (lastCommonTime != time)
                {
                listener.lookupTransform("map" ,frameAr_pose_markerName.str(),
                                                                                                                        ros::Time(0), transform);
 
                ros::Duration dt = ros::Time::now() - lastCommonTime;
 
                transformBase_Link.setOrigin(tf::Vector3(transform.getOrigin().x()+vx*dt.toSec(), transform.getOrigin().y()+vy*dt.toSec(), 0.0));
 
                time = lastCommonTime;
                double roll, pitch, yaw;
                tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw);
 
                tf::Quaternion qBase_link;
          qBase_link.setRPY(0, 0, yaw+angularz*dt.toSec());
          transformBase_Link.setRotation(qBase_link);
 
                x = transform.getOrigin().x();
                y = transform.getOrigin().y();
 
                fi = yaw;
        }
        }
        catch (tf::TransformException &ex) {
         ROS_ERROR("%s",ex.what());
         //ros::Duration(1.0).sleep();
       }
}
 
void Robot::EKFOdometryCallback(const   nav_msgs::Odometry & msg)
{
 
        tf::Quaternion qBase_link;
 
 
        tf::Quaternion q(
    msg.pose.pose.orientation.x,
    msg.pose.pose.orientation.y,
    msg.pose.pose.orientation.z,
    msg.pose.pose.orientation.w);
        tf::Matrix3x3 m(q);
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);
 
        fi = yaw;
        x = msg.pose.pose.position.x;
        y = msg.pose.pose.position.y;
        
        qBase_link.setRPY(0, 0, yaw);
        transformBase_Link.setRotation(qBase_link);
 
        transformBase_Link.setOrigin(tf::Vector3(x, y, 0.0));
 
        this->transmitFrames();
}
 
void Robot::odometryCallback(const      nav_msgs::Odometry & msg)
{
 
        ros::Duration dt = ros::Time::now() - time_2;
        time_2 = ros::Time::now();
 
        tf::Quaternion qBase_link;
 
        double vx_beagle = msg.twist.twist.linear.x;
        double vy_beagle = msg.twist.twist.linear.y;
 
        double angularz = msg.twist.twist.angular.z;
        fi = fi+angularz*dt.toSec();
 
        tf::Quaternion q(
    msg.pose.pose.orientation.x,
    msg.pose.pose.orientation.y,
    msg.pose.pose.orientation.z,
    msg.pose.pose.orientation.w);
        tf::Matrix3x3 m(q);
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);
 
        double v = vx_beagle/cos(yaw);
 
        vx = v*cos(fi);
        vy = v*sin(fi);
 
        qBase_link.setRPY(0, 0, fi);
        transformBase_Link.setRotation(qBase_link);
 
        x = x + vx*dt.toSec();
        y = y + vy*dt.toSec();
 
        transformBase_Link.setOrigin(tf::Vector3(x, y, 0.0));
 
}
 
void Robot::transmitOdom()
{
        odom.header.stamp = time;
        odom.header.frame_id = frameOdomName.str();
 
        //set the position
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(fi);
 
        //set the velocity
        odom.child_frame_id = frameBase_linkName.str();
        odom.twist.twist.linear.x = vx;
        odom.twist.twist.linear.y = vy;
        odom.twist.twist.angular.z = angularz;
 
        //publish the message
        odom_publisher.publish(odom);
}
 
void Robot::transmitLaserScan(Robot robotek_list[], int list_length, Field *field)
{
        std::vector<Segment_2> segments;
        for(std::vector<Obstacle>::iterator it_obstacles = field->obstacles.begin(); it_obstacles != field->obstacles.end(); ++it_obstacles) {
                 for(std::vector<Segment_2>::iterator it_segments = (*it_obstacles).segments.begin(); it_segments != (*it_obstacles).segments.end(); ++ it_segments){
                         segments.push_back(*it_segments);
                 }
        }
        std::vector<Obstacle>::iterator it_obstacle_first = field->obstacles.begin();
        Point_2 *points = &((*it_obstacle_first).points[0]);
        Polygon_2 pgn(points, points+(*it_obstacle_first).points.size()-1);
        for (int i = 0; i<list_length; i++){
                        if (robotek_list[i].name != name){
 
                                Point_2 h1(robotek_list[i].x+0.1*std::cos(3.14159/4. + fi), robotek_list[i].y + 0.1*std::sin(3.1415926/4. + fi));
                                Point_2 h2(robotek_list[i].x+0.1*std::cos(3.14159*3./4. + fi), robotek_list[i].y + 0.1*std::sin(3.1415926*3./4. + fi));
                                Point_2 h3(robotek_list[i].x+0.1*std::cos(3.14159*5./4. + fi), robotek_list[i].y + 0.1*std::sin(3.1415926*5./4. + fi));
                                Point_2 h4(robotek_list[i].x+0.1*std::cos(3.14159*7./4. + fi), robotek_list[i].y + 0.1*std::sin(3.1415926*7./4. + fi));
 
                                segments.push_back(Segment_2(h1, h2));
                                segments.push_back(Segment_2(h2, h3));
                                segments.push_back(Segment_2(h3, h4));
                                segments.push_back(Segment_2(h4, h1));
                        }
                }
                Arrangement_2 env;
                CGAL::insert(env,segments.begin(),segments.end());
                Point_2 query_point(x, y);
 
                Arrangement_2::Face_const_handle * face;
                CGAL::Arr_naive_point_location<Arrangement_2> pl(env);
                CGAL::Arr_point_location_result<Arrangement_2>::Type obj = pl.locate(query_point);
 
                // The query point locates in the interior of a face
                face = boost::get<Arrangement_2::Face_const_handle> (&obj);
 
                CGAL::Bounded_side bside = pgn.bounded_side(query_point);
                if (bside == CGAL::ON_BOUNDED_SIDE) {
 
                Arrangement_2 output_arr;
                TEV tev(env);
                Face_handle fh = tev.compute_visibility(query_point, *face, output_arr);
 
          Arrangement_2::Ccb_halfedge_circulator curr = fh->outer_ccb();
 
                sensor_msgs::LaserScan LaserScan_msg;
 
                LaserScan_msg.header.stamp = ros::Time::now();
                LaserScan_msg.header.frame_id = frameBase_linkName.str();;
 
                LaserScan_msg.angle_min = 0.;
                LaserScan_msg.angle_max = 2*3.1415926;
 
                LaserScan_msg.angle_increment = 3.1415926/40.;
                LaserScan_msg.time_increment = 0.001;
 
                LaserScan_msg.scan_time = 0.1;
 
                LaserScan_msg.range_min = 0.01;
                LaserScan_msg.range_max = 4.0;
                int size = (int)((LaserScan_msg.angle_max - LaserScan_msg.angle_min)/LaserScan_msg.angle_increment+2);
                LaserScan_msg.ranges.resize(size);
 
                float ray_angle = LaserScan_msg.angle_increment;
                int index = 0;
                while (ray_angle <= LaserScan_msg.angle_max){
                        Ray_2 ray(query_point, Vector_2(std::cos(fi+ray_angle), std::sin(fi+ray_angle)));
 
                        auto result = CGAL::intersection(ray, Segment_2(curr->source()->point(), curr->target()->point()));
                        if(result){
                                Point_2* p = boost::get<Point_2>(&*result);
                                        double distance = std::pow(CGAL::to_double(squared_distance(query_point, *p)), 0.5);
                                                LaserScan_msg.ranges.at(index) = distance;
                                                                }
 
                  while (++curr != fh->outer_ccb()){
 
                                auto result = CGAL::intersection(ray, Segment_2(curr->source()->point(), curr->target()->point()));
                                if(result){
                                        Point_2* p = boost::get<Point_2>(&*result);
                                        double distance = std::pow(CGAL::to_double(squared_distance(query_point, *p)), 0.5);
                                        LaserScan_msg.ranges.at(index) = distance;
                                }
                                }
                        index++;
 
                        ray_angle+=LaserScan_msg.angle_increment;
                }
                LaserScan_publisher.publish(LaserScan_msg);
                }
}
 
void Robot::transmitFrames(){
        // Method to transmit robo frames
        ros::Time transmit_time = ros::Time::now();
        br.sendTransform(tf::StampedTransform(transformMap, transmit_time, "map", frameMapName.str()));
        br.sendTransform(tf::StampedTransform(transformOdom, transmit_time, frameMapName.str(), frameOdomName.str()));
        br.sendTransform(tf::StampedTransform(transformBase_Link, transmit_time, frameOdomName.str(), frameBase_linkName.str()));
 
}
 
WebotsRobot::WebotsRobot(){
        wheelDistance = 0.1;
        wheelRadius = 0.015;
};
 
// Constructor for webots robot
void WebotsRobot::init(ros::NodeHandle nh, std::string controllerName)
{
        cmd_vel_subscriber_name << "robot_" << name << "/cmd_vel";
 
        controllerNameinWebots = controllerName;
        gyroNameinWebots = "gyro";
 
        wheelSrv.request.value = INFINITY;
 
        // Setting wheels to velocity control
        leftWheelPositionClient =
        nh.serviceClient<webots_ros::set_float>(controllerNameinWebots + "/wheel1/set_position");
        leftWheelPositionClient.call(wheelSrv);
        rightWheelPositionClient =
                nh.serviceClient<webots_ros::set_float>(controllerNameinWebots + "/wheel2/set_position");
        rightWheelPositionClient.call(wheelSrv);
        leftWheelVelocityClient =
                nh.serviceClient<webots_ros::set_float>(controllerNameinWebots + "/wheel1/set_velocity");
        rightWheelVelocityClient =
                nh.serviceClient<webots_ros::set_float>(controllerNameinWebots + "/wheel2/set_velocity");
 
                wheelSrv.request.value = 0.;
                rightWheelVelocityClient.call(wheelSrv);
                wheelSrv.request.value = 0.;
                leftWheelVelocityClient.call(wheelSrv);
}
 
void WebotsRobot::moveCallback(const geometry_msgs::Twist& msg){
        // Callback method for subsrciber
 
  float vel = msg.linear.x;
        float rotation = msg.angular.z;
 
        float wheel_right = (vel*2./wheelDistance - rotation)*wheelDistance/(4.*3.14159265*wheelRadius);
  float wheel_left = vel/(3.14159265*wheelRadius) - wheel_right;
 
  wheelSrv.request.value = -wheel_right*5.;
  rightWheelVelocityClient.call(wheelSrv);
  wheelSrv.request.value = -wheel_left*5.;
  leftWheelVelocityClient.call(wheelSrv);
}
 
void WebotsRobot::transmitLaserScan(WebotsRobot robotek_list[], int list_length, Field *field)
{
        std::vector<Segment_2> segments;
        for(std::vector<Obstacle>::iterator it_obstacles = field->obstacles.begin(); it_obstacles != field->obstacles.end(); ++it_obstacles) {
                 for(std::vector<Segment_2>::iterator it_segments = (*it_obstacles).segments.begin(); it_segments != (*it_obstacles).segments.end(); ++ it_segments){
                         segments.push_back(*it_segments);
                 }
        }
        std::vector<Obstacle>::iterator it_obstacle_first = field->obstacles.begin();
        Point_2 *points = &((*it_obstacle_first).points[0]);
        Polygon_2 pgn(points, points+(*it_obstacle_first).points.size()-1);
        for (int i = 0; i<list_length; i++){
                        if (robotek_list[i].name != name){
 
                                Point_2 h1(robotek_list[i].x+0.1*std::cos(3.14159/4. + fi), robotek_list[i].y + 0.1*std::sin(3.1415926/4. + fi));
                                Point_2 h2(robotek_list[i].x+0.1*std::cos(3.14159*3./4. + fi), robotek_list[i].y + 0.1*std::sin(3.1415926*3./4. + fi));
                                Point_2 h3(robotek_list[i].x+0.1*std::cos(3.14159*5./4. + fi), robotek_list[i].y + 0.1*std::sin(3.1415926*5./4. + fi));
                                Point_2 h4(robotek_list[i].x+0.1*std::cos(3.14159*7./4. + fi), robotek_list[i].y + 0.1*std::sin(3.1415926*7./4. + fi));
 
                                segments.push_back(Segment_2(h1, h2));
                                segments.push_back(Segment_2(h2, h3));
                                segments.push_back(Segment_2(h3, h4));
                                segments.push_back(Segment_2(h4, h1));
                        }
                }
                Arrangement_2 env;
                CGAL::insert(env,segments.begin(),segments.end());
                Point_2 query_point(x, y);
 
                Arrangement_2::Face_const_handle * face;
                CGAL::Arr_naive_point_location<Arrangement_2> pl(env);
                CGAL::Arr_point_location_result<Arrangement_2>::Type obj = pl.locate(query_point);
 
                // The query point locates in the interior of a face
                face = boost::get<Arrangement_2::Face_const_handle> (&obj);
 
                CGAL::Bounded_side bside = pgn.bounded_side(query_point);
                if (bside == CGAL::ON_BOUNDED_SIDE) {
 
                Arrangement_2 output_arr;
                TEV tev(env);
                Face_handle fh = tev.compute_visibility(query_point, *face, output_arr);
 
          Arrangement_2::Ccb_halfedge_circulator curr = fh->outer_ccb();
 
                sensor_msgs::LaserScan LaserScan_msg;
 
                LaserScan_msg.header.stamp = ros::Time::now();
                LaserScan_msg.header.frame_id = frameBase_linkName.str();;
 
                LaserScan_msg.angle_min = 0.;
                LaserScan_msg.angle_max = 2*3.1415926;
 
                LaserScan_msg.angle_increment = 3.1415926/40.;
                LaserScan_msg.time_increment = 0.001;
 
                LaserScan_msg.scan_time = 0.1;
 
                LaserScan_msg.range_min = 0.01;
                LaserScan_msg.range_max = 4.0;
                int size = (int)((LaserScan_msg.angle_max - LaserScan_msg.angle_min)/LaserScan_msg.angle_increment+2);
                LaserScan_msg.ranges.resize(size);
 
                float ray_angle = LaserScan_msg.angle_increment;
                int index = 0;
                while (ray_angle <= LaserScan_msg.angle_max){
                        Ray_2 ray(query_point, Vector_2(std::cos(fi+ray_angle), std::sin(fi+ray_angle)));
 
                        auto result = CGAL::intersection(ray, Segment_2(curr->source()->point(), curr->target()->point()));
                        if(result){
                                Point_2* p = boost::get<Point_2>(&*result);
                                        double distance = std::pow(CGAL::to_double(squared_distance(query_point, *p)), 0.5);
                                                LaserScan_msg.ranges.at(index) = distance;
                                                                }
 
                  while (++curr != fh->outer_ccb()){
 
                                auto result = CGAL::intersection(ray, Segment_2(curr->source()->point(), curr->target()->point()));
                                if(result){
                                        Point_2* p = boost::get<Point_2>(&*result);
                                        double distance = std::pow(CGAL::to_double(squared_distance(query_point, *p)), 0.5);
                                        LaserScan_msg.ranges.at(index) = distance;
                                }
                                }
                        index++;
 
                        ray_angle+=LaserScan_msg.angle_increment;
                }
                LaserScan_publisher.publish(LaserScan_msg);
                }
}
 
void WebotsRobot::subscribeMove(ros::NodeHandle nh){
        // subscriber to twist topic
        subCmd_vel = nh.subscribe(cmd_vel_subscriber_name.str(), 1, &WebotsRobot::moveCallback, this);
}
 
void WebotsRobot::enableGyro(ros::NodeHandle nh){
        // Method to enable camera in simulator
  ros::ServiceClient enableGyroClient =
  nh.serviceClient<webots_ros::set_int>(controllerNameinWebots + '/' + gyroNameinWebots + "/enable");
    webots_ros::set_int enableGyroSrv;
 
  enableGyroSrv.request.value = 2 * TIME_STEP;
  if (enableGyroClient.call(enableGyroSrv) && enableGyroSrv.response.success) {
    ROS_INFO("Gyro enabled with sampling period %d.", enableGyroSrv.request.value);}
}
 
void WebotsRobot::gyroCallback(const sensor_msgs::Imu &imu){
        // publihses odometry from gyro
 
}
 
void WebotsRobot::subscribeGyro(ros::NodeHandle nh){
        // Method to subscribe to camera in webots ssimulator
  subGyro = nh.subscribe(controllerNameinWebots + '/' + gyroNameinWebots + "/values", 1, &WebotsRobot::gyroCallback, this);
  while (subGyro.getNumPublishers() == 0) {
    }
}
 
Camera::Camera(){
        name = "camera";
}
 
 
void Camera::setPositionOrientation(tf::Vector3 position, tf::Quaternion orientation)
{
        // Set poseition and orientation fo webots camera
  transformCamera.setOrigin(position);
  transformCamera.setRotation(orientation);
 
}
 
void Camera::setFrames()
{
        // Set frames of camera
  transformCamera.setOrigin(tf::Vector3(0.0, 0.0, 2.0));
  tf::Quaternion qCamera(0.0, 0.0, 0.0, 1.0);
  qCamera.setRPY(3.14159, 0, 0);
  transformCamera.setRotation(qCamera);
  frameCameraName << name << "_link";
}
 
void Camera::transmitFrames()
{
        // Transmit frames
  br.sendTransform(tf::StampedTransform(transformCamera, ros::Time::now(), "map", frameCameraName.str()));
}
 
void Camera::setName(std::string cameraName){
        // set name of camera
        name = cameraName;
 
        imagePublisherName << cameraName << "/image_raw";
        infoPublisherName << cameraName << "/camera_info";
}
 
WebotsCamera::WebotsCamera(ros::NodeHandle nh, image_transport::ImageTransport imgt, std::string controllerName)
{
 
                // Constructor of webots camera
                std::string imagePublisherName_ = imagePublisherName.str();
    pub = imgt.advertise("/camera/image_raw", 1);
    pub_info = nh.advertise<sensor_msgs::CameraInfo>("/camera/camera_info", 100);
    controllerNameinWebots = controllerName;
 
    ros::ServiceClient deviceListClient = nh.serviceClient<webots_ros::robot_get_device_list>(controllerNameinWebots + "/robot/get_device_list");
    std::vector<std::string> deviceList;
    webots_ros::robot_get_device_list deviceListSrv;
 
    if (deviceListClient.call(deviceListSrv)) {
    deviceList = deviceListSrv.response.list;
    ROS_INFO("The controller has %lu devices availables:", deviceList.size());
    for (unsigned int i = 0; i < deviceList.size(); i++)
      ROS_INFO("Device [%d]: %s.", i, deviceList[i].c_str());
  } else
    ROS_ERROR("Failed to call service deviceList.");
 
    cameraNameinWebots = deviceList[0];
 
    ros::ServiceClient cameraGetInfoClient =
      nh.serviceClient<webots_ros::camera_get_info>(controllerNameinWebots + '/' + cameraNameinWebots + "/get_info");
 
    webots_ros::camera_get_info cameraGetInfoSrv;
      if (cameraGetInfoClient.call(cameraGetInfoSrv)) {
        width = cameraGetInfoSrv.response.width;
        height = cameraGetInfoSrv.response.height;
        ROS_INFO("Camera size is %d x %d.", width, height);
      } else
        ROS_ERROR("Failed to call service range_finder_get_info.");
}
 
void WebotsCamera::setCamInfo(){
        // Method to set camera parameters
 
  cam_info.header.stamp = ros::Time::now();
  cam_info.header.frame_id = "camera_link";
  cam_info.distortion_model = "plumb_bob";
  cam_info.height = height;
  cam_info.width = width;
  cam_info.D.resize(5);
  std::fill (cam_info.D.begin(),cam_info.D.end(),0.0);
 
  cam_info.K.at(0) = 1104;
  cam_info.K.at(1) = 1.0;
  cam_info.K.at(2) = 640.5;
  cam_info.K.at(3) = 0.0;
  cam_info.K.at(4) = 1104;
  cam_info.K.at(5) = 480.5;
  cam_info.K.at(6) = 0.0;
  cam_info.K.at(7) = 0.0;
  cam_info.K.at(8) = 1.0;
 
  cam_info.D.at(0) = 1104;
  cam_info.D.at(0) = 0.0;
  cam_info.D.at(0) = 640.5;
  cam_info.D.at(0) = 0.0;
  cam_info.D.at(0) = 0.0;
  cam_info.D.at(0) = 1104;
  cam_info.D.at(0) = 480.5;
  cam_info.D.at(0) = 0.0;
  cam_info.D.at(0) = 0.0;
  cam_info.D.at(0) = 0.0;
  cam_info.D.at(0) = 1.0;
  cam_info.D.at(0) = 0.0;
}
 
void WebotsCamera::cameraCallback(const sensor_msgs::Image::ConstPtr &image){
        // Callback method for subscriber
 
  int size = image->width * image->height;
  imageCamera.resize(size);
 
  const float *depth_data = reinterpret_cast<const float *>(&image->data[0]);
  for (int i = 0; i < size; ++i)
    imageCamera[i] = depth_data[i];
}
 
void WebotsCamera::enable(ros::NodeHandle nh){
        // Method to enable camera in simulator
  ros::ServiceClient enableCameraClient =
  nh.serviceClient<webots_ros::set_int>(controllerNameinWebots + '/' + cameraNameinWebots + "/enable");
    webots_ros::set_int enableCameraSrv;
 
  enableCameraSrv.request.value = 2 * TIME_STEP;
  if (enableCameraClient.call(enableCameraSrv) && enableCameraSrv.response.success) {
    ROS_INFO("Camera enabled with sampling period %d.", enableCameraSrv.request.value);}
 
}
 
void WebotsCamera::subscribe(ros::NodeHandle nh){
        // Method to subscribe to camera in webots ssimulator
  subCameraCamera = nh.subscribe(controllerNameinWebots + '/' + cameraNameinWebots + "/image", 1, &WebotsCamera::cameraCallback, this);
  while (subCameraCamera.getNumPublishers() == 0) {
    }
}
 
void WebotsCamera::publish(){
        // Method to publish camera from webots in different topic
  cv::Mat image(height, width, CV_32FC1);
  memcpy(image.data,imageCamera.data(),imageCamera.size()*sizeof(float));
 
  sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgra8", image).toImageMsg();
  msg->header.frame_id = frameCameraName.str();
  msg->header.stamp = ros::Time::now();
  cam_info.header.stamp = ros::Time::now();
  cam_info.header.frame_id = frameCameraName.str();
  pub_info.publish(cam_info);
  pub.publish(msg);
}