costmap_2d_ros.h
Go to the documentation of this file.
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   bool got_footprint_;
00252   std::vector<geometry_msgs::Point> unpadded_footprint_;
00253   std::vector<geometry_msgs::Point> padded_footprint_;
00254   float footprint_padding_;
00255   costmap_2d::Costmap2DConfig old_config_;
00256 };
00257 // class Costmap2DROS
00258 }  // namespace costmap_2d
00259 
00260 #endif  // COSTMAP_2D_COSTMAP_2D_ROS_H


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:21