diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp index fe90502d829..66b8acde360 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp +++ b/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 +#include #include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/clear_entire_costmap.hpp" @@ -49,6 +50,18 @@ class ClearEntireCostmapService : public BtServiceNode>("plugins", "List of costmap layer names to clear. If empty, all layers will be cleared") + }); + } }; /** @@ -86,7 +99,8 @@ class ClearCostmapExceptRegionService { BT::InputPort( "reset_distance", 1, - "Distance from the robot above which obstacles are cleared") + "Distance from the robot above which obstacles are cleared"), + BT::InputPort>("plugins", "List of costmap layer names to clear. If empty, all layers will be cleared") }); } }; @@ -125,7 +139,8 @@ class ClearCostmapAroundRobotService : public BtServiceNode( "reset_distance", 1, - "Distance from the robot under which obstacles are cleared") + "Distance from the robot under which obstacles are cleared"), + BT::InputPort>("plugins", "List of costmap layer names to clear. If empty, all layers will be cleared") }); } }; @@ -166,11 +181,12 @@ class ClearCostmapAroundPoseService : public BtServiceNode( "reset_distance", 1.0, - "Distance from the pose under which obstacles are cleared") + "Distance from the pose under which obstacles are cleared"), + BT::InputPort>("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_ \ No newline at end of file diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index d637571c740..a1a3e676d9e 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp" @@ -29,6 +30,10 @@ ClearEntireCostmapService::ClearEntireCostmapService( void ClearEntireCostmapService::on_tick() { + std::vector 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 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 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 plugins; + if (getInput("plugins", plugins)) { + request_->plugins = plugins; + } + increment_recovery_count(); } diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 93d3b310dc2..80e78c76b3b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/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 & 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 & plugins); /** * @brief Clears all layers */ - void clearEntirely(); + void clearEntirely(const std::vector & 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 & costmap, double pose_x, double pose_y, double reset_distance, - bool invert); + std::shared_ptr & 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, + const std::vector & plugins) const; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index eec73de6f9d..d7d4b7a9378 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -84,7 +84,7 @@ 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( @@ -92,7 +92,7 @@ void ClearCostmapService::clearAroundRobotCallback( const shared_ptr request, const shared_ptr/*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/*request_header*/, - const std::shared_ptr/*request*/, + const std::shared_ptr request, const std::shared_ptr/*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 & 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(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 & 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(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 & plugins) { - std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); - costmap_.resetLayers(); + if (plugins.empty()) { + // Default behavior: clear all layers + std::unique_lock 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(layer); + std::unique_lock 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, + const std::vector & 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 diff --git a/nav2_msgs/srv/ClearCostmapAroundPose.srv b/nav2_msgs/srv/ClearCostmapAroundPose.srv index 5d244279f67..04c8c18b296 100644 --- a/nav2_msgs/srv/ClearCostmapAroundPose.srv +++ b/nav2_msgs/srv/ClearCostmapAroundPose.srv @@ -2,5 +2,6 @@ geometry_msgs/PoseStamped pose float64 reset_distance +string[] plugins --- std_msgs/Empty response diff --git a/nav2_msgs/srv/ClearCostmapAroundRobot.srv b/nav2_msgs/srv/ClearCostmapAroundRobot.srv index e3b41bf584e..1e115afb3f0 100644 --- a/nav2_msgs/srv/ClearCostmapAroundRobot.srv +++ b/nav2_msgs/srv/ClearCostmapAroundRobot.srv @@ -1,5 +1,6 @@ # Clears the costmap within a distance float32 reset_distance +string[] plugins --- std_msgs/Empty response diff --git a/nav2_msgs/srv/ClearCostmapExceptRegion.srv b/nav2_msgs/srv/ClearCostmapExceptRegion.srv index 56bb2f4938e..e3ee961f541 100644 --- a/nav2_msgs/srv/ClearCostmapExceptRegion.srv +++ b/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 diff --git a/nav2_msgs/srv/ClearEntireCostmap.srv b/nav2_msgs/srv/ClearEntireCostmap.srv index ed1b675f2d4..7e5d20d511e 100644 --- a/nav2_msgs/srv/ClearEntireCostmap.srv +++ b/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