costmap_2d: costmap_2d: costmap_2d_publisher.cpp Source File

00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
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     //the user might not want to run the loop every cycle
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       //check if we have new data to publish
00071       if(new_data_){
00072         //we are about to publish the latest data
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       //make sure to sleep for the remainder of our cycle time
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         //if(costmap.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || costmap.getCost(i, j) == costmap_2d::NO_INFORMATION)
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     //let the publisher know we have new data to publish
00113     new_data_ = true;
00114     ROS_DEBUG("Set new_data_ to: %d", new_data_);
00115 
00116     lock_.unlock();
00117 
00118     //we'll publish the footprint at the rate at which we get data about it since it is so small
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     //create a polygon message for the footprint
00130     geometry_msgs::PolygonStamped footprint_poly;
00131     footprint_poly.header.frame_id = global_frame_;
00132     footprint_poly.header.stamp = ros::Time::now();
00133 
00134     //if the footprint size is less than 3 we'll assume a circular robot
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     //create a GridCells message for the obstacles
00175     nav_msgs::GridCells obstacle_cells;
00176     obstacle_cells.header.frame_id = global_frame_;
00177     obstacle_cells.header.stamp = ros::Time::now();
00178 
00179     //set the width and height appropriately
00180     obstacle_cells.cell_width = resolution;
00181     obstacle_cells.cell_height = resolution;
00182 
00183     //set the size equal to the number of obstacles
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     //set the size equal to the number of inflated obstacles
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 };