Called off API changes in costmap, added "restore_outdated_footprint"

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

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

@@ -163,8 +163,6 @@ class Costmap2D
    */
   void setCost(unsigned int mx, unsigned int my, unsigned char cost);
 
-  void setCost(unsigned int index, unsigned char cost);
-
   /**
    * @brief  Convert from map coordinates to world coordinates
    * @param  mx The x map coordinate
@@ -313,36 +311,6 @@ class Costmap2D
     const std::vector<geometry_msgs::msg::Point> & polygon,
     unsigned char cost_value);
 
-  template<typename CostSetter>
-  bool setConvexPolygonCost(
-    const std::vector<geometry_msgs::msg::Point> & polygon,
-    CostSetter cost_setter)
-  {
-    // 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) {
-      MapLocation loc;
-      if (!worldToMap(polygon[i].x, polygon[i].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);
-
-    // 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_setter(index);
-    }
-    return true;
-  }
-
   /**
    * @brief  Get the map cells that make up the outline of a polygon
    * @param polygon The polygon in map coordinates to rasterize

nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

@@ -154,6 +154,17 @@ class StaticLayer : public CostmapLayer
    */
   unsigned char interpretValue(unsigned char value);
 
+  void getCellsOccupiedByFootprint(
+    std::vector<MapLocation> & cells_occupied_by_footprint,
+    const std::vector<geometry_msgs::msg::Point> & footprint);
+
+  void resetCells(
+    const std::vector<MapLocation> & resetting_cells, unsigned char cost);
+
+  void restoreCellsFromMap(
+    const std::vector<MapLocation> & restoring_cells,
+    const nav_msgs::msg::OccupancyGrid::SharedPtr & map_buffer);
+
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message
@@ -162,8 +173,10 @@ class StaticLayer : public CostmapLayer
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 
   std::vector<geometry_msgs::msg::Point> transformed_footprint_;
-  std::vector<unsigned int> cleared_indexes_in_map_;
+  std::vector<MapLocation> cells_cleared_by_footprint_;
   bool footprint_clearing_enabled_;
+  bool restore_outdated_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_outdated_footprint", rclcpp::ParameterValue(false));
 
   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_outdated_footprint", restore_outdated_footprint_);
   node->get_parameter(name_ + "." + "map_topic", map_topic_);
   map_topic_ = joinWithParentNamespace(map_topic_);
   node->get_parameter(
@@ -322,6 +324,46 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u
   has_updated_data_ = true;
 }
 
+void
+StaticLayer::getCellsOccupiedByFootprint(
+  std::vector<MapLocation> & cells_occupied_by_footprint,
+  const std::vector<geometry_msgs::msg::Point> & footprint)
+{
+  // we assume the polygon is given in the global_frame...
+  // we need to transform it to map coordinates
+  std::vector<MapLocation> map_polygon;
+  for (const auto & point : footprint) {
+    MapLocation loc;
+    if (!this->worldToMap(point.x, point.y, loc.x, loc.y)) {
+      // ("Polygon lies outside map bounds, so we can't fill it");
+      return;
+    }
+    map_polygon.push_back(loc);
+  }
+
+  // get the cells that fill the polygon
+  this->convexFillCells(map_polygon, cells_occupied_by_footprint);
+}
+
+void StaticLayer::resetCells(
+  const std::vector<MapLocation> & resetting_cells, unsigned char cost)
+{
+
+  for (const auto & cell : resetting_cells) {
+    setCost(cell.x, cell.y, cost);
+  }
+}
+
+void StaticLayer::restoreCellsFromMap(
+  const std::vector<MapLocation> & restoring_cells,
+  const nav_msgs::msg::OccupancyGrid::SharedPtr & map_buffer)
+{
+
+  for (const auto & cell : restoring_cells) {
+    unsigned int index = getIndex(cell.x, cell.y);
+    costmap_[index] = interpretValue(map_buffer->data[index]);
+  }
+}
 
 void
 StaticLayer::updateBounds(
@@ -374,10 +416,17 @@ StaticLayer::updateFootprint(
 {
   if (!footprint_clearing_enabled_) {return;}
 
+  if (restore_outdated_footprint_) {
+    // Increase the bounds to make the outdated footprint restored by layered costmap
+    for (const auto & point : transformed_footprint_) {
+      touch(point.x, point.y, min_x, min_y, max_x, max_y);
+    }
+  }
+
   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);
+  for (const auto & point : transformed_footprint_) {
+    touch(point.x, point.y, min_x, min_y, max_x, max_y);
   }
 }
 
@@ -400,6 +449,17 @@ StaticLayer::updateCosts(
     return;
   }
 
+  if (footprint_clearing_enabled_) {
+    if (restore_outdated_footprint_) {
+      restoreCellsFromMap(cells_cleared_by_footprint_, map_buffer_);
+    }
+
+    cells_cleared_by_footprint_.clear();
+    getCellsOccupiedByFootprint(cells_cleared_by_footprint_, transformed_footprint_);
+
+    resetCells(cells_cleared_by_footprint_, 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_) {
@@ -444,16 +504,6 @@ StaticLayer::updateCosts(
     }
   }
 
-  if (footprint_clearing_enabled_) {
-    for (const auto index : cleared_indexes_in_map_) {
-      master_grid.setCost(index, interpretValue(map_buffer_->data[index]));
-    }
-    cleared_indexes_in_map_.clear();
-    master_grid.setConvexPolygonCost(transformed_footprint_, [this](unsigned int index) {
-        cleared_indexes_in_map_.push_back(index);
-        return nav2_costmap_2d::FREE_SPACE;
-      });
-  }
   current_ = true;
 }
 
@@ -496,6 +546,8 @@ StaticLayer::dynamicParametersCallback(
     } else if (param_type == ParameterType::PARAMETER_BOOL) {
       if (param_name == name_ + "." + "footprint_clearing_enabled") {
         footprint_clearing_enabled_ = parameter.as_bool();
+      } else if (param_name == name_ + "." + "restore_outdated_footprint") {
+        restore_outdated_footprint_ = parameter.as_bool();
       }
     }
   }

nav2_costmap_2d/src/costmap_2d.cpp

@@ -276,11 +276,6 @@ void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
   costmap_[getIndex(mx, my)] = cost;
 }
 
-void Costmap2D::setCost(unsigned int index, unsigned char cost)
-{
-  costmap_[index] = cost;
-}
-
 void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const
 {
   wx = origin_x_ + (mx + 0.5) * resolution_;