├── src ├── laser_simulator_node.cpp ├── mobile_robot_simulator_node.cpp ├── simulator_node.cpp ├── mobile_robot_simulator.cpp └── laser_simulator.cpp ├── launch └── simulator.launch ├── package.xml ├── include └── mobile_robot_simulator │ ├── mobile_robot_simulator.h │ └── laser_simulator.h ├── README.md └── CMakeLists.txt /src/laser_simulator_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | 3 | #include "mobile_robot_simulator/laser_simulator.h" 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc,argv, "laser_simulator"); 8 | ros::NodeHandle nh("~"); 9 | 10 | LaserScannerSimulator laser_sim(&nh); 11 | ros::AsyncSpinner spinner(1); 12 | 13 | ROS_INFO("--- Starting LaserScanner simulator"); 14 | 15 | ros::Duration(0.5).sleep(); 16 | 17 | laser_sim.start(); 18 | 19 | spinner.start(); 20 | while (nh.ok()) { 21 | //ros::spinOnce(); 22 | ros::Duration(0.01).sleep(); 23 | } 24 | spinner.stop(); 25 | 26 | laser_sim.stop(); 27 | 28 | return 0; 29 | 30 | } // end main 31 | -------------------------------------------------------------------------------- /src/mobile_robot_simulator_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | 3 | #include "mobile_robot_simulator/mobile_robot_simulator.h" 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc,argv, "mobile_robot_simulator"); 8 | ros::NodeHandle nh("~"); 9 | ros::Duration(0.1).sleep(); 10 | 11 | MobileRobotSimulator mob_sim(&nh); 12 | ros::AsyncSpinner spinner(1); 13 | 14 | ROS_INFO("--- Starting MobileRobot simulator"); 15 | 16 | ros::Duration(0.1).sleep(); 17 | 18 | mob_sim.start(); 19 | 20 | spinner.start(); 21 | while (nh.ok()) { 22 | //ros::spinOnce(); 23 | ros::Duration(0.01).sleep(); 24 | } 25 | spinner.stop(); 26 | 27 | mob_sim.stop(); 28 | 29 | return 0; 30 | 31 | } // end main 32 | -------------------------------------------------------------------------------- /src/simulator_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | 3 | #include "mobile_robot_simulator/mobile_robot_simulator.h" 4 | #include "mobile_robot_simulator/laser_simulator.h" 5 | 6 | int main(int argc, char **argv) 7 | { 8 | ros::init(argc,argv, "mobile_robot_simulator"); 9 | ros::NodeHandle nh; 10 | 11 | // global rate 12 | float rate = 10.0; 13 | 14 | MobileRobotSimulator mob_sim(&nh); 15 | LaserScannerSimulator laser_sim(&nh); 16 | 17 | ROS_INFO("--- Starting simulator"); 18 | 19 | ros::Duration(1.0).sleep(); 20 | ros::AsyncSpinner spinner(2); 21 | 22 | mob_sim.publish_map_transform = true; 23 | mob_sim.start(); 24 | laser_sim.start(); 25 | 26 | ros::Time tic = ros::Time::now(); 27 | 28 | spinner.start(); 29 | while (nh.ok() && ros::Time::now()-tic 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mobile_robot_simulator 4 | 1.0.0 5 | The mobile_robot_simulator package 6 | 7 | 8 | 9 | 10 | Mikkel Rath Pedersen 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | geometry_msgs 44 | nav_msgs 45 | roscpp 46 | tf 47 | sensor_msgs 48 | geometry_msgs 49 | nav_msgs 50 | roscpp 51 | tf 52 | sensor_msgs 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /include/mobile_robot_simulator/mobile_robot_simulator.h: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "ros/console.h" 3 | 4 | #include "geometry_msgs/Twist.h" 5 | #include "tf/LinearMath/Transform.h" 6 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 7 | #include "nav_msgs/Odometry.h" 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #ifndef MOBILE_ROBOT_SIMULATOR 15 | #define MOBILE_ROBOT_SIMULATOR 16 | 17 | class MobileRobotSimulator { 18 | 19 | public: 20 | 21 | MobileRobotSimulator(ros::NodeHandle *nh); // default constructor 22 | ~MobileRobotSimulator(); // default destructor 23 | 24 | /*! start the simulation loop */ 25 | void start(); // 26 | 27 | /*! stop everything */ 28 | void stop(); 29 | 30 | bool publish_map_transform; // whether or not to publish the map transform 31 | 32 | 33 | private: 34 | 35 | /*! gets parameters from the parameter server */ 36 | void get_params(); 37 | 38 | /*! main update loop */ 39 | void update_loop(const ros::TimerEvent& event); 40 | 41 | /*! update the odometry info based on velocity and duration */ 42 | void update_odom_from_vel(geometry_msgs::Twist vel, ros::Duration time_diff); 43 | 44 | /*! generate transform from odom */ 45 | void get_tf_from_odom(nav_msgs::Odometry odom); 46 | 47 | /*! callback function for velocity */ 48 | void vel_callback(const geometry_msgs::Twist::ConstPtr& msg); 49 | 50 | /*! initial pose callback function */ 51 | void init_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg); 52 | 53 | double publish_rate; 54 | 55 | nav_msgs::Odometry odom; // odometry message 56 | tf::StampedTransform odom_trans; // odometry transform 57 | tf::StampedTransform map_trans; // transformation from odom to map 58 | 59 | ros::Time last_vel; // last incoming velocity command 60 | ros::Time last_update; // last time the odom was published 61 | ros::Time measure_time; // this incoming velocity command 62 | bool message_received = false; 63 | ros::NodeHandle * nh_ptr; 64 | 65 | bool is_running; 66 | 67 | // ROS interfaces 68 | ros::Publisher odom_pub; 69 | ros::Subscriber vel_sub; 70 | ros::Subscriber init_pose_sub; 71 | tf::TransformBroadcaster tf_broadcaster; 72 | 73 | //Topics 74 | std::string velocity_topic; 75 | std::string odometry_topic; 76 | 77 | ros::Timer loop_timer; // timer for the update loop 78 | 79 | double th = 0.0; // current pose (only need yaw, rest is calculated) 80 | 81 | 82 | 83 | }; // end class 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mobile_robot_simulator 2 | A simple ROS simulator for mobile robots. Two nodes are included, that simulate a mobile base and a laser scanner. 3 | This is useful if some high-level simulation is needed. The package is inspired by the industrial_robot_simulator package in ROS Industrial. 4 | 5 | The package includes two nodes, as well as a library for each of the simulators to include in other projects. For most uses, the included launch file should be sufficient -- see description of parameters for each node below. 6 | 7 | ## mobile_robot_simulator_node 8 | Subscribes to incoming velocity commands, and updates odometry based on this. Also (optionally) publishes localization on tf. 9 | 10 | ### Publications 11 | - `/odom` (nav_msgs/Odometry) - odometry of the mobile robot, calculated based on the incoming velocity commands 12 | - `/tf` - publishes 2 transforms: /odom -> /base_link and /map -> /odom (last transform optional) 13 | 14 | ### Subscriptions 15 | - `/cmd_vel` (geometry_msgs/Twist) - velocity commands 16 | - `/initialpose` (geometry_msgs/PoseWithCovarianceStamped) - (optional) current pose estimate of the robot with respect to the /map frame 17 | 18 | ### Parameters 19 | - `publish_map_transform` - whether to publish the /map -> /odom transform. Default value: `false` 20 | - `publish_rate` - rate of the simulations loop -- and thus the published topics. Default value: `10.0` 21 | - `velocity_topic` - topic for subscribed velocity commands. Default value: `/cmd_vel` 22 | - `odometry_topic` - topic for publishing odometry messages. Default value: `/odom` 23 | 24 | 25 | ## laser_scanner_simulator_node 26 | Does raytracing on an available occupancy grid map, using [1]. Optionally includes the noise model from [2] and applies it on the ranges. Default laser parameters are those of a Sick S300 laser range scanner. 27 | 28 | ### Publications 29 | - `/scan` (sensor_msgs/LaserScan) - simulated laser scan, constructed by raytracing the occupancy grid map 30 | 31 | ### Called services 32 | - `/static_map` (nav_msgs/GetMap) - service that returns the occupancy grid map (as provided by the `map_server` package) 33 | 34 | ### Parameters 35 | - `laser_topic` - topic for publishing the laser scan. Default value: `/scan` 36 | - `map_service` - service to get the map from. Default value: `/static_map` 37 | 38 | Laser scanner parameters: 39 | 40 | - `laser_frame_id` - frame_id of the laser scanner. Default value: `/base_link` 41 | - `laser_fov` - field of view of the laser scanner in radians. Default value: `4.71` (270 degrees) 42 | - `laser_beam_count` - number of beams per laser scan . Default value: `541` 43 | - `laser_max_range` - maximum range of the laser scanner in meters. Default value: `30.0` 44 | - `laser_min_range` - minimum range of the laser scanner in meters. Default value: `0.05` 45 | - `laser_frequency` - frequency of the laser scanner (and thus of the publisher). Default value: `10.0` 46 | 47 | Noise model parameters (see [2]): 48 | 49 | - `apply_noise` - whether to apply the noise model. Default value: `true` 50 | - `sigma_hit` - standard deviation for gaussian noise on good readings. Default value: `0.005` 51 | - `lambda_short` - lambda coefficient for exponential distribution that determines short readings. Default value: `2` 52 | 53 | Noise model mixing coefficients. Ideally, these should be normalized, if not the node will normalize before applying the parameters: 54 | - `z_hit` - coefficient for good readings. Default value: `0.995` 55 | - `z_short` - coefficient for short readings. Default value: `0.0` 56 | - `z_max` - coefficient for readings with maximum range. Default value: `0.005` 57 | - `z_rand` - coefficient for "phantom", random, readings. Default value: `0.0` 58 | 59 | ## References 60 | 61 | 1. J. Amanatides, A. Woo, "A Fast Voxel Traversal Algorithm for Ray Tracing", University of Toronto, 2010 62 | 2. D. Fox, S. Thrun, W. Burgard, "Probabilistic Robotics", MIT Press, 2006, ISBN: 978-0262201629 (noise model is in chapter 6) 63 | -------------------------------------------------------------------------------- /include/mobile_robot_simulator/laser_simulator.h: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "ros/console.h" 3 | 4 | #include "nav_msgs/OccupancyGrid.h" 5 | #include "nav_msgs/GetMap.h" 6 | #include "sensor_msgs/LaserScan.h" 7 | 8 | #include "tf/transform_datatypes.h" 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #ifndef LASER_SIMULATOR 15 | #define LASER_SIMULATOR 16 | 17 | class LaserScannerSimulator { 18 | 19 | public: 20 | 21 | LaserScannerSimulator(ros::NodeHandle *nh); 22 | ~LaserScannerSimulator(); 23 | 24 | /*! updates the laser scanner parameters */ 25 | void set_laser_params(std::string frame_id, double fov, unsigned int beam_count, double max_range, double min_range, double l_frequency); 26 | 27 | /*! updates the noise model parameters */ 28 | void set_noise_params(bool use_model, double sigma_hit_reading, double lambda_short_reading, double z_hit, double z_short, double z_max, double z_rand); 29 | 30 | /*! start the simulation loop */ 31 | void start(); // 32 | 33 | /*! stop everything */ 34 | void stop(); 35 | 36 | 37 | private: 38 | 39 | void update_loop(const ros::TimerEvent& event); 40 | 41 | /*! gets the current map */ 42 | void get_map(); 43 | 44 | /*! gets parameters from the parameter server */ 45 | void get_params(); 46 | 47 | /*! finds the pose of the laser in the map frame */ 48 | void get_laser_pose(double * x, double * y, double * theta); 49 | 50 | /*! updates the laser scan based on current 2D pose of the scanner in map coordinates */ 51 | void update_scan(double x, double y, double theta); 52 | 53 | /*! raytracing, calculates intersection with the map for a single ray */ 54 | double find_map_range(double x, double y, double theta); 55 | 56 | /*! applies sensor noise to the calculated range */ 57 | double apply_range_noise(double range_reading); 58 | 59 | /*! get map cell corresponding to real-world coordinates */ 60 | void get_world2map_coordinates(double x, double y, int * map_x, int * map_y); 61 | 62 | /*! get real-world coordinates of map cell */ 63 | void get_map2world_coordinates(int map_x, int map_y, double * x, double * y); 64 | 65 | /*! get occupancy of specified cell */ 66 | int get_map_occupancy(int x, int y); 67 | 68 | 69 | ros::NodeHandle * nh_ptr; 70 | ros::Publisher laser_pub; // scan publisher 71 | tf::TransformListener tl; 72 | 73 | ros::Timer loop_timer; // timer for the update loop 74 | bool is_running; 75 | 76 | // map 77 | std::string map_service; 78 | nav_msgs::OccupancyGrid map; //map data 79 | bool have_map; 80 | 81 | std::string l_scan_topic; 82 | 83 | // laser parameters 84 | std::string l_frame; 85 | double l_fov; // field of view, centered at pose of laser 86 | int l_beams; // number of beams per scan 87 | double l_max_range; // max range of the laser scan 88 | double l_min_range; // min range of the laser scan 89 | double l_frequency; // frequency of laser scans 90 | tf::StampedTransform rob_laser_tf; // transform from robot to laser (assumed static) 91 | 92 | // noise model parameters (see Probabilistic Robotics eq. 6.12) 93 | bool use_noise_model; 94 | double sigma_hit; // stddev of measurement noise 95 | double lambda_short; // intrinsic parameter for short readings (1/mu in exp pdf) 96 | double z_mix[4]; // mixing coefficients of the noise model - [z_hit, z_short, z_max, z_rand] 97 | // noise model distributions and generators 98 | std::default_random_engine rand_gen; // generator 99 | std::uniform_real_distribution selector; // selector for which noise distribution to draw from 100 | std::normal_distribution p_hit; // gaussian noise on detection 101 | std::exponential_distribution p_short; // short readings 102 | std::uniform_real_distribution p_rand; // random, "phantom" readings 103 | 104 | // output 105 | sensor_msgs::LaserScan output_scan; 106 | 107 | }; // end class 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /src/mobile_robot_simulator.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | 3 | #include "mobile_robot_simulator/mobile_robot_simulator.h" 4 | 5 | MobileRobotSimulator::MobileRobotSimulator(ros::NodeHandle *nh) 6 | { 7 | nh_ptr = nh; 8 | // get parameters 9 | get_params(); 10 | odom_pub = nh_ptr->advertise(odometry_topic,50); // odometry publisher 11 | vel_sub = nh_ptr->subscribe(velocity_topic,5,&MobileRobotSimulator::vel_callback,this); // velocity subscriber 12 | 13 | // initialize timers 14 | last_update = ros::Time::now(); 15 | last_vel = last_update - ros::Duration(0.1); 16 | // initialize forst odom message 17 | update_odom_from_vel(geometry_msgs::Twist(), ros::Duration(0.1)); 18 | odom.header.stamp = last_update; 19 | get_tf_from_odom(odom); 20 | // Initialize tf from map to odom 21 | if (publish_map_transform) 22 | { 23 | init_pose_sub = nh_ptr->subscribe("/initialpose",5,&MobileRobotSimulator::init_pose_callback,this); // initial pose callback 24 | map_trans.frame_id_ = "/map"; 25 | map_trans.stamp_ = last_update; 26 | map_trans.child_frame_id_ = "/odom"; 27 | map_trans.setIdentity(); 28 | } 29 | 30 | ROS_INFO("Initialized mobile robot simulator"); 31 | 32 | } 33 | 34 | MobileRobotSimulator::~MobileRobotSimulator() 35 | { 36 | if (is_running) stop(); 37 | } 38 | 39 | void MobileRobotSimulator::get_params() 40 | { 41 | nh_ptr->param("publish_map_transform", publish_map_transform , false); 42 | nh_ptr->param("publish_rate", publish_rate, 10.0); 43 | nh_ptr->param("velocity_topic", velocity_topic, "/cmd_vel"); 44 | nh_ptr->param("odometry_topic", odometry_topic, "/odom"); 45 | } 46 | 47 | 48 | void MobileRobotSimulator::start() 49 | { 50 | loop_timer = nh_ptr->createTimer(ros::Duration(1.0/publish_rate),&MobileRobotSimulator::update_loop, this); 51 | loop_timer.start(); // should not be necessary 52 | is_running = true; 53 | ROS_INFO("Started mobile robot simulator update loop, listening on cmd_vel topic"); 54 | } 55 | 56 | void MobileRobotSimulator::stop() 57 | { 58 | loop_timer.stop(); 59 | is_running = false; 60 | ROS_INFO("Stopped mobile robot simulator"); 61 | } 62 | 63 | void MobileRobotSimulator::update_loop(const ros::TimerEvent& event) 64 | { 65 | last_update = event.current_real; 66 | // If we didn't receive a message, send the old odometry info with a new timestamp 67 | if (!message_received) 68 | { 69 | odom.header.stamp = last_update; 70 | odom_trans.stamp_ = last_update; 71 | } 72 | // publish odometry and tf 73 | odom_pub.publish(odom); 74 | get_tf_from_odom(odom); 75 | tf_broadcaster.sendTransform(odom_trans); // odom -> base_link 76 | message_received = false; 77 | // should we publish the map transform? 78 | if (!publish_map_transform) return; 79 | map_trans.stamp_ = last_update; 80 | tf_broadcaster.sendTransform(map_trans); // map -> odom 81 | } 82 | 83 | void MobileRobotSimulator::update_odom_from_vel(geometry_msgs::Twist vel, ros::Duration time_diff) 84 | { 85 | ROS_DEBUG_STREAM("Velocity - x: " << vel.linear.x << " y: " << vel.linear.y << " th: " << vel.angular.z); 86 | //compute odometry in a typical way given the velocities of the robot 87 | double delta_x = (vel.linear.x * cos(th) - vel.linear.y * sin(th)) * time_diff.toSec(); 88 | double delta_y = (vel.linear.x * sin(th) + vel.linear.y * cos(th)) * time_diff.toSec(); 89 | double delta_th = vel.angular.z * time_diff.toSec(); 90 | ROS_DEBUG_STREAM("Delta - x: " << delta_x << " y: " << delta_y << " th: " << delta_th); 91 | 92 | // update odometry 93 | odom.header.stamp = measure_time; 94 | odom.header.frame_id = "odom"; 95 | odom.pose.pose.position.x += delta_x; 96 | odom.pose.pose.position.y += delta_y; 97 | // generate quaternion based on current yaw 98 | th += delta_th; 99 | odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(th); 100 | // set velocity 101 | odom.child_frame_id = "base_link"; 102 | odom.twist.twist = vel; 103 | ROS_DEBUG_STREAM("Odometry - x: " << odom.pose.pose.position.x << " y: " << odom.pose.pose.position.y << " th: " << th); 104 | } 105 | 106 | void MobileRobotSimulator::get_tf_from_odom(nav_msgs::Odometry odom) 107 | { 108 | geometry_msgs::TransformStamped odom_tmp; 109 | // copy from odmoetry message 110 | odom_tmp.header = odom.header; 111 | odom_tmp.child_frame_id = odom.child_frame_id; 112 | odom_tmp.transform.translation.x = odom.pose.pose.position.x; 113 | odom_tmp.transform.translation.y = odom.pose.pose.position.y; 114 | odom_tmp.transform.translation.z = 0.0; 115 | odom_tmp.transform.rotation = odom.pose.pose.orientation; 116 | // convert and update 117 | tf::transformStampedMsgToTF(odom_tmp, odom_trans); 118 | } 119 | 120 | void MobileRobotSimulator::vel_callback(const geometry_msgs::Twist::ConstPtr& msg) 121 | { 122 | ROS_DEBUG("Received message on cmd_vel"); 123 | measure_time = ros::Time::now(); 124 | ros::Duration dt = measure_time - last_vel; 125 | last_vel = measure_time; 126 | if (dt >= ros::Duration(0.5)) dt = ros::Duration(0.1); 127 | message_received = true; 128 | geometry_msgs::Twist vel = *msg; 129 | update_odom_from_vel(vel,dt); 130 | } 131 | 132 | void MobileRobotSimulator::init_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) 133 | { 134 | if (msg->header.frame_id != "map") { 135 | ROS_ERROR("Initial pose not specified in map frame, ignoring"); 136 | return; 137 | } 138 | ROS_INFO("Received pose estimate of mobile base"); 139 | 140 | // msg is map -> base_link 141 | tf::StampedTransform msg_t; 142 | msg_t.setOrigin(tf::Vector3(msg->pose.pose.position.x,msg->pose.pose.position.y,msg->pose.pose.position.z)); 143 | msg_t.setRotation(tf::Quaternion(msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z,msg->pose.pose.orientation.w)); 144 | ROS_DEBUG_STREAM("map -> base_link - x: " << msg_t.getOrigin().getX() << " y: " << msg_t.getOrigin().getY()); 145 | // get odom -> base_link 146 | ROS_DEBUG_STREAM("odom -> base_link - x: " << odom_trans.getOrigin().getX() << " y: " << odom_trans.getOrigin().getY()); 147 | // calculate map -> odom and save as stamped 148 | tf::StampedTransform map_t = tf::StampedTransform(msg_t * odom_trans.inverse(), msg->header.stamp, "map", "odom"); 149 | ROS_DEBUG_STREAM("map -> odom - x: " << map_t.getOrigin().getX() << " y: " << map_t.getOrigin().getY()); 150 | map_trans = map_t; 151 | } 152 | 153 | 154 | 155 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mobile_robot_simulator) 3 | 4 | # using c++11 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | geometry_msgs 13 | nav_msgs 14 | tf 15 | sensor_msgs 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a run_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # geometry_msgs# nav_msgs 76 | # ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if you package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | INCLUDE_DIRS include 109 | # LIBRARIES mobile_robot_simulator 110 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp tf sensor_msgs 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories(include) 121 | include_directories( 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Declare a C++ library 126 | add_library(mobile_robot_simulator 127 | src/mobile_robot_simulator.cpp 128 | ) 129 | add_library(laser_simulator 130 | src/laser_simulator.cpp 131 | ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | add_dependencies(mobile_robot_simulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | add_dependencies(laser_simulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 138 | 139 | ## Declare a C++ executable 140 | add_executable(mobile_robot_simulator_node src/mobile_robot_simulator_node.cpp) 141 | add_executable(laser_scanner_simulator_node src/laser_simulator_node.cpp) 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | add_dependencies(mobile_robot_simulator_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | add_dependencies(laser_scanner_simulator_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | target_link_libraries(mobile_robot_simulator ${catkin_LIBRARIES}) 150 | target_link_libraries(laser_simulator ${catkin_LIBRARIES}) 151 | 152 | 153 | target_link_libraries(mobile_robot_simulator_node 154 | ${catkin_LIBRARIES} mobile_robot_simulator laser_simulator 155 | ) 156 | target_link_libraries(laser_scanner_simulator_node 157 | ${catkin_LIBRARIES} laser_simulator mobile_robot_simulator 158 | ) 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # install(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables and/or libraries for installation 175 | # install(TARGETS mobile_robot_simulator mobile_robot_simulator_node 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mobile_robot_simulator.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /src/laser_simulator.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | 3 | #include "mobile_robot_simulator/laser_simulator.h" 4 | 5 | LaserScannerSimulator::LaserScannerSimulator(ros::NodeHandle *nh) 6 | { 7 | nh_ptr = nh; 8 | // get parameters 9 | get_params(); 10 | laser_pub = nh_ptr->advertise(l_scan_topic,10); // scan publisher 11 | // get map 12 | get_map(); 13 | ROS_INFO("Initialized laser scanner simulator"); 14 | } 15 | 16 | LaserScannerSimulator::~LaserScannerSimulator() 17 | { 18 | if (is_running) stop(); 19 | } 20 | 21 | void LaserScannerSimulator::get_params() 22 | { 23 | nh_ptr->param("laser_topic", l_scan_topic, "scan"); 24 | nh_ptr->param("map_service", map_service, "static_map"); 25 | //laser parameters - defaults are appriximately that of a Sick S300 26 | nh_ptr->param("laser_frame_id", l_frame, "base_link"); 27 | nh_ptr->param("laser_fov", l_fov, 1.5*M_PI); 28 | nh_ptr->param("laser_beam_count", l_beams, 541); 29 | nh_ptr->param("laser_max_range", l_max_range, 30.0); 30 | nh_ptr->param("laser_min_range", l_min_range, 0.05); 31 | nh_ptr->param("laser_frequency", l_frequency, 10.0); 32 | // noise model parameters (see Probabilistic Robotics eq. 6.12) 33 | nh_ptr->param("apply_noise", use_noise_model, true); 34 | nh_ptr->param("sigma_hit", sigma_hit, 0.005); 35 | nh_ptr->param("lambda_short", lambda_short, 2.0); 36 | nh_ptr->param("z_hit", z_mix[0], 0.995); 37 | nh_ptr->param("z_short", z_mix[1], 0.0); 38 | nh_ptr->param("z_max", z_mix[2], 0.005); 39 | nh_ptr->param("z_rand", z_mix[3], 0.0); 40 | // update the noise model internally 41 | set_noise_params(use_noise_model, sigma_hit, lambda_short, z_mix[0], z_mix[1], z_mix[2], z_mix[3]); 42 | } 43 | 44 | void LaserScannerSimulator::start() 45 | { 46 | loop_timer = nh_ptr->createTimer(ros::Duration(1.0/l_frequency),&LaserScannerSimulator::update_loop, this); 47 | loop_timer.start(); // should not be necessary 48 | is_running = true; 49 | ROS_INFO("Started laser scanner simulator update loop"); 50 | } 51 | 52 | void LaserScannerSimulator::stop() 53 | { 54 | loop_timer.stop(); 55 | is_running = false; 56 | ROS_INFO("Stopped laser scanner simulator"); 57 | } 58 | 59 | void LaserScannerSimulator::update_loop(const ros::TimerEvent& event) 60 | { 61 | // If we don't have a map, try to get one 62 | if (!have_map) get_map(); 63 | // first, get the pose of the laser in the map frame 64 | double l_x, l_y, l_theta; 65 | get_laser_pose(&l_x,&l_y,&l_theta); 66 | //ROS_INFO_STREAM_THROTTLE(2,"x: " << l_x << " y: " << l_y << " theta: " << l_theta); 67 | update_scan(l_x,l_y,l_theta); 68 | output_scan.header.stamp = event.current_real; 69 | laser_pub.publish(output_scan); 70 | } 71 | 72 | void LaserScannerSimulator::get_map() 73 | { 74 | nav_msgs::GetMapRequest req; 75 | nav_msgs::GetMapResponse resp; 76 | if (ros::service::call(map_service, req, resp)) 77 | { 78 | map = resp.map; 79 | ROS_INFO_STREAM("Got a " << map.info.width << "x" << map.info.height << " map with resolution " << map.info.resolution); 80 | have_map = true; 81 | } 82 | else 83 | { 84 | ROS_WARN_THROTTLE(10,"No map received - service '/static_map' not available (will publish only max_range)"); 85 | have_map = false; 86 | } 87 | } 88 | 89 | void LaserScannerSimulator::set_laser_params(std::string frame_id, double fov, unsigned int beam_count, double max_range, double min_range, double update_frequency) 90 | { 91 | l_frame = frame_id; 92 | l_fov = fov; 93 | l_beams = beam_count; 94 | l_max_range = max_range; 95 | l_min_range = min_range; 96 | l_frequency = update_frequency; 97 | ROS_INFO("Updated parameters of simulated laser"); 98 | } 99 | 100 | void LaserScannerSimulator::get_laser_pose(double * x, double * y, double * theta) 101 | { 102 | ros::Time now = ros::Time::now(); 103 | tf::StampedTransform transf; 104 | try 105 | { 106 | tl.waitForTransform("/map",l_frame,now,ros::Duration(1.0)); 107 | tl.lookupTransform("/map",l_frame,now,transf); 108 | } 109 | catch (tf::TransformException ex) 110 | { 111 | ROS_ERROR("%s",ex.what()); 112 | *x = 0.0; *y = 0.0, *theta * 0.0; 113 | return; 114 | } 115 | *x = transf.getOrigin().getX(); 116 | *y = transf.getOrigin().getY(); 117 | *theta = tf::getYaw(transf.getRotation()); 118 | } 119 | 120 | void LaserScannerSimulator::update_scan(double x, double y, double theta) 121 | { 122 | //timing 123 | //ros::Time start = ros::Time::now(); 124 | // laser params 125 | output_scan.angle_min = -l_fov/2.0; 126 | output_scan.angle_max = l_fov/2.0; 127 | output_scan.angle_increment = l_fov/l_beams; 128 | output_scan.range_min = l_min_range; 129 | output_scan.range_max = l_max_range+0.001; 130 | output_scan.time_increment = (1.0/l_frequency) / l_beams; 131 | output_scan.scan_time = (1.0/l_frequency); 132 | 133 | std::vector ranges; 134 | double this_range; 135 | double this_ang; 136 | // header 137 | output_scan.header.frame_id = l_frame; 138 | output_scan.header.stamp = ros::Time::now(); 139 | 140 | for (unsigned int i=0; i<=l_beams; i++) 141 | { 142 | if (!have_map) 143 | { 144 | ranges.push_back((float)l_max_range); 145 | continue; 146 | } 147 | this_ang = theta + output_scan.angle_min + i*output_scan.angle_increment; 148 | this_range = find_map_range(x,y,this_ang); 149 | //ROS_INFO_STREAM_THROTTLE(1,"x: " << x << " y: " << y << " theta: " << this_ang); 150 | ranges.push_back(this_range); 151 | } 152 | output_scan.ranges = ranges; 153 | //ROS_INFO_STREAM_THROTTLE(2, "Simulated scan in " << (ros::Time::now()-start).toSec() << "s"); 154 | } 155 | 156 | double LaserScannerSimulator::find_map_range(double x, double y, double theta) 157 | { 158 | // using "A Faster Voxel Traversal Algorithm for Ray Tracing" by Amanatides & Woo 159 | // ======== initialization phase ======== 160 | //ROS_INFO_STREAM_THROTTLE(1,"x,y,theta " << x << ", " << y << ", " << theta); 161 | double origin[2]; // u 162 | origin[0] = x; 163 | origin[1] = y; 164 | //ROS_INFO_STREAM_THROTTLE(1,"origin " << origin[0] << ", " << origin[1]); 165 | double dir[2]; // v 166 | dir[0] = cos(theta); 167 | dir[1] = sin(theta); 168 | //ROS_INFO_STREAM_THROTTLE(1,"dir " << dir[0] << ", " << dir[1]); 169 | int start_x, start_y; 170 | get_world2map_coordinates(x,y,&start_x,&start_y); 171 | //ROS_INFO_STREAM_THROTTLE(1,"start " << start_x << ", " << start_y); 172 | if (start_x<0 || start_y<0 || start_x >= map.info.width || start_y >= map.info.height) 173 | { 174 | //outside the map, find entry point 175 | double delta_x = abs(map.info.origin.position.x - x); 176 | double delta_y = abs(map.info.origin.position.y - y); 177 | double intersect_x = x + (dir[0] * delta_x); 178 | double intersect_y = y + (dir[1] * delta_y); 179 | get_world2map_coordinates(intersect_x,intersect_y,&start_x,&start_y); 180 | } 181 | int current[2]; // X, Y 182 | current[0] = start_x; 183 | current[1] = start_y; 184 | //ROS_INFO_STREAM_THROTTLE(1,"current " << current[0] << ", " << current[1]); 185 | 186 | int step[2]; // stepX, stepY 187 | double tMax[2]; // tMaxX, tMaxY 188 | double tDelta[2]; // tDeltaX, tDeltaY 189 | 190 | double voxel_border[2]; 191 | get_map2world_coordinates(current[0], current[1], &voxel_border[0], &voxel_border[1]); 192 | voxel_border[0] -= 0.5 * map.info.resolution; //this is the lower left voxel corner 193 | voxel_border[1] -= 0.5 * map.info.resolution; //this is the lower left voxel corner 194 | //ROS_INFO_STREAM_THROTTLE(1,"voxel_border " << voxel_border[0] << ", " << voxel_border[1]); 195 | 196 | for (unsigned int i=0;i<2;i++) // for each dimension (x,y) 197 | { 198 | // determine step direction 199 | if (dir[i] > 0.0) step[i] = 1; 200 | else if (dir[i] < 0.0) step[i] = -1; 201 | else step[i] = 0; 202 | 203 | // determine tMax, tDelta 204 | if (step[i] != 0) 205 | { 206 | // corner point of voxel (in direction of ray) 207 | if (step[i] == 1) 208 | { 209 | voxel_border[i] += (float) (step[i] * map.info.resolution); 210 | } 211 | // tMax - voxel boundary crossing 212 | tMax[i] = (voxel_border[i] - origin[i]) / dir[i]; 213 | // tDelta - distance along ray so that veritcal/horizontal component equals one voxel 214 | tDelta[i] = map.info.resolution / fabs(dir[i]); 215 | } 216 | else 217 | { 218 | tMax[i] = std::numeric_limits::max(); 219 | tDelta[i] = std::numeric_limits::max(); 220 | } 221 | 222 | } 223 | //ROS_INFO_STREAM_THROTTLE(1,"step " << step[0] << ", " << step[1]); 224 | //ROS_INFO_STREAM_THROTTLE(1,"tMax " << tMax[0] << ", " << tMax[1]); 225 | //ROS_INFO_STREAM_THROTTLE(1,"tDelta " << tDelta[0] << ", " << tDelta[1]); 226 | 227 | //ROS_DEBUG_STREAM("Starting at index " << start_x << "," << start_y); 228 | 229 | // ======== incremental traversal ======== 230 | while (true) 231 | { 232 | // advance 233 | unsigned int dim; // X or Y direction 234 | if (tMax[0] < tMax[1]) dim = 0; 235 | else dim = 1; 236 | // advance one voxel 237 | current[dim] += step[dim]; 238 | tMax[dim] += tDelta[dim]; 239 | //ROS_DEBUG_STREAM("Examining index " << current[0] << "," << current[1]); 240 | // are we outside the map? 241 | if (current[0] < 0 || current[0] >= map.info.width || current[1] < 0 || current[1] >= map.info.height) 242 | { 243 | return l_max_range; 244 | } 245 | // determine current range 246 | double current_range = sqrt(pow((current[0] - start_x),2) + pow((current[1] - start_y),2)) * map.info.resolution; 247 | // are we at max range? 248 | if (current_range > l_max_range) return l_max_range; 249 | else { 250 | int occ = get_map_occupancy(current[0],current[1]); 251 | if (occ >= 60) // current cell is occupied 252 | { 253 | // are we below the minimum range of the laser scanner? 254 | if (current_range < l_min_range) continue; 255 | // if not, return the current range, with noise (optionally) applied 256 | if (use_noise_model) return apply_range_noise(current_range); 257 | else return current_range; 258 | } 259 | } 260 | } // end while 261 | } 262 | 263 | double LaserScannerSimulator::apply_range_noise(double range_reading) 264 | { 265 | // using Measurement model of laser range scanner, following method from chapter 6.3.1 of Probabilistic Robotics 266 | double noise_modifier = selector(rand_gen); 267 | // hit: gaussian noise on reading 268 | if (noise_modifier < z_mix[0]) return range_reading + p_hit(rand_gen); 269 | // short: short readings, exponential function 270 | else if (noise_modifier < z_mix[0] + z_mix[1]) return p_short(rand_gen); 271 | // rand: random reading, uniform distribution 272 | else if (noise_modifier < z_mix[0] + z_mix[1] + z_mix[3]) return p_rand(rand_gen); 273 | // max: no detection, max reading 274 | else return l_max_range; 275 | } 276 | 277 | void LaserScannerSimulator::set_noise_params(bool use_model, double sigma_hit_reading, double lambda_short_reading, double z_hit, double z_short, double z_max, double z_rand) 278 | { 279 | use_noise_model = use_model; 280 | sigma_hit = sigma_hit_reading; 281 | lambda_short = lambda_short_reading; 282 | z_mix[0] = z_hit; 283 | z_mix[1] = z_short; 284 | z_mix[2] = z_max; 285 | z_mix[3] = z_rand; 286 | // check that weights are normalized 287 | double z_sum = z_mix[0]+z_mix[1]+z_mix[2]+z_mix[3]; 288 | if (z_sum != 1) 289 | { 290 | ROS_WARN_STREAM("Noise model weighting sums not normalized (sum is " << z_sum << "), normalizing them"); 291 | z_mix[0] = z_mix[0]/z_sum; 292 | z_mix[1] = z_mix[1]/z_sum; 293 | z_mix[2] = z_mix[2]/z_sum; 294 | z_mix[3] = z_mix[3]/z_sum; 295 | ROS_WARN_STREAM("After normalization - z_hit " << z_mix[0] << ", z_short " << z_mix[1] << ", z_max " << z_mix[2] << ", z_rand " << z_mix[3]); 296 | 297 | } 298 | // reset distributions 299 | p_hit = std::normal_distribution(0.0,sigma_hit); 300 | p_short = std::exponential_distribution(lambda_short); 301 | p_rand = std::uniform_real_distribution(0.0,l_max_range); 302 | selector = std::uniform_real_distribution(0.0,1.0); 303 | } 304 | 305 | void LaserScannerSimulator::get_world2map_coordinates(double world_x, double world_y, int * map_x, int * map_y) 306 | { 307 | *map_x = (int) (std::floor((world_x - map.info.origin.position.x) / map.info.resolution)); 308 | *map_y = (int) (std::floor((world_y - map.info.origin.position.y) / map.info.resolution)); 309 | //ROS_INFO_STREAM_THROTTLE(1, "world2map - x: " << world_x << " map_x: " << *map_x); 310 | } 311 | 312 | void LaserScannerSimulator::get_map2world_coordinates(int map_x, int map_y, double * world_x, double * world_y) 313 | { 314 | *world_x = (map_x * map.info.resolution) + map.info.origin.position.x + 0.5*map.info.resolution; 315 | *world_y = (map_y * map.info.resolution) + map.info.origin.position.y + 0.5*map.info.resolution; 316 | //ROS_INFO_STREAM_THROTTLE(1, "map2world - map_x: " << map_x << " x: " << *world_x); 317 | } 318 | 319 | int LaserScannerSimulator::get_map_occupancy(int x, int y) 320 | { 321 | //ROS_DEBUG_STREAM("x: " << x << " y: " << y << " index: " << y*map.info.width + x); 322 | return map.data[y*map.info.width + x]; 323 | } 324 | 325 | --------------------------------------------------------------------------------