costmap_2d_ros.cpp
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  *********************************************************************/
40 #include <cstdio>
41 #include <string>
42 #include <algorithm>
43 #include <vector>
44 
45 
46 using namespace std;
47 
48 namespace costmap_2d
49 {
50 
51 void move_parameter(ros::NodeHandle& old_h, ros::NodeHandle& new_h, std::string name, bool should_delete = true)
52 {
53  if (!old_h.hasParam(name))
54  return;
55 
56  XmlRpc::XmlRpcValue value;
57  old_h.getParam(name, value);
58  new_h.setParam(name, value);
59  if (should_delete) old_h.deleteParam(name);
60 }
61 
62 Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :
63  layered_costmap_(NULL),
64  name_(name),
65  tf_(tf),
66  transform_tolerance_(0.3),
67  map_update_thread_shutdown_(false),
68  stop_updates_(false),
69  initialized_(true),
70  stopped_(false),
71  robot_stopped_(false),
72  map_update_thread_(NULL),
73  last_publish_(0),
74  plugin_loader_("costmap_2d", "costmap_2d::Layer"),
75  publisher_(NULL),
76  dsrv_(NULL),
77  footprint_padding_(0.0)
78 {
79  // Initialize old pose with something
80  old_pose_.setIdentity();
81  old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));
82 
83  ros::NodeHandle private_nh("~/" + name);
84  ros::NodeHandle g_nh;
85 
86  // get our tf prefix
87  ros::NodeHandle prefix_nh;
88  std::string tf_prefix = tf::getPrefixParam(prefix_nh);
89 
90  // get two frames
91  private_nh.param("global_frame", global_frame_, std::string("/map"));
92  private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
93 
94  // make sure that we set the frames appropriately based on the tf_prefix
97 
98  ros::Time last_error = ros::Time::now();
99  std::string tf_error;
100  // we need to make sure that the transform between the robot base frame and the global frame is available
101  while (ros::ok()
103  &tf_error))
104  {
105  ros::spinOnce();
106  if (last_error + ros::Duration(5.0) < ros::Time::now())
107  {
108  ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
109  robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
110  last_error = ros::Time::now();
111  }
112  // The error string will accumulate and errors will typically be the same, so the last
113  // will do for the warning above. Reset the string here to avoid accumulation.
114  tf_error.clear();
115  }
116 
117  // check if we want a rolling window version of the costmap
118  bool rolling_window, track_unknown_space, always_send_full_costmap;
119  private_nh.param("rolling_window", rolling_window, false);
120  private_nh.param("track_unknown_space", track_unknown_space, false);
121  private_nh.param("always_send_full_costmap", always_send_full_costmap, false);
122 
123  layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
124 
125  if (!private_nh.hasParam("plugins"))
126  {
127  resetOldParameters(private_nh);
128  }
129 
130  if (private_nh.hasParam("plugins"))
131  {
132  XmlRpc::XmlRpcValue my_list;
133  private_nh.getParam("plugins", my_list);
134  for (int32_t i = 0; i < my_list.size(); ++i)
135  {
136  std::string pname = static_cast<std::string>(my_list[i]["name"]);
137  std::string type = static_cast<std::string>(my_list[i]["type"]);
138  ROS_INFO("Using plugin \"%s\"", pname.c_str());
139 
140  boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
141  layered_costmap_->addPlugin(plugin);
142  plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
143  }
144  }
145 
146  // subscribe to the footprint topic
147  std::string topic_param, topic;
148  if (!private_nh.searchParam("footprint_topic", topic_param))
149  {
150  topic_param = "footprint_topic";
151  }
152 
153  private_nh.param(topic_param, topic, std::string("footprint"));
155 
156  if (!private_nh.searchParam("published_footprint_topic", topic_param))
157  {
158  topic_param = "published_footprint";
159  }
160 
161  private_nh.param(topic_param, topic, std::string("oriented_footprint"));
162  footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);
163 
165 
166  publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
167  always_send_full_costmap);
168 
169  // create a thread to handle updating the map
170  stop_updates_ = false;
171  initialized_ = true;
172  stopped_ = false;
173 
174  // Create a time r to check if the robot is moving
175  robot_stopped_ = false;
176  timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);
177 
178  dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
179  dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,
180  _2);
181  dsrv_->setCallback(cb);
182 }
183 
184 void Costmap2DROS::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
185 {
187 }
188 
190 {
192  if (map_update_thread_ != NULL)
193  {
194  map_update_thread_->join();
195  delete map_update_thread_;
196  }
197  if (publisher_ != NULL)
198  delete publisher_;
199 
200  delete layered_costmap_;
201  delete dsrv_;
202 }
203 
205 {
206  ROS_INFO("Loading from pre-hydro parameter style");
207  bool flag;
208  std::string s;
209  std::vector < XmlRpc::XmlRpcValue > plugins;
210 
212  SuperValue super_map;
213  SuperValue super_array;
214 
215  if (nh.getParam("static_map", flag) && flag)
216  {
217  map["name"] = XmlRpc::XmlRpcValue("static_layer");
218  map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer");
219  super_map.setStruct(&map);
220  plugins.push_back(super_map);
221 
222  ros::NodeHandle map_layer(nh, "static_layer");
223  move_parameter(nh, map_layer, "map_topic");
224  move_parameter(nh, map_layer, "unknown_cost_value");
225  move_parameter(nh, map_layer, "lethal_cost_threshold");
226  move_parameter(nh, map_layer, "track_unknown_space", false);
227  }
228 
229  ros::NodeHandle obstacles(nh, "obstacle_layer");
230  if (nh.getParam("map_type", s) && s == "voxel")
231  {
232  map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
233  map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer");
234  super_map.setStruct(&map);
235  plugins.push_back(super_map);
236 
237  move_parameter(nh, obstacles, "origin_z");
238  move_parameter(nh, obstacles, "z_resolution");
239  move_parameter(nh, obstacles, "z_voxels");
240  move_parameter(nh, obstacles, "mark_threshold");
241  move_parameter(nh, obstacles, "unknown_threshold");
242  move_parameter(nh, obstacles, "publish_voxel_map");
243  }
244  else
245  {
246  map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
247  map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer");
248  super_map.setStruct(&map);
249  plugins.push_back(super_map);
250  }
251 
252  move_parameter(nh, obstacles, "max_obstacle_height");
253  move_parameter(nh, obstacles, "raytrace_range");
254  move_parameter(nh, obstacles, "obstacle_range");
255  move_parameter(nh, obstacles, "track_unknown_space", true);
256  nh.param("observation_sources", s, std::string(""));
257  std::stringstream ss(s);
258  std::string source;
259  while (ss >> source)
260  {
261  move_parameter(nh, obstacles, source);
262  }
263  move_parameter(nh, obstacles, "observation_sources");
264 
265  ros::NodeHandle inflation(nh, "inflation_layer");
266  move_parameter(nh, inflation, "cost_scaling_factor");
267  move_parameter(nh, inflation, "inflation_radius");
268  map["name"] = XmlRpc::XmlRpcValue("inflation_layer");
269  map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer");
270  super_map.setStruct(&map);
271  plugins.push_back(super_map);
272 
273  super_array.setArray(&plugins);
274  nh.setParam("plugins", super_array);
275 }
276 
277 void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
278 {
279  transform_tolerance_ = config.transform_tolerance;
280  if (map_update_thread_ != NULL)
281  {
283  map_update_thread_->join();
284  delete map_update_thread_;
285  }
287  double map_update_frequency = config.update_frequency;
288 
289  double map_publish_frequency = config.publish_frequency;
290  if (map_publish_frequency > 0)
291  publish_cycle = ros::Duration(1 / map_publish_frequency);
292  else
294 
295  // find size parameters
296  double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =
297  config.origin_x,
298  origin_y = config.origin_y;
299 
301  {
302  layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
303  (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
304  }
305 
306  // If the padding has changed, call setUnpaddedRobotFootprint() to
307  // re-apply the padding.
308  if (footprint_padding_ != config.footprint_padding)
309  {
310  footprint_padding_ = config.footprint_padding;
312  }
313 
315 
316  old_config_ = config;
317 
318  map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
319 }
320 
321 void Costmap2DROS::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
322  const costmap_2d::Costmap2DConfig &old_config)
323 {
324  // Only change the footprint if footprint or robot_radius has
325  // changed. Otherwise we might overwrite a footprint sent on a
326  // topic by a dynamic_reconfigure call which was setting some other
327  // variable.
328  if (new_config.footprint == old_config.footprint &&
329  new_config.robot_radius == old_config.robot_radius)
330  {
331  return;
332  }
333 
334  if (new_config.footprint != "" && new_config.footprint != "[]")
335  {
336  std::vector<geometry_msgs::Point> new_footprint;
337  if (makeFootprintFromString(new_config.footprint, new_footprint))
338  {
339  setUnpaddedRobotFootprint(new_footprint);
340  }
341  else
342  {
343  ROS_ERROR("Invalid footprint string from dynamic reconfigure");
344  }
345  }
346  else
347  {
348  // robot_radius may be 0, but that must be intended at this point.
349  setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius));
350  }
351 }
352 
353 void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
354 {
355  unpadded_footprint_ = points;
356  padded_footprint_ = points;
358 
360 }
361 
363 {
364  // don't allow configuration to happen while this check occurs
365  // boost::recursive_mutex::scoped_lock mcl(configuration_mutex_);
366 
367  tf::Stamped < tf::Pose > new_pose;
368 
369  if (!getRobotPose(new_pose))
370  {
371  ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration");
372  robot_stopped_ = false;
373  }
374  // make sure that the robot is not moving
375  else if (fabs((old_pose_.getOrigin() - new_pose.getOrigin()).length()) < 1e-3
376  && fabs(old_pose_.getRotation().angle(new_pose.getRotation())) < 1e-3)
377  {
378  old_pose_ = new_pose;
379  robot_stopped_ = true;
380  }
381  else
382  {
383  old_pose_ = new_pose;
384  robot_stopped_ = false;
385  }
386 }
387 
388 void Costmap2DROS::mapUpdateLoop(double frequency)
389 {
390  // the user might not want to run the loop every cycle
391  if (frequency == 0.0)
392  return;
393 
394  ros::NodeHandle nh;
395  ros::Rate r(frequency);
396  while (nh.ok() && !map_update_thread_shutdown_)
397  {
398  struct timeval start, end;
399  double start_t, end_t, t_diff;
400  gettimeofday(&start, NULL);
401 
402  updateMap();
403 
404  gettimeofday(&end, NULL);
405  start_t = start.tv_sec + double(start.tv_usec) / 1e6;
406  end_t = end.tv_sec + double(end.tv_usec) / 1e6;
407  t_diff = end_t - start_t;
408  ROS_DEBUG("Map update time: %.9f", t_diff);
410  {
411  unsigned int x0, y0, xn, yn;
412  layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
413  publisher_->updateBounds(x0, xn, y0, yn);
414 
415  ros::Time now = ros::Time::now();
416  if (last_publish_ + publish_cycle < now)
417  {
419  last_publish_ = now;
420  }
421  }
422  r.sleep();
423  // make sure to sleep for the remainder of our cycle time
424  if (r.cycleTime() > ros::Duration(1 / frequency))
425  ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
426  r.cycleTime().toSec());
427  }
428 }
429 
431 {
432  if (!stop_updates_)
433  {
434  // get global pose
436  if (getRobotPose (pose))
437  {
438  double x = pose.getOrigin().x(),
439  y = pose.getOrigin().y(),
440  yaw = tf::getYaw(pose.getRotation());
441 
442  layered_costmap_->updateMap(x, y, yaw);
443 
444  geometry_msgs::PolygonStamped footprint;
445  footprint.header.frame_id = global_frame_;
446  footprint.header.stamp = ros::Time::now();
447  transformFootprint(x, y, yaw, padded_footprint_, footprint);
448  footprint_pub_.publish(footprint);
449 
450  initialized_ = true;
451  }
452  }
453 }
454 
456 {
457  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
458  // check if we're stopped or just paused
459  if (stopped_)
460  {
461  // if we're stopped we need to re-subscribe to topics
462  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
463  ++plugin)
464  {
465  (*plugin)->activate();
466  }
467  stopped_ = false;
468  }
469  stop_updates_ = false;
470 
471  // block until the costmap is re-initialized.. meaning one update cycle has run
472  ros::Rate r(100.0);
473  while (ros::ok() && !initialized_)
474  r.sleep();
475 }
476 
478 {
479  stop_updates_ = true;
480  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
481  // unsubscribe from topics
482  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
483  ++plugin)
484  {
485  (*plugin)->deactivate();
486  }
487  initialized_ = false;
488  stopped_ = true;
489 }
490 
492 {
493  stop_updates_ = true;
494  initialized_ = false;
495 }
496 
498 {
499  stop_updates_ = false;
500 
501  // block until the costmap is re-initialized.. meaning one update cycle has run
502  ros::Rate r(100.0);
503  while (!initialized_)
504  r.sleep();
505 }
506 
507 
509 {
511  top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
512  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
513  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
514  ++plugin)
515  {
516  (*plugin)->reset();
517  }
518 }
519 
521 {
522  global_pose.setIdentity();
523  tf::Stamped < tf::Pose > robot_pose;
524  robot_pose.setIdentity();
525  robot_pose.frame_id_ = robot_base_frame_;
526  robot_pose.stamp_ = ros::Time();
527  ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
528 
529  // get the global pose of the robot
530  try
531  {
532  tf_.transformPose(global_frame_, robot_pose, global_pose);
533  }
534  catch (tf::LookupException& ex)
535  {
536  ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
537  return false;
538  }
539  catch (tf::ConnectivityException& ex)
540  {
541  ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
542  return false;
543  }
544  catch (tf::ExtrapolationException& ex)
545  {
546  ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
547  return false;
548  }
549  // check global_pose timeout
550  if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
551  {
552  ROS_WARN_THROTTLE(1.0,
553  "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
554  current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
555  return false;
556  }
557 
558  return true;
559 }
560 
561 void Costmap2DROS::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
562 {
563  tf::Stamped<tf::Pose> global_pose;
564  if (!getRobotPose(global_pose))
565  return;
566 
567  double yaw = tf::getYaw(global_pose.getRotation());
568  transformFootprint(global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw,
569  padded_footprint_, oriented_footprint);
570 }
571 
572 } // namespace costmap_2d
std::vector< geometry_msgs::Point > makeFootprintFromParams(ros::NodeHandle &nh)
Read the ros-params "footprint" and/or "robot_radius" from the given NodeHandle using searchParam() t...
Definition: footprint.cpp:212
void start()
Subscribes to sensor topics if necessary and starts costmap updates, can be called to restart the cos...
#define ROS_WARN_THROTTLE(rate,...)
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
bool getRobotPose(tf::Stamped< tf::Pose > &global_pose) const
Get the pose of the robot in the global frame of the costmap.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int size() const
bool deleteParam(const std::string &key) const
static double getYaw(const Quaternion &bt_q)
XmlRpcServer s
Duration cycleTime() const
costmap_2d::Costmap2DConfig old_config_
void pause()
Stops the costmap from updating, but sensor data still comes in over the wire.
dynamic_reconfigure::Server< costmap_2d::Costmap2DConfig > * dsrv_
void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, const costmap_2d::Costmap2DConfig &old_config)
Set the footprint from the new_config object.
void setFootprint(const std::vector< geometry_msgs::Point > &footprint_spec)
Updates the stored footprint, updates the circumscribed and inscribed radii, and calls onFootprintCha...
tf::TransformListener * tf_
void getOrientedFootprint(std::vector< geometry_msgs::Point > &oriented_footprint) const
Build the oriented footprint of the robot at the robot&#39;s current pose.
TFSIMD_FORCE_INLINE const tfScalar & y() const
std::vector< boost::shared_ptr< Layer > > * getPlugins()
#define ROS_WARN(...)
boost::thread * map_update_thread_
A thread for updating the map.
void getBounds(unsigned int *x0, unsigned int *xn, unsigned int *y0, unsigned int *yn)
bool makeFootprintFromString(const std::string &footprint_string, std::vector< geometry_msgs::Point > &footprint)
Make the footprint from the given string.
Definition: footprint.cpp:170
tf::TransformListener & tf_
Used for transforming point clouds.
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
std::string resolve(const std::string &prefix, const std::string &frame_name)
std::vector< geometry_msgs::Point > toPointVector(geometry_msgs::Polygon polygon)
Convert Polygon msg to vector of Points.
Definition: footprint.cpp:96
LayeredCostmap * layered_costmap_
#define ROS_ERROR_THROTTLE(rate,...)
std::map< std::string, XmlRpcValue > ValueStruct
ros::Subscriber footprint_sub_
void addPlugin(boost::shared_ptr< Layer > plugin)
void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon &footprint)
Set the footprint of the robot to be the given polygon, padded by footprint_padding.
std::string global_frame_
The global frame for the costmap.
#define ROS_INFO(...)
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::vector< geometry_msgs::Point > makeFootprintFromRadius(double radius)
Create a circular footprint from a given radius.
Definition: footprint.cpp:150
void padFootprint(std::vector< geometry_msgs::Point > &footprint, double padding)
Adds the specified amount of padding to the footprint (in place)
Definition: footprint.cpp:138
ROSCPP_DECL bool ok()
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void movementCB(const ros::TimerEvent &event)
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:431
Costmap2DPublisher * publisher_
pluginlib::ClassLoader< Layer > plugin_loader_
bool sleep()
std::string robot_base_frame_
The frame_id of the robot base.
void move_parameter(ros::NodeHandle &old_h, ros::NodeHandle &new_h, std::string name, bool should_delete=true)
ros::Time stamp_
A tool to periodically publish visualization data from a Costmap2D.
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:426
void updateBounds(unsigned int x0, unsigned int xn, unsigned int y0, unsigned int yn)
Include the given bounds in the changed-rectangle.
void mapUpdateLoop(double frequency)
std::string frame_id_
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
void resetLayers()
Reset each individual layer.
void resume()
Resumes costmap updates.
bool getParam(const std::string &key, std::string &s) const
double transform_tolerance_
timeout before transform errors
void setUnpaddedRobotFootprint(const std::vector< geometry_msgs::Point > &points)
Set the footprint of the robot to be the given set of points, padded by footprint_padding.
static Time now()
ros::Publisher footprint_pub_
void stop()
Stops costmap updates and unsubscribes from sensor topics.
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
tf::Stamped< tf::Pose > old_pose_
bool ok() const
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
bool hasParam(const std::string &key) const
Instantiates different layer plugins and aggregates them into one score.
void updateMap(double robot_x, double robot_y, double robot_yaw)
Update the underlying costmap with new data. If you want to update the map outside of the update loop...
ROSCPP_DECL void spinOnce()
void setArray(XmlRpc::XmlRpcValue::ValueArray *a)
#define ROS_ERROR(...)
void setStruct(XmlRpc::XmlRpcValue::ValueStruct *a)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
void transformFootprint(double x, double y, double theta, const std::vector< geometry_msgs::Point > &footprint_spec, std::vector< geometry_msgs::Point > &oriented_footprint)
Given a pose and base footprint, build the oriented footprint of the robot (list of Points) ...
Definition: footprint.cpp:106
std::vector< geometry_msgs::Point > padded_footprint_
void publishCostmap()
Publishes the visualization data over ROS.
#define ROS_DEBUG(...)
void resetOldParameters(ros::NodeHandle &nh)
void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
Definition: costmap_2d.cpp:93
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