costmap_converter_interface.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann, Otniel Rinaldo
37  *********************************************************************/
38 
39 #ifndef COSTMAP_CONVERTER_INTERFACE_H_
40 #define COSTMAP_CONVERTER_INTERFACE_H_
41 
42 //#include <costmap_2d/costmap_2d_ros.h>
43 #include <ros/ros.h>
44 #include <ros/callback_queue.h>
45 #include <boost/thread.hpp>
46 #include <costmap_2d/costmap_2d.h>
48 #include <geometry_msgs/Polygon.h>
49 #include <costmap_converter/ObstacleArrayMsg.h>
50 
51 
53 {
58 
63 
64 
78 {
79 public:
80 
85  virtual void initialize(ros::NodeHandle nh) = 0;
86 
91  {
92  stopWorker();
93  }
94 
95 
102  virtual void setCostmap2D(costmap_2d::Costmap2D* costmap) = 0;
103 
109  virtual void updateCostmap2D() = 0;
110 
114  virtual void compute() = 0;
115 
125  virtual PolygonContainerConstPtr getPolygons(){return PolygonContainerConstPtr();}
126 
135  virtual ObstacleArrayConstPtr getObstacles()
136  {
137  ObstacleArrayPtr obstacles = boost::make_shared<ObstacleArrayMsg>();
138  PolygonContainerConstPtr polygons = getPolygons();
139  if (polygons)
140  {
141  for (const geometry_msgs::Polygon& polygon : *polygons)
142  {
143  obstacles->obstacles.emplace_back();
144  obstacles->obstacles.back().polygon = polygon;
145  }
146  }
147  return obstacles;
148  }
149 
157  virtual void setOdomTopic(const std::string& odom_topic) {}
158 
164  virtual bool stackedCostmapConversion() {return false;}
165 
177  void startWorker(ros::Rate rate, costmap_2d::Costmap2D* costmap, bool spin_thread = false)
178  {
179  setCostmap2D(costmap);
180 
181  if (spin_thread_)
182  {
183  {
184  boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
185  need_to_terminate_ = true;
186  }
187  spin_thread_->join();
188  delete spin_thread_;
189  }
190 
191  if (spin_thread)
192  {
193  ROS_DEBUG_NAMED("costmap_converter", "Spinning up a thread for the CostmapToPolygons plugin");
194  need_to_terminate_ = false;
195  spin_thread_ = new boost::thread(boost::bind(&BaseCostmapToPolygons::spinThread, this));
197  }
198  else
199  {
200  spin_thread_ = NULL;
202  }
203 
205  }
206 
210  void stopWorker()
211  {
213  if (spin_thread_)
214  {
215  {
216  boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
217  need_to_terminate_ = true;
218  }
219  spin_thread_->join();
220  delete spin_thread_;
221  }
222  }
223 
224 protected:
225 
229  BaseCostmapToPolygons() : nh_("~costmap_to_polygons"), spin_thread_(NULL), need_to_terminate_(false) {}
230 
234  void spinThread()
235  {
236  while (nh_.ok())
237  {
238  {
239  boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
240  if (need_to_terminate_)
241  break;
242  }
244  }
245  }
246 
251  {
252  updateCostmap2D();
253  compute();
254  }
255 
256 private:
259  boost::thread* spin_thread_;
261  boost::mutex terminate_mutex_;
263 };
264 
265 
273 {
274 public:
275 
282  void loadStaticCostmapConverterPlugin(const std::string& plugin_name, ros::NodeHandle nh_parent)
283  {
284  try
285  {
286  static_costmap_converter_ = static_converter_loader_.createInstance(plugin_name);
287 
288  if(boost::dynamic_pointer_cast<BaseCostmapToDynamicObstacles>(static_costmap_converter_))
289  {
290  throw pluginlib::PluginlibException("The specified plugin for static costmap conversion is a dynamic plugin. Specify a static plugin.");
291  }
292  std::string raw_plugin_name = static_converter_loader_.getName(plugin_name);
293  static_costmap_converter_->initialize(ros::NodeHandle(nh_parent, raw_plugin_name));
294  setStaticCostmapConverterPlugin(static_costmap_converter_);
295  ROS_INFO_STREAM("CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles " << plugin_name << " loaded.");
296  }
297  catch(const pluginlib::PluginlibException& ex)
298  {
299  ROS_WARN("CostmapToDynamicObstacles: The specified costmap converter plugin cannot be loaded. Continuing without subsequent conversion of static obstacles. Error message: %s", ex.what());
300  static_costmap_converter_.reset();
301  }
302  }
303 
309  {
310  static_costmap_converter_ = static_costmap_converter;
311  }
312 
318  {
319  static_costmap_converter_->setCostmap2D(static_costmap.get());
320  }
321 
326  {
327  static_costmap_converter_->compute();
328  }
329 
334  PolygonContainerConstPtr getStaticPolygons()
335  {
336  return static_costmap_converter_->getPolygons();
337  }
338 
345  {
346  if(static_costmap_converter_)
347  return true;
348  else
349  return false;
350  }
351 
352 protected:
356  BaseCostmapToDynamicObstacles() : static_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), static_costmap_converter_() {}
357 
358 private:
361 };
362 
363 
364 }
365 
366 
367 
368 #endif // end COSTMAP_CONVERTER_INTERFACE_H_
void convertStaticObstacles()
Apply the underlying plugin for static costmap conversion.
virtual ObstacleArrayConstPtr getObstacles()
Get a shared instance of the current obstacle container If this method is not overwritten by the unde...
boost::shared_ptr< BaseCostmapToPolygons > static_costmap_converter_
f
void setStaticCostmapConverterPlugin(boost::shared_ptr< BaseCostmapToPolygons > static_costmap_converter)
Set the underlying plugin for subsequent costmap conversion of the static background of the costmap...
virtual void initialize(ros::NodeHandle nh)=0
Initialize the plugin.
void stop()
void workerCallback(const ros::TimerEvent &)
The callback of the worker that performs the actual work (updating the costmap and converting it to p...
virtual bool stackedCostmapConversion()
Determines whether an additional plugin for subsequent costmap conversion is specified.
pluginlib::ClassLoader< BaseCostmapToPolygons > static_converter_loader_
virtual void setOdomTopic(const std::string &odom_topic)
Set name of robot&#39;s odometry topic.
#define ROS_WARN(...)
BaseCostmapToDynamicObstacles()
Protected constructor that should be called by subclasses.
virtual PolygonContainerConstPtr getPolygons()
Get a shared instance of the current polygon container.
ROSCPP_DECL CallbackQueue * getGlobalCallbackQueue()
virtual void setCostmap2D(costmap_2d::Costmap2D *costmap)=0
Pass a pointer to the costap to the plugin.
#define ROS_DEBUG_NAMED(name,...)
void stopWorker()
Stop the worker that repeatedly converts the costmap to polygons.
void loadStaticCostmapConverterPlugin(const std::string &plugin_name, ros::NodeHandle nh_parent)
Load underlying static costmap conversion plugin via plugin-loader.
void spinThread()
Blocking method that checks for new timer events (active if startWorker() is called with spin_thread ...
void setCallbackQueue(CallbackQueueInterface *queue)
This abstract class defines the interface for plugins that convert the costmap into polygon types...
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void setStaticCostmap(boost::shared_ptr< costmap_2d::Costmap2D > static_costmap)
Set the costmap for the underlying plugin.
BaseCostmapToPolygons()
Protected constructor that should be called by subclasses.
boost::shared_ptr< ObstacleArrayMsg > ObstacleArrayPtr
Typedef for a shared dynamic obstacle container.
virtual void compute()=0
This method performs the actual work (conversion of the costmap to polygons)
#define ROS_INFO_STREAM(args)
boost::shared_ptr< const std::vector< geometry_msgs::Polygon > > PolygonContainerConstPtr
Typedef for a shared polygon container (read-only access)
boost::shared_ptr< const ObstacleArrayMsg > ObstacleArrayConstPtr
Typedef for a shared dynamic obstacle container (read-only access)
boost::shared_ptr< std::vector< geometry_msgs::Polygon > > PolygonContainerPtr
Typedef for a shared polygon container.
bool ok() const
void startWorker(ros::Rate rate, costmap_2d::Costmap2D *costmap, bool spin_thread=false)
Instantiate a worker that repeatedly coverts the most recent costmap to polygons. The worker is imple...
virtual void updateCostmap2D()=0
Get updated data from the previously set Costmap2D.
PolygonContainerConstPtr getStaticPolygons()
Get the converted polygons from the underlying plugin.
bool stackedCostmapConversion()
Determines whether an additional plugin for subsequent costmap conversion is specified.


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat May 16 2020 03:19:18