Add "plugins" Parameter to BT XML for Selective Clearing of Costmap Layers

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp

@@ -16,6 +16,7 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
 
 #include <string>
+#include <vector>
 
 #include "nav2_behavior_tree/bt_service_node.hpp"
 #include "nav2_msgs/srv/clear_entire_costmap.hpp"
@@ -49,6 +50,18 @@ class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEnti
    * @return BT::NodeStatus Status of tick execution
    */
   void on_tick() override;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::InputPort<std::vector<std::string>>("plugins", "List of costmap layer names to clear. If empty, all layers will be cleared")
+      });
+  }
 };
 
 /**
@@ -86,7 +99,8 @@ class ClearCostmapExceptRegionService
       {
         BT::InputPort<double>(
           "reset_distance", 1,
-          "Distance from the robot above which obstacles are cleared")
+          "Distance from the robot above which obstacles are cleared"),
+        BT::InputPort<std::vector<std::string>>("plugins", "List of costmap layer names to clear. If empty, all layers will be cleared")
       });
   }
 };
@@ -125,7 +139,8 @@ class ClearCostmapAroundRobotService : public BtServiceNode<nav2_msgs::srv::Clea
       {
         BT::InputPort<double>(
           "reset_distance", 1,
-          "Distance from the robot under which obstacles are cleared")
+          "Distance from the robot under which obstacles are cleared"),
+        BT::InputPort<std::vector<std::string>>("plugins", "List of costmap layer names to clear. If empty, all layers will be cleared")
       });
   }
 };
@@ -166,11 +181,12 @@ class ClearCostmapAroundPoseService : public BtServiceNode<nav2_msgs::srv::Clear
           "pose", "Pose around which to clear the costmap"),
         BT::InputPort<double>(
           "reset_distance", 1.0,
-          "Distance from the pose under which obstacles are cleared")
+          "Distance from the pose under which obstacles are cleared"),
+        BT::InputPort<std::vector<std::string>>("plugins", "List of costmap layer names to clear. If empty, all layers will be cleared")
       });
   }
 };
 
 }  // namespace nav2_behavior_tree
 
-#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_

nav2_behavior_tree/plugins/action/clear_costmap_service.cpp

@@ -14,6 +14,7 @@
 
 #include <string>
 #include <memory>
+#include <vector>
 
 #include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
 
@@ -29,6 +30,10 @@ ClearEntireCostmapService::ClearEntireCostmapService(
 
 void ClearEntireCostmapService::on_tick()
 {
+  std::vector<std::string> plugins;
+  if (getInput("plugins", plugins)) {
+    request_->plugins = plugins;
+  }
   increment_recovery_count();
 }
 
@@ -42,6 +47,11 @@ ClearCostmapExceptRegionService::ClearCostmapExceptRegionService(
 void ClearCostmapExceptRegionService::on_tick()
 {
   getInput("reset_distance", request_->reset_distance);
+  std::vector<std::string> plugins;
+  if (getInput("plugins", plugins)) {
+    request_->plugins = plugins;
+  }
+
   increment_recovery_count();
 }
 
@@ -55,6 +65,12 @@ ClearCostmapAroundRobotService::ClearCostmapAroundRobotService(
 void ClearCostmapAroundRobotService::on_tick()
 {
   getInput("reset_distance", request_->reset_distance);
+
+  std::vector<std::string> plugins;
+  if (getInput("plugins", plugins)) {
+    request_->plugins = plugins;
+  }
+
   increment_recovery_count();
 }
 
@@ -69,6 +85,12 @@ void ClearCostmapAroundPoseService::on_tick()
 {
   getInput("pose", request_->pose);
   getInput("reset_distance", request_->reset_distance);
+
+  std::vector<std::string> plugins;
+  if (getInput("plugins", plugins)) {
+    request_->plugins = plugins;
+  }
+
   increment_recovery_count();
 }
 

nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp

@@ -59,17 +59,17 @@ class ClearCostmapService
   /**
    * @brief Clears the region outside of a user-specified area reverting to the static map
    */
