Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <costmap_2d/costmap_2d_publisher.h>
00038 #include <geometry_msgs/PolygonStamped.h>
00039
00040 namespace costmap_2d {
00041 Costmap2DPublisher::Costmap2DPublisher(ros::NodeHandle ros_node, double publish_frequency, std::string global_frame)
00042 : global_frame_(global_frame), visualizer_thread_(NULL), active_(false), new_data_(false), resolution_(0.0), visualizer_thread_shutdown_(false){
00043
00044 obs_pub_ = ros_node.advertise<nav_msgs::GridCells>("obstacles", 1);
00045 inf_obs_pub_ = ros_node.advertise<nav_msgs::GridCells>("inflated_obstacles", 1);
00046 unknown_space_pub_ = ros_node.advertise<nav_msgs::GridCells>("unknown_space", 1);
00047 footprint_pub_ = ros_node.advertise<geometry_msgs::PolygonStamped>("robot_footprint", 1);
00048
00049 visualizer_thread_ = new boost::thread(boost::bind(&Costmap2DPublisher::mapPublishLoop, this, publish_frequency));
00050 }
00051
00052 Costmap2DPublisher::~Costmap2DPublisher(){
00053 visualizer_thread_shutdown_ = true;
00054 if(visualizer_thread_ != NULL){
00055 visualizer_thread_->join();
00056 delete visualizer_thread_;
00057 }
00058 }
00059
00060 void Costmap2DPublisher::mapPublishLoop(double frequency){
00061
00062 if(frequency == 0.0)
00063 return;
00064
00065 active_ = true;
00066 ros::NodeHandle n;
00067 ros::Rate r(frequency);
00068 while(n.ok() && !visualizer_thread_shutdown_){
00069 ROS_DEBUG("In publish loop new data is: %d", new_data_);
00070
00071 if(new_data_){
00072
00073 ROS_DEBUG("Publishing costmap");
00074 publishCostmap();
00075 new_data_ = false;
00076 ROS_DEBUG("Finished publishing map and set new_data_ to: %d", new_data_);
00077 }
00078
00079 r.sleep();
00080
00081 if(r.cycleTime() > ros::Duration(1 / frequency))
00082 ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency, r.cycleTime().toSec());
00083 }
00084 }
00085
00086 void Costmap2DPublisher::updateCostmapData(const Costmap2D& costmap, const std::vector<geometry_msgs::Point>& footprint, const tf::Stamped<tf::Pose>& global_pose){
00087 std::vector< std::pair<double, double> > raw_obstacles, inflated_obstacles, unknown_space;
00088 for(unsigned int i = 0; i < costmap.getSizeInCellsX(); i++){
00089 for(unsigned int j = 0; j < costmap.getSizeInCellsY(); j++){
00090 double wx, wy;
00091 costmap.mapToWorld(i, j, wx, wy);
00092 std::pair<double, double> p(wx, wy);
00093
00094
00095 if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
00096 raw_obstacles.push_back(p);
00097 else if(costmap.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
00098 inflated_obstacles.push_back(p);
00099 else if(costmap.getCost(i, j) == costmap_2d::NO_INFORMATION)
00100 unknown_space.push_back(p);
00101 }
00102 }
00103 lock_.lock();
00104 resolution_ = costmap.getResolution();
00105 raw_obstacles_ = raw_obstacles;
00106 inflated_obstacles_ = inflated_obstacles;
00107 unknown_space_ = unknown_space;
00108 inscribed_radius_ = costmap.getInscribedRadius();
00109 footprint_ = footprint;
00110 global_pose_ = global_pose;
00111
00112
00113 new_data_ = true;
00114 ROS_DEBUG("Set new_data_ to: %d", new_data_);
00115
00116 lock_.unlock();
00117
00118
00119 publishFootprint();
00120 }
00121
00122 void Costmap2DPublisher::publishFootprint(){
00123 std::vector<geometry_msgs::Point> footprint;
00124
00125 lock_.lock();
00126 footprint = footprint_;
00127 lock_.unlock();
00128
00129
00130 geometry_msgs::PolygonStamped footprint_poly;
00131 footprint_poly.header.frame_id = global_frame_;
00132 footprint_poly.header.stamp = ros::Time::now();
00133
00134
00135 if(footprint.size() < 3){
00136 double angle = 0;
00137 double step = 2 * M_PI / 72;
00138 while(angle < 2 * M_PI){
00139 geometry_msgs::Point32 pt;
00140 pt.x = inscribed_radius_ * cos(angle) + global_pose_.getOrigin().x();
00141 pt.y = inscribed_radius_ * sin(angle) + global_pose_.getOrigin().y();
00142 pt.z = 0.0;
00143 footprint_poly.polygon.points.push_back(pt);
00144 angle += step;
00145 }
00146 }
00147 else{
00148 footprint_poly.polygon.points.resize(footprint.size());
00149
00150 for(unsigned int i = 0; i < footprint.size(); ++i){
00151 footprint_poly.polygon.points[i].x = footprint[i].x;
00152 footprint_poly.polygon.points[i].y = footprint[i].y;
00153 footprint_poly.polygon.points[i].z = footprint[i].z;
00154 }
00155 }
00156
00157 ROS_DEBUG("Publishing footprint");
00158 footprint_pub_.publish(footprint_poly);
00159 }
00160
00161 void Costmap2DPublisher::publishCostmap(){
00162 std::vector< std::pair<double, double> > raw_obstacles, inflated_obstacles, unknown_space;
00163 double resolution;
00164
00165 lock_.lock();
00166 raw_obstacles = raw_obstacles_;
00167 inflated_obstacles = inflated_obstacles_;
00168 unknown_space = unknown_space_;
00169 resolution = resolution_;
00170 lock_.unlock();
00171
00172 unsigned int point_count = raw_obstacles.size();
00173
00174
00175 nav_msgs::GridCells obstacle_cells;
00176 obstacle_cells.header.frame_id = global_frame_;
00177 obstacle_cells.header.stamp = ros::Time::now();
00178
00179
00180 obstacle_cells.cell_width = resolution;
00181 obstacle_cells.cell_height = resolution;
00182
00183
00184 obstacle_cells.cells.resize(point_count);
00185
00186 for(unsigned int i=0;i<point_count;i++){
00187 obstacle_cells.cells[i].x = raw_obstacles[i].first;
00188 obstacle_cells.cells[i].y = raw_obstacles[i].second;
00189 obstacle_cells.cells[i].z = 0;
00190 }
00191
00192 ROS_DEBUG("Publishing obstacles");
00193 obs_pub_.publish(obstacle_cells);
00194
00195
00196 point_count = inflated_obstacles.size();
00197 obstacle_cells.cells.resize(point_count);
00198
00199 for(unsigned int i=0;i<point_count;i++){
00200 obstacle_cells.cells[i].x = inflated_obstacles[i].first;
00201 obstacle_cells.cells[i].y = inflated_obstacles[i].second;
00202 obstacle_cells.cells[i].z = 0;
00203 }
00204
00205 ROS_DEBUG("Publishing inflated obstacles");
00206 inf_obs_pub_.publish(obstacle_cells);
00207
00208 point_count = unknown_space.size();
00209 obstacle_cells.cells.resize(point_count);
00210
00211 for(unsigned int i=0;i<point_count;i++){
00212 obstacle_cells.cells[i].x = unknown_space[i].first;
00213 obstacle_cells.cells[i].y = unknown_space[i].second;
00214 obstacle_cells.cells[i].z = 0;
00215 }
00216
00217 ROS_DEBUG("Publishing unknown space");
00218 unknown_space_pub_.publish(obstacle_cells);
00219
00220 }
00221
00222 };