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,8 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
 
 #include <string>
+#include <vector>
+#include <memory>
 
 #include "nav2_behavior_tree/bt_service_node.hpp"
 #include "nav2_msgs/srv/clear_entire_costmap.hpp"
@@ -49,6 +51,27 @@ class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEnti
    * @return BT::NodeStatus Status of tick execution
    */
   void on_tick() override;
+
+  /**
+   * @brief Check the service response and return appropriate BT status
+   * @param response Service response containing success status
+   * @return BT::NodeStatus SUCCESS if service succeeded, FAILURE otherwise
+   */
+  BT::NodeStatus on_completion(
+    std::shared_ptr<typename nav2_msgs::srv::ClearEntireCostmap::Response> response) 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 plugin names to clear. If empty, all plugins will be cleared")
+      });
+  }
 };
 
 /**
@@ -76,6 +99,14 @@ class ClearCostmapExceptRegionService
    */
   void on_tick() override;
 
+  /**
+   * @brief Check the service response and return appropriate BT status
+   * @param response Service response containing success status
+   * @return BT::NodeStatus SUCCESS if service succeeded, FAILURE otherwise
+   */
+  BT::NodeStatus on_completion(
+    std::shared_ptr<typename nav2_msgs::srv::ClearCostmapExceptRegion::Response> response) override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -86,7 +117,9 @@ 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 plugin names to clear. If empty, all plugins will be cleared")
       });
   }
 };
@@ -115,6 +148,14 @@ class ClearCostmapAroundRobotService : public BtServiceNode<nav2_msgs::srv::Clea
    */
   void on_tick() override;
 
+  /**
+   * @brief Check the service response and return appropriate BT status
+   * @param response Service response containing success status
+   * @return BT::NodeStatus SUCCESS if service succeeded, FAILURE otherwise
+   */
+  BT::NodeStatus on_completion(
+    std::shared_ptr<typename nav2_msgs::srv::ClearCostmapAroundRobot::Response> response) override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -125,7 +166,9 @@ 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 plugin names to clear. If empty, all plugins will be cleared")
       });
   }
 };
@@ -154,6 +197,14 @@ class ClearCostmapAroundPoseService : public BtServiceNode<nav2_msgs::srv::Clear
    */
   void on_tick() override;
 
+  /**
+   * @brief Check the service response and return appropriate BT status
+   * @param response Service response containing success status
+   * @return BT::NodeStatus SUCCESS if service succeeded, FAILURE otherwise
+   */
+  BT::NodeStatus on_completion(
+    std::shared_ptr<typename nav2_msgs::srv::ClearCostmapAroundPose::Response> response) override;
+
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -166,7 +217,9 @@ 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 plugin names to clear. If empty, all plugins will be cleared")
       });
   }
 };

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -67,25 +67,29 @@
     <Action ID="ClearEntireCostmap">
       <input_port name="service_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="plugins">List of specific plugins to clear.</input_port>
     </Action>
 
     <Action ID="ClearCostmapExceptRegion">
       <input_port name="service_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <input_port name="reset_distance">Distance from the robot above which obstacles are cleared</input_port>
+      <input_port name="plugins">List of specific plugins to clear.</input_port>
     </Action>
 
     <Action ID="ClearCostmapAroundRobot">
       <input_port name="service_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <input_port name="reset_distance">Distance from the robot under which obstacles are cleared</input_port>
+      <input_port name="plugins">List of specific plugins to clear.</input_port>
     </Action>
 
     <Action ID="ClearCostmapAroundPose">
       <input_port name="service_name">Service name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <input_port name="pose">Pose around which to clear the costmap</input_port>
       <input_port name="reset_distance">Distance from the pose under which obstacles are cleared</input_port>
+      <input_port name="plugins">List of specific plugins to clear.</input_port>
     </Action>
 
     <Action ID="ComputePathToPose">

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,9 +30,23 @@ ClearEntireCostmapService::ClearEntireCostmapService(
 
 void ClearEntireCostmapService::on_tick()
 {
+  if (!getInput("plugins", request_->plugins)) {
+    request_->plugins.clear();
+  }
   increment_recovery_count();
 }
 
+BT::NodeStatus ClearEntireCostmapService::on_completion(
+  std::shared_ptr<typename nav2_msgs::srv::ClearEntireCostmap::Response> response)
+{
+  if (response->success) {
+    return BT::NodeStatus::SUCCESS;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "ClearEntireCostmap: Failed to clear costmap layers");
+    return BT::NodeStatus::FAILURE;
+  }
+}
+
 ClearCostmapExceptRegionService::ClearCostmapExceptRegionService(
   const std::string & service_node_name,
   const BT::NodeConfiguration & conf)
@@ -42,9 +57,24 @@ ClearCostmapExceptRegionService::ClearCostmapExceptRegionService(
 void ClearCostmapExceptRegionService::on_tick()
 {
   getInput("reset_distance", request_->reset_distance);
+  if (!getInput("plugins", request_->plugins)) {
+    request_->plugins.clear();
+  }
+
   increment_recovery_count();
 }
 
+BT::NodeStatus ClearCostmapExceptRegionService::on_completion(
+  std::shared_ptr<typename nav2_msgs::srv::ClearCostmapExceptRegion::Response> response)
+{
+  if (response->success) {
+    return BT::NodeStatus::SUCCESS;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "ClearCostmapExceptRegion: Failed to clear costmap layers");
+    return BT::NodeStatus::FAILURE;
+  }
+}
+
 ClearCostmapAroundRobotService::ClearCostmapAroundRobotService(
   const std::string & service_node_name,
   const BT::NodeConfiguration & conf)
