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 #include <tf2/convert.h>
45 #include <tf2/utils.h>
47 
48 using namespace std;
49 
50 namespace costmap_2d
51 {
52 
53 void move_parameter(ros::NodeHandle& old_h, ros::NodeHandle& new_h, std::string name, bool should_delete = true)
54 {
55  if (!old_h.hasParam(name))
56  return;
57 
58  XmlRpc::XmlRpcValue value;
59  old_h.getParam(name, value);
60  new_h.setParam(name, value);
61  if (should_delete) old_h.deleteParam(name);
62 }
63 
64 Costmap2DROS::Costmap2DROS(const std::string& name, tf2_ros::Buffer& tf) :
65  layered_costmap_(NULL),
66  name_(name),
67  tf_(tf),
68  transform_tolerance_(0.3),
69  map_update_thread_shutdown_(false),
70  stop_updates_(false),
71  initialized_(true),
72  stopped_(false),
73  robot_stopped_(false),
74  map_update_thread_(NULL),
75  last_publish_(0),
76  plugin_loader_("costmap_2d", "costmap_2d::Layer"),
77  publisher_(NULL),
78  dsrv_(NULL),
79  footprint_padding_(0.0)
80 {
81  // Initialize old pose with something
83 
84  ros::NodeHandle private_nh("~/" + name);
85  ros::NodeHandle g_nh;
86 
87  // get global and robot base frame names
88  private_nh.param("global_frame", global_frame_, std::string("map"));
89  private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
90 
91  ros::Time last_error = ros::Time::now();
92  std::string tf_error;
93  // we need to make sure that the transform between the robot base frame and the global frame is available
94  while (ros::ok()
96  {
97  ros::spinOnce();
98  if (last_error + ros::Duration(5.0) < ros::Time::now())
99  {
100  ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
101  robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
102  last_error = ros::Time::now();
103  }
104  // The error string will accumulate and errors will typically be the same, so the last
105  // will do for the warning above. Reset the string here to avoid accumulation.
106  tf_error.clear();
107  }
108 
109  // check if we want a rolling window version of the costmap
110  bool rolling_window, track_unknown_space, always_send_full_costmap;
111  private_nh.param("rolling_window", rolling_window, false);
112  private_nh.param("track_unknown_space", track_unknown_space, false);
113  private_nh.param("always_send_full_costmap", always_send_full_costmap, false);
114 
115  layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
116 
117  if (!private_nh.hasParam("plugins"))
118  {
119  loadOldParameters(private_nh);
120  } else {
121  warnForOldParameters(private_nh);
122  }
123 
124  if (private_nh.hasParam("plugins"))
125  {
126  XmlRpc::XmlRpcValue my_list;
127  private_nh.getParam("plugins", my_list);
128  for (int32_t i = 0; i < my_list.size(); ++i)
129  {
130  std::string pname = static_cast<std::string>(my_list[i]["name"]);
131  std::string type = static_cast<std::string>(my_list[i]["type"]);
132  ROS_INFO("%s: Using plugin \"%s\"", name_.c_str(), pname.c_str());
133 
134  copyParentParameters(pname, type, private_nh);
135 
136  boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
137  layered_costmap_->addPlugin(plugin);
138  plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
139  }
140  }
141 
142  // subscribe to the footprint topic
143  std::string topic_param, topic;
144  if (!private_nh.searchParam("footprint_topic", topic_param))
145  {
146  topic_param = "footprint_topic";
147  }
148 
149  private_nh.param(topic_param, topic, std::string("footprint"));
151 
152  if (!private_nh.searchParam("published_footprint_topic", topic_param))
153  {
154  topic_param = "published_footprint";
155  }
156 
157  private_nh.param(topic_param, topic, std::string("footprint")); // TODO: revert to oriented_footprint in N-turtle
158  footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>(topic, 1);
159 
161 
162  publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
163  always_send_full_costmap);
164 
165  // create a thread to handle updating the map
166  stop_updates_ = false;
167  initialized_ = true;
168  stopped_ = false;
169 
170  // Create a time r to check if the robot is moving
171  robot_stopped_ = false;
172  timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);
173 
174  dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
175  dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,
176  _2);
177  dsrv_->setCallback(cb);
178 }
179 
180 void Costmap2DROS::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
181 {
183 }
184 
186 {
187  timer_.stop();
188 
190  if (map_update_thread_ != NULL)
191  {
192  map_update_thread_->join();
193  delete map_update_thread_;
194  }
195  if (publisher_ != NULL)
196  delete publisher_;
197 
198  delete layered_costmap_;
199  delete dsrv_;
200 }
201 
203 {
204  ROS_WARN("%s: Parameter \"plugins\" not provided, loading pre-Hydro parameters", name_.c_str());
205  bool flag;
206  std::string s;
207  std::vector < XmlRpc::XmlRpcValue > plugins;
208 
210  SuperValue super_map;
211  SuperValue super_array;
212 
213  if (nh.getParam("static_map", flag) && flag)
214  {
215  map["name"] = XmlRpc::XmlRpcValue("static_layer");
216  map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer");
217  super_map.setStruct(&map);
218  plugins.push_back(super_map);
219 
220  ros::NodeHandle map_layer(nh, "static_layer");
221  move_parameter(nh, map_layer, "map_topic");
222  move_parameter(nh, map_layer, "unknown_cost_value");
223  move_parameter(nh, map_layer, "lethal_cost_threshold");
224  move_parameter(nh, map_layer, "track_unknown_space", false);
225  }
226 
227  ros::NodeHandle obstacles(nh, "obstacle_layer");
228  if (nh.getParam("map_type", s) && s == "voxel")
229  {
230  map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
231  map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer");
232  super_map.setStruct(&map);
233  plugins.push_back(super_map);
234 
235  move_parameter(nh, obstacles, "origin_z");
236  move_parameter(nh, obstacles, "z_resolution");
237  move_parameter(nh, obstacles, "z_voxels");
238  move_parameter(nh, obstacles, "mark_threshold");
239  move_parameter(nh, obstacles, "unknown_threshold");
240  move_parameter(nh, obstacles, "publish_voxel_map");
241  }
242  else
243  {
244  map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
245  map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer");
246  super_map.setStruct(&map);
247  plugins.push_back(super_map);
248  }
249 
250  move_parameter(nh, obstacles, "max_obstacle_height");
251  move_parameter(nh, obstacles, "raytrace_range");
252  move_parameter(nh, obstacles, "obstacle_range");
253  move_parameter(nh, obstacles, "track_unknown_space", true);
254  nh.param("observation_sources", s, std::string(""));
255  std::stringstream ss(s);
256  std::string source;
257  while (ss >> source)
258  {
259  move_parameter(nh, obstacles, source);
260  }
261  move_parameter(nh, obstacles, "observation_sources");
262 
263  ros::NodeHandle inflation(nh, "inflation_layer");
264  move_parameter(nh, inflation, "cost_scaling_factor");
265  move_parameter(nh, inflation, "inflation_radius");
266  map["name"] = XmlRpc::XmlRpcValue("inflation_layer");
267  map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer");
268  super_map.setStruct(&map);
269  plugins.push_back(super_map);
270 
271  super_array.setArray(&plugins);
272  nh.setParam("plugins", super_array);
273 }
274 
275 void Costmap2DROS::copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh)
276 {
277  ros::NodeHandle target_layer(nh, plugin_name);
278 
279  if(plugin_type == "costmap_2d::StaticLayer")
280  {
281  move_parameter(nh, target_layer, "map_topic", false);
282  move_parameter(nh, target_layer, "unknown_cost_value", false);
283  move_parameter(nh, target_layer, "lethal_cost_threshold", false);
284  move_parameter(nh, target_layer, "track_unknown_space", false);
285  }
286  else if(plugin_type == "costmap_2d::VoxelLayer")
287  {
288  move_parameter(nh, target_layer, "origin_z", false);
289  move_parameter(nh, target_layer, "z_resolution", false);
290  move_parameter(nh, target_layer, "z_voxels", false);
291  move_parameter(nh, target_layer, "mark_threshold", false);
292  move_parameter(nh, target_layer, "unknown_threshold", false);
293  move_parameter(nh, target_layer, "publish_voxel_map", false);
294  }
295  else if(plugin_type == "costmap_2d::ObstacleLayer")
296  {
297  move_parameter(nh, target_layer, "max_obstacle_height", false);
298  move_parameter(nh, target_layer, "raytrace_range", false);
299  move_parameter(nh, target_layer, "obstacle_range", false);
300  move_parameter(nh, target_layer, "track_unknown_space", false);
301  }
302  else if(plugin_type == "costmap_2d::InflationLayer")
303  {
304  move_parameter(nh, target_layer, "cost_scaling_factor", false);
305  move_parameter(nh, target_layer, "inflation_radius", false);
306  }
307 }
308 
310 {
311  checkOldParam(nh, "static_map");
312  checkOldParam(nh, "map_type");
313 }
314 
315 void Costmap2DROS::checkOldParam(ros::NodeHandle& nh, const std::string &param_name){
316  if(nh.hasParam(param_name)){
317  ROS_WARN("%s: Pre-Hydro parameter \"%s\" unused since \"plugins\" is provided", name_.c_str(), param_name.c_str());
318  }
319 }
320 
321 void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
322 {
323  transform_tolerance_ = config.transform_tolerance;
324  if (map_update_thread_ != NULL)
325  {
327  map_update_thread_->join();
328  delete map_update_thread_;
329  }
331  double map_update_frequency = config.update_frequency;
332 
333  double map_publish_frequency = config.publish_frequency;
334  if (map_publish_frequency > 0)
335  publish_cycle = ros::Duration(1 / map_publish_frequency);
336  else
338 
339  // find size parameters
340  double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =
341  config.origin_x,
342  origin_y = config.origin_y;
343 
345  {
346  layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
347  (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
348  }
349 
350  // If the padding has changed, call setUnpaddedRobotFootprint() to
351  // re-apply the padding.
352  if (footprint_padding_ != config.footprint_padding)
353  {
354  footprint_padding_ = config.footprint_padding;
356  }
357 
359 
360  old_config_ = config;
361 
362  map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
363 }
364 
365 void Costmap2DROS::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
366  const costmap_2d::Costmap2DConfig &old_config)
367 {
368  // Only change the footprint if footprint or robot_radius has
369  // changed. Otherwise we might overwrite a footprint sent on a
370  // topic by a dynamic_reconfigure call which was setting some other
371  // variable.
372  if (new_config.footprint == old_config.footprint &&
373  new_config.robot_radius == old_config.robot_radius)
374  {
375  return;
376  }
377 
378  if (new_config.footprint != "" && new_config.footprint != "[]")
379  {
380  std::vector<geometry_msgs::Point> new_footprint;
381  if (makeFootprintFromString(new_config.footprint, new_footprint))
382  {
383  setUnpaddedRobotFootprint(new_footprint);
384  }
385  else
386  {
387  ROS_ERROR("Invalid footprint string from dynamic reconfigure");
388  }
389  }
390  else
391  {
392  // robot_radius may be 0, but that must be intended at this point.
393  setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius));
394  }
395 }
396 
397 void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
398 {
399  unpadded_footprint_ = points;
400  padded_footprint_ = points;
402 
404 }
405 
407 {
408  // don't allow configuration to happen while this check occurs
409  // boost::recursive_mutex::scoped_lock mcl(configuration_mutex_);
410 
411  geometry_msgs::PoseStamped new_pose;
412 
413  if (!getRobotPose(new_pose))
414  {
415  ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration");
416  robot_stopped_ = false;
417  }
418  // make sure that the robot is not moving
419  else
420  {
421  old_pose_ = new_pose;
422 
423  robot_stopped_ = (tf2::Vector3(old_pose_.pose.position.x, old_pose_.pose.position.y,
424  old_pose_.pose.position.z).distance(tf2::Vector3(new_pose.pose.position.x,
425  new_pose.pose.position.y, new_pose.pose.position.z)) < 1e-3) &&
426  (tf2::Quaternion(old_pose_.pose.orientation.x,
427  old_pose_.pose.orientation.y,
428  old_pose_.pose.orientation.z,
429  old_pose_.pose.orientation.w).angle(tf2::Quaternion(new_pose.pose.orientation.x,
430  new_pose.pose.orientation.y,
431  new_pose.pose.orientation.z,
432  new_pose.pose.orientation.w)) < 1e-3);
433  }
434 }
435 
436 void Costmap2DROS::mapUpdateLoop(double frequency)
437 {
438  // the user might not want to run the loop every cycle
439  if (frequency == 0.0)
440  return;
441 
442  ros::NodeHandle nh;
443  ros::Rate r(frequency);
444  while (nh.ok() && !map_update_thread_shutdown_)
445  {
446  #ifdef HAVE_SYS_TIME_H
447  struct timeval start, end;
448  double start_t, end_t, t_diff;
449  gettimeofday(&start, NULL);
450  #endif
451 
452  updateMap();
453 
454  #ifdef HAVE_SYS_TIME_H
455  gettimeofday(&end, NULL);
456  start_t = start.tv_sec + double(start.tv_usec) / 1e6;
457  end_t = end.tv_sec + double(end.tv_usec) / 1e6;
458  t_diff = end_t - start_t;
459  ROS_DEBUG("Map update time: %.9f", t_diff);
460  #endif
461 
463  {
464  unsigned int x0, y0, xn, yn;
465  layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
466  publisher_->updateBounds(x0, xn, y0, yn);
467 
468  ros::Time now = ros::Time::now();
469  if (last_publish_ + publish_cycle < now)
470  {
472  last_publish_ = now;
473  }
474  }
475  r.sleep();
476  // make sure to sleep for the remainder of our cycle time
477  if (r.cycleTime() > ros::Duration(1 / frequency))
478  ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
479  r.cycleTime().toSec());
480  }
481 }
482 
484 {
485  if (!stop_updates_)
486  {
487  // get global pose
488  geometry_msgs::PoseStamped pose;
489  if (getRobotPose (pose))
490  {
491  double x = pose.pose.position.x,
492  y = pose.pose.position.y,
493  yaw = tf2::getYaw(pose.pose.orientation);
494 
495  layered_costmap_->updateMap(x, y, yaw);
496 
497  geometry_msgs::PolygonStamped footprint;
498  footprint.header.frame_id = global_frame_;
499  footprint.header.stamp = ros::Time::now();
500  transformFootprint(x, y, yaw, padded_footprint_, footprint);
501  footprint_pub_.publish(footprint);
502 
503  initialized_ = true;
504  }
505  }
506 }
507 
509 {
510  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
511  // check if we're stopped or just paused
512  if (stopped_)
513  {
514  // if we're stopped we need to re-subscribe to topics
515  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
516  ++plugin)
517  {
518  (*plugin)->activate();
519  }
520  stopped_ = false;
521  }
522  stop_updates_ = false;
523 
524  // block until the costmap is re-initialized.. meaning one update cycle has run
525  ros::Rate r(100.0);
526  while (ros::ok() && !initialized_)
527  r.sleep();
528 }
529 
531 {
532  stop_updates_ = true;
533  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
534  // unsubscribe from topics
535  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
536  ++plugin)
537  {
538  (*plugin)->deactivate();
539  }
540  initialized_ = false;
541  stopped_ = true;
542 }
543 
545 {
546  stop_updates_ = true;
547  initialized_ = false;
548 }
549 
551 {
552  stop_updates_ = false;
553 
554  // block until the costmap is re-initialized.. meaning one update cycle has run
555  ros::Rate r(100.0);
556  while (!initialized_)
557  r.sleep();
558 }
559 
560 
562 {
564  top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
565  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
566  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
567  ++plugin)
568  {
569  (*plugin)->reset();
570  }
571 }
572 
573 bool Costmap2DROS::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
574 {
575  tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
576  geometry_msgs::PoseStamped robot_pose;
577  tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
578  robot_pose.header.frame_id = robot_base_frame_;
579  robot_pose.header.stamp = ros::Time();
580  ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
581 
582  // get the global pose of the robot
583  try
584  {
585  // use current time if possible (makes sure it's not in the future)
586  if (tf_.canTransform(global_frame_, robot_base_frame_, current_time))
587  {
588  geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time);
589  tf2::doTransform(robot_pose, global_pose, transform);
590  }
591  // use the latest otherwise
592  else
593  {
594  tf_.transform(robot_pose, global_pose, global_frame_);
595  }
596  }
597  catch (tf2::LookupException& ex)
598  {
599  ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
600  return false;
601  }
602  catch (tf2::ConnectivityException& ex)
603  {
604  ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
605  return false;
606  }
607  catch (tf2::ExtrapolationException& ex)
608  {
609  ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
610  return false;
611  }
612  // check global_pose timeout
613  if (current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_)
614  {
615  ROS_WARN_THROTTLE(1.0,
616  "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
617  current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
618  return false;
619  }
620 
621  return true;
622 }
623 
624 void Costmap2DROS::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
625 {
626  geometry_msgs::PoseStamped global_pose;
627  if (!getRobotPose(global_pose))
628  return;
629 
630  double yaw = tf2::getYaw(global_pose.pose.orientation);
631  transformFootprint(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
632  padded_footprint_, oriented_footprint);
633 }
634 
635 } // namespace costmap_2d
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:430
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...
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
tf2_ros::Buffer & tf_
Used for transforming point clouds.
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:435
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
tf2Scalar angle(const Quaternion &q) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void warnForOldParameters(ros::NodeHandle &nh)
XmlRpcServer s
costmap_2d::Costmap2DConfig old_config_
void pause()
Stops the costmap from updating, but sensor data still comes in over the wire.
void stop()
tf2_ros::Buffer * tf_
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.
bool deleteParam(const std::string &key) const
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
void setFootprint(const std::vector< geometry_msgs::Point > &footprint_spec)
Updates the stored footprint, updates the circumscribed and inscribed radii, and calls onFootprintCha...
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)
void loadOldParameters(ros::NodeHandle &nh)
bool makeFootprintFromString(const std::string &footprint_string, std::vector< geometry_msgs::Point > &footprint)
Make the footprint from the given string.
Definition: footprint.cpp:170
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &target_time, const ros::Duration timeout, std::string *errstr=NULL) const
std::vector< geometry_msgs::Point > toPointVector(geometry_msgs::Polygon polygon)
Convert Polygon msg to vector of Points.
Definition: footprint.cpp:96
LayeredCostmap * layered_costmap_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::map< std::string, XmlRpcValue > ValueStruct
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
void publish(const boost::shared_ptr< M > &message) const
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.
#define ROS_WARN_THROTTLE(period,...)
std::string global_frame_
The global frame for the costmap.
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_THROTTLE(period,...)
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()
static const Transform & getIdentity()
void movementCB(const ros::TimerEvent &event)
bool getRobotPose(geometry_msgs::PoseStamped &global_pose) const
Get the pose of the robot in the global frame of the costmap.
void checkOldParam(ros::NodeHandle &nh, const std::string &param_name)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
Duration cycleTime() const
Costmap2DPublisher * publisher_
geometry_msgs::PoseStamped old_pose_
bool hasParam(const std::string &key) const
pluginlib::ClassLoader< Layer > plugin_loader_
bool sleep()
double getYaw(const A &a)
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)
B toMsg(const A &a)
bool searchParam(const std::string &key, std::string &result) const
A tool to periodically publish visualization data from a Costmap2D.
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)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
void resetLayers()
Reset each individual layer.
void resume()
Resumes costmap updates.
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.
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
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()
bool ok() const
void setArray(XmlRpc::XmlRpcValue::ValueArray *a)
void copyParentParameters(const std::string &plugin_name, const std::string &plugin_type, ros::NodeHandle &nh)
#define ROS_ERROR(...)
void setStruct(XmlRpc::XmlRpcValue::ValueStruct *a)
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 getOrientedFootprint(std::vector< geometry_msgs::Point > &oriented_footprint) const
Build the oriented footprint of the robot at the robot&#39;s current pose.
void publishCostmap()
Publishes the visualization data over ROS.
#define ROS_DEBUG(...)
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 Wed Jun 22 2022 02:07:03