costmap_2d_ros.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, 2013, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Eitan Marder-Eppstein
36  * David V. Lu!!
37  *********************************************************************/
38 #ifndef COSTMAP_2D_COSTMAP_2D_ROS_H_
39 #define COSTMAP_2D_COSTMAP_2D_ROS_H_
40 
42 #include <costmap_2d/layer.h>
44 #include <costmap_2d/Costmap2DConfig.h>
45 #include <costmap_2d/footprint.h>
46 #include <geometry_msgs/Polygon.h>
47 #include <geometry_msgs/PolygonStamped.h>
48 #include <dynamic_reconfigure/server.h>
49 #include <pluginlib/class_loader.h>
50 
52 {
53 public:
55  {
56  _type = TypeStruct;
57  _value.asStruct = new XmlRpc::XmlRpcValue::ValueStruct(*a);
58  }
60  {
61  _type = TypeArray;
62  _value.asArray = new std::vector<XmlRpc::XmlRpcValue>(*a);
63  }
64 };
65 
66 namespace costmap_2d
67 {
68 
73 {
74 public:
80  Costmap2DROS(std::string name, tf::TransformListener& tf);
81  ~Costmap2DROS();
82 
88  void start();
89 
93  void stop();
94 
98  void pause();
99 
103  void resume();
104 
105  void updateMap();
106 
110  void resetLayers();
111 
113  bool isCurrent()
114  {
115  return layered_costmap_->isCurrent();
116  }
117 
123  bool getRobotPose(tf::Stamped<tf::Pose>& global_pose) const;
124 
129  {
130  return layered_costmap_->getCostmap();
131  }
132 
137  std::string getGlobalFrameID()
138  {
139  return global_frame_;
140  }
141 
146  std::string getBaseFrameID()
147  {
148  return robot_base_frame_;
149  }
151  {
152  return layered_costmap_;
153  }
154 
156  geometry_msgs::Polygon getRobotFootprintPolygon()
157  {
158  return costmap_2d::toPolygon(padded_footprint_);
159  }
160 
169  std::vector<geometry_msgs::Point> getRobotFootprint()
170  {
171  return padded_footprint_;
172  }
173 
181  std::vector<geometry_msgs::Point> getUnpaddedRobotFootprint()
182  {
183  return unpadded_footprint_;
184  }
185 
190  void getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const;
191 
202  void setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points);
203 
214  void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint);
215 
216 protected:
218  std::string name_;
220  std::string global_frame_;
221  std::string robot_base_frame_;
223 
224 private:
229  void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
230  const costmap_2d::Costmap2DConfig &old_config);
231 
232  void resetOldParameters(ros::NodeHandle& nh);
233  void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
234  void movementCB(const ros::TimerEvent &event);
235  void mapUpdateLoop(double frequency);
237  bool stop_updates_, initialized_, stopped_, robot_stopped_;
238  boost::thread* map_update_thread_;
245  dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
246 
247  boost::recursive_mutex configuration_mutex_;
248 
251  std::vector<geometry_msgs::Point> unpadded_footprint_;
252  std::vector<geometry_msgs::Point> padded_footprint_;
254  costmap_2d::Costmap2DConfig old_config_;
255 };
256 // class Costmap2DROS
257 } // namespace costmap_2d
258 
259 #endif // COSTMAP_2D_COSTMAP_2D_ROS_H
boost::recursive_mutex configuration_mutex_
std::vector< XmlRpcValue > ValueArray
costmap_2d::Costmap2DConfig old_config_
std::string getGlobalFrameID()
Returns the global frame of the costmap.
dynamic_reconfigure::Server< costmap_2d::Costmap2DConfig > * dsrv_
boost::thread * map_update_thread_
A thread for updating the map.
tf::TransformListener & tf_
Used for transforming point clouds.
LayeredCostmap * layered_costmap_
union XmlRpc::XmlRpcValue::@0 _value
std::map< std::string, XmlRpcValue > ValueStruct
ros::Subscriber footprint_sub_
std::string global_frame_
The global frame for the costmap.
std::string getBaseFrameID()
Returns the local frame of the costmap.
Costmap2DPublisher * publisher_
pluginlib::ClassLoader< Layer > plugin_loader_
std::string robot_base_frame_
The frame_id of the robot base.
geometry_msgs::Polygon toPolygon(std::vector< geometry_msgs::Point > pts)
Convert vector of Points to Polygon msg.
Definition: footprint.cpp:87
A tool to periodically publish visualization data from a Costmap2D.
geometry_msgs::Polygon getRobotFootprintPolygon()
Returns the current padded footprint as a geometry_msgs::Polygon.
double transform_tolerance_
timeout before transform errors
bool isCurrent()
Same as getLayeredCostmap()->isCurrent().
ros::Publisher footprint_pub_
tf::Stamped< tf::Pose > old_pose_
std::vector< geometry_msgs::Point > getUnpaddedRobotFootprint()
Return the current unpadded footprint of the robot as a vector of points.
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
Costmap2D * getCostmap()
Return a pointer to the "master" costmap which receives updates from all the layers.
A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacl...
std::vector< geometry_msgs::Point > getRobotFootprint()
Return the current footprint of the robot as a vector of points.
Instantiates different layer plugins and aggregates them into one score.
void setArray(XmlRpc::XmlRpcValue::ValueArray *a)
void setStruct(XmlRpc::XmlRpcValue::ValueStruct *a)
std::vector< geometry_msgs::Point > padded_footprint_
LayeredCostmap * getLayeredCostmap()
std::vector< geometry_msgs::Point > unpadded_footprint_


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42