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 00079 class BaseCostmapToPolygons 00080 { 00081 public: 00082 00087 virtual void initialize(ros::NodeHandle nh) = 0; 00088 00092 virtual ~BaseCostmapToPolygons() 00093 { 00094 stopWorker(); 00095 } 00096 00097 00104 virtual void setCostmap2D(costmap_2d::Costmap2D* costmap) = 0; 00105 00111 virtual void updateCostmap2D() = 0; 00112 00116 virtual void compute() = 0; 00117 00127 virtual PolygonContainerConstPtr getPolygons(){return PolygonContainerConstPtr();} 00128 00137 virtual ObstacleArrayConstPtr getObstacles() 00138 { 00139 ObstacleArrayPtr obstacles = boost::make_shared<ObstacleArrayMsg>(); 00140 PolygonContainerConstPtr polygons = getPolygons(); 00141 if (polygons) 00142 { 00143 for (const geometry_msgs::Polygon& polygon : *polygons) 00144 { 00145 obstacles->obstacles.emplace_back(); 00146 obstacles->obstacles.back().polygon = polygon; 00147 } 00148 } 00149 return obstacles; 00150 } 00151 00159 virtual void setOdomTopic(const std::string& odom_topic) {} 00160 00172 void startWorker(ros::Rate rate, costmap_2d::Costmap2D* costmap, bool spin_thread = false) 00173 { 00174 setCostmap2D(costmap); 00175 00176 if (spin_thread_) 00177 { 00178 { 00179 boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 00180 need_to_terminate_ = true; 00181 } 00182 spin_thread_->join(); 00183 delete spin_thread_; 00184 } 00185 00186 if (spin_thread) 00187 { 00188 ROS_DEBUG_NAMED("costmap_converter", "Spinning up a thread for the CostmapToPolygons plugin"); 00189 need_to_terminate_ = false; 00190 spin_thread_ = new boost::thread(boost::bind(&BaseCostmapToPolygons::spinThread, this)); 00191 nh_.setCallbackQueue(&callback_queue_); 00192 } 00193 else 00194 { 00195 spin_thread_ = NULL; 00196 nh_.setCallbackQueue(ros::getGlobalCallbackQueue()); 00197 } 00198 00199 worker_timer_ = nh_.createTimer(rate, &BaseCostmapToPolygons::workerCallback, this); 00200 } 00201 00205 void stopWorker() 00206 { 00207 worker_timer_.stop(); 00208 if (spin_thread_) 00209 { 00210 { 00211 boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 00212 need_to_terminate_ = true; 00213 } 00214 spin_thread_->join(); 00215 delete spin_thread_; 00216 } 00217 } 00218 00219 protected: 00220 00224 BaseCostmapToPolygons() : nh_("~costmap_to_polygons"), spin_thread_(NULL), need_to_terminate_(false) {} 00225 00229 void spinThread() 00230 { 00231 while (nh_.ok()) 00232 { 00233 { 00234 boost::mutex::scoped_lock terminate_lock(terminate_mutex_); 00235 if (need_to_terminate_) 00236 break; 00237 } 00238 callback_queue_.callAvailable(ros::WallDuration(0.1f)); 00239 } 00240 } 00241 00245 void workerCallback(const ros::TimerEvent&) 00246 { 00247 updateCostmap2D(); 00248 compute(); 00249 } 00250 00251 private: 00252 ros::Timer worker_timer_; 00253 ros::NodeHandle nh_; 00254 boost::thread* spin_thread_; 00255 ros::CallbackQueue callback_queue_; 00256 boost::mutex terminate_mutex_; 00257 bool need_to_terminate_; 00258 }; 00259 00260 } 00261 00262 00263 00264 #endif // end COSTMAP_CONVERTER_INTERFACE_H_