costmap_converter_interface.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Christoph Rösmann, Otniel Rinaldo
00037  *********************************************************************/
00038 
00039 #ifndef COSTMAP_CONVERTER_INTERFACE_H_
00040 #define COSTMAP_CONVERTER_INTERFACE_H_
00041 
00042 //#include <costmap_2d/costmap_2d_ros.h>
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_


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat Jun 8 2019 20:53:15