From 088c423deb97a76f5a5f4ca133cb122338576fe1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cihat=20Kurtulu=C5=9F=20Alt=C4=B1parmak?= Date: Tue, 4 Feb 2025 20:06:16 +0000 Subject: [PATCH] Fixed the problem of static layer not restoring old map values for footprint (#4824) * Fix Signed-off-by: CihatAltiparmak * Fixed some bugs and removed the unnecessary variables from class Signed-off-by: CihatAltiparmak * Keep the branch updated with main Signed-off-by: CihatAltiparmak * Called off API changes in costmap, added "restore_outdated_footprint" Signed-off-by: CihatAltiparmak * Added the documentation for the new methods in static layer Signed-off-by: CihatAltiparmak * Make CI happy Signed-off-by: CihatAltiparmak * Apply suggestions and minimize diffs Signed-off-by: CihatAltiparmak * Fix ament_cpp_lint Signed-off-by: CihatAltiparmak * Revert changes in the method updateFootprint Signed-off-by: CihatAltiparmak * Used cached data to restore map and broken up setConvexPolygonCost into two methods Signed-off-by: CihatAltiparmak * Rename newly added methods Signed-off-by: CihatAltiparmak * Added restore_outdated_map parameter Signed-off-by: CihatAltiparmak * Make CI happy Signed-off-by: CihatAltiparmak * Changed the name of the newly added parameter - changed the name of the new param - fixed bug in the setConvexPolygonCost - Used reserve method of the vector instances - Added checks in dynamic parameter update. Signed-off-by: CihatAltiparmak * Remove unnecessary log Signed-off-by: CihatAltiparmak * Add new member to MapLocation Signed-off-by: CihatAltiparmak * Remove unnecessary header Signed-off-by: CihatAltiparmak * Set the parameters footprint_clearing_enabled and restore_cleared_footprint enable in nav2_system_tests Signed-off-by: CihatAltiparmak --------- Signed-off-by: CihatAltiparmak --- .../include/nav2_costmap_2d/costmap_2d.hpp | 28 +++++++++++ .../include/nav2_costmap_2d/static_layer.hpp | 1 + nav2_costmap_2d/plugins/static_layer.cpp | 19 +++++++- nav2_costmap_2d/src/costmap_2d.cpp | 46 +++++++++++++++---- .../src/system/nav2_system_params.yaml | 2 + 5 files changed, 85 insertions(+), 11 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 927e5806122..b30179977d5 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -58,6 +58,7 @@ struct MapLocation { unsigned int x; unsigned int y; + unsigned char cost; }; /** @@ -311,6 +312,32 @@ class Costmap2D const std::vector & polygon, unsigned char cost_value); + /** + * @brief Gets the map region occupied by polygon + * @param polygon The polygon to perform the operation on + * @param polygon_map_region The map region occupied by the polygon + * @return True if the polygon_map_region was filled... false if it could not be filled + */ + bool getMapRegionOccupiedByPolygon( + const std::vector & polygon, + std::vector & polygon_map_region); + + /** + * @brief Sets the given map region to desired value + * @param polygon_map_region The map region to perform the operation on + * @param new_cost_value The value to set costs to + */ + void setMapRegionOccupiedByPolygon( + const std::vector & polygon_map_region, + unsigned char new_cost_value); + + /** + * @brief Restores the corresponding map region using given map region + * @param polygon_map_region The map region to perform the operation on + */ + void restoreMapRegionOccupiedByPolygon( + const std::vector & polygon_map_region); + /** * @brief Get the map cells that make up the outline of a polygon * @param polygon The polygon in map coordinates to rasterize @@ -568,6 +595,7 @@ class Costmap2D { MapLocation loc; costmap_.indexToCells(offset, loc.x, loc.y); + loc.cost = costmap_.getCost(loc.x, loc.y); cells_.push_back(loc); } diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index f0735904a9d..c938f30f55e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -163,6 +163,7 @@ class StaticLayer : public CostmapLayer std::vector transformed_footprint_; bool footprint_clearing_enabled_; + bool restore_cleared_footprint_; /** * @brief Clear costmap layer info below the robot's footprint */ diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index ac43c018673..9f0a69f80ed 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -138,6 +138,7 @@ StaticLayer::getParameters() declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); declareParameter("map_topic", rclcpp::ParameterValue("map")); declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false)); + declareParameter("restore_cleared_footprint", rclcpp::ParameterValue(true)); auto node = node_.lock(); if (!node) { @@ -147,6 +148,7 @@ StaticLayer::getParameters() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "restore_cleared_footprint", restore_cleared_footprint_); node->get_parameter(name_ + "." + "map_topic", map_topic_); map_topic_ = joinWithParentNamespace(map_topic_); node->get_parameter( @@ -411,8 +413,11 @@ StaticLayer::updateCosts( return; } + std::vector map_region_to_restore; if (footprint_clearing_enabled_) { - setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE); + 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()) { @@ -458,6 +463,11 @@ StaticLayer::updateCosts( } } } + + 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; } @@ -498,6 +508,13 @@ StaticLayer::dynamicParametersCallback( 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."); + } } } } diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 0302801297b..1d9997de8da 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -399,29 +399,54 @@ void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y) bool Costmap2D::setConvexPolygonCost( const std::vector & polygon, unsigned char cost_value) +{ + std::vector polygon_map_region; + polygon_map_region.reserve(100); + if (!getMapRegionOccupiedByPolygon(polygon, polygon_map_region)) { + return false; + } + + // set the cost of those cells + setMapRegionOccupiedByPolygon(polygon_map_region, cost_value); + return true; +} + +void Costmap2D::setMapRegionOccupiedByPolygon( + const std::vector & polygon_map_region, + unsigned char new_cost_value) +{ + for (const auto & cell : polygon_map_region) { + setCost(cell.x, cell.y, new_cost_value); + } +} + +void Costmap2D::restoreMapRegionOccupiedByPolygon( + const std::vector & polygon_map_region) +{ + for (const auto & cell : polygon_map_region) { + setCost(cell.x, cell.y, cell.cost); + } +} + +bool Costmap2D::getMapRegionOccupiedByPolygon( + const std::vector & polygon, + std::vector & polygon_map_region) { // we assume the polygon is given in the global_frame... // we need to transform it to map coordinates std::vector map_polygon; - for (unsigned int i = 0; i < polygon.size(); ++i) { + for (const auto & cell : polygon) { MapLocation loc; - if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) { + if (!worldToMap(cell.x, cell.y, loc.x, loc.y)) { // ("Polygon lies outside map bounds, so we can't fill it"); return false; } map_polygon.push_back(loc); } - std::vector polygon_cells; - // get the cells that fill the polygon - convexFillCells(map_polygon, polygon_cells); + convexFillCells(map_polygon, polygon_map_region); - // set the cost of those cells - for (unsigned int i = 0; i < polygon_cells.size(); ++i) { - unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y); - costmap_[index] = cost_value; - } return true; } @@ -506,6 +531,7 @@ void Costmap2D::convexFillCells( for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) { pt.x = x; pt.y = y; + pt.cost = getCost(x, y); polygon_cells.push_back(pt); } } diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 5ee29e6536e..756f0958f9f 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -196,6 +196,8 @@ global_costmap: static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True + footprint_clearing_enabled: True + restore_cleared_footprint: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0