-  void clearRegion(double reset_distance, bool invert);
+  void clearRegion(double reset_distance, bool invert, const std::vector<std::string> & plugins);
 
   /**
    * @brief Clears the region around a specific pose
    */
-  void clearAroundPose(const geometry_msgs::msg::PoseStamped & pose, double reset_distance);
+  void clearAroundPose(const geometry_msgs::msg::PoseStamped & pose, double reset_distance, const std::vector<std::string> & plugins);
 
   /**
    * @brief Clears all layers
    */
-  void clearEntirely();
+  void clearEntirely(const std::vector<std::string> & plugins);
 
 private:
   // The Logger object for logging
@@ -126,13 +126,17 @@ class ClearCostmapService
    * @brief  Function used to clear a given costmap layer
    */
   void clearLayerRegion(
-    std::shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance,
-    bool invert);
+    std::shared_ptr<CostmapLayer> & costmap,
+    double pose_x, double pose_y, double reset_distance, bool invert);
 
   /**
    * @brief Get the robot's position in the costmap using the master costmap
    */
   bool getPosition(double & x, double & y) const;
+
+  bool shouldClearLayer(
+    const std::shared_ptr<Layer> & layer,
+    const std::vector<std::string> & plugins) const;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/src/clear_costmap_service.cpp

@@ -84,15 +84,15 @@ void ClearCostmapService::clearExceptRegionCallback(
     logger_, "%s",
     ("Received request to clear except a region the " + costmap_.getName()).c_str());
 
