From ee0982614fa67b342f1ace3ceb69d6e6eed837cc Mon Sep 17 00:00:00 2001 From: Jetson Date: Mon, 9 Feb 2026 21:04:12 +0000 Subject: [PATCH 1/3] Refactor updateCosts to improve handling of grid map data and ensure proper merging into the master costmap --- .../src/gridmap_layer.cpp | 75 ++++++++++--------- 1 file changed, 39 insertions(+), 36 deletions(-) diff --git a/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp b/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp index 0f65f605..43d23e94 100644 --- a/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp +++ b/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp @@ -170,50 +170,53 @@ bool GridmapLayer::getTransform( void GridmapLayer::updateCosts(nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) { std::lock_guard guard(*getMutex()); - if (!enabled_ || !has_updated_data_) { + if (!enabled_) { return; } - geometry_msgs::msg::TransformStamped transform; - if (!getTransform(transform)) { - return; - } - unsigned char *master_array = master_grid.getCharMap(); - has_updated_data_ = false; - geometry_msgs::msg::PointStamped grid_map_point, costmap_point; - - // Iterate through the grid map and copy the values to the costmap - for (grid_map::GridMapIterator it(gridmap_in_); !it.isPastEnd(); ++it) { - const grid_map::Index index(*it); - const float value = gridmap_in_.at(layer_name_, index); - const auto cost = interpretValue(value); - if (cost == NO_INFORMATION) { - continue; + // When new grid map data has arrived, copy it into the internal buffer + if (has_updated_data_) { + geometry_msgs::msg::TransformStamped transform; + if (!getTransform(transform)) { + return; } - // Convert grid_map index to world coordinates - grid_map::Position position; - gridmap_in_.getPosition(index, position); - - // Transform the position to the costmap frame - grid_map_point.point.x = position.x(); - grid_map_point.point.y = position.y(); - grid_map_point.point.z = 0.0; - tf2::doTransform(grid_map_point, costmap_point, transform); - - // Convert world coordinates to costmap coordinates - unsigned int mx, my; - const bool isValid = - worldToMap(costmap_point.point.x, costmap_point.point.y, mx, my); - if (isValid) { - auto &index = master_array[getIndex(mx, my)]; - if (use_maximum_) { - index = std::max(index, cost); - } else { - index = cost; + has_updated_data_ = false; + geometry_msgs::msg::PointStamped grid_map_point, costmap_point; + + // Iterate through the grid map and copy values to the internal costmap + for (grid_map::GridMapIterator it(gridmap_in_); !it.isPastEnd(); ++it) { + const grid_map::Index index(*it); + const float value = gridmap_in_.at(layer_name_, index); + const auto cost = interpretValue(value); + + // Convert grid_map index to world coordinates + grid_map::Position position; + gridmap_in_.getPosition(index, position); + + // Transform the position to the costmap frame + grid_map_point.point.x = position.x(); + grid_map_point.point.y = position.y(); + grid_map_point.point.z = 0.0; + tf2::doTransform(grid_map_point, costmap_point, transform); + + // Convert world coordinates to costmap cell coordinates + unsigned int mx, my; + const bool isValid = + worldToMap(costmap_point.point.x, costmap_point.point.y, mx, my); + if (isValid) { + // Write to the layer's own internal buffer (not the master) + setCost(mx, my, cost); } } } + + // Always merge the internal buffer into the master costmap + if (use_maximum_) { + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + } else { + updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j); + } current_ = true; } From 03fe79cde7f984f8ccc3a5d0a3518a02ebd44fe4 Mon Sep 17 00:00:00 2001 From: Jetson Date: Wed, 11 Feb 2026 18:02:05 +0000 Subject: [PATCH 2/3] ADdress PR comments --- .../src/gridmap_layer.cpp | 46 +- .../cprt_costmap_plugins/src/static_layer.cpp | 540 ++++++++++++++++++ 2 files changed, 583 insertions(+), 3 deletions(-) create mode 100644 src/Nav/cprt_costmap_plugins/src/static_layer.cpp diff --git a/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp b/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp index 43d23e94..d6c04032 100644 --- a/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp +++ b/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp @@ -118,6 +118,12 @@ void GridmapLayer::incomingMap( void GridmapLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y) { + if (!map_received_) { + map_received_in_update_bounds_ = false; + return; + } + map_received_in_update_bounds_ = true; + std::lock_guard guard(*getMutex()); if (layered_costmap_->isRolling()) { @@ -174,14 +180,45 @@ void GridmapLayer::updateCosts(nav2_costmap_2d::Costmap2D &master_grid, return; } - // When new grid map data has arrived, copy it into the internal buffer - if (has_updated_data_) { + // Don't write to master costmap until we've received at least one map + if (!map_received_in_update_bounds_) { + static int count = 0; + // throttle warning down to only 1/10 message rate + if (++count == 10) { + RCLCPP_WARN(logger_, "Can't update gridmap costmap layer, no map received"); + count = 0; + } + return; + } + + // When new grid map data has arrived OR using rolling costmap, copy it into the internal buffer + // For rolling costmaps, we need to re-apply the transformation even without new data + // to keep the internal layer synchronized with the moving costmap frame + if (has_updated_data_ || layered_costmap_->isRolling()) { geometry_msgs::msg::TransformStamped transform; if (!getTransform(transform)) { return; } - has_updated_data_ = false; + // Check if the required layer exists in the grid map to avoid exceptions + if (!gridmap_in_.exists(layer_name_)) { + RCLCPP_ERROR( + logger_, + "GridMap layer '%s' does not exist in received grid map. " + "Available layers: [%s]. Check your configuration.", + layer_name_.c_str(), + [&]() { + std::string layers; + for (const auto& layer : gridmap_in_.getLayers()) { + if (!layers.empty()) layers += ", "; + layers += layer; + } + return layers; + }().c_str() + ); + return; + } + geometry_msgs::msg::PointStamped grid_map_point, costmap_point; // Iterate through the grid map and copy values to the internal costmap @@ -209,6 +246,9 @@ void GridmapLayer::updateCosts(nav2_costmap_2d::Costmap2D &master_grid, setCost(mx, my, cost); } } + + // Only mark as processed after successful copy + has_updated_data_ = false; } // Always merge the internal buffer into the master costmap diff --git a/src/Nav/cprt_costmap_plugins/src/static_layer.cpp b/src/Nav/cprt_costmap_plugins/src/static_layer.cpp new file mode 100644 index 00000000..bb3015b8 --- /dev/null +++ b/src/Nav/cprt_costmap_plugins/src/static_layer.cpp @@ -0,0 +1,540 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * Copyright (c) 2015, Fetch Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +#include "nav2_costmap_2d/static_layer.hpp" + +#include +#include + +#include "pluginlib/class_list_macros.hpp" +#include "tf2/convert.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_ros_common/validate_messages.hpp" + +#define EPSILON 1e-5 + +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) + +using nav2_costmap_2d::NO_INFORMATION; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using nav2_costmap_2d::FREE_SPACE; +using rcl_interfaces::msg::ParameterType; + +namespace nav2_costmap_2d +{ + +StaticLayer::StaticLayer() +: map_buffer_(nullptr) +{ +} + +StaticLayer::~StaticLayer() +{ +} + +void +StaticLayer::onInitialize() +{ + global_frame_ = layered_costmap_->getGlobalFrameID(); + + getParameters(); + + rclcpp::QoS map_qos = nav2::qos::StandardTopicQoS(); // initialize to default + if (map_subscribe_transient_local_) { + map_qos = nav2::qos::LatchedSubscriptionQoS(3); + } + + RCLCPP_INFO( + logger_, + "Subscribing to the map topic (%s) with %s durability", + map_topic_.c_str(), + map_subscribe_transient_local_ ? "transient local" : "volatile"); + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + map_sub_ = node->create_subscription( + map_topic_, + std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1), + map_qos); + + if (subscribe_to_updates_) { + RCLCPP_INFO(logger_, "Subscribing to updates"); + map_update_sub_ = node->create_subscription( + map_topic_ + "_updates", + std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1)); + } +} + +void +StaticLayer::activate() +{ +} + +void +StaticLayer::deactivate() +{ + auto node = node_.lock(); + if (dyn_params_handler_ && node) { + node->remove_on_set_parameters_callback(dyn_params_handler_.get()); + } + dyn_params_handler_.reset(); +} + +void +StaticLayer::reset() +{ + has_updated_data_ = true; + current_ = false; +} + +void +StaticLayer::getParameters() +{ + int temp_lethal_threshold = 0; + double temp_tf_tol = 0.0; + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true); + subscribe_to_updates_ = node->declare_or_get_parameter( + name_ + "." + "subscribe_to_updates", false); + footprint_clearing_enabled_ = node->declare_or_get_parameter( + name_ + "." + "footprint_clearing_enabled", false); + restore_cleared_footprint_ = node->declare_or_get_parameter( + name_ + "." + "restore_cleared_footprint", true); + map_topic_ = node->declare_or_get_parameter( + name_ + "." + "map_topic", std::string("map")); + map_topic_ = joinWithParentNamespace(map_topic_); + map_subscribe_transient_local_ = node->declare_or_get_parameter( + name_ + "." + "map_subscribe_transient_local", true); + node->get_parameter("track_unknown_space", track_unknown_space_); + node->get_parameter("use_maximum", use_maximum_); + node->get_parameter("lethal_cost_threshold", temp_lethal_threshold); + node->get_parameter("inscribed_obstacle_cost_value", inscribed_obstacle_cost_value_); + node->get_parameter("unknown_cost_value", unknown_cost_value_); + node->get_parameter("trinary_costmap", trinary_costmap_); + node->get_parameter("transform_tolerance", temp_tf_tol); + + // Enforce bounds + lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); + map_received_ = false; + map_received_in_update_bounds_ = false; + + transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &StaticLayer::dynamicParametersCallback, + this, std::placeholders::_1)); +} + +void +StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) +{ + RCLCPP_DEBUG(logger_, "StaticLayer: Process map"); + + unsigned int size_x = new_map.info.width; + unsigned int size_y = new_map.info.height; + + RCLCPP_DEBUG( + logger_, + "StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y, + new_map.info.resolution); + + // resize costmap if size, resolution or origin do not match + Costmap2D * master = layered_costmap_->getCostmap(); + if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x || + master->getSizeInCellsY() != size_y || + !isEqual(master->getResolution(), new_map.info.resolution, EPSILON) || + !isEqual(master->getOriginX(), new_map.info.origin.position.x, EPSILON) || + !isEqual(master->getOriginY(), new_map.info.origin.position.y, EPSILON) || + !layered_costmap_->isSizeLocked())) + { + // Update the size of the layered costmap (and all layers, including this one) + RCLCPP_INFO( + logger_, + "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y, + new_map.info.resolution); + layered_costmap_->resizeMap( + size_x, size_y, new_map.info.resolution, + new_map.info.origin.position.x, + new_map.info.origin.position.y, + true); + } else if (size_x_ != size_x || size_y_ != size_y || // NOLINT + !isEqual(resolution_, new_map.info.resolution, EPSILON) || + !isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) || + !isEqual(origin_y_, new_map.info.origin.position.y, EPSILON)) + { + // only update the size of the costmap stored locally in this layer + RCLCPP_INFO( + logger_, + "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y, + new_map.info.resolution); + resizeMap( + size_x, size_y, new_map.info.resolution, + new_map.info.origin.position.x, new_map.info.origin.position.y); + } + + unsigned int index = 0; + + // we have a new map, update full size of map + std::lock_guard guard(*getMutex()); + + // initialize the costmap with static data + for (unsigned int i = 0; i < size_y; ++i) { + for (unsigned int j = 0; j < size_x; ++j) { + unsigned char value = new_map.data[index]; + costmap_[index] = interpretValue(value); + ++index; + } + } + + map_frame_ = new_map.header.frame_id; + + x_ = y_ = 0; + width_ = size_x_; + height_ = size_y_; + has_updated_data_ = true; + + current_ = true; +} + +void +StaticLayer::matchSize() +{ + // If we are using rolling costmap, the static map size is + // unrelated to the size of the layered costmap + if (!layered_costmap_->isRolling()) { + Costmap2D * master = layered_costmap_->getCostmap(); + resizeMap( + master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), + master->getOriginX(), master->getOriginY()); + } +} + +unsigned char +StaticLayer::interpretValue(unsigned char value) +{ + // check if the static value is above the unknown or lethal thresholds + if (track_unknown_space_ && value == unknown_cost_value_) { + return NO_INFORMATION; + } else if (!track_unknown_space_ && value == unknown_cost_value_) { + return FREE_SPACE; + } else if (value == inscribed_obstacle_cost_value_) { + return INSCRIBED_INFLATED_OBSTACLE; + } else if (value >= lethal_threshold_) { + return LETHAL_OBSTACLE; + } else if (trinary_costmap_) { + return FREE_SPACE; + } + + double scale = static_cast(value) / lethal_threshold_; + return scale * LETHAL_OBSTACLE; +} + +void +StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & new_map) +{ + if (!nav2::validateMsg(*new_map)) { + RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting."); + return; + } + if (!map_received_) { + processMap(*new_map); + map_received_ = true; + return; + } + std::lock_guard guard(*getMutex()); + map_buffer_ = new_map; +} + +void +StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update) +{ + std::lock_guard guard(*getMutex()); + if (update->y < static_cast(y_) || + y_ + height_ < update->y + update->height || + update->x < static_cast(x_) || + x_ + width_ < update->x + update->width) + { + RCLCPP_WARN( + logger_, + "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n" + "Static layer origin: %d, %d bounds: %d X %d\n" + "Update origin: %d, %d bounds: %d X %d", + x_, y_, width_, height_, update->x, update->y, update->width, + update->height); + return; + } + + if (update->header.frame_id != map_frame_) { + RCLCPP_WARN( + logger_, + "StaticLayer: Map update ignored. Current map is in frame %s " + "but update was in frame %s", + map_frame_.c_str(), update->header.frame_id.c_str()); + return; + } + + unsigned int di = 0; + for (unsigned int y = 0; y < update->height; y++) { + unsigned int index_base = (update->y + y) * size_x_; + for (unsigned int x = 0; x < update->width; x++) { + unsigned int index = index_base + x + update->x; + costmap_[index] = interpretValue(update->data[di++]); + } + } + + has_updated_data_ = true; +} + + +void +StaticLayer::updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, + double * max_x, + double * max_y) +{ + if (!map_received_) { + map_received_in_update_bounds_ = false; + return; + } + map_received_in_update_bounds_ = true; + + std::lock_guard guard(*getMutex()); + + // If there is a new available map, load it. + if (map_buffer_) { + processMap(*map_buffer_); + map_buffer_ = nullptr; + } + + if (!layered_costmap_->isRolling() ) { + if (!(has_updated_data_ || has_extra_bounds_)) { + return; + } + } + + useExtraBounds(min_x, min_y, max_x, max_y); + + double wx, wy; + + mapToWorld(x_, y_, wx, wy); + *min_x = std::min(wx, *min_x); + *min_y = std::min(wy, *min_y); + + mapToWorld(x_ + width_, y_ + height_, wx, wy); + *max_x = std::max(wx, *max_x); + *max_y = std::max(wy, *max_y); + + has_updated_data_ = false; + + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void +StaticLayer::updateFootprint( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, + double * max_y) +{ + if (!footprint_clearing_enabled_) {return;} + + transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + + for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { + touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); + } +} + +void +StaticLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) +{ + std::lock_guard guard(*getMutex()); + if (!enabled_) { + return; + } + if (!map_received_in_update_bounds_) { + static int count = 0; + // throttle warning down to only 1/10 message rate + if (++count == 10) { + RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received"); + count = 0; + } + return; + } + + std::vector map_region_to_restore; + if (footprint_clearing_enabled_) { + map_region_to_restore.reserve(100); + getMapRegionOccupiedByPolygon(transformed_footprint_, map_region_to_restore); + setMapRegionOccupiedByPolygon(map_region_to_restore, nav2_costmap_2d::FREE_SPACE); + } + + if (!layered_costmap_->isRolling()) { + // if not rolling, the layered costmap (master_grid) has same coordinates as this layer + if (!use_maximum_) { + updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j); + } else { + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + } + } else { + // If rolling window, the master_grid is unlikely to have same coordinates as this layer + unsigned int mx, my; + double wx, wy; + // Might even be in a different frame + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_->lookupTransform( + map_frame_, global_frame_, tf2::TimePointZero, + transform_tolerance_); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what()); + return; + } + // Copy map data given proper transformations + tf2::Transform tf2_transform; + tf2::fromMsg(transform.transform, tf2_transform); + + for (int i = min_i; i < max_i; ++i) { + for (int j = min_j; j < max_j; ++j) { + // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates + layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy); + // Transform from global_frame_ to map_frame_ + tf2::Vector3 p(wx, wy, 0); + p = tf2_transform * p; + // Set master_grid with cell from map + if (worldToMap(p.x(), p.y(), mx, my)) { + if (!use_maximum_) { + master_grid.setCost(i, j, getCost(mx, my)); + } else { + master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j))); + } + } + } + } + } + + if (footprint_clearing_enabled_ && restore_cleared_footprint_) { + // restore the map region occupied by the polygon using cached data + restoreMapRegionOccupiedByPolygon(map_region_to_restore); + } + current_ = true; +} + +/** + * @brief Check if two floating point numbers are equal within a given epsilon + * @param a First number + * @param b Second number + * @param epsilon Tolerance for equality check + * @return True if numbers are equal within the tolerance, false otherwise + */ +bool StaticLayer::isEqual(double a, double b, double epsilon) +{ + return std::abs(a - b) < epsilon; +} + +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +StaticLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } + + if (param_name == name_ + "." + "map_subscribe_transient_local" || + param_name == name_ + "." + "map_topic" || + param_name == name_ + "." + "subscribe_to_updates") + { + RCLCPP_WARN( + logger_, "%s is not a dynamic parameter " + "cannot be changed while running. Rejecting parameter update.", param_name.c_str()); + } else if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + "transform_tolerance") { + transform_tolerance_ = tf2::durationFromSec(parameter.as_double()); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { + enabled_ = parameter.as_bool(); + + x_ = y_ = 0; + width_ = size_x_; + height_ = size_y_; + has_updated_data_ = true; + current_ = false; + } else if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } else if (param_name == name_ + "." + "restore_cleared_footprint") { + if (footprint_clearing_enabled_) { + restore_cleared_footprint_ = parameter.as_bool(); + } else { + RCLCPP_WARN( + logger_, "restore_cleared_footprint cannot be used " + "when footprint_clearing_enabled is False. Rejecting parameter update."); + } + } + } + } + result.successful = true; + return result; +} + +} // namespace nav2_costmap_2d \ No newline at end of file From 974f147292b3b0c587ec6ebe9797c51795906c09 Mon Sep 17 00:00:00 2001 From: Erik Caldwell Date: Wed, 18 Feb 2026 01:14:21 -0500 Subject: [PATCH 3/3] Optimize gridmap layer and fix persistance bug --- .../src/gridmap_layer.cpp | 124 ++-- .../cprt_costmap_plugins/src/static_layer.cpp | 540 ------------------ 2 files changed, 70 insertions(+), 594 deletions(-) delete mode 100644 src/Nav/cprt_costmap_plugins/src/static_layer.cpp diff --git a/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp b/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp index d6c04032..b3b5476f 100644 --- a/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp +++ b/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp @@ -185,72 +185,88 @@ void GridmapLayer::updateCosts(nav2_costmap_2d::Costmap2D &master_grid, static int count = 0; // throttle warning down to only 1/10 message rate if (++count == 10) { - RCLCPP_WARN(logger_, "Can't update gridmap costmap layer, no map received"); + RCLCPP_WARN(logger_, + "Can't update gridmap costmap layer, no map received"); count = 0; } return; } - // When new grid map data has arrived OR using rolling costmap, copy it into the internal buffer - // For rolling costmaps, we need to re-apply the transformation even without new data - // to keep the internal layer synchronized with the moving costmap frame - if (has_updated_data_ || layered_costmap_->isRolling()) { - geometry_msgs::msg::TransformStamped transform; - if (!getTransform(transform)) { - return; + // FIX: For rolling costmaps, we MUST re-project every cycle to prevent + // flickering For static costmaps, only update when new data arrives + if (!has_updated_data_ && !layered_costmap_->isRolling()) { + // No new data and not rolling - skip the update loop but still merge + if (use_maximum_) { + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + } else { + updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j); } + current_ = true; + return; + } - // Check if the required layer exists in the grid map to avoid exceptions - if (!gridmap_in_.exists(layer_name_)) { - RCLCPP_ERROR( - logger_, - "GridMap layer '%s' does not exist in received grid map. " - "Available layers: [%s]. Check your configuration.", - layer_name_.c_str(), - [&]() { - std::string layers; - for (const auto& layer : gridmap_in_.getLayers()) { - if (!layers.empty()) layers += ", "; - layers += layer; - } - return layers; - }().c_str() - ); - return; - } + geometry_msgs::msg::TransformStamped transform_stamped; + if (!getTransform(transform_stamped)) { + return; + } - geometry_msgs::msg::PointStamped grid_map_point, costmap_point; - - // Iterate through the grid map and copy values to the internal costmap - for (grid_map::GridMapIterator it(gridmap_in_); !it.isPastEnd(); ++it) { - const grid_map::Index index(*it); - const float value = gridmap_in_.at(layer_name_, index); - const auto cost = interpretValue(value); - - // Convert grid_map index to world coordinates - grid_map::Position position; - gridmap_in_.getPosition(index, position); - - // Transform the position to the costmap frame - grid_map_point.point.x = position.x(); - grid_map_point.point.y = position.y(); - grid_map_point.point.z = 0.0; - tf2::doTransform(grid_map_point, costmap_point, transform); - - // Convert world coordinates to costmap cell coordinates - unsigned int mx, my; - const bool isValid = - worldToMap(costmap_point.point.x, costmap_point.point.y, mx, my); - if (isValid) { - // Write to the layer's own internal buffer (not the master) - setCost(mx, my, cost); - } + // Check if the required layer exists in the grid map to avoid exceptions + if (!gridmap_in_.exists(layer_name_)) { + RCLCPP_ERROR(logger_, + "GridMap layer '%s' does not exist in received grid map. " + "Available layers: [%s]. Check your configuration.", + layer_name_.c_str(), + [&]() { + std::string layers; + for (const auto &layer : gridmap_in_.getLayers()) { + if (!layers.empty()) + layers += ", "; + layers += layer; + } + return layers; + }() + .c_str()); + return; + } + + // OPTIMIZATION: Convert transform once before the loop to avoid repeated + // conversions + tf2::Transform tf2_transform; + tf2::fromMsg(transform_stamped.transform, tf2_transform); + + // Iterate through the grid map and copy values to the internal costmap + for (grid_map::GridMapIterator it(gridmap_in_); !it.isPastEnd(); ++it) { + const grid_map::Index index(*it); + const float value = gridmap_in_.at(layer_name_, index); + const auto cost = interpretValue(value); + + // PRESERVE DATA: Skip unknown values to prevent overwriting valid obstacles + if (cost == NO_INFORMATION) { + continue; } - // Only mark as processed after successful copy - has_updated_data_ = false; + // Convert grid_map index to world coordinates + grid_map::Position position; + gridmap_in_.getPosition(index, position); + + // OPTIMIZATION: Use direct tf2::Transform multiplication instead of + // doTransform + tf2::Vector3 grid_map_point(position.x(), position.y(), 0.0); + tf2::Vector3 costmap_point = tf2_transform * grid_map_point; + + // Convert world coordinates to costmap cell coordinates + unsigned int mx, my; + const bool isValid = + worldToMap(costmap_point.x(), costmap_point.y(), mx, my); + if (isValid) { + // Write to the layer's own internal buffer (not the master) + setCost(mx, my, cost); + } } + // Mark as processed after successful copy + has_updated_data_ = false; + // Always merge the internal buffer into the master costmap if (use_maximum_) { updateWithMax(master_grid, min_i, min_j, max_i, max_j); diff --git a/src/Nav/cprt_costmap_plugins/src/static_layer.cpp b/src/Nav/cprt_costmap_plugins/src/static_layer.cpp deleted file mode 100644 index bb3015b8..00000000 --- a/src/Nav/cprt_costmap_plugins/src/static_layer.cpp +++ /dev/null @@ -1,540 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, 2013, Willow Garage, Inc. - * Copyright (c) 2015, Fetch Robotics, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Eitan Marder-Eppstein - * David V. Lu!! - *********************************************************************/ - -#include "nav2_costmap_2d/static_layer.hpp" - -#include -#include - -#include "pluginlib/class_list_macros.hpp" -#include "tf2/convert.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav2_ros_common/validate_messages.hpp" - -#define EPSILON 1e-5 - -PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) - -using nav2_costmap_2d::NO_INFORMATION; -using nav2_costmap_2d::LETHAL_OBSTACLE; -using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; -using nav2_costmap_2d::FREE_SPACE; -using rcl_interfaces::msg::ParameterType; - -namespace nav2_costmap_2d -{ - -StaticLayer::StaticLayer() -: map_buffer_(nullptr) -{ -} - -StaticLayer::~StaticLayer() -{ -} - -void -StaticLayer::onInitialize() -{ - global_frame_ = layered_costmap_->getGlobalFrameID(); - - getParameters(); - - rclcpp::QoS map_qos = nav2::qos::StandardTopicQoS(); // initialize to default - if (map_subscribe_transient_local_) { - map_qos = nav2::qos::LatchedSubscriptionQoS(3); - } - - RCLCPP_INFO( - logger_, - "Subscribing to the map topic (%s) with %s durability", - map_topic_.c_str(), - map_subscribe_transient_local_ ? "transient local" : "volatile"); - - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - - map_sub_ = node->create_subscription( - map_topic_, - std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1), - map_qos); - - if (subscribe_to_updates_) { - RCLCPP_INFO(logger_, "Subscribing to updates"); - map_update_sub_ = node->create_subscription( - map_topic_ + "_updates", - std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1)); - } -} - -void -StaticLayer::activate() -{ -} - -void -StaticLayer::deactivate() -{ - auto node = node_.lock(); - if (dyn_params_handler_ && node) { - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); - } - dyn_params_handler_.reset(); -} - -void -StaticLayer::reset() -{ - has_updated_data_ = true; - current_ = false; -} - -void -StaticLayer::getParameters() -{ - int temp_lethal_threshold = 0; - double temp_tf_tol = 0.0; - - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - - enabled_ = node->declare_or_get_parameter(name_ + "." + "enabled", true); - subscribe_to_updates_ = node->declare_or_get_parameter( - name_ + "." + "subscribe_to_updates", false); - footprint_clearing_enabled_ = node->declare_or_get_parameter( - name_ + "." + "footprint_clearing_enabled", false); - restore_cleared_footprint_ = node->declare_or_get_parameter( - name_ + "." + "restore_cleared_footprint", true); - map_topic_ = node->declare_or_get_parameter( - name_ + "." + "map_topic", std::string("map")); - map_topic_ = joinWithParentNamespace(map_topic_); - map_subscribe_transient_local_ = node->declare_or_get_parameter( - name_ + "." + "map_subscribe_transient_local", true); - node->get_parameter("track_unknown_space", track_unknown_space_); - node->get_parameter("use_maximum", use_maximum_); - node->get_parameter("lethal_cost_threshold", temp_lethal_threshold); - node->get_parameter("inscribed_obstacle_cost_value", inscribed_obstacle_cost_value_); - node->get_parameter("unknown_cost_value", unknown_cost_value_); - node->get_parameter("trinary_costmap", trinary_costmap_); - node->get_parameter("transform_tolerance", temp_tf_tol); - - // Enforce bounds - lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); - map_received_ = false; - map_received_in_update_bounds_ = false; - - transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); - - // Add callback for dynamic parameters - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind( - &StaticLayer::dynamicParametersCallback, - this, std::placeholders::_1)); -} - -void -StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) -{ - RCLCPP_DEBUG(logger_, "StaticLayer: Process map"); - - unsigned int size_x = new_map.info.width; - unsigned int size_y = new_map.info.height; - - RCLCPP_DEBUG( - logger_, - "StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y, - new_map.info.resolution); - - // resize costmap if size, resolution or origin do not match - Costmap2D * master = layered_costmap_->getCostmap(); - if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x || - master->getSizeInCellsY() != size_y || - !isEqual(master->getResolution(), new_map.info.resolution, EPSILON) || - !isEqual(master->getOriginX(), new_map.info.origin.position.x, EPSILON) || - !isEqual(master->getOriginY(), new_map.info.origin.position.y, EPSILON) || - !layered_costmap_->isSizeLocked())) - { - // Update the size of the layered costmap (and all layers, including this one) - RCLCPP_INFO( - logger_, - "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y, - new_map.info.resolution); - layered_costmap_->resizeMap( - size_x, size_y, new_map.info.resolution, - new_map.info.origin.position.x, - new_map.info.origin.position.y, - true); - } else if (size_x_ != size_x || size_y_ != size_y || // NOLINT - !isEqual(resolution_, new_map.info.resolution, EPSILON) || - !isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) || - !isEqual(origin_y_, new_map.info.origin.position.y, EPSILON)) - { - // only update the size of the costmap stored locally in this layer - RCLCPP_INFO( - logger_, - "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y, - new_map.info.resolution); - resizeMap( - size_x, size_y, new_map.info.resolution, - new_map.info.origin.position.x, new_map.info.origin.position.y); - } - - unsigned int index = 0; - - // we have a new map, update full size of map - std::lock_guard guard(*getMutex()); - - // initialize the costmap with static data - for (unsigned int i = 0; i < size_y; ++i) { - for (unsigned int j = 0; j < size_x; ++j) { - unsigned char value = new_map.data[index]; - costmap_[index] = interpretValue(value); - ++index; - } - } - - map_frame_ = new_map.header.frame_id; - - x_ = y_ = 0; - width_ = size_x_; - height_ = size_y_; - has_updated_data_ = true; - - current_ = true; -} - -void -StaticLayer::matchSize() -{ - // If we are using rolling costmap, the static map size is - // unrelated to the size of the layered costmap - if (!layered_costmap_->isRolling()) { - Costmap2D * master = layered_costmap_->getCostmap(); - resizeMap( - master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), - master->getOriginX(), master->getOriginY()); - } -} - -unsigned char -StaticLayer::interpretValue(unsigned char value) -{ - // check if the static value is above the unknown or lethal thresholds - if (track_unknown_space_ && value == unknown_cost_value_) { - return NO_INFORMATION; - } else if (!track_unknown_space_ && value == unknown_cost_value_) { - return FREE_SPACE; - } else if (value == inscribed_obstacle_cost_value_) { - return INSCRIBED_INFLATED_OBSTACLE; - } else if (value >= lethal_threshold_) { - return LETHAL_OBSTACLE; - } else if (trinary_costmap_) { - return FREE_SPACE; - } - - double scale = static_cast(value) / lethal_threshold_; - return scale * LETHAL_OBSTACLE; -} - -void -StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & new_map) -{ - if (!nav2::validateMsg(*new_map)) { - RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting."); - return; - } - if (!map_received_) { - processMap(*new_map); - map_received_ = true; - return; - } - std::lock_guard guard(*getMutex()); - map_buffer_ = new_map; -} - -void -StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update) -{ - std::lock_guard guard(*getMutex()); - if (update->y < static_cast(y_) || - y_ + height_ < update->y + update->height || - update->x < static_cast(x_) || - x_ + width_ < update->x + update->width) - { - RCLCPP_WARN( - logger_, - "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n" - "Static layer origin: %d, %d bounds: %d X %d\n" - "Update origin: %d, %d bounds: %d X %d", - x_, y_, width_, height_, update->x, update->y, update->width, - update->height); - return; - } - - if (update->header.frame_id != map_frame_) { - RCLCPP_WARN( - logger_, - "StaticLayer: Map update ignored. Current map is in frame %s " - "but update was in frame %s", - map_frame_.c_str(), update->header.frame_id.c_str()); - return; - } - - unsigned int di = 0; - for (unsigned int y = 0; y < update->height; y++) { - unsigned int index_base = (update->y + y) * size_x_; - for (unsigned int x = 0; x < update->width; x++) { - unsigned int index = index_base + x + update->x; - costmap_[index] = interpretValue(update->data[di++]); - } - } - - has_updated_data_ = true; -} - - -void -StaticLayer::updateBounds( - double robot_x, double robot_y, double robot_yaw, double * min_x, - double * min_y, - double * max_x, - double * max_y) -{ - if (!map_received_) { - map_received_in_update_bounds_ = false; - return; - } - map_received_in_update_bounds_ = true; - - std::lock_guard guard(*getMutex()); - - // If there is a new available map, load it. - if (map_buffer_) { - processMap(*map_buffer_); - map_buffer_ = nullptr; - } - - if (!layered_costmap_->isRolling() ) { - if (!(has_updated_data_ || has_extra_bounds_)) { - return; - } - } - - useExtraBounds(min_x, min_y, max_x, max_y); - - double wx, wy; - - mapToWorld(x_, y_, wx, wy); - *min_x = std::min(wx, *min_x); - *min_y = std::min(wy, *min_y); - - mapToWorld(x_ + width_, y_ + height_, wx, wy); - *max_x = std::max(wx, *max_x); - *max_y = std::max(wy, *max_y); - - has_updated_data_ = false; - - updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); -} - -void -StaticLayer::updateFootprint( - double robot_x, double robot_y, double robot_yaw, - double * min_x, double * min_y, - double * max_x, - double * max_y) -{ - if (!footprint_clearing_enabled_) {return;} - - transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); - - for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { - touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); - } -} - -void -StaticLayer::updateCosts( - nav2_costmap_2d::Costmap2D & master_grid, - int min_i, int min_j, int max_i, int max_j) -{ - std::lock_guard guard(*getMutex()); - if (!enabled_) { - return; - } - if (!map_received_in_update_bounds_) { - static int count = 0; - // throttle warning down to only 1/10 message rate - if (++count == 10) { - RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received"); - count = 0; - } - return; - } - - std::vector map_region_to_restore; - if (footprint_clearing_enabled_) { - map_region_to_restore.reserve(100); - getMapRegionOccupiedByPolygon(transformed_footprint_, map_region_to_restore); - setMapRegionOccupiedByPolygon(map_region_to_restore, nav2_costmap_2d::FREE_SPACE); - } - - if (!layered_costmap_->isRolling()) { - // if not rolling, the layered costmap (master_grid) has same coordinates as this layer - if (!use_maximum_) { - updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j); - } else { - updateWithMax(master_grid, min_i, min_j, max_i, max_j); - } - } else { - // If rolling window, the master_grid is unlikely to have same coordinates as this layer - unsigned int mx, my; - double wx, wy; - // Might even be in a different frame - geometry_msgs::msg::TransformStamped transform; - try { - transform = tf_->lookupTransform( - map_frame_, global_frame_, tf2::TimePointZero, - transform_tolerance_); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what()); - return; - } - // Copy map data given proper transformations - tf2::Transform tf2_transform; - tf2::fromMsg(transform.transform, tf2_transform); - - for (int i = min_i; i < max_i; ++i) { - for (int j = min_j; j < max_j; ++j) { - // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates - layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy); - // Transform from global_frame_ to map_frame_ - tf2::Vector3 p(wx, wy, 0); - p = tf2_transform * p; - // Set master_grid with cell from map - if (worldToMap(p.x(), p.y(), mx, my)) { - if (!use_maximum_) { - master_grid.setCost(i, j, getCost(mx, my)); - } else { - master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j))); - } - } - } - } - } - - if (footprint_clearing_enabled_ && restore_cleared_footprint_) { - // restore the map region occupied by the polygon using cached data - restoreMapRegionOccupiedByPolygon(map_region_to_restore); - } - current_ = true; -} - -/** - * @brief Check if two floating point numbers are equal within a given epsilon - * @param a First number - * @param b Second number - * @param epsilon Tolerance for equality check - * @return True if numbers are equal within the tolerance, false otherwise - */ -bool StaticLayer::isEqual(double a, double b, double epsilon) -{ - return std::abs(a - b) < epsilon; -} - -/** - * @brief Callback executed when a parameter change is detected - * @param event ParameterEvent message - */ -rcl_interfaces::msg::SetParametersResult -StaticLayer::dynamicParametersCallback( - std::vector parameters) -{ - std::lock_guard guard(*getMutex()); - rcl_interfaces::msg::SetParametersResult result; - - for (auto parameter : parameters) { - const auto & param_type = parameter.get_type(); - const auto & param_name = parameter.get_name(); - if (param_name.find(name_ + ".") != 0) { - continue; - } - - if (param_name == name_ + "." + "map_subscribe_transient_local" || - param_name == name_ + "." + "map_topic" || - param_name == name_ + "." + "subscribe_to_updates") - { - RCLCPP_WARN( - logger_, "%s is not a dynamic parameter " - "cannot be changed while running. Rejecting parameter update.", param_name.c_str()); - } else if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == name_ + "." + "transform_tolerance") { - transform_tolerance_ = tf2::durationFromSec(parameter.as_double()); - } - } else if (param_type == ParameterType::PARAMETER_BOOL) { - if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { - enabled_ = parameter.as_bool(); - - x_ = y_ = 0; - width_ = size_x_; - height_ = size_y_; - has_updated_data_ = true; - current_ = false; - } else if (param_name == name_ + "." + "footprint_clearing_enabled") { - footprint_clearing_enabled_ = parameter.as_bool(); - } else if (param_name == name_ + "." + "restore_cleared_footprint") { - if (footprint_clearing_enabled_) { - restore_cleared_footprint_ = parameter.as_bool(); - } else { - RCLCPP_WARN( - logger_, "restore_cleared_footprint cannot be used " - "when footprint_clearing_enabled is False. Rejecting parameter update."); - } - } - } - } - result.successful = true; - return result; -} - -} // namespace nav2_costmap_2d \ No newline at end of file