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..3470ba1d0ff 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,8 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_ #include +#include +#include #include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/clear_entire_costmap.hpp" @@ -49,6 +51,27 @@ class ClearEntireCostmapService : public BtServiceNode 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>("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 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( "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 plugin names to clear. If empty, all plugins will be cleared") }); } }; @@ -115,6 +148,14 @@ class ClearCostmapAroundRobotService : public BtServiceNode 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( "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 plugin names to clear. If empty, all plugins will be cleared") }); } }; @@ -154,6 +197,14 @@ class ClearCostmapAroundPoseService : public BtServiceNode 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( "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 plugin names to clear. If empty, all plugins will be cleared") }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 7161f6419ce..c7ef7859cd1 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -67,18 +67,21 @@ Service name Server timeout + List of specific plugins to clear. Service name Server timeout Distance from the robot above which obstacles are cleared + List of specific plugins to clear. Service name Server timeout Distance from the robot under which obstacles are cleared + List of specific plugins to clear. @@ -86,6 +89,7 @@ Server timeout Pose around which to clear the costmap Distance from the pose under which obstacles are cleared + List of specific plugins to clear. diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index d637571c740..2b52c9a3f1b 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,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 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 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 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 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" diff --git a/nav2_behavior_tree/test/utils/test_service.hpp b/nav2_behavior_tree/test/utils/test_service.hpp index e9abad8c0c9..e018cc6cfa8 100644 --- a/nav2_behavior_tree/test/utils/test_service.hpp +++ b/nav2_behavior_tree/test/utils/test_service.hpp @@ -48,11 +48,22 @@ class TestService : public rclcpp::Node const std::shared_ptr response) { (void)request_header; - (void)response; current_request_ = request; + setSuccessIfExists(response); } private: + template + auto setSuccessIfExists(std::shared_ptr response) -> decltype(response->success, void()) + { + response->success = true; + } + + template + void setSuccessIfExists(T) + { + } + typename rclcpp::Service::SharedPtr server_; std::shared_ptr current_request_; }; 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..811ef31d2dc 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 @@ -18,6 +18,7 @@ #include #include #include +#include #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 & 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 & plugins); /** * @brief Clears all layers + * @return true if at least one layer was successfully cleared, false otherwise */ - void clearEntirely(); + bool clearEntirely(const std::vector & 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 & 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; + + /** + * @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 & requested_plugins, + const std::vector> * layers, + std::vector & valid_plugins, + std::vector & 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 & plugins, + const std::vector> * layers, + std::function &)> 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, + 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..03548ff0032 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -78,50 +78,51 @@ ClearCostmapService::~ClearCostmapService() void ClearCostmapService::clearExceptRegionCallback( const shared_ptr/*request_header*/, const shared_ptr request, - const shared_ptr/*response*/) + const shared_ptr response) { RCLCPP_INFO( logger_, "%s", ("Received request to clear except a region the " + costmap_.getName()).c_str()); - clearRegion(request->reset_distance, true); + response->success = clearRegion(request->reset_distance, true, request->plugins); } void ClearCostmapService::clearAroundRobotCallback( const shared_ptr/*request_header*/, const shared_ptr request, - const shared_ptr/*response*/) + const shared_ptr response) { - clearRegion(request->reset_distance, false); + response->success = clearRegion(request->reset_distance, false, request->plugins); } void ClearCostmapService::clearAroundPoseCallback( const shared_ptr/*request_header*/, const shared_ptr request, - const shared_ptr/*response*/) + const shared_ptr response) { RCLCPP_INFO( logger_, "%s", ("Received request to clear around pose for " + costmap_.getName()).c_str()); - clearAroundPose(request->pose, request->reset_distance); + response->success = 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/*response*/) + 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(); + response->success = clearEntirely(request->plugins); } -void ClearCostmapService::clearAroundPose( +bool ClearCostmapService::clearAroundPose( const geometry_msgs::msg::PoseStamped & pose, - const double reset_distance) + const double reset_distance, + const std::vector & plugins) { double x, y; @@ -138,7 +139,7 @@ void ClearCostmapService::clearAroundPose( logger_, "Cannot clear map around pose because pose cannot be transformed to costmap frame: %s", ex.what()); - return; + return false; } x = global_pose.pose.position.x; @@ -146,15 +147,31 @@ void ClearCostmapService::clearAroundPose( auto layers = costmap_.getLayeredCostmap()->getPlugins(); - for (auto & layer : *layers) { - if (layer->isClearable()) { - auto costmap_layer = std::static_pointer_cast(layer); - clearLayerRegion(costmap_layer, x, y, reset_distance, false); + if (!plugins.empty()) { + return validateAndClearPlugins( + plugins, layers, + [this, x, y, reset_distance](std::shared_ptr & layer) { + clearLayerRegion(layer, x, y, reset_distance, false); + }, + "clear costmap around pose"); + } else { + // Clear all clearable layers (default behavior) + bool any_plugin_cleared = false; + for (auto & layer : *layers) { + if (layer->isClearable()) { + auto costmap_layer = std::static_pointer_cast(layer); + clearLayerRegion(costmap_layer, x, y, reset_distance, false); + any_plugin_cleared = true; + } } + + return any_plugin_cleared; } } -void ClearCostmapService::clearRegion(const double reset_distance, bool invert) +bool ClearCostmapService::clearRegion( + const double reset_distance, bool invert, + const std::vector & plugins) { double x, y; @@ -162,20 +179,31 @@ void ClearCostmapService::clearRegion(const double reset_distance, bool invert) RCLCPP_ERROR( logger_, "%s", "Cannot clear map because robot pose cannot be retrieved."); - return; + return false; } auto layers = costmap_.getLayeredCostmap()->getPlugins(); - for (auto & layer : *layers) { - if (layer->isClearable()) { - auto costmap_layer = std::static_pointer_cast(layer); - clearLayerRegion(costmap_layer, x, y, reset_distance, invert); + if (!plugins.empty()) { + return validateAndClearPlugins( + plugins, layers, + [this, x, y, reset_distance, invert](std::shared_ptr & layer) { + clearLayerRegion(layer, x, y, reset_distance, invert); + }, + "clear costmap region"); + } else { + // Clear all clearable layers (default behavior) + bool any_plugin_cleared = false; + for (auto & layer : *layers) { + if (layer->isClearable()) { + auto costmap_layer = std::static_pointer_cast(layer); + clearLayerRegion(costmap_layer, x, y, reset_distance, invert); + any_plugin_cleared = true; + } } - } - // AlexeyMerzlyakov: No need to clear layer region for costmap filters - // as they are always supposed to be not clearable. + return any_plugin_cleared; + } } void ClearCostmapService::clearLayerRegion( @@ -200,10 +228,143 @@ void ClearCostmapService::clearLayerRegion( costmap->addExtraBounds(ox, oy, ox + width, oy + height); } -void ClearCostmapService::clearEntirely() +bool 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(); + return true; + } else { + // Clear only specified plugins + std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); + auto layers = costmap_.getLayeredCostmap()->getPlugins(); + + bool result = validateAndClearPlugins( + plugins, layers, + [this](std::shared_ptr & layer) { + RCLCPP_INFO(logger_, "Clearing entire layer: %s", layer->getName().c_str()); + layer->resetMap(0, 0, layer->getSizeInCellsX(), layer->getSizeInCellsY()); + }, + "clear costmap entirely"); + + if (result) { + RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); + costmap_.getCostmap()->resetMap(0, 0, + costmap_.getCostmap()->getSizeInCellsX(), + costmap_.getCostmap()->getSizeInCellsY()); + } + + return result; + } +} + +void ClearCostmapService::validateAndCategorizePlugins( + const std::vector & requested_plugins, + const std::vector> * layers, + std::vector & valid_plugins, + std::vector & invalid_plugins) const +{ + valid_plugins.clear(); + invalid_plugins.clear(); + + for (const auto & requested_plugin : requested_plugins) { + bool found = false; + bool clearable = false; + + for (auto & layer : *layers) { + if (layer->getName() == requested_plugin) { + found = true; + clearable = layer->isClearable(); + break; + } + } + + if (!found) { + invalid_plugins.push_back(requested_plugin + " (not found)"); + } else if (!clearable) { + invalid_plugins.push_back(requested_plugin + " (not clearable)"); + } else { + valid_plugins.push_back(requested_plugin); + } + } +} + +bool ClearCostmapService::validateAndClearPlugins( + const std::vector & plugins, + const std::vector> * layers, + std::function &)> clear_callback, + const std::string & operation_name) const +{ + std::vector invalid_plugins; + std::vector valid_plugins; + + // Validate all requested plugins + validateAndCategorizePlugins(plugins, layers, valid_plugins, invalid_plugins); + + // Log validation errors + if (!invalid_plugins.empty()) { + std::string error_msg = "Invalid plugin(s) requested for clearing: "; + for (size_t i = 0; i < invalid_plugins.size(); ++i) { + error_msg += invalid_plugins[i]; + if (i < invalid_plugins.size() - 1) { + error_msg += ", "; + } + } + RCLCPP_ERROR(logger_, "%s", error_msg.c_str()); + } + + // Clear all valid plugins using the provided callback + bool any_plugin_cleared = false; + for (auto & layer : *layers) { + if (std::find(valid_plugins.begin(), valid_plugins.end(), + layer->getName()) != valid_plugins.end()) + { + auto costmap_layer = std::static_pointer_cast(layer); + clear_callback(costmap_layer); + any_plugin_cleared = true; + } + } + + // Return failure if any requested plugin was invalid + if (!invalid_plugins.empty()) { + RCLCPP_ERROR( + logger_, + "Failed to %s: %zu invalid plugin(s) out of %zu requested", + operation_name.c_str(), invalid_plugins.size(), plugins.size()); + return false; + } + + if (!any_plugin_cleared) { + RCLCPP_ERROR(logger_, "No plugins were cleared in %s", operation_name.c_str()); + return false; + } + + return true; +} + +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..e255b825a1d 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 +bool success diff --git a/nav2_msgs/srv/ClearCostmapAroundRobot.srv b/nav2_msgs/srv/ClearCostmapAroundRobot.srv index e3b41bf584e..ff8e8870245 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 +bool success diff --git a/nav2_msgs/srv/ClearCostmapExceptRegion.srv b/nav2_msgs/srv/ClearCostmapExceptRegion.srv index 56bb2f4938e..0c0d75c686a 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 +bool success diff --git a/nav2_msgs/srv/ClearEntireCostmap.srv b/nav2_msgs/srv/ClearEntireCostmap.srv index ed1b675f2d4..e7e95281ba7 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 +bool success diff --git a/nav2_system_tests/src/behavior_tree/server_handler.cpp b/nav2_system_tests/src/behavior_tree/server_handler.cpp index 9eed165cab3..276ab1c0175 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.cpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.cpp @@ -31,9 +31,9 @@ ServerHandler::ServerHandler() { node_ = rclcpp::Node::make_shared("behavior_tree_tester"); - clear_local_costmap_server = std::make_unique>( + clear_local_costmap_server = std::make_unique( node_, "local_costmap/clear_entirely_local_costmap"); - clear_global_costmap_server = std::make_unique>( + clear_global_costmap_server = std::make_unique( node_, "global_costmap/clear_entirely_global_costmap"); compute_path_to_pose_server = std::make_unique(node_); follow_path_server = std::make_unique(node_); diff --git a/nav2_system_tests/src/behavior_tree/server_handler.hpp b/nav2_system_tests/src/behavior_tree/server_handler.hpp index 2b852cd085f..93843e91789 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.hpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.hpp @@ -95,6 +95,22 @@ class DummyFollowPathActionServer : public DummyActionServer +{ +public: + explicit DummyClearCostmapService( + const rclcpp::Node::SharedPtr & node, + std::string service_name) + : DummyService(node, service_name) {} + +protected: + void fillResponse( + const std::shared_ptr/*request*/, + const std::shared_ptr response) override + { + response->success = true; + } +}; class ServerHandler { @@ -114,8 +130,8 @@ class ServerHandler void reset() const; public: - std::unique_ptr> clear_local_costmap_server; - std::unique_ptr> clear_global_costmap_server; + std::unique_ptr clear_local_costmap_server; + std::unique_ptr clear_global_costmap_server; std::unique_ptr compute_path_to_pose_server; std::unique_ptr follow_path_server; std::unique_ptr> spin_server;