-  clearRegion(request->reset_distance, true);
+  clearRegion(request->reset_distance, true, request->plugins);
 }
 
 void ClearCostmapService::clearAroundRobotCallback(
   const shared_ptr<rmw_request_id_t>/*request_header*/,
   const shared_ptr<ClearAroundRobot::Request> request,
   const shared_ptr<ClearAroundRobot::Response>/*response*/)
 {
-  clearRegion(request->reset_distance, false);
+  clearRegion(request->reset_distance, false, request->plugins);
 }
 
 void ClearCostmapService::clearAroundPoseCallback(
@@ -104,24 +104,25 @@ void ClearCostmapService::clearAroundPoseCallback(
     logger_, "%s",
     ("Received request to clear around pose for " + costmap_.getName()).c_str());
 
-  clearAroundPose(request->pose, request->reset_distance);
+  clearAroundPose(request->pose, request->reset_distance, request->plugins);
 }
 
 void ClearCostmapService::clearEntireCallback(
   const std::shared_ptr<rmw_request_id_t>/*request_header*/,
-  const std::shared_ptr<ClearEntirely::Request>/*request*/,
+  const std::shared_ptr<ClearEntirely::Request> request,
   const std::shared_ptr<ClearEntirely::Response>/*response*/)
 {
   RCLCPP_INFO(
     logger_, "%s",
     ("Received request to clear entirely the " + costmap_.getName()).c_str());
 
-  clearEntirely();
+  clearEntirely(request->plugins);
 }
 
 void ClearCostmapService::clearAroundPose(
   const geometry_msgs::msg::PoseStamped & pose,
-  const double reset_distance)
+  const double reset_distance,
+  const std::vector<std::string> & plugins)
 {
   double x, y;
 
@@ -147,14 +148,14 @@ void ClearCostmapService::clearAroundPose(
   auto layers = costmap_.getLayeredCostmap()->getPlugins();
 
   for (auto & layer : *layers) {
-    if (layer->isClearable()) {
+    if (shouldClearLayer(layer, plugins)) {
       auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
       clearLayerRegion(costmap_layer, x, y, reset_distance, false);
     }
   }
 }
 
-void ClearCostmapService::clearRegion(const double reset_distance, bool invert)
+void ClearCostmapService::clearRegion(const double reset_distance, bool invert, const std::vector<std::string> & plugins)
 {
   double x, y;
 
@@ -168,7 +169,7 @@ void ClearCostmapService::clearRegion(const double reset_distance, bool invert)
   auto layers = costmap_.getLayeredCostmap()->getPlugins();
 
   for (auto & layer : *layers) {
-    if (layer->isClearable()) {
+    if (shouldClearLayer(layer, plugins)) {
       auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
       clearLayerRegion(costmap_layer, x, y, reset_distance, invert);
     }
@@ -200,10 +201,56 @@ void ClearCostmapService::clearLayerRegion(
   costmap->addExtraBounds(ox, oy, ox + width, oy + height);
 }
 
-void ClearCostmapService::clearEntirely()
+void ClearCostmapService::clearEntirely(const std::vector<std::string> & plugins)
 {
-  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getCostmap()->getMutex()));
-  costmap_.resetLayers();
+  if (plugins.empty()) {
+    // Default behavior: clear all layers
+    std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getCostmap()->getMutex()));
+    RCLCPP_INFO(logger_, "Clearing all layers in costmap: %s", costmap_.getName().c_str()); 
+    costmap_.resetLayers();
+  } else {
+    // Clear only specified plugins
+    auto layers = costmap_.getLayeredCostmap()->getPlugins();
+
+    for (auto & layer : *layers) {
+      if (shouldClearLayer(layer, plugins)) {
+        if (layer->isClearable()) {
+          RCLCPP_INFO(logger_, "Clearing entire layer: %s", layer->getName().c_str());
+          auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
+          std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_layer->getMutex())); 
+          costmap_layer->resetMap(0, 0, costmap_layer->getSizeInCellsX(), costmap_layer->getSizeInCellsY());
+        } else {
+          RCLCPP_WARN(
+            logger_,
+            "Layer '%s' is not clearable, skipping.",
+            layer->getName().c_str());
+        }
+      }
+    }
+  }
+}
+
+bool ClearCostmapService::shouldClearLayer(
+  const std::shared_ptr<Layer> & layer,
+  const std::vector<std::string> & plugins) const
+{
+  // If no specific plugins requested, use default behavior (clear all clearable layers)
+  if (plugins.empty()) {
+    return layer->isClearable();
+  }
+
+  // If specific plugins requested, check if this layer is in the list AND clearable
+  bool is_in_plugin_list = std::find(plugins.begin(), plugins.end(), layer->getName()) != plugins.end();
+
+  if (is_in_plugin_list && !layer->isClearable()) {
+    RCLCPP_WARN(
+      logger_,
+      "Plugin '%s' was requested to be cleared but is not clearable. Skipping.",
+      layer->getName().c_str());
+    return false;
+  }
+
+  return is_in_plugin_list && layer->isClearable();
 }
 
 bool ClearCostmapService::getPosition(double & x, double & y) const

nav2_msgs/srv/ClearCostmapAroundPose.srv

@@ -2,5 +2,6 @@
 
 geometry_msgs/PoseStamped pose
 float64 reset_distance
+string[] plugins
 ---
 std_msgs/Empty response

nav2_msgs/srv/ClearCostmapAroundRobot.srv

@@ -1,5 +1,6 @@
 # Clears the costmap within a distance
 
 float32 reset_distance
+string[] plugins
 ---
 std_msgs/Empty response

nav2_msgs/srv/ClearCostmapExceptRegion.srv

@@ -1,5 +1,6 @@
 # Clears the costmap except a rectangular region specified by reset_distance
 
 float32 reset_distance
+string[] plugins
 ---
 std_msgs/Empty response

nav2_msgs/srv/ClearEntireCostmap.srv

@@ -1,5 +1,6 @@
 # Clears all layers on the costmap
 
 std_msgs/Empty request
+string[] plugins
 ---
 std_msgs/Empty response