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
00038
00039 #ifndef COSTMAP_CONVERTER_INTERFACE_H_
00040 #define COSTMAP_CONVERTER_INTERFACE_H_
00041
00042
00043 #include <ros/ros.h>
00044 #include <ros/callback_queue.h>
00045 #include <boost/thread.hpp>
00046 #include <costmap_2d/costmap_2d.h>
00047 #include <costmap_2d/costmap_2d_ros.h>
00048 #include <geometry_msgs/Polygon.h>
00049 #include <costmap_converter/ObstacleArrayMsg.h>
00050
00051
00052 namespace costmap_converter
00053 {
00055 typedef boost::shared_ptr<ObstacleArrayMsg> ObstacleArrayPtr;
00057 typedef boost::shared_ptr< const ObstacleArrayMsg > ObstacleArrayConstPtr;
00058
00060 typedef boost::shared_ptr< std::vector<geometry_msgs::Polygon> > PolygonContainerPtr;
00062 typedef boost::shared_ptr< const std::vector<geometry_msgs::Polygon> > PolygonContainerConstPtr;
00063
00064
00077 class BaseCostmapToPolygons
00078 {
00079 public:
00080
00085 virtual void initialize(ros::NodeHandle nh) = 0;
00086
00090 virtual ~BaseCostmapToPolygons()
00091 {
00092 stopWorker();
00093 }
00094
00095
00102 virtual void setCostmap2D(costmap_2d::Costmap2D* costmap) = 0;
00103
00109 virtual void updateCostmap2D() = 0;
00110
00114 virtual void compute() = 0;
00115
00125 virtual PolygonContainerConstPtr getPolygons(){return PolygonContainerConstPtr();}
00126
00135 virtual ObstacleArrayConstPtr getObstacles()
00136 {
00137 ObstacleArrayPtr obstacles = boost::make_shared<ObstacleArrayMsg>();
00138 PolygonContainerConstPtr polygons = getPolygons();
00139 if (polygons)
00140 {
00141 for (const geometry_msgs::Polygon& polygon : *polygons)
00142 {
00143 obstacles->obstacles.emplace_back();
00144 obstacles->obstacles.back().polygon = polygon;
00145 }
00146 }
00147 return obstacles;
00148 }
00149
00157 virtual void setOdomTopic(const std::string& odom_topic) {}
00158
00164 virtual bool stackedCostmapConversion() {return false;}
00165
00177 void startWorker(ros::Rate rate, costmap_2d::Costmap2D* costmap, bool spin_thread = false)
00178 {
00179 setCostmap2D(costmap);
00180
00181 if (spin_thread_)
00182 {
00183 {
00184 boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00185 need_to_terminate_ = true;
00186 }
00187 spin_thread_->join();
00188 delete spin_thread_;
00189 }
00190
00191 if (spin_thread)
00192 {
00193 ROS_DEBUG_NAMED("costmap_converter", "Spinning up a thread for the CostmapToPolygons plugin");
00194 need_to_terminate_ = false;
00195 spin_thread_ = new boost::thread(boost::bind(&BaseCostmapToPolygons::spinThread, this));
00196 nh_.setCallbackQueue(&callback_queue_);
00197 }
00198 else
00199 {
00200 spin_thread_ = NULL;
00201 nh_.setCallbackQueue(ros::getGlobalCallbackQueue());
00202 }
00203
00204 worker_timer_ = nh_.createTimer(rate, &BaseCostmapToPolygons::workerCallback, this);
00205 }
00206
00210 void stopWorker()
00211 {
00212 worker_timer_.stop();
00213 if (spin_thread_)
00214 {
00215 {
00216 boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00217 need_to_terminate_ = true;
00218 }
00219 spin_thread_->join();
00220 delete spin_thread_;
00221 }
00222 }
00223
00224 protected:
00225
00229 BaseCostmapToPolygons() : nh_("~costmap_to_polygons"), spin_thread_(NULL), need_to_terminate_(false) {}
00230
00234 void spinThread()
00235 {
00236 while (nh_.ok())
00237 {
00238 {
00239 boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00240 if (need_to_terminate_)
00241 break;
00242 }
00243 callback_queue_.callAvailable(ros::WallDuration(0.1f));
00244 }
00245 }
00246
00250 void workerCallback(const ros::TimerEvent&)
00251 {
00252 updateCostmap2D();
00253 compute();
00254 }
00255
00256 private:
00257 ros::Timer worker_timer_;
00258 ros::NodeHandle nh_;
00259 boost::thread* spin_thread_;
00260 ros::CallbackQueue callback_queue_;
00261 boost::mutex terminate_mutex_;
00262 bool need_to_terminate_;
00263 };
00264
00265
00272 class BaseCostmapToDynamicObstacles : public BaseCostmapToPolygons
00273 {
00274 public:
00275
00282 void loadStaticCostmapConverterPlugin(const std::string& plugin_name, ros::NodeHandle nh_parent)
00283 {
00284 try
00285 {
00286 static_costmap_converter_ = static_converter_loader_.createInstance(plugin_name);
00287
00288 if(boost::dynamic_pointer_cast<BaseCostmapToDynamicObstacles>(static_costmap_converter_))
00289 {
00290 throw pluginlib::PluginlibException("The specified plugin for static costmap conversion is a dynamic plugin. Specify a static plugin.");
00291 }
00292 std::string raw_plugin_name = static_converter_loader_.getName(plugin_name);
00293 static_costmap_converter_->initialize(ros::NodeHandle(nh_parent, raw_plugin_name));
00294 setStaticCostmapConverterPlugin(static_costmap_converter_);
00295 ROS_INFO_STREAM("CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles " << plugin_name << " loaded.");
00296 }
00297 catch(const pluginlib::PluginlibException& ex)
00298 {
00299 ROS_WARN("CostmapToDynamicObstacles: The specified costmap converter plugin cannot be loaded. Continuing without subsequent conversion of static obstacles. Error message: %s", ex.what());
00300 static_costmap_converter_.reset();
00301 }
00302 }
00303
00308 void setStaticCostmapConverterPlugin(boost::shared_ptr<BaseCostmapToPolygons> static_costmap_converter)
00309 {
00310 static_costmap_converter_ = static_costmap_converter;
00311 }
00312
00317 void setStaticCostmap(boost::shared_ptr<costmap_2d::Costmap2D> static_costmap)
00318 {
00319 static_costmap_converter_->setCostmap2D(static_costmap.get());
00320 }
00321
00325 void convertStaticObstacles()
00326 {
00327 static_costmap_converter_->compute();
00328 }
00329
00334 PolygonContainerConstPtr getStaticPolygons()
00335 {
00336 return static_costmap_converter_->getPolygons();
00337 }
00338
00344 bool stackedCostmapConversion()
00345 {
00346 if(static_costmap_converter_)
00347 return true;
00348 else
00349 return false;
00350 }
00351
00352 protected:
00356 BaseCostmapToDynamicObstacles() : static_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), static_costmap_converter_() {}
00357
00358 private:
00359 pluginlib::ClassLoader<BaseCostmapToPolygons> static_converter_loader_;
00360 boost::shared_ptr<BaseCostmapToPolygons> static_costmap_converter_;
00361 };
00362
00363
00364 }
00365
00366
00367
00368 #endif // end COSTMAP_CONVERTER_INTERFACE_H_