Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
static_layer.cpp
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, 2013, Willow Garage, Inc.
6  * Copyright (c) 2015, Fetch Robotics, Inc.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Eitan Marder-Eppstein
37  * David V. Lu!!
38  *********************************************************************/
39 
40 #include "nav2_costmap_2d/static_layer.hpp"
41 
42 #include <algorithm>
43 #include <string>
44 
45 #include "pluginlib/class_list_macros.hpp"
46 #include "tf2/convert.hpp"
47 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
48 #include "nav2_ros_common/validate_messages.hpp"
49 #include "nav2_util/robot_utils.hpp"
50 
51 #define EPSILON 1e-5
52 
54 
55 using nav2_costmap_2d::NO_INFORMATION;
56 using nav2_costmap_2d::LETHAL_OBSTACLE;
57 using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
58 using nav2_costmap_2d::FREE_SPACE;
59 using rcl_interfaces::msg::ParameterType;
60 
61 namespace nav2_costmap_2d
62 {
63 
65 : map_buffer_(nullptr)
66 {
67 }
68 
70 {
71 }
72 
73 void
75 {
76  global_frame_ = layered_costmap_->getGlobalFrameID();
77 
78  getParameters();
79 
80  rclcpp::QoS map_qos = nav2::qos::StandardTopicQoS(); // initialize to default
81  if (map_subscribe_transient_local_) {
83  }
84 
85  RCLCPP_INFO(
86  logger_,
87  "Subscribing to the map topic (%s) with %s durability",
88  map_topic_.c_str(),
89  map_subscribe_transient_local_ ? "transient local" : "volatile");
90 
91  auto node = node_.lock();
92  if (!node) {
93  throw std::runtime_error{"Failed to lock node"};
94  }
95 
96  map_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
97  map_topic_,
98  std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1),
99  map_qos);
100 
101  if (subscribe_to_updates_) {
102  RCLCPP_INFO(logger_, "Subscribing to updates");
103  map_update_sub_ = node->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
104  map_topic_ + "_updates",
105  std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
106  }
107 }
108 
109 void
111 {
112  auto node = node_.lock();
113  // Add callback for dynamic parameters
114  post_set_params_handler_ = node->add_post_set_parameters_callback(
115  std::bind(
117  this, std::placeholders::_1));
118  on_set_params_handler_ = node->add_on_set_parameters_callback(
119  std::bind(
121  this, std::placeholders::_1));
122 }
123 
124 void
126 {
127  auto node = node_.lock();
128  if (post_set_params_handler_ && node) {
129  node->remove_post_set_parameters_callback(post_set_params_handler_.get());
130  }
131  post_set_params_handler_.reset();
132  if (on_set_params_handler_ && node) {
133  node->remove_on_set_parameters_callback(on_set_params_handler_.get());
134  }
135  on_set_params_handler_.reset();
136 }
137 
138 void
140 {
141  has_updated_data_ = true;
142  setCurrent(false);
143 }
144 
145 void
147 {
148  int temp_lethal_threshold = 0;
149 
150  auto node = node_.lock();
151  if (!node) {
152  throw std::runtime_error{"Failed to lock node"};
153  }
154 
155  enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true);
156  resize_master_ = node->declare_or_get_parameter(name_ + "." + "resize_master", true);
157  subscribe_to_updates_ = node->declare_or_get_parameter(
158  name_ + "." + "subscribe_to_updates", false);
159  footprint_clearing_enabled_ = node->declare_or_get_parameter(
160  name_ + "." + "footprint_clearing_enabled", false);
161  restore_cleared_footprint_ = node->declare_or_get_parameter(
162  name_ + "." + "restore_cleared_footprint", true);
163  map_topic_ = node->declare_or_get_parameter(
164  name_ + "." + "map_topic", std::string("map"));
165  map_topic_ = joinWithParentNamespace(map_topic_);
166  map_subscribe_transient_local_ = node->declare_or_get_parameter(
167  name_ + "." + "map_subscribe_transient_local", true);
168  transform_staleness_threshold_ = node->declare_or_get_parameter(
169  name_ + "." + "transform_staleness_threshold", 0.0);
170  node->get_parameter("track_unknown_space", track_unknown_space_);
171  node->get_parameter("use_maximum", use_maximum_);
172  node->get_parameter("lethal_cost_threshold", temp_lethal_threshold);
173  node->get_parameter("inscribed_obstacle_cost_value", inscribed_obstacle_cost_value_);
174  node->get_parameter("unknown_cost_value", unknown_cost_value_);
175  node->get_parameter("trinary_costmap", trinary_costmap_);
176 
177  // Enforce bounds
178  lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
179  map_received_ = false;
180  map_received_in_update_bounds_ = false;
181 }
182 
183 void
184 StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
185 {
186  RCLCPP_DEBUG(logger_, "StaticLayer: Process map");
187 
188  unsigned int size_x = new_map.info.width;
189  unsigned int size_y = new_map.info.height;
190 
191  RCLCPP_DEBUG(
192  logger_,
193  "StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y,
194  new_map.info.resolution);
195 
196  // resize costmap if size, resolution or origin do not match
197  Costmap2D * master = layered_costmap_->getCostmap();
198  if (usesMasterCostmapSize() && (master->getSizeInCellsX() != size_x ||
199  master->getSizeInCellsY() != size_y ||
200  !isEqual(master->getResolution(), new_map.info.resolution, EPSILON) ||
201  !isEqual(master->getOriginX(), new_map.info.origin.position.x, EPSILON) ||
202  !isEqual(master->getOriginY(), new_map.info.origin.position.y, EPSILON) ||
203  !layered_costmap_->isSizeLocked()))
204  {
205  // Update the size of the layered costmap (and all layers, including this one)
206  RCLCPP_INFO(
207  logger_,
208  "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y,
209  new_map.info.resolution);
210 
211  double fmod_x = std::fmod(new_map.info.origin.position.x, new_map.info.resolution);
212  double fmod_y = std::fmod(new_map.info.origin.position.y, new_map.info.resolution);
213 
214  if (std::abs(fmod_x) > EPSILON || std::abs(fmod_y) > EPSILON) {
215  RCLCPP_WARN(
216  logger_,
217  "StaticLayer: Costmap origin coordinates are not perfectly aligned with the resolution. "
218  "This may cause misalignment aliasing between rolling and non-rolling costmaps.\n"
219  "Map origin: (%.f, %.f) | Resolution: %.f",
220  new_map.info.origin.position.x, new_map.info.origin.position.y,
221  new_map.info.resolution);
222  }
223 
224  layered_costmap_->resizeMap(
225  size_x, size_y, new_map.info.resolution,
226  new_map.info.origin.position.x,
227  new_map.info.origin.position.y,
228  true);
229  } else if (size_x_ != size_x || size_y_ != size_y || // NOLINT
230  !isEqual(resolution_, new_map.info.resolution, EPSILON) ||
231  !isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) ||
232  !isEqual(origin_y_, new_map.info.origin.position.y, EPSILON))
233  {
234  // only update the size of the costmap stored locally in this layer
235  RCLCPP_INFO(
236  logger_,
237  "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y,
238  new_map.info.resolution);
239  if (!layered_costmap_->isRolling() && !resize_master_ && size_x_ > 0 && size_y_ > 0) {
240  // Re-render the previous map's extent so a moved or shrunk map clears what it covered
242  origin_x_, origin_y_,
243  origin_x_ + size_x_ * resolution_, origin_y_ + size_y_ * resolution_);
244  }
245  resizeMap(
246  size_x, size_y, new_map.info.resolution,
247  new_map.info.origin.position.x, new_map.info.origin.position.y);
248  }
249 
250  unsigned int index = 0;
251 
252  // we have a new map, update full size of map
253  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
254 
255  // initialize the costmap with static data
256  for (unsigned int i = 0; i < size_y; ++i) {
257  for (unsigned int j = 0; j < size_x; ++j) {
258  unsigned char value = new_map.data[index];
259  costmap_[index] = interpretValue(value);
260  ++index;
261  }
262  }
263 
264  map_frame_ = new_map.header.frame_id;
265 
266  x_ = y_ = 0;
267  width_ = size_x_;
268  height_ = size_y_;
269  has_updated_data_ = true;
270 
271  setCurrent(true);
272 }
273 
274 void
276 {
277  // If we are using rolling costmap or not resizing the master, the static map size is
278  // unrelated to the size of the layered costmap
279  if (usesMasterCostmapSize()) {
280  Costmap2D * master = layered_costmap_->getCostmap();
281  resizeMap(
282  master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
283  master->getOriginX(), master->getOriginY());
284  } else {
285  // The master was resized (and cleared) by someone else: repaint our extent into it
286  has_updated_data_ = true;
287  }
288 }
289 
290 bool
292 {
293  return !layered_costmap_->isRolling() && resize_master_;
294 }
295 
296 unsigned char
297 StaticLayer::interpretValue(unsigned char value)
298 {
299  // check if the static value is above the unknown or lethal thresholds
300  if (track_unknown_space_ && value == unknown_cost_value_) {
301  return NO_INFORMATION;
302  } else if (!track_unknown_space_ && value == unknown_cost_value_) {
303  return FREE_SPACE;
304  } else if (value == inscribed_obstacle_cost_value_) {
305  return INSCRIBED_INFLATED_OBSTACLE;
306  } else if (value >= lethal_threshold_) {
307  return LETHAL_OBSTACLE;
308  } else if (trinary_costmap_) {
309  return FREE_SPACE;
310  }
311 
312  double scale = static_cast<double>(value) / lethal_threshold_;
313  return scale * LETHAL_OBSTACLE;
314 }
315 
316 void
317 StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & new_map)
318 {
319  if (!nav2::validateMsg(*new_map)) {
320  RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting.");
321  return;
322  }
323  if (!layered_costmap_->isRolling() && !resize_master_ &&
324  new_map->header.frame_id != global_frame_)
325  {
326  // Non-rolling bounds are reported in the map frame, so it must be the costmap frame
327  RCLCPP_ERROR_THROTTLE(
328  logger_, *clock_, 10000,
329  "StaticLayer: Map in frame %s ignored: with resize_master false on a non-rolling costmap "
330  "the map must be in the costmap global frame (%s)",
331  new_map->header.frame_id.c_str(), global_frame_.c_str());
332  return;
333  }
334  if (!map_received_) {
335  processMap(*new_map);
336  map_received_ = true;
337  return;
338  }
339  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
340  map_buffer_ = new_map;
341  setCurrent(false);
342 }
343 
344 void
345 StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
346 {
347  if (!nav2::validateMsg(*update)) {
348  RCLCPP_ERROR(logger_, "Received map update is malformed. Rejecting.");
349  return;
350  }
351 
352  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
353  if (update->y < static_cast<int32_t>(y_) ||
354  y_ + height_ < update->y + update->height ||
355  update->x < static_cast<int32_t>(x_) ||
356  x_ + width_ < update->x + update->width)
357  {
358  RCLCPP_WARN(
359  logger_,
360  "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n"
361  "Static layer origin: %d, %d bounds: %d X %d\n"
362  "Update origin: %d, %d bounds: %d X %d",
363  x_, y_, width_, height_, update->x, update->y, update->width,
364  update->height);
365  return;
366  }
367 
368  if (update->header.frame_id != map_frame_) {
369  RCLCPP_WARN(
370  logger_,
371  "StaticLayer: Map update ignored. Current map is in frame %s "
372  "but update was in frame %s",
373  map_frame_.c_str(), update->header.frame_id.c_str());
374  return;
375  }
376 
377  unsigned int di = 0;
378  for (unsigned int y = 0; y < update->height; y++) {
379  unsigned int index_base = (update->y + y) * size_x_;
380  for (unsigned int x = 0; x < update->width; x++) {
381  unsigned int index = index_base + x + update->x;
382  costmap_[index] = interpretValue(update->data[di++]);
383  }
384  }
385 
386  has_updated_data_ = true;
387 }
388 
389 
390 void
392  double robot_x, double robot_y, double robot_yaw, double * min_x,
393  double * min_y,
394  double * max_x,
395  double * max_y)
396 {
397  if (!map_received_) {
398  map_received_in_update_bounds_ = false;
399  return;
400  }
401  map_received_in_update_bounds_ = true;
402 
403  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
404 
405  // If there is a new available map, load it.
406  if (map_buffer_) {
407  processMap(*map_buffer_);
408  map_buffer_ = nullptr;
409  }
410 
411  if (!layered_costmap_->isRolling() ) {
412  if (!(has_updated_data_ || has_extra_bounds_)) {
413  return;
414  }
415  }
416 
417  useExtraBounds(min_x, min_y, max_x, max_y);
418 
419  if (layered_costmap_->isRolling()) {
420  // For rolling costmaps the global_frame (e.g. odom) differs from the
421  // map frame. mapToWorld() returns coordinates in the map frame, but
422  // the layered costmap interprets bounds in its global_frame. Report
423  // bounds that cover the full rolling window using the robot pose,
424  // which is already in the correct frame. updateCosts() handles the
425  // per-cell map↔odom transform itself.
426  Costmap2D * master = layered_costmap_->getCostmap();
427  double half_w = master->getSizeInMetersX() / 2.0;
428  double half_h = master->getSizeInMetersY() / 2.0;
429  *min_x = std::min(robot_x - half_w, *min_x);
430  *min_y = std::min(robot_y - half_h, *min_y);
431  *max_x = std::max(robot_x + half_w, *max_x);
432  *max_y = std::max(robot_y + half_h, *max_y);
433  } else {
434  double wx, wy;
435 
436  mapToWorld(x_, y_, wx, wy);
437  *min_x = std::min(wx, *min_x);
438  *min_y = std::min(wy, *min_y);
439 
440  mapToWorld(x_ + width_, y_ + height_, wx, wy);
441  *max_x = std::max(wx, *max_x);
442  *max_y = std::max(wy, *max_y);
443  }
444 
445  has_updated_data_ = false;
446 
447  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
448 }
449 
450 void
452  double robot_x, double robot_y, double robot_yaw,
453  double * min_x, double * min_y,
454  double * max_x,
455  double * max_y)
456 {
457  if (!footprint_clearing_enabled_) {return;}
458 
459  transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
460 
461  for (unsigned int i = 0; i < transformed_footprint_.size(); i++) {
462  touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
463  }
464 }
465 
466 void
468  nav2_costmap_2d::Costmap2D & master_grid,
469  int min_i, int min_j, int max_i, int max_j)
470 {
471  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
472  if (!enabled_) {
473  return;
474  }
475  if (!map_received_in_update_bounds_) {
476  static int count = 0;
477  // throttle warning down to only 1/10 message rate
478  if (++count == 10) {
479  RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received");
480  count = 0;
481  }
482  return;
483  }
484 
485  // global_frame_ -> map_frame_; only needed for rolling costmaps, where the frames differ
486  tf2::Transform tf2_transform = tf2::Transform::getIdentity();
487  if (layered_costmap_->isRolling()) {
488  geometry_msgs::msg::TransformStamped transform;
489  if (!nav2_util::lookupTransformWithStalenessCheck(
490  *tf_, map_frame_, global_frame_, clock_->now(),
491  transform_staleness_threshold_, transform))
492  {
493  RCLCPP_ERROR(logger_, "StaticLayer: transform lookup failed or returned stale data");
494  has_updated_data_ = true;
495  setCurrent(false);
496  return;
497  }
498  tf2::fromMsg(transform.transform, tf2_transform);
499  }
500 
501  std::vector<MapLocation> map_region_to_restore;
502  if (footprint_clearing_enabled_) {
503  // The footprint is in global_frame_ but is rasterized into this layer's map_frame_ grid
504  std::vector<geometry_msgs::msg::Point> footprint_in_map_frame = transformed_footprint_;
505  if (layered_costmap_->isRolling()) {
506  for (auto & point : footprint_in_map_frame) {
507  const tf2::Vector3 p = tf2_transform * tf2::Vector3(point.x, point.y, 0);
508  point.x = p.x();
509  point.y = p.y();
510  }
511  }
512  map_region_to_restore.reserve(100);
513  getMapRegionOccupiedByPolygon(footprint_in_map_frame, map_region_to_restore);
514  setMapRegionOccupiedByPolygon(map_region_to_restore, nav2_costmap_2d::FREE_SPACE);
515  }
516 
517  if (usesMasterCostmapSize()) {
518  // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
519  if (!use_maximum_) {
520  updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
521  } else {
522  updateWithMax(master_grid, min_i, min_j, max_i, max_j);
523  }
524  } else {
525  // If rolling window or not resizing the master, the master_grid is unlikely to have
526  // same coordinates as this layer
527  unsigned int mx, my;
528  double wx, wy;
529 
530  for (int i = min_i; i < max_i; ++i) {
531  for (int j = min_j; j < max_j; ++j) {
532  // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
533  layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
534  // Transform from global_frame_ to map_frame_
535  tf2::Vector3 p(wx, wy, 0);
536  p = tf2_transform * p;
537  // Set master_grid with cell from map
538  if (worldToMap(p.x(), p.y(), mx, my)) {
539  const unsigned char cost = getCost(mx, my);
540  if (!use_maximum_) {
541  master_grid.setCost(i, j, cost);
542  } else if (cost != NO_INFORMATION) {
543  // Same rule as updateWithMax: unknown is transparent, known beats unknown
544  const unsigned char old_cost = master_grid.getCost(i, j);
545  if (old_cost == NO_INFORMATION || cost > old_cost) {
546  master_grid.setCost(i, j, cost);
547  }
548  }
549  }
550  }
551  }
552  }
553 
554  if (footprint_clearing_enabled_ && restore_cleared_footprint_) {
555  // restore the map region occupied by the polygon using cached data
556  restoreMapRegionOccupiedByPolygon(map_region_to_restore);
557  }
558  setCurrent(true);
559 }
560 
568 bool StaticLayer::isEqual(double a, double b, double epsilon)
569 {
570  return std::abs(a - b) < epsilon;
571 }
572 
573 rcl_interfaces::msg::SetParametersResult StaticLayer::validateParameterUpdatesCallback(
574  const std::vector<rclcpp::Parameter> & parameters)
575 {
576  rcl_interfaces::msg::SetParametersResult result;
577  result.successful = true;
578  for (const auto & parameter : parameters) {
579  const auto & param_type = parameter.get_type();
580  const auto & param_name = parameter.get_name();
581  if (param_name.find(name_ + ".") != 0) {
582  continue;
583  }
584 
585  if (param_name == name_ + "." + "map_subscribe_transient_local" ||
586  param_name == name_ + "." + "map_topic" ||
587  param_name == name_ + "." + "subscribe_to_updates" ||
588  param_name == name_ + "." + "resize_master")
589  {
590  RCLCPP_WARN(
591  logger_, "%s is not a dynamic parameter "
592  "cannot be changed while running. Rejecting parameter update.", param_name.c_str());
593  } else if (param_type == ParameterType::PARAMETER_BOOL && // NOLINT
594  param_name == name_ + "." + "restore_cleared_footprint")
595  {
596  if (!footprint_clearing_enabled_) {
597  RCLCPP_WARN(
598  logger_, "restore_cleared_footprint cannot be used "
599  "when footprint_clearing_enabled is False. Rejecting parameter update.");
600  result.successful = false;
601  }
602  }
603  }
604  return result;
605 }
606 
607 void
609  const std::vector<rclcpp::Parameter> & parameters)
610 {
611  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
612 
613  for (const auto & parameter : parameters) {
614  const auto & param_type = parameter.get_type();
615  const auto & param_name = parameter.get_name();
616  if (param_name.find(name_ + ".") != 0) {
617  continue;
618  }
619 
620  if (param_type == ParameterType::PARAMETER_BOOL) {
621  if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
622  enabled_ = parameter.as_bool();
623 
624  x_ = y_ = 0;
625  width_ = size_x_;
626  height_ = size_y_;
627  has_updated_data_ = true;
628  setCurrent(false);
629  } else if (param_name == name_ + "." + "footprint_clearing_enabled") {
630  footprint_clearing_enabled_ = parameter.as_bool();
631  } else if (param_name == name_ + "." + "restore_cleared_footprint") {
632  restore_cleared_footprint_ = parameter.as_bool();
633  }
634  } else if (param_type == ParameterType::PARAMETER_DOUBLE && // NOLINT
635  param_name == name_ + "." + "transform_staleness_threshold")
636  {
637  transform_staleness_threshold_ = parameter.as_double();
638  }
639  }
640 }
641 
642 } // namespace nav2_costmap_2d
A QoS profile for latched, reliable topics with a history of 10 messages.
A QoS profile for standard reliable topics with a history of 10 messages.
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.hpp:69
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
Convert from map coordinates to world coordinates.
Definition: costmap_2d.cpp:280
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
Resize the costmap.
Definition: costmap_2d.cpp:111
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
Definition: costmap_2d.cpp:265
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
Convert from world coordinates to map coordinates.
Definition: costmap_2d.cpp:292
double getResolution() const
Accessor for the resolution of the costmap.
Definition: costmap_2d.cpp:578
double getSizeInMetersY() const
Accessor for the y size of the costmap in meters.
Definition: costmap_2d.cpp:563
double getSizeInMetersX() const
Accessor for the x size of the costmap in meters.
Definition: costmap_2d.cpp:558
void setMapRegionOccupiedByPolygon(const std::vector< MapLocation > &polygon_map_region, unsigned char new_cost_value)
Sets the given map region to desired value.
Definition: costmap_2d.cpp:421
void restoreMapRegionOccupiedByPolygon(const std::vector< MapLocation > &polygon_map_region)
Restores the corresponding map region using given map region.
Definition: costmap_2d.cpp:430
bool getMapRegionOccupiedByPolygon(const std::vector< geometry_msgs::msg::Point > &polygon, std::vector< MapLocation > &polygon_map_region)
Gets the map region occupied by polygon.
Definition: costmap_2d.cpp:438
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:548
double getOriginY() const
Accessor for the y origin of the costmap.
Definition: costmap_2d.cpp:573
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:553
double getOriginX() const
Accessor for the x origin of the costmap.
Definition: costmap_2d.cpp:568
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
Set the cost of a cell in the costmap.
Definition: costmap_2d.cpp:275
void addExtraBounds(double mx0, double my0, double mx1, double my1)
void touch(double x, double y, double *min_x, double *min_y, double *max_x, double *max_y)
Abstract class for layered costmap plugin implementations.
Definition: layer.hpp:62
std::string joinWithParentNamespace(const std::string &topic)
Definition: layer.cpp:83
void setCurrent(bool current)
Set whether the data in the layer is up to date.
Definition: layer.hpp:149
const std::vector< geometry_msgs::msg::Point > & getFootprint() const
Convenience function for layered_costmap_->getFootprint().
Definition: layer.cpp:70
bool isRolling()
If this costmap is rolling or not.
Costmap2D * getCostmap()
Get the costmap pointer to the master costmap.
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
Resize the map to a new size, resolution, or origin.
bool isSizeLocked()
Get if the size of the costmap is locked.
Takes in a map generated from SLAM to add costs to costmap.
void getParameters()
Get parameters of layer.
bool usesMasterCostmapSize() const
Whether this layer's grid is the master's grid (non-rolling, resize_master true). Otherwise the map k...
virtual ~StaticLayer()
Static Layer destructor.
virtual void deactivate()
Deactivate this layer.
bool has_updated_data_
frame that map is located in
void updateParametersCallback(const std::vector< rclcpp::Parameter > &parameters)
Apply parameter updates after validation This callback is executed when parameters have been successf...
unsigned char interpretValue(unsigned char value)
Interpret the value in the static map given on the topic to convert into costs for the costmap to uti...
StaticLayer()
Static Layer constructor.
virtual void matchSize()
Match the size of the master costmap.
virtual void onInitialize()
Initialization process of layer on startup.
void updateFootprint(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
Clear costmap layer info below the robot's footprint.
virtual void activate()
Activate this layer.
void incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
Callback to update the costmap's map from the map_server (or SLAM) with an update in a particular are...
bool isEqual(double a, double b, double epsilon)
Check if two double values are equal within a given epsilon.
double transform_staleness_threshold_
Maximum map-frame transform age in seconds; zero disables the check.
std::string global_frame_
The global frame for the costmap.
void processMap(const nav_msgs::msg::OccupancyGrid &new_map)
Process a new map coming from a topic.
virtual void updateCosts(nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
Update the costs in the master costmap in the window.
rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(const std::vector< rclcpp::Parameter > &parameters)
Validate incoming parameter updates before applying them. This callback is triggered when one or more...
void incomingMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr &new_map)
Callback to update the costmap's map from the map_server.
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
Update the bounds of the master costmap by this layer's update dimensions.
virtual void reset()
Reset this costmap.
void transformFootprint(double x, double y, double theta, const std::vector< geometry_msgs::msg::Point > &footprint_spec, std::vector< geometry_msgs::msg::Point > &oriented_footprint)
Given a pose and base footprint, build the oriented footprint of the robot (list of Points)
Definition: footprint.cpp:112