obstacle_layer.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  *********************************************************************/
41 
43 
46 using costmap_2d::FREE_SPACE;
47 
49 using costmap_2d::Observation;
50 
51 namespace costmap_2d
52 {
53 
54 void ObstacleLayer::onInitialize()
55 {
56  ros::NodeHandle nh("~/" + name_), g_nh;
57  rolling_window_ = layered_costmap_->isRolling();
58 
59  bool track_unknown_space;
60  nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
61  if (track_unknown_space)
62  default_value_ = NO_INFORMATION;
63  else
64  default_value_ = FREE_SPACE;
65 
66  ObstacleLayer::matchSize();
67  current_ = true;
68 
69  global_frame_ = layered_costmap_->getGlobalFrameID();
70  double transform_tolerance;
71  nh.param("transform_tolerance", transform_tolerance, 0.2);
72 
73  std::string topics_string;
74  // get the topics that we'll subscribe to from the parameter server
75  nh.param("observation_sources", topics_string, std::string(""));
76  ROS_INFO(" Subscribed to Topics: %s", topics_string.c_str());
77 
78  // get our tf prefix
79  ros::NodeHandle prefix_nh;
80  const std::string tf_prefix = tf::getPrefixParam(prefix_nh);
81 
82  // now we need to split the topics based on whitespace which we can use a stringstream for
83  std::stringstream ss(topics_string);
84 
85  std::string source;
86  while (ss >> source)
87  {
88  ros::NodeHandle source_node(nh, source);
89 
90  // get the parameters for the specific topic
91  double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
92  std::string topic, sensor_frame, data_type;
93  bool inf_is_valid, clearing, marking;
94 
95  source_node.param("topic", topic, source);
96  source_node.param("sensor_frame", sensor_frame, std::string(""));
97  source_node.param("observation_persistence", observation_keep_time, 0.0);
98  source_node.param("expected_update_rate", expected_update_rate, 0.0);
99  source_node.param("data_type", data_type, std::string("PointCloud"));
100  source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
101  source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
102  source_node.param("inf_is_valid", inf_is_valid, false);
103  source_node.param("clearing", clearing, false);
104  source_node.param("marking", marking, true);
105 
106  if (!sensor_frame.empty())
107  {
108  sensor_frame = tf::resolve(tf_prefix, sensor_frame);
109  }
110 
111  if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
112  {
113  ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
114  throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
115  }
116 
117  std::string raytrace_range_param_name, obstacle_range_param_name;
118 
119  // get the obstacle range for the sensor
120  double obstacle_range = 2.5;
121  if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
122  {
123  source_node.getParam(obstacle_range_param_name, obstacle_range);
124  }
125 
126  // get the raytrace range for the sensor
127  double raytrace_range = 3.0;
128  if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
129  {
130  source_node.getParam(raytrace_range_param_name, raytrace_range);
131  }
132 
133  ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(),
134  sensor_frame.c_str());
135 
136  // create an observation buffer
137  observation_buffers_.push_back(
138  boost::shared_ptr < ObservationBuffer
139  > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
140  max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
141  sensor_frame, transform_tolerance)));
142 
143  // check if we'll add this buffer to our marking observation buffers
144  if (marking)
145  marking_buffers_.push_back(observation_buffers_.back());
146 
147  // check if we'll also add this buffer to our clearing observation buffers
148  if (clearing)
149  clearing_buffers_.push_back(observation_buffers_.back());
150 
151  ROS_DEBUG(
152  "Created an observation buffer for source %s, topic %s, global frame: %s, "
153  "expected update rate: %.2f, observation persistence: %.2f",
154  source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
155 
156  // create a callback for the topic
157  if (data_type == "LaserScan")
158  {
160  > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
161 
163  > filter(new tf::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50));
164 
165  if (inf_is_valid)
166  {
167  filter->registerCallback(
168  boost::bind(&ObstacleLayer::laserScanValidInfCallback, this, _1, observation_buffers_.back()));
169  }
170  else
171  {
172  filter->registerCallback(
173  boost::bind(&ObstacleLayer::laserScanCallback, this, _1, observation_buffers_.back()));
174  }
175 
176  observation_subscribers_.push_back(sub);
177  observation_notifiers_.push_back(filter);
178 
179  observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
180  }
181  else if (data_type == "PointCloud")
182  {
184  > sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
185 
186  if (inf_is_valid)
187  {
188  ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
189  }
190 
192  > filter(new tf::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50));
193  filter->registerCallback(
194  boost::bind(&ObstacleLayer::pointCloudCallback, this, _1, observation_buffers_.back()));
195 
196  observation_subscribers_.push_back(sub);
197  observation_notifiers_.push_back(filter);
198  }
199  else
200  {
202  > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
203 
204  if (inf_is_valid)
205  {
206  ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
207  }
208 
210  > filter(new tf::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50));
211  filter->registerCallback(
212  boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back()));
213 
214  observation_subscribers_.push_back(sub);
215  observation_notifiers_.push_back(filter);
216  }
217 
218  if (sensor_frame != "")
219  {
220  std::vector < std::string > target_frames;
221  target_frames.push_back(global_frame_);
222  target_frames.push_back(sensor_frame);
223  observation_notifiers_.back()->setTargetFrames(target_frames);
224  }
225  }
226 
227  dsrv_ = NULL;
228  setupDynamicReconfigure(nh);
229 }
230 
231 void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
232 {
233  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
234  dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb = boost::bind(
235  &ObstacleLayer::reconfigureCB, this, _1, _2);
236  dsrv_->setCallback(cb);
237 }
238 
239 ObstacleLayer::~ObstacleLayer()
240 {
241  if (dsrv_)
242  delete dsrv_;
243 }
244 void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
245 {
246  enabled_ = config.enabled;
247  footprint_clearing_enabled_ = config.footprint_clearing_enabled;
248  max_obstacle_height_ = config.max_obstacle_height;
249  combination_method_ = config.combination_method;
250 }
251 
252 void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
254 {
255  // project the laser into a point cloud
256  sensor_msgs::PointCloud2 cloud;
257  cloud.header = message->header;
258 
259  // project the scan into a point cloud
260  try
261  {
262  projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
263  }
264  catch (tf::TransformException &ex)
265  {
266  ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
267  ex.what());
268  projector_.projectLaser(*message, cloud);
269  }
270 
271  // buffer the point cloud
272  buffer->lock();
273  buffer->bufferCloud(cloud);
274  buffer->unlock();
275 }
276 
277 void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message,
279 {
280  // Filter positive infinities ("Inf"s) to max_range.
281  float epsilon = 0.0001; // a tenth of a millimeter
282  sensor_msgs::LaserScan message = *raw_message;
283  for (size_t i = 0; i < message.ranges.size(); i++)
284  {
285  float range = message.ranges[ i ];
286  if (!std::isfinite(range) && range > 0)
287  {
288  message.ranges[ i ] = message.range_max - epsilon;
289  }
290  }
291 
292  // project the laser into a point cloud
293  sensor_msgs::PointCloud2 cloud;
294  cloud.header = message.header;
295 
296  // project the scan into a point cloud
297  try
298  {
299  projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
300  }
301  catch (tf::TransformException &ex)
302  {
303  ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
304  global_frame_.c_str(), ex.what());
305  projector_.projectLaser(message, cloud);
306  }
307 
308  // buffer the point cloud
309  buffer->lock();
310  buffer->bufferCloud(cloud);
311  buffer->unlock();
312 }
313 
314 void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message,
316 {
317  sensor_msgs::PointCloud2 cloud2;
318 
319  if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2))
320  {
321  ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
322  return;
323  }
324 
325  // buffer the point cloud
326  buffer->lock();
327  buffer->bufferCloud(cloud2);
328  buffer->unlock();
329 }
330 
331 void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
333 {
334  // buffer the point cloud
335  buffer->lock();
336  buffer->bufferCloud(*message);
337  buffer->unlock();
338 }
339 
340 void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
341  double* min_y, double* max_x, double* max_y)
342 {
343  if (rolling_window_)
344  updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
345  if (!enabled_)
346  return;
347  useExtraBounds(min_x, min_y, max_x, max_y);
348 
349  bool current = true;
350  std::vector<Observation> observations, clearing_observations;
351 
352  // get the marking observations
353  current = current && getMarkingObservations(observations);
354 
355  // get the clearing observations
356  current = current && getClearingObservations(clearing_observations);
357 
358  // update the global current status
359  current_ = current;
360 
361  // raytrace freespace
362  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
363  {
364  raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
365  }
366 
367  // place the new obstacles into a priority queue... each with a priority of zero to begin with
368  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
369  {
370  const Observation& obs = *it;
371 
372  const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
373 
374  double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
375 
376  for (unsigned int i = 0; i < cloud.points.size(); ++i)
377  {
378  double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
379 
380  // if the obstacle is too high or too far away from the robot we won't add it
381  if (pz > max_obstacle_height_)
382  {
383  ROS_DEBUG("The point is too high");
384  continue;
385  }
386 
387  // compute the squared distance from the hitpoint to the pointcloud's origin
388  double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
389  + (pz - obs.origin_.z) * (pz - obs.origin_.z);
390 
391  // if the point is far enough away... we won't consider it
392  if (sq_dist >= sq_obstacle_range)
393  {
394  ROS_DEBUG("The point is too far away");
395  continue;
396  }
397 
398  // now we need to compute the map coordinates for the observation
399  unsigned int mx, my;
400  if (!worldToMap(px, py, mx, my))
401  {
402  ROS_DEBUG("Computing map coords failed");
403  continue;
404  }
405 
406  unsigned int index = getIndex(mx, my);
407  costmap_[index] = LETHAL_OBSTACLE;
408  touch(px, py, min_x, min_y, max_x, max_y);
409  }
410  }
411 
412  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
413 }
414 
415 void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
416  double* max_x, double* max_y)
417 {
418  if (!footprint_clearing_enabled_) return;
419  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
420 
421  for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
422  {
423  touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
424  }
425 }
426 
427 void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
428 {
429  if (!enabled_)
430  return;
431 
432  if (footprint_clearing_enabled_)
433  {
434  setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
435  }
436 
437  switch (combination_method_)
438  {
439  case 0: // Overwrite
440  updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
441  break;
442  case 1: // Maximum
443  updateWithMax(master_grid, min_i, min_j, max_i, max_j);
444  break;
445  default: // Nothing
446  break;
447  }
448 }
449 
450 void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
451 {
452  if (marking)
453  static_marking_observations_.push_back(obs);
454  if (clearing)
455  static_clearing_observations_.push_back(obs);
456 }
457 
458 void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
459 {
460  if (marking)
461  static_marking_observations_.clear();
462  if (clearing)
463  static_clearing_observations_.clear();
464 }
465 
466 bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
467 {
468  bool current = true;
469  // get the marking observations
470  for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
471  {
472  marking_buffers_[i]->lock();
473  marking_buffers_[i]->getObservations(marking_observations);
474  current = marking_buffers_[i]->isCurrent() && current;
475  marking_buffers_[i]->unlock();
476  }
477  marking_observations.insert(marking_observations.end(),
478  static_marking_observations_.begin(), static_marking_observations_.end());
479  return current;
480 }
481 
482 bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
483 {
484  bool current = true;
485  // get the clearing observations
486  for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
487  {
488  clearing_buffers_[i]->lock();
489  clearing_buffers_[i]->getObservations(clearing_observations);
490  current = clearing_buffers_[i]->isCurrent() && current;
491  clearing_buffers_[i]->unlock();
492  }
493  clearing_observations.insert(clearing_observations.end(),
494  static_clearing_observations_.begin(), static_clearing_observations_.end());
495  return current;
496 }
497 
498 void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
499  double* max_x, double* max_y)
500 {
501  double ox = clearing_observation.origin_.x;
502  double oy = clearing_observation.origin_.y;
503  pcl::PointCloud < pcl::PointXYZ > cloud = *(clearing_observation.cloud_);
504 
505  // get the map coordinates of the origin of the sensor
506  unsigned int x0, y0;
507  if (!worldToMap(ox, oy, x0, y0))
508  {
510  1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
511  ox, oy);
512  return;
513  }
514 
515  // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
516  double origin_x = origin_x_, origin_y = origin_y_;
517  double map_end_x = origin_x + size_x_ * resolution_;
518  double map_end_y = origin_y + size_y_ * resolution_;
519 
520 
521  touch(ox, oy, min_x, min_y, max_x, max_y);
522 
523  // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
524  for (unsigned int i = 0; i < cloud.points.size(); ++i)
525  {
526  double wx = cloud.points[i].x;
527  double wy = cloud.points[i].y;
528 
529  // now we also need to make sure that the enpoint we're raytracing
530  // to isn't off the costmap and scale if necessary
531  double a = wx - ox;
532  double b = wy - oy;
533 
534  // the minimum value to raytrace from is the origin
535  if (wx < origin_x)
536  {
537  double t = (origin_x - ox) / a;
538  wx = origin_x;
539  wy = oy + b * t;
540  }
541  if (wy < origin_y)
542  {
543  double t = (origin_y - oy) / b;
544  wx = ox + a * t;
545  wy = origin_y;
546  }
547 
548  // the maximum value to raytrace to is the end of the map
549  if (wx > map_end_x)
550  {
551  double t = (map_end_x - ox) / a;
552  wx = map_end_x - .001;
553  wy = oy + b * t;
554  }
555  if (wy > map_end_y)
556  {
557  double t = (map_end_y - oy) / b;
558  wx = ox + a * t;
559  wy = map_end_y - .001;
560  }
561 
562  // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
563  unsigned int x1, y1;
564 
565  // check for legality just in case
566  if (!worldToMap(wx, wy, x1, y1))
567  continue;
568 
569  unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
570  MarkCell marker(costmap_, FREE_SPACE);
571  // and finally... we can execute our trace to clear obstacles along that line
572  raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
573 
574  updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
575  }
576 }
577 
578 void ObstacleLayer::activate()
579 {
580  // if we're stopped we need to re-subscribe to topics
581  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
582  {
583  if (observation_subscribers_[i] != NULL)
584  observation_subscribers_[i]->subscribe();
585  }
586 
587  for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
588  {
589  if (observation_buffers_[i])
590  observation_buffers_[i]->resetLastUpdated();
591  }
592 }
593 void ObstacleLayer::deactivate()
594 {
595  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
596  {
597  if (observation_subscribers_[i] != NULL)
598  observation_subscribers_[i]->unsubscribe();
599  }
600 }
601 
602 void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
603  double* min_x, double* min_y, double* max_x, double* max_y)
604 {
605  double dx = wx-ox, dy = wy-oy;
606  double full_distance = hypot(dx, dy);
607  double scale = std::min(1.0, range / full_distance);
608  double ex = ox + dx * scale, ey = oy + dy * scale;
609  touch(ex, ey, min_x, min_y, max_x, max_y);
610 }
611 
612 void ObstacleLayer::reset()
613 {
614  deactivate();
615  resetMaps();
616  current_ = true;
617  activate();
618 }
619 
620 } // namespace costmap_2d
geometry_msgs::Point origin_
Definition: observation.h:97
double epsilon
#define ROS_FATAL(...)
std::string getPrefixParam(ros::NodeHandle &nh)
pcl::PointCloud< pcl::PointXYZ > * cloud_
Definition: observation.h:98
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
Stores an observation in terms of a point cloud and the origin of the source.
Definition: observation.h:47
tf::TransformListener * tf_
TFSIMD_FORCE_INLINE const tfScalar & y() const
static const unsigned char FREE_SPACE
Definition: cost_values.h:45
#define ROS_WARN(...)
std::string resolve(const std::string &prefix, const std::string &frame_name)
Takes in point clouds from sensors, transforms them to the desired frame, and stores them...
#define ROS_WARN_THROTTLE(period,...)
#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
TFSIMD_FORCE_INLINE const tfScalar & x() const
static bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:43
static const unsigned char NO_INFORMATION
Definition: cost_values.h:42
bool getParam(const std::string &key, std::string &s) const
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
#define ROS_ERROR(...)
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
#define ROS_DEBUG(...)


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:17