@@ -55,9 +85,25 @@ ClearCostmapAroundRobotService::ClearCostmapAroundRobotService(
 void ClearCostmapAroundRobotService::on_tick()
 {
   getInput("reset_distance", request_->reset_distance);
+
+  if (!getInput("plugins", request_->plugins)) {
+    request_->plugins.clear();
+  }
+
   increment_recovery_count();
 }
 
+BT::NodeStatus ClearCostmapAroundRobotService::on_completion(
+  std::shared_ptr<typename nav2_msgs::srv::ClearCostmapAroundRobot::Response> response)
+{
+  if (response->success) {
+    return BT::NodeStatus::SUCCESS;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "ClearCostmapAroundRobot: Failed to clear costmap layers");
+    return BT::NodeStatus::FAILURE;
+  }
+}
+
 ClearCostmapAroundPoseService::ClearCostmapAroundPoseService(
   const std::string & service_node_name,
   const BT::NodeConfiguration & conf)
@@ -69,9 +115,25 @@ void ClearCostmapAroundPoseService::on_tick()
 {
   getInput("pose", request_->pose);
   getInput("reset_distance", request_->reset_distance);
+
+  if (!getInput("plugins", request_->plugins)) {
+    request_->plugins.clear();
+  }
+
   increment_recovery_count();
 }
 
+BT::NodeStatus ClearCostmapAroundPoseService::on_completion(
+  std::shared_ptr<typename nav2_msgs::srv::ClearCostmapAroundPose::Response> response)
+{
+  if (response->success) {
+    return BT::NodeStatus::SUCCESS;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "ClearCostmapAroundPose: Failed to clear costmap layers");
+    return BT::NodeStatus::FAILURE;
+  }
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"

nav2_behavior_tree/test/utils/test_service.hpp

@@ -48,11 +48,22 @@ class TestService : public rclcpp::Node
     const std::shared_ptr<typename ServiceT::Response> response)
   {
     (void)request_header;
-    (void)response;
     current_request_ = request;
+    setSuccessIfExists(response);
   }
 
 private:
+  template<typename T>
+  auto setSuccessIfExists(std::shared_ptr<T> response) -> decltype(response->success, void())
+  {
+    response->success = true;
+  }
+
+  template<typename T>
+  void setSuccessIfExists(T)
+  {
+  }
+
   typename rclcpp::Service<ServiceT>::SharedPtr server_;
   std::shared_ptr<typename ServiceT::Request> current_request_;
 };

nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp

@@ -18,6 +18,7 @@
 #include <vector>
 #include <string>
 #include <memory>
+#include <functional>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_msgs/srv/clear_costmap_except_region.hpp"
@@ -58,18 +59,23 @@ class ClearCostmapService
 
   /**
    * @brief Clears the region outside of a user-specified area reverting to the static map
+   * @return true if at least one layer was successfully cleared, false otherwise
    */
-  void clearRegion(double reset_distance, bool invert);
+  bool clearRegion(double reset_distance, bool invert, const std::vector<std::string> & plugins);
 
   /**
    * @brief Clears the region around a specific pose
+   * @return true if at least one layer was successfully cleared, false otherwise
    */
-  void clearAroundPose(const geometry_msgs::msg::PoseStamped & pose, double reset_distance);
+  bool clearAroundPose(
+    const geometry_msgs::msg::PoseStamped & pose, double reset_distance,
+    const std::vector<std::string> & plugins);
 
   /**
    * @brief Clears all layers
+   * @return true if at least one layer was successfully cleared, false otherwise
    */
-  void clearEntirely();
+  bool clearEntirely(const std::vector<std::string> & plugins);
 
 private:
   // The Logger object for logging
@@ -126,13 +132,47 @@ 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;
+
+  /**
+   * @brief Validates requested plugins and returns lists of valid and invalid plugins
+   * @param requested_plugins List of plugin names to validate
+   * @param layers Pointer to all available layers
+   * @param valid_plugins Output: list of valid plugin names
+   * @param invalid_plugins Output: list of invalid plugins with reasons
+   */
+  void validateAndCategorizePlugins(
+    const std::vector<std::string> & requested_plugins,
+    const std::vector<std::shared_ptr<Layer>> * layers,
+    std::vector<std::string> & valid_plugins,
+    std::vector<std::string> & invalid_plugins) const;
+
+  /**
+   * @brief Validates plugins and clears valid ones using provided callback
+   * @param plugins List of plugin names to clear
+   * @param layers Pointer to all available layers
+   * @param clear_callback Function to call for each valid plugin
+   * @param operation_name Name of the operation for logging (e.g. "clearAroundPose")
+   * @return true if all requested plugins were valid and cleared, false otherwise
+   */
+  bool validateAndClearPlugins(
+    const std::vector<std::string> & plugins,
+    const std::vector<std::shared_ptr<Layer>> * layers,
+    std::function<void(std::shared_ptr<CostmapLayer> &)> clear_callback,
+    const std::string & operation_name) const;
+
+  /**
+   * @brief Determines whether a specific layer should be cleared based on plugin list and clearable status
+   */
+  bool shouldClearLayer(
+    const std::shared_ptr<Layer> & layer,
+    const std::vector<std::string> & plugins) const;
 };
 
 }  // namespace nav2_costmap_2d