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 eabba3a4b09..a97fce6ca54 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_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 93d3b310dc2..0e5a7819ced 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 @@ -58,18 +58,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 +131,20 @@ 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 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..a157c3debc2 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,23 +139,44 @@ 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; y = global_pose.pose.position.y; auto layers = costmap_.getLayeredCostmap()->getPlugins(); + bool any_plugin_cleared = false; + bool any_clearable_plugin_failed = false; 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); + any_plugin_cleared = true; + } else { + // Check if this was a clearable plugin that failed to clear + bool is_in_plugin_list = std::find(plugins.begin(), plugins.end(), + layer->getName()) != plugins.end(); + if (is_in_plugin_list && layer->isClearable()) { + RCLCPP_ERROR(logger_, "Clearable plugin '%s' failed to clear in clearAroundPose", + layer->getName().c_str()); + any_clearable_plugin_failed = true; + } } } + + if (!any_plugin_cleared) { + RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearAroundPose"); + } + + // Return false if any clearable plugin failed to clear OR if no plugins were cleared at all + return !any_clearable_plugin_failed && 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 +184,39 @@ 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(); + bool any_plugin_cleared = false; + bool any_clearable_plugin_failed = false; 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); + any_plugin_cleared = true; + } else { + // Check if this was a clearable plugin that failed to clear + bool is_in_plugin_list = std::find(plugins.begin(), plugins.end(), + layer->getName()) != plugins.end(); + if (is_in_plugin_list && layer->isClearable()) { + RCLCPP_ERROR(logger_, "Clearable plugin '%s' failed to clear in clearRegion", + layer->getName().c_str()); + any_clearable_plugin_failed = true; + } } } // AlexeyMerzlyakov: No need to clear layer region for costmap filters // as they are always supposed to be not clearable. + + if (!any_plugin_cleared) { + RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearRegion"); + } + + // Return false if any clearable plugin failed to clear OR if no plugins were cleared at all + return !any_clearable_plugin_failed && any_plugin_cleared; } void ClearCostmapService::clearLayerRegion( @@ -200,10 +241,72 @@ 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 any_plugin_cleared = false; + bool any_clearable_plugin_failed = false; + for (auto & layer : *layers) { + if (shouldClearLayer(layer, plugins)) { + RCLCPP_INFO(logger_, "Clearing entire layer: %s", layer->getName().c_str()); + auto costmap_layer = std::static_pointer_cast(layer); + costmap_layer->resetMap(0, 0, costmap_layer->getSizeInCellsX(), + costmap_layer->getSizeInCellsY()); + any_plugin_cleared = true; + } else { + // Check if this was a clearable plugin that failed to clear + bool is_in_plugin_list = std::find(plugins.begin(), plugins.end(), + layer->getName()) != plugins.end(); + if (is_in_plugin_list && layer->isClearable()) { + RCLCPP_ERROR(logger_, "Clearable plugin '%s' failed to clear", + layer->getName().c_str()); + any_clearable_plugin_failed = true; + } + } + } + if (any_plugin_cleared) { + RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); + costmap_.getCostmap()->resetMap(0, 0, + costmap_.getCostmap()->getSizeInCellsX(), + costmap_.getCostmap()->getSizeInCellsY()); + } else { + RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearEntirely"); + } + // Return false if any clearable plugin failed to clear OR if no plugins were cleared at all + return !any_clearable_plugin_failed && any_plugin_cleared; + } +} + +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