rosinljubljana.h

Go to the documentation of this file.

// -*- lsst-c++ -*-
/*
 * This file is part of webost_sim.
 *
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
 #include <stdint.h>
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <iostream>
#include <signal.h>
#include <cmath>
#include <time.h>
#include <tf/transform_broadcaster.h>
#include <camera_info_manager/camera_info_manager.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/GetMap.h>
#include <nav_msgs/OccupancyGrid.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Imu.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <webots_ros/camera_get_info.h>
#include <webots_ros/robot_get_device_list.h>
#include <webots_ros/save_image.h>
#include <webots_ros/get_int.h>
#include <webots_ros/set_int.h>
#include <webots_ros/get_string.h>
#include <webots_ros/set_float.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Simple_polygon_visibility_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arr_naive_point_location.h>
#include <CGAL/Polygon_2.h>
#include <istream>
#include <CGAL/Triangular_expansion_visibility_2.h>
#include <CGAL/squared_distance_2.h>
#include <vector>
 
// Define the used kernel and arrangement
typedef CGAL::Exact_predicates_exact_constructions_kernel       Kernel;
typedef Kernel::Point_2                                         Point_2;
typedef Kernel::Vector_2                                                                                                                                                                Vector_2;
typedef Kernel::Segment_2                                       Segment_2;
typedef Kernel::Ray_2                                                                                                                                                                   Ray_2;
typedef CGAL::Arr_segment_traits_2<Kernel>                      Traits_2;
typedef CGAL::Arrangement_2<Traits_2>                           Arrangement_2;
typedef Arrangement_2::Halfedge_const_handle                    Halfedge_const_handle;
typedef Arrangement_2::Face_handle                              Face_handle;
typedef Arrangement_2::Edge_const_iterator                      Edge_const_iterator;
// Define the used visibility class
typedef CGAL::Triangular_expansion_visibility_2<Arrangement_2>  TEV;
typedef CGAL::Polygon_2<Kernel> Polygon_2;
 
 
class Obstacle
{
public:
        Obstacle (std::string);
 
        void setSegments(std::vector<Point_2>);
 
        void clearSegments();
 
        std::vector<Point_2> points;
 
        std::vector<Segment_2> segments;
 
        std::string name;
};
 
class Field
{
public:
        Field(ros::NodeHandle);
 
        void addObstacle(std::string, std::vector<Point_2>);
 
        void addObstacle(std::string);
 
        void modifyObstacle(std::string, std::vector<Point_2>);
 
        void deleteObstacle(std::string);
 
        void deleteAll();
 
        void updateObstacleFromAlvar(std::string, std::vector<int>);
 
  void generateOccupancyGrid();
 
        void publishOccupancyGrid(ros::NodeHandle n);
 
 
        std::vector<Obstacle> obstacles;
 
  std::vector<signed char> data;
 
        tf::TransformListener listener;
 
        std::stringstream frameAr_pose_markerName;
 
        float mapResolution;
        uint32_t mapWidth;
        uint32_t mapHeight;
        geometry_msgs::Pose origin;
 
        ros::Publisher map_pub;
};
 
 
class Robot
{
 
public:
        Robot();
 
        void setName(std::string, ros::NodeHandle nh);
 
        void setFrames();
 
  void transmitFrames();
 
  void setPositionOrientation(tf::Vector3, tf::Quaternion);
 
        void updatePositionOrientation();
 
 
        void EKFOdometryCallback(const nav_msgs::Odometry &msg);
 
        void transmitPoseFromAlvarPoseMsg();
 
        void odometryCallback(const nav_msgs::Odometry & msg);
 
        void transmitLaserScan(Robot [], int list_length, Field *field);
 
        void transmitOdom();
 
        ros::Subscriber sub_ekf_odom;
        std::stringstream robot_ekf_odom_subscriber_name;
 
        std::string name;
 
        std::stringstream frameMapName;
 
        std::stringstream frameOdomName;
 
        std::stringstream frameBase_linkName;
 
        std::stringstream LaserScan_publisher_name;
 
        std::stringstream odom_publisher_name;
        std::stringstream pose_alvar_publisher_name;
 
        std::stringstream frameAr_pose_markerName;
 
  tf::Transform transformMap;
  tf::Transform transformOdom;
  tf::Transform transformBase_Link;
 
  tf::TransformBroadcaster br;
        tf::TransformListener listener;
 
        ros::Publisher LaserScan_publisher;
        ros::Publisher odom_publisher;
        ros::Publisher pose_alvar_publisher;
 
        nav_msgs::Odometry odom;
 
        geometry_msgs::PoseWithCovarianceStamped pose_alvar;
 
        ros::Time time;
        ros::Time time_2;
 
        float vx, vy, angularz;
        float x, y, fi;
};
 
class WebotsRobot: public Robot
{
public:
 
        WebotsRobot();
 
        void init(ros::NodeHandle, std::string);
 
        void moveCallback(const geometry_msgs::Twist& msg);
 
        void subscribeMove(ros::NodeHandle);
 
        void gyroCallback(const sensor_msgs::Imu &imu);
 
        void transmitLaserScan(WebotsRobot [], int list_length, Field *field);
 
        void enableGyro(ros::NodeHandle);
 
        void subscribeGyro(ros::NodeHandle);
 
        std::string controllerNameinWebots;
 
        std::string robotNameinWebots;
 
        webots_ros::set_float wheelSrv;
 
        ros::ServiceClient rightWheelPositionClient;
        ros::ServiceClient leftWheelPositionClient;
        ros::ServiceClient rightWheelVelocityClient;
        ros::ServiceClient leftWheelVelocityClient;
 
        std::stringstream cmd_vel_subscriber_name;
        std::string gyroNameinWebots;
 
        float wheelDistance;
        float wheelRadius;
 
        ros::Subscriber subCmd_vel;
        ros::Subscriber subGyro;
 
};
 
class Camera
{
public:
        Camera();
 
        void setName(std::string);
 
        void setFrames();
 
        void transmitFrames();
 
        void setPositionOrientation(tf::Vector3, tf::Quaternion);
 
        std::stringstream frameCameraName;
 
        tf::Transform transformCamera;
        tf::TransformBroadcaster br;
 
        std::stringstream imagePublisherName;
        std::stringstream infoPublisherName;
 
        std::string name;
};
 
class WebotsCamera: public Camera
{
public:
        WebotsCamera(ros::NodeHandle, image_transport::ImageTransport, std::string);
 
  void enable(ros::NodeHandle);
 
  void cameraCallback(const sensor_msgs::Image::ConstPtr &image);
 
        void subscribe(ros::NodeHandle);
 
  void setCamInfo();
 
  void publish();
 
        int width;
        int height;
 
  image_transport::Publisher pub;
 
  std::vector<float> imageCamera;
 
  ros::Publisher pub_info;
 
        std::string controllerNameinWebots;
  std::string cameraNameinWebots;
 
        sensor_msgs::CameraInfo cam_info;
 
        ros::Subscriber subCameraCamera;
 
};