Fixed the problem of static layer not restoring old map values for footprint (#4824)

* Fix

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Fixed some bugs and removed the unnecessary variables from class

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Keep the branch updated with main

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Called off API changes in costmap, added "restore_outdated_footprint"

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Added the documentation for the new methods in static layer

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Make CI happy

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Apply suggestions and minimize diffs

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Fix ament_cpp_lint

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Revert changes in the method updateFootprint

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Used cached data to restore map and broken up setConvexPolygonCost into two methods

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Rename newly added methods

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Added restore_outdated_map parameter

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Make CI happy

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* 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 <cihataltiparmak1@gmail.com>

* Remove unnecessary log

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Add new member to MapLocation

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Remove unnecessary header

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

* Set the parameters footprint_clearing_enabled and restore_cleared_footprint enable in nav2_system_tests

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

---------

Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com>

Author: CihatAltiparmak

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<geometry_msgs::msg::Point> & 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<geometry_msgs::msg::Point> & polygon,
+    std::vector<MapLocation> & 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<MapLocation> & 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<MapLocation> & 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);
     }
 

nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

@@ -163,6 +163,7 @@ class StaticLayer : public CostmapLayer
 
   std::vector<geometry_msgs::msg::Point> transformed_footprint_;
   bool footprint_clearing_enabled_;
+  bool restore_cleared_footprint_;
   /**
    * @brief Clear costmap layer info below the robot's footprint
    */

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<MapLocation> 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.");
+        }
       }
     }
   }

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<geometry_msgs::msg::Point> & polygon,
   unsigned char cost_value)
+{
+  std::vector<MapLocation> 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<MapLocation> & 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<MapLocation> & polygon_map_region)
+{
+  for (const auto & cell : polygon_map_region) {
+    setCost(cell.x, cell.y, cell.cost);
+  }
+}
+
+bool Costmap2D::getMapRegionOccupiedByPolygon(
+  const std::vector<geometry_msgs::msg::Point> & polygon,
+  std::vector<MapLocation> & polygon_map_region)
 {
   // we assume the polygon is given in the global_frame...
   // we need to transform it to map coordinates
   std::vector<MapLocation> 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<MapLocation> 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);
     }
   }

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