00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2008, 2013, 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 Willow Garage, Inc. 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 * David V. Lu!! 00037 *********************************************************************/ 00038 #ifndef COSTMAP_2D_COSTMAP_2D_ROS_H_ 00039 #define COSTMAP_2D_COSTMAP_2D_ROS_H_ 00040 00041 #include <costmap_2d/layered_costmap.h> 00042 #include <costmap_2d/layer.h> 00043 #include <costmap_2d/costmap_2d_publisher.h> 00044 #include <costmap_2d/Costmap2DConfig.h> 00045 #include <costmap_2d/footprint.h> 00046 #include <geometry_msgs/Polygon.h> 00047 #include <geometry_msgs/PolygonStamped.h> 00048 #include <dynamic_reconfigure/server.h> 00049 #include <pluginlib/class_loader.h> 00050 00051 class SuperValue : public XmlRpc::XmlRpcValue 00052 { 00053 public: 00054 void setStruct(XmlRpc::XmlRpcValue::ValueStruct* a) 00055 { 00056 _type = TypeStruct; 00057 _value.asStruct = new XmlRpc::XmlRpcValue::ValueStruct(*a); 00058 } 00059 void setArray(XmlRpc::XmlRpcValue::ValueArray* a) 00060 { 00061 _type = TypeArray; 00062 _value.asArray = new std::vector<XmlRpc::XmlRpcValue>(*a); 00063 } 00064 }; 00065 00066 namespace costmap_2d 00067 { 00068 00072 class Costmap2DROS 00073 { 00074 public: 00080 Costmap2DROS(std::string name, tf::TransformListener& tf); 00081 ~Costmap2DROS(); 00082 00088 void start(); 00089 00093 void stop(); 00094 00098 void pause(); 00099 00103 void resume(); 00104 00105 void updateMap(); 00106 00110 void resetLayers(); 00111 00113 bool isCurrent() 00114 { 00115 return layered_costmap_->isCurrent(); 00116 } 00117 00123 bool getRobotPose(tf::Stamped<tf::Pose>& global_pose) const; 00124 00128 Costmap2D* getCostmap() 00129 { 00130 return layered_costmap_->getCostmap(); 00131 } 00132 00137 std::string getGlobalFrameID() 00138 { 00139 return global_frame_; 00140 } 00141 00146 std::string getBaseFrameID() 00147 { 00148 return robot_base_frame_; 00149 } 00150 LayeredCostmap* getLayeredCostmap() 00151 { 00152 return layered_costmap_; 00153 } 00154 00156 geometry_msgs::Polygon getRobotFootprintPolygon() 00157 { 00158 return costmap_2d::toPolygon(padded_footprint_); 00159 } 00160 00169 std::vector<geometry_msgs::Point> getRobotFootprint() 00170 { 00171 return padded_footprint_; 00172 } 00173 00181 std::vector<geometry_msgs::Point> getUnpaddedRobotFootprint() 00182 { 00183 return unpadded_footprint_; 00184 } 00185 00190 void getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const; 00191 00202 void setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points); 00203 00214 void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint); 00215 00216 protected: 00217 LayeredCostmap* layered_costmap_; 00218 std::string name_; 00219 tf::TransformListener& tf_; 00220 std::string global_frame_; 00221 std::string robot_base_frame_; 00222 double transform_tolerance_; 00223 00224 private: 00229 void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, 00230 const costmap_2d::Costmap2DConfig &old_config); 00231 00232 void resetOldParameters(ros::NodeHandle& nh); 00233 void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level); 00234 void movementCB(const ros::TimerEvent &event); 00235 void mapUpdateLoop(double frequency); 00236 bool map_update_thread_shutdown_; 00237 bool stop_updates_, initialized_, stopped_, robot_stopped_; 00238 boost::thread* map_update_thread_; 00239 ros::Timer timer_; 00240 ros::Time last_publish_; 00241 ros::Duration publish_cycle; 00242 pluginlib::ClassLoader<Layer> plugin_loader_; 00243 tf::Stamped<tf::Pose> old_pose_; 00244 Costmap2DPublisher* publisher_; 00245 dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_; 00246 00247 boost::recursive_mutex configuration_mutex_; 00248 00249 ros::Subscriber footprint_sub_; 00250 ros::Publisher footprint_pub_; 00251 std::vector<geometry_msgs::Point> unpadded_footprint_; 00252 std::vector<geometry_msgs::Point> padded_footprint_; 00253 float footprint_padding_; 00254 costmap_2d::Costmap2DConfig old_config_; 00255 }; 00256 // class Costmap2DROS 00257 } // namespace costmap_2d 00258 00259 #endif // COSTMAP_2D_COSTMAP_2D_ROS_H