From 7feff71a7800dc0bca141fe48221b89ae9c67c93 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Tue, 23 Sep 2025 21:24:11 +0300 Subject: [PATCH 01/26] Add plugins Parameter to BT XML for Selective Clearing of Costmap Layers Signed-off-by: BCKSELFDRIVEWORLD --- .../plugins/action/clear_costmap_service.hpp | 26 +++++- .../plugins/action/clear_costmap_service.cpp | 30 +++++++ .../nav2_costmap_2d/clear_costmap_service.hpp | 16 ++-- nav2_costmap_2d/src/clear_costmap_service.cpp | 80 ++++++++++++++++--- nav2_msgs/srv/ClearCostmapAroundPose.srv | 1 + nav2_msgs/srv/ClearCostmapAroundRobot.srv | 1 + nav2_msgs/srv/ClearCostmapExceptRegion.srv | 1 + nav2_msgs/srv/ClearEntireCostmap.srv | 1 + 8 files changed, 136 insertions(+), 20 deletions(-) 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 fe90502d82..fd8b5d7e29 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,19 @@ class ClearEntireCostmapService : public BtServiceNode>("plugins", + "List of costmap layer names to clear. If empty, all layers will be cleared") + }); + } }; /** @@ -86,7 +100,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 layer names to clear. If empty, all layers will be cleared") }); } }; @@ -125,7 +141,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 layer names to clear. If empty, all layers will be cleared") }); } }; @@ -166,7 +184,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 layer names to clear. If empty, all layers will be cleared") }); } }; diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index d637571c74..9f84676abc 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,12 @@ ClearEntireCostmapService::ClearEntireCostmapService( void ClearEntireCostmapService::on_tick() { + std::vector plugins; + if (getInput("plugins", plugins)) { + request_->plugins = plugins; + } else { + request_->plugins = std::vector(); + } increment_recovery_count(); } @@ -42,6 +49,13 @@ ClearCostmapExceptRegionService::ClearCostmapExceptRegionService( void ClearCostmapExceptRegionService::on_tick() { getInput("reset_distance", request_->reset_distance); + std::vector plugins; + if (getInput("plugins", plugins)) { + request_->plugins = plugins; + } else { + request_->plugins = std::vector(); + } + increment_recovery_count(); } @@ -55,6 +69,14 @@ ClearCostmapAroundRobotService::ClearCostmapAroundRobotService( void ClearCostmapAroundRobotService::on_tick() { getInput("reset_distance", request_->reset_distance); + + std::vector plugins; + if (getInput("plugins", plugins)) { + request_->plugins = plugins; + } else { + request_->plugins = std::vector(); + } + increment_recovery_count(); } @@ -69,6 +91,14 @@ void ClearCostmapAroundPoseService::on_tick() { getInput("pose", request_->pose); getInput("reset_distance", request_->reset_distance); + + std::vector plugins; + if (getInput("plugins", plugins)) { + request_->plugins = plugins; + } else { + request_->plugins = std::vector(); + } + 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 93d3b310dc..1fd9b895e9 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,19 @@ 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 +128,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 eec73de6f9..59ecc79c18 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,16 @@ 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 +171,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 +203,63 @@ 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()); + } + } + } + // TODO(BCKSELFDRIVEWORLD): Optimize to reset only affected regions instead of entire master costmap + // Reset master costmap to ensure consistency + RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); + costmap_.getCostmap()->resetMap(0, 0, + costmap_.getCostmap()->getSizeInCellsX(), + costmap_.getCostmap()->getSizeInCellsY()); + costmap_.updateMap(); + } +} + +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 5d244279f6..04c8c18b29 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 e3b41bf584..1e115afb3f 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 56bb2f4938..e3ee961f54 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 ed1b675f2d..7e5d20d511 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 From 16981f66ed2cae9e027d0a830693d6c16e1be7c9 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Tue, 23 Sep 2025 21:29:57 +0300 Subject: [PATCH 02/26] fix typo Signed-off-by: BCKSELFDRIVEWORLD --- nav2_costmap_2d/src/clear_costmap_service.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 59ecc79c18..3524fc692b 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -229,8 +229,8 @@ void ClearCostmapService::clearEntirely(const std::vector & plugins } } } - // TODO(BCKSELFDRIVEWORLD): Optimize to reset only affected regions instead of entire master costmap - // Reset master costmap to ensure consistency + // TODO(BCKSELFDRIVEWORLD): Optimize to reset only affected regions + // instead of entire master costmap RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); costmap_.getCostmap()->resetMap(0, 0, costmap_.getCostmap()->getSizeInCellsX(), From 2dc81eb99933689168bdd1dfef40d32e9ef803ac Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Wed, 24 Sep 2025 00:04:29 +0300 Subject: [PATCH 03/26] remove TODO Signed-off-by: BCKSELFDRIVEWORLD --- nav2_costmap_2d/src/clear_costmap_service.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 3524fc692b..50ac15e534 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -229,8 +229,6 @@ void ClearCostmapService::clearEntirely(const std::vector & plugins } } } - // TODO(BCKSELFDRIVEWORLD): Optimize to reset only affected regions - // instead of entire master costmap RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); costmap_.getCostmap()->resetMap(0, 0, costmap_.getCostmap()->getSizeInCellsX(), From 7ad01da0baf44fc466b590a34235323c6d5168f8 Mon Sep 17 00:00:00 2001 From: Burak Can Kaya <146545020+BCKSELFDRIVEWORLD@users.noreply.github.com> Date: Thu, 25 Sep 2025 15:06:41 +0300 Subject: [PATCH 04/26] Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp Co-authored-by: Sushant Chavan Signed-off-by: Burak Can Kaya <146545020+BCKSELFDRIVEWORLD@users.noreply.github.com> Signed-off-by: BCKSELFDRIVEWORLD --- .../nav2_behavior_tree/plugins/action/clear_costmap_service.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 fd8b5d7e29..5d8bec5ee2 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 @@ -60,7 +60,7 @@ class ClearEntireCostmapService : public BtServiceNode>("plugins", - "List of costmap layer names to clear. If empty, all layers will be cleared") + "List of costmap plugin names to clear. If empty, all plugins will be cleared") }); } }; From b7243995dbc6580cc799b5f2ae366d74ccd8a7a7 Mon Sep 17 00:00:00 2001 From: Burak Can Kaya <146545020+BCKSELFDRIVEWORLD@users.noreply.github.com> Date: Thu, 25 Sep 2025 15:07:03 +0300 Subject: [PATCH 05/26] Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp Co-authored-by: Sushant Chavan Signed-off-by: Burak Can Kaya <146545020+BCKSELFDRIVEWORLD@users.noreply.github.com> Signed-off-by: BCKSELFDRIVEWORLD --- .../nav2_behavior_tree/plugins/action/clear_costmap_service.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 5d8bec5ee2..d471f10551 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 @@ -102,7 +102,7 @@ class ClearCostmapExceptRegionService "reset_distance", 1, "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") + "List of costmap plugin names to clear. If empty, all plugins will be cleared") }); } }; From 92d8d74cda8093a02876b7136a9366d44f6add01 Mon Sep 17 00:00:00 2001 From: Burak Can Kaya <146545020+BCKSELFDRIVEWORLD@users.noreply.github.com> Date: Thu, 25 Sep 2025 15:07:10 +0300 Subject: [PATCH 06/26] Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp Co-authored-by: Sushant Chavan Signed-off-by: Burak Can Kaya <146545020+BCKSELFDRIVEWORLD@users.noreply.github.com> Signed-off-by: BCKSELFDRIVEWORLD --- .../nav2_behavior_tree/plugins/action/clear_costmap_service.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 d471f10551..034676b90a 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 @@ -143,7 +143,7 @@ class ClearCostmapAroundRobotService : public BtServiceNode>("plugins", - "List of costmap layer names to clear. If empty, all layers will be cleared") + "List of costmap plugin names to clear. If empty, all plugins will be cleared") }); } }; From f5d167b4c7954b6f5681de6b0739f12f676f15a5 Mon Sep 17 00:00:00 2001 From: Burak Can Kaya <146545020+BCKSELFDRIVEWORLD@users.noreply.github.com> Date: Thu, 25 Sep 2025 15:07:19 +0300 Subject: [PATCH 07/26] Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp Co-authored-by: Sushant Chavan Signed-off-by: Burak Can Kaya <146545020+BCKSELFDRIVEWORLD@users.noreply.github.com> Signed-off-by: BCKSELFDRIVEWORLD --- .../nav2_behavior_tree/plugins/action/clear_costmap_service.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 034676b90a..ca148f5200 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 @@ -186,7 +186,7 @@ class ClearCostmapAroundPoseService : public BtServiceNode>("plugins", - "List of costmap layer names to clear. If empty, all layers will be cleared") + "List of costmap plugin names to clear. If empty, all plugins will be cleared") }); } }; From ffa981423f293eece490ae743018fb930c0392e8 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Thu, 25 Sep 2025 15:17:53 +0300 Subject: [PATCH 08/26] Reset master costmap only if a layer was cleared Signed-off-by: BCKSELFDRIVEWORLD --- nav2_costmap_2d/src/clear_costmap_service.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 50ac15e534..03d5e946f6 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -213,14 +213,15 @@ void ClearCostmapService::clearEntirely(const std::vector & plugins } else { // Clear only specified plugins auto layers = costmap_.getLayeredCostmap()->getPlugins(); + bool layer_was_cleared = false; 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()); + layer_was_cleared = true; } else { RCLCPP_WARN( logger_, @@ -229,11 +230,13 @@ void ClearCostmapService::clearEntirely(const std::vector & plugins } } } - RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); - costmap_.getCostmap()->resetMap(0, 0, - costmap_.getCostmap()->getSizeInCellsX(), - costmap_.getCostmap()->getSizeInCellsY()); - costmap_.updateMap(); + if (layer_was_cleared) { + RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); + costmap_.getCostmap()->resetMap(0, 0, + costmap_.getCostmap()->getSizeInCellsX(), + costmap_.getCostmap()->getSizeInCellsY()); + costmap_.updateMap(); + } } } From 685397ee04dacd557cf5337a65a95ee505c7d16b Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Thu, 25 Sep 2025 15:40:07 +0300 Subject: [PATCH 09/26] fix typo input Signed-off-by: BCKSELFDRIVEWORLD --- .../plugins/action/clear_costmap_service.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index 9f84676abc..c4be80e257 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -31,10 +31,8 @@ ClearEntireCostmapService::ClearEntireCostmapService( void ClearEntireCostmapService::on_tick() { std::vector plugins; - if (getInput("plugins", plugins)) { - request_->plugins = plugins; - } else { - request_->plugins = std::vector(); + if (!getInput("plugins", request_->plugins)) { + request_->plugins.clear(); } increment_recovery_count(); } @@ -71,10 +69,8 @@ void ClearCostmapAroundRobotService::on_tick() getInput("reset_distance", request_->reset_distance); std::vector plugins; - if (getInput("plugins", plugins)) { - request_->plugins = plugins; - } else { - request_->plugins = std::vector(); + if (!getInput("plugins", request_->plugins)) { + request_->plugins.clear(); } increment_recovery_count(); @@ -93,10 +89,8 @@ void ClearCostmapAroundPoseService::on_tick() getInput("reset_distance", request_->reset_distance); std::vector plugins; - if (getInput("plugins", plugins)) { - request_->plugins = plugins; - } else { - request_->plugins = std::vector(); + if (!getInput("plugins", request_->plugins)) { + request_->plugins.clear(); } increment_recovery_count(); From 1b522be3e6350046cc668bd628bdcffb137c2acc Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Thu, 25 Sep 2025 15:46:03 +0300 Subject: [PATCH 10/26] update nav2_tree_node.xml Signed-off-by: BCKSELFDRIVEWORLD --- nav2_behavior_tree/nav2_tree_nodes.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index eabba3a4b0..a97fce6ca5 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. From 20ba7230e9bbfc3316294468a2f4544b440ebc7c Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Thu, 25 Sep 2025 18:31:25 +0300 Subject: [PATCH 11/26] fix Signed-off-by: BCKSELFDRIVEWORLD --- .../plugins/action/clear_costmap_service.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index c4be80e257..acd134cb04 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -30,7 +30,6 @@ ClearEntireCostmapService::ClearEntireCostmapService( void ClearEntireCostmapService::on_tick() { - std::vector plugins; if (!getInput("plugins", request_->plugins)) { request_->plugins.clear(); } @@ -47,11 +46,8 @@ ClearCostmapExceptRegionService::ClearCostmapExceptRegionService( void ClearCostmapExceptRegionService::on_tick() { getInput("reset_distance", request_->reset_distance); - std::vector plugins; - if (getInput("plugins", plugins)) { - request_->plugins = plugins; - } else { - request_->plugins = std::vector(); + if (!getInput("plugins", request_->plugins)) { + request_->plugins.clear(); } increment_recovery_count(); @@ -68,7 +64,6 @@ void ClearCostmapAroundRobotService::on_tick() { getInput("reset_distance", request_->reset_distance); - std::vector plugins; if (!getInput("plugins", request_->plugins)) { request_->plugins.clear(); } @@ -88,7 +83,6 @@ void ClearCostmapAroundPoseService::on_tick() getInput("pose", request_->pose); getInput("reset_distance", request_->reset_distance); - std::vector plugins; if (!getInput("plugins", request_->plugins)) { request_->plugins.clear(); } From e036748813125cef3574d11f3cbe96a12fd72972 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Fri, 26 Sep 2025 00:41:51 +0300 Subject: [PATCH 12/26] added doxygen and mutex and delete updatemap after clearing Signed-off-by: BCKSELFDRIVEWORLD --- .../include/nav2_costmap_2d/clear_costmap_service.hpp | 3 +++ nav2_costmap_2d/src/clear_costmap_service.cpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) 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 1fd9b895e9..56b5abc43b 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 @@ -136,6 +136,9 @@ class ClearCostmapService */ 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; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 03d5e946f6..35d373aba0 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -212,6 +212,7 @@ void ClearCostmapService::clearEntirely(const std::vector & plugins costmap_.resetLayers(); } else { // Clear only specified plugins + std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); auto layers = costmap_.getLayeredCostmap()->getPlugins(); bool layer_was_cleared = false; for (auto & layer : *layers) { @@ -235,7 +236,6 @@ void ClearCostmapService::clearEntirely(const std::vector & plugins costmap_.getCostmap()->resetMap(0, 0, costmap_.getCostmap()->getSizeInCellsX(), costmap_.getCostmap()->getSizeInCellsY()); - costmap_.updateMap(); } } } From 04e32b60559e83541ca7c1968dd9ecece1a5f8ec Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Fri, 26 Sep 2025 00:54:02 +0300 Subject: [PATCH 13/26] ClearCostmapService to return success status Signed-off-by: BCKSELFDRIVEWORLD --- .../nav2_costmap_2d/clear_costmap_service.hpp | 9 +++-- nav2_costmap_2d/src/clear_costmap_service.cpp | 40 +++++++++++-------- nav2_msgs/srv/ClearCostmapAroundPose.srv | 2 +- nav2_msgs/srv/ClearCostmapAroundRobot.srv | 2 +- nav2_msgs/srv/ClearCostmapExceptRegion.srv | 2 +- nav2_msgs/srv/ClearEntireCostmap.srv | 2 +- 6 files changed, 34 insertions(+), 23 deletions(-) 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 56b5abc43b..0e5a7819ce 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,20 +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, const std::vector & plugins); + 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( + 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(const std::vector & plugins); + bool clearEntirely(const std::vector & plugins); private: // The Logger object for logging diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 35d373aba0..33798c8600 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -78,48 +78,48 @@ 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, request->plugins); + 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, request->plugins); + 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, request->plugins); + 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 response) { RCLCPP_INFO( logger_, "%s", ("Received request to clear entirely the " + costmap_.getName()).c_str()); - clearEntirely(request->plugins); + response->success = clearEntirely(request->plugins); } -void ClearCostmapService::clearAroundPose( +bool ClearCostmapService::clearAroundPose( const geometry_msgs::msg::PoseStamped & pose, const double reset_distance, const std::vector & plugins) @@ -139,23 +139,26 @@ 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 plugin_was_cleared = false; for (auto & layer : *layers) { if (shouldClearLayer(layer, plugins)) { auto costmap_layer = std::static_pointer_cast(layer); clearLayerRegion(costmap_layer, x, y, reset_distance, false); + plugin_was_cleared = true; } } + return plugin_was_cleared; } -void ClearCostmapService::clearRegion( +bool ClearCostmapService::clearRegion( const double reset_distance, bool invert, const std::vector & plugins) { @@ -165,20 +168,23 @@ void ClearCostmapService::clearRegion( RCLCPP_ERROR( logger_, "%s", "Cannot clear map because robot pose cannot be retrieved."); - return; + return false; } auto layers = costmap_.getLayeredCostmap()->getPlugins(); + bool plugin_was_cleared = false; for (auto & layer : *layers) { if (shouldClearLayer(layer, plugins)) { auto costmap_layer = std::static_pointer_cast(layer); clearLayerRegion(costmap_layer, x, y, reset_distance, invert); + plugin_was_cleared = true; } } // AlexeyMerzlyakov: No need to clear layer region for costmap filters // as they are always supposed to be not clearable. + return plugin_was_cleared; } void ClearCostmapService::clearLayerRegion( @@ -203,18 +209,19 @@ void ClearCostmapService::clearLayerRegion( costmap->addExtraBounds(ox, oy, ox + width, oy + height); } -void ClearCostmapService::clearEntirely(const std::vector & plugins) +bool ClearCostmapService::clearEntirely(const std::vector & plugins) { 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; // resetLayers() always succeeds } else { // Clear only specified plugins std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); auto layers = costmap_.getLayeredCostmap()->getPlugins(); - bool layer_was_cleared = false; + bool plugin_was_cleared = false; for (auto & layer : *layers) { if (shouldClearLayer(layer, plugins)) { if (layer->isClearable()) { @@ -222,7 +229,7 @@ void ClearCostmapService::clearEntirely(const std::vector & plugins auto costmap_layer = std::static_pointer_cast(layer); costmap_layer->resetMap(0, 0, costmap_layer->getSizeInCellsX(), costmap_layer->getSizeInCellsY()); - layer_was_cleared = true; + plugin_was_cleared = true; } else { RCLCPP_WARN( logger_, @@ -231,12 +238,13 @@ void ClearCostmapService::clearEntirely(const std::vector & plugins } } } - if (layer_was_cleared) { + if (plugin_was_cleared) { RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); costmap_.getCostmap()->resetMap(0, 0, costmap_.getCostmap()->getSizeInCellsX(), costmap_.getCostmap()->getSizeInCellsY()); } + return plugin_was_cleared; } } diff --git a/nav2_msgs/srv/ClearCostmapAroundPose.srv b/nav2_msgs/srv/ClearCostmapAroundPose.srv index 04c8c18b29..f2fdfab12a 100644 --- a/nav2_msgs/srv/ClearCostmapAroundPose.srv +++ b/nav2_msgs/srv/ClearCostmapAroundPose.srv @@ -4,4 +4,4 @@ geometry_msgs/PoseStamped pose float64 reset_distance string[] plugins --- -std_msgs/Empty response +bool success \ No newline at end of file diff --git a/nav2_msgs/srv/ClearCostmapAroundRobot.srv b/nav2_msgs/srv/ClearCostmapAroundRobot.srv index 1e115afb3f..4431c9b733 100644 --- a/nav2_msgs/srv/ClearCostmapAroundRobot.srv +++ b/nav2_msgs/srv/ClearCostmapAroundRobot.srv @@ -3,4 +3,4 @@ float32 reset_distance string[] plugins --- -std_msgs/Empty response +bool success \ No newline at end of file diff --git a/nav2_msgs/srv/ClearCostmapExceptRegion.srv b/nav2_msgs/srv/ClearCostmapExceptRegion.srv index e3ee961f54..4b0fdaddf5 100644 --- a/nav2_msgs/srv/ClearCostmapExceptRegion.srv +++ b/nav2_msgs/srv/ClearCostmapExceptRegion.srv @@ -3,4 +3,4 @@ float32 reset_distance string[] plugins --- -std_msgs/Empty response +bool success \ No newline at end of file diff --git a/nav2_msgs/srv/ClearEntireCostmap.srv b/nav2_msgs/srv/ClearEntireCostmap.srv index 7e5d20d511..d3df671720 100644 --- a/nav2_msgs/srv/ClearEntireCostmap.srv +++ b/nav2_msgs/srv/ClearEntireCostmap.srv @@ -3,4 +3,4 @@ std_msgs/Empty request string[] plugins --- -std_msgs/Empty response +bool success \ No newline at end of file From 20f222dbae369abfb0ee2ca23f5812c97d08ac01 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Fri, 26 Sep 2025 01:00:19 +0300 Subject: [PATCH 14/26] fix typo Signed-off-by: BCKSELFDRIVEWORLD --- nav2_costmap_2d/src/clear_costmap_service.cpp | 2 +- nav2_msgs/srv/ClearCostmapAroundPose.srv | 2 +- nav2_msgs/srv/ClearCostmapAroundRobot.srv | 2 +- nav2_msgs/srv/ClearCostmapExceptRegion.srv | 2 +- nav2_msgs/srv/ClearEntireCostmap.srv | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 33798c8600..8c0ed1546c 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -216,7 +216,7 @@ bool ClearCostmapService::clearEntirely(const std::vector & plugins std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); RCLCPP_INFO(logger_, "Clearing all layers in costmap: %s", costmap_.getName().c_str()); costmap_.resetLayers(); - return true; // resetLayers() always succeeds + return true; } else { // Clear only specified plugins std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); diff --git a/nav2_msgs/srv/ClearCostmapAroundPose.srv b/nav2_msgs/srv/ClearCostmapAroundPose.srv index f2fdfab12a..e255b825a1 100644 --- a/nav2_msgs/srv/ClearCostmapAroundPose.srv +++ b/nav2_msgs/srv/ClearCostmapAroundPose.srv @@ -4,4 +4,4 @@ geometry_msgs/PoseStamped pose float64 reset_distance string[] plugins --- -bool success \ No newline at end of file +bool success diff --git a/nav2_msgs/srv/ClearCostmapAroundRobot.srv b/nav2_msgs/srv/ClearCostmapAroundRobot.srv index 4431c9b733..ff8e887024 100644 --- a/nav2_msgs/srv/ClearCostmapAroundRobot.srv +++ b/nav2_msgs/srv/ClearCostmapAroundRobot.srv @@ -3,4 +3,4 @@ float32 reset_distance string[] plugins --- -bool success \ No newline at end of file +bool success diff --git a/nav2_msgs/srv/ClearCostmapExceptRegion.srv b/nav2_msgs/srv/ClearCostmapExceptRegion.srv index 4b0fdaddf5..0c0d75c686 100644 --- a/nav2_msgs/srv/ClearCostmapExceptRegion.srv +++ b/nav2_msgs/srv/ClearCostmapExceptRegion.srv @@ -3,4 +3,4 @@ float32 reset_distance string[] plugins --- -bool success \ No newline at end of file +bool success diff --git a/nav2_msgs/srv/ClearEntireCostmap.srv b/nav2_msgs/srv/ClearEntireCostmap.srv index d3df671720..e7e95281ba 100644 --- a/nav2_msgs/srv/ClearEntireCostmap.srv +++ b/nav2_msgs/srv/ClearEntireCostmap.srv @@ -3,4 +3,4 @@ std_msgs/Empty request string[] plugins --- -bool success \ No newline at end of file +bool success From a9876555a55cfe2b2982bb13f1c24b6fcc057e56 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Fri, 26 Sep 2025 01:17:33 +0300 Subject: [PATCH 15/26] Add on_completion methods to ClearCostmap services for handling service responses Signed-off-by: BCKSELFDRIVEWORLD --- .../plugins/action/clear_costmap_service.hpp | 28 +++++++++++++++ .../plugins/action/clear_costmap_service.cpp | 36 +++++++++++++++++++ 2 files changed, 64 insertions(+) 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 ca148f5200..1fa3aaf2d6 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 @@ -51,6 +51,13 @@ class ClearEntireCostmapService : public BtServiceNode response) override; + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -90,6 +97,13 @@ 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 @@ -131,6 +145,13 @@ class ClearCostmapAroundRobotService : public BtServiceNode response) override; + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -172,6 +193,13 @@ class ClearCostmapAroundPoseService : public BtServiceNode response) override; + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index acd134cb04..060feb0bc5 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -36,6 +36,15 @@ void ClearEntireCostmapService::on_tick() increment_recovery_count(); } +BT::NodeStatus ClearEntireCostmapService::on_completion( + std::shared_ptr response) +{ + if (response->success) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + ClearCostmapExceptRegionService::ClearCostmapExceptRegionService( const std::string & service_node_name, const BT::NodeConfiguration & conf) @@ -53,6 +62,15 @@ void ClearCostmapExceptRegionService::on_tick() increment_recovery_count(); } +BT::NodeStatus ClearCostmapExceptRegionService::on_completion( + std::shared_ptr response) +{ + if (response->success) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + ClearCostmapAroundRobotService::ClearCostmapAroundRobotService( const std::string & service_node_name, const BT::NodeConfiguration & conf) @@ -71,6 +89,15 @@ void ClearCostmapAroundRobotService::on_tick() increment_recovery_count(); } +BT::NodeStatus ClearCostmapAroundRobotService::on_completion( + std::shared_ptr response) +{ + if (response->success) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + ClearCostmapAroundPoseService::ClearCostmapAroundPoseService( const std::string & service_node_name, const BT::NodeConfiguration & conf) @@ -90,6 +117,15 @@ void ClearCostmapAroundPoseService::on_tick() increment_recovery_count(); } +BT::NodeStatus ClearCostmapAroundPoseService::on_completion( + std::shared_ptr response) +{ + if (response->success) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" From 37df7fa920a99ca40dd5177864ef8d6bc69c767c Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Fri, 26 Sep 2025 01:23:35 +0300 Subject: [PATCH 16/26] fix typo Signed-off-by: BCKSELFDRIVEWORLD --- .../plugins/action/clear_costmap_service.hpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) 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 1fa3aaf2d6..3470ba1d0f 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 @@ -17,6 +17,7 @@ #include #include +#include #include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/clear_entire_costmap.hpp" @@ -56,7 +57,8 @@ class ClearEntireCostmapService : public BtServiceNode response) override; + BT::NodeStatus on_completion( + std::shared_ptr response) override; /** * @brief Creates list of BT ports @@ -102,7 +104,8 @@ class ClearCostmapExceptRegionService * @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; + BT::NodeStatus on_completion( + std::shared_ptr response) override; /** * @brief Creates list of BT ports @@ -150,7 +153,8 @@ class ClearCostmapAroundRobotService : public BtServiceNode response) override; + BT::NodeStatus on_completion( + std::shared_ptr response) override; /** * @brief Creates list of BT ports @@ -198,7 +202,8 @@ class ClearCostmapAroundPoseService : public BtServiceNode response) override; + BT::NodeStatus on_completion( + std::shared_ptr response) override; /** * @brief Creates list of BT ports From 73049fa6a6c326a6c47267a55119269462fa5715 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Mon, 29 Sep 2025 17:25:03 +0300 Subject: [PATCH 17/26] ClearCostmapService to return success status based on all plugins being cleared Signed-off-by: BCKSELFDRIVEWORLD --- nav2_costmap_2d/src/clear_costmap_service.cpp | 53 +++++++++++-------- 1 file changed, 30 insertions(+), 23 deletions(-) diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 8c0ed1546c..f947b3c089 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -146,16 +146,19 @@ bool ClearCostmapService::clearAroundPose( y = global_pose.pose.position.y; auto layers = costmap_.getLayeredCostmap()->getPlugins(); - bool plugin_was_cleared = false; + bool all_plugins_cleared = true; for (auto & layer : *layers) { - if (shouldClearLayer(layer, plugins)) { - auto costmap_layer = std::static_pointer_cast(layer); - clearLayerRegion(costmap_layer, x, y, reset_distance, false); - plugin_was_cleared = true; + if (layer->isClearable()) { + if (shouldClearLayer(layer, plugins)) { + auto costmap_layer = std::static_pointer_cast(layer); + clearLayerRegion(costmap_layer, x, y, reset_distance, false); + } else { + all_plugins_cleared = false; + } } } - return plugin_was_cleared; + return all_plugins_cleared; } bool ClearCostmapService::clearRegion( @@ -172,19 +175,22 @@ bool ClearCostmapService::clearRegion( } auto layers = costmap_.getLayeredCostmap()->getPlugins(); - bool plugin_was_cleared = false; + bool all_plugins_cleared = true; for (auto & layer : *layers) { - if (shouldClearLayer(layer, plugins)) { - auto costmap_layer = std::static_pointer_cast(layer); - clearLayerRegion(costmap_layer, x, y, reset_distance, invert); - plugin_was_cleared = true; + if (layer->isClearable()) { + if (shouldClearLayer(layer, plugins)) { + auto costmap_layer = std::static_pointer_cast(layer); + clearLayerRegion(costmap_layer, x, y, reset_distance, invert); + } else { + all_plugins_cleared = false; + } } } // AlexeyMerzlyakov: No need to clear layer region for costmap filters // as they are always supposed to be not clearable. - return plugin_was_cleared; + return all_plugins_cleared; } void ClearCostmapService::clearLayerRegion( @@ -221,30 +227,31 @@ bool ClearCostmapService::clearEntirely(const std::vector & plugins // Clear only specified plugins std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); auto layers = costmap_.getLayeredCostmap()->getPlugins(); - bool plugin_was_cleared = false; + bool all_plugins_cleared = true; for (auto & layer : *layers) { - if (shouldClearLayer(layer, plugins)) { - if (layer->isClearable()) { + if (layer->isClearable()) { + 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()); - plugin_was_cleared = true; } else { - RCLCPP_WARN( - logger_, - "Layer '%s' is not clearable, skipping.", - layer->getName().c_str()); + all_plugins_cleared = false; } + } else { + RCLCPP_WARN( + logger_, + "Layer '%s' is not clearable, skipping.", + layer->getName().c_str()); } } - if (plugin_was_cleared) { + if (all_plugins_cleared) { RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); costmap_.getCostmap()->resetMap(0, 0, costmap_.getCostmap()->getSizeInCellsX(), costmap_.getCostmap()->getSizeInCellsY()); } - return plugin_was_cleared; + return all_plugins_cleared; } } @@ -268,7 +275,7 @@ bool ClearCostmapService::shouldClearLayer( return false; } - return is_in_plugin_list && layer->isClearable(); + return layer->isClearable(); } bool ClearCostmapService::getPosition(double & x, double & y) const From 3cf4e641ecb26737f37d80ed05bfd7ad03e62641 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Mon, 29 Sep 2025 17:35:15 +0300 Subject: [PATCH 18/26] added log(error) Signed-off-by: BCKSELFDRIVEWORLD --- nav2_costmap_2d/src/clear_costmap_service.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index f947b3c089..99062ba83c 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -158,6 +158,11 @@ bool ClearCostmapService::clearAroundPose( } } } + + if (!all_plugins_cleared) { + RCLCPP_ERROR(logger_, "Not all requested plugins were cleared in clearAroundPose"); + } + return all_plugins_cleared; } @@ -190,6 +195,11 @@ bool ClearCostmapService::clearRegion( // AlexeyMerzlyakov: No need to clear layer region for costmap filters // as they are always supposed to be not clearable. + + if (!all_plugins_cleared) { + RCLCPP_ERROR(logger_, "Not all requested plugins were cleared in clearRegion"); + } + return all_plugins_cleared; } @@ -250,6 +260,8 @@ bool ClearCostmapService::clearEntirely(const std::vector & plugins costmap_.getCostmap()->resetMap(0, 0, costmap_.getCostmap()->getSizeInCellsX(), costmap_.getCostmap()->getSizeInCellsY()); + } else { + RCLCPP_ERROR(logger_, "Not all requested plugins were cleared in clearEntirely"); } return all_plugins_cleared; } From bdb8262cba4bc5b20357ec47ee0d6c0a7f54299c Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Mon, 29 Sep 2025 17:40:31 +0300 Subject: [PATCH 19/26] fix typo Signed-off-by: BCKSELFDRIVEWORLD --- nav2_costmap_2d/src/clear_costmap_service.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 99062ba83c..2afbcef263 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -158,11 +158,11 @@ bool ClearCostmapService::clearAroundPose( } } } - + if (!all_plugins_cleared) { RCLCPP_ERROR(logger_, "Not all requested plugins were cleared in clearAroundPose"); } - + return all_plugins_cleared; } @@ -195,11 +195,11 @@ bool ClearCostmapService::clearRegion( // AlexeyMerzlyakov: No need to clear layer region for costmap filters // as they are always supposed to be not clearable. - + if (!all_plugins_cleared) { RCLCPP_ERROR(logger_, "Not all requested plugins were cleared in clearRegion"); } - + return all_plugins_cleared; } From ac22786c8b57178a3a047426ad3e030cbbd67f44 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Mon, 29 Sep 2025 22:02:32 +0300 Subject: [PATCH 20/26] Updated the logic Signed-off-by: BCKSELFDRIVEWORLD --- .../plugins/action/clear_costmap_service.cpp | 4 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 68 ++++++++----------- 2 files changed, 30 insertions(+), 42 deletions(-) diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index 060feb0bc5..34d96eff6e 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -41,8 +41,10 @@ BT::NodeStatus ClearEntireCostmapService::on_completion( { if (response->success) { return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(node_->get_logger(), "ClearEntireCostmap: Failed to clear costmap layers"); + return BT::NodeStatus::FAILURE; } - return BT::NodeStatus::FAILURE; } ClearCostmapExceptRegionService::ClearCostmapExceptRegionService( diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 2afbcef263..9401afbaeb 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -146,24 +146,21 @@ bool ClearCostmapService::clearAroundPose( y = global_pose.pose.position.y; auto layers = costmap_.getLayeredCostmap()->getPlugins(); - bool all_plugins_cleared = true; + bool any_plugin_cleared = 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); - } else { - all_plugins_cleared = false; - } + if (shouldClearLayer(layer, plugins)) { + auto costmap_layer = std::static_pointer_cast(layer); + clearLayerRegion(costmap_layer, x, y, reset_distance, false); + any_plugin_cleared = true; } } - if (!all_plugins_cleared) { - RCLCPP_ERROR(logger_, "Not all requested plugins were cleared in clearAroundPose"); + if (!any_plugin_cleared) { + RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearAroundPose"); } - return all_plugins_cleared; + return any_plugin_cleared; } bool ClearCostmapService::clearRegion( @@ -180,27 +177,24 @@ bool ClearCostmapService::clearRegion( } auto layers = costmap_.getLayeredCostmap()->getPlugins(); - bool all_plugins_cleared = true; + bool any_plugin_cleared = 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); - } else { - all_plugins_cleared = false; - } + if (shouldClearLayer(layer, plugins)) { + 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. - if (!all_plugins_cleared) { - RCLCPP_ERROR(logger_, "Not all requested plugins were cleared in clearRegion"); + if (!any_plugin_cleared) { + RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearRegion"); } - return all_plugins_cleared; + return any_plugin_cleared; } void ClearCostmapService::clearLayerRegion( @@ -237,33 +231,25 @@ bool ClearCostmapService::clearEntirely(const std::vector & plugins // Clear only specified plugins std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); auto layers = costmap_.getLayeredCostmap()->getPlugins(); - bool all_plugins_cleared = true; + bool any_plugin_cleared = false; for (auto & layer : *layers) { - if (layer->isClearable()) { - 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()); - } else { - all_plugins_cleared = false; - } - } else { - RCLCPP_WARN( - logger_, - "Layer '%s' is not clearable, skipping.", - layer->getName().c_str()); + 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; } } - if (all_plugins_cleared) { + 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_, "Not all requested plugins were cleared in clearEntirely"); + RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearEntirely"); } - return all_plugins_cleared; + return any_plugin_cleared; } } @@ -287,7 +273,7 @@ bool ClearCostmapService::shouldClearLayer( return false; } - return layer->isClearable(); + return is_in_plugin_list && layer->isClearable(); } bool ClearCostmapService::getPosition(double & x, double & y) const From 05f7704c25e9acb80a5962a692b14ce8bc1862b5 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Mon, 29 Sep 2025 22:07:32 +0300 Subject: [PATCH 21/26] added logs Signed-off-by: BCKSELFDRIVEWORLD --- .../plugins/action/clear_costmap_service.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index 34d96eff6e..2b52c9a3f1 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -69,8 +69,10 @@ BT::NodeStatus ClearCostmapExceptRegionService::on_completion( { if (response->success) { return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(node_->get_logger(), "ClearCostmapExceptRegion: Failed to clear costmap layers"); + return BT::NodeStatus::FAILURE; } - return BT::NodeStatus::FAILURE; } ClearCostmapAroundRobotService::ClearCostmapAroundRobotService( @@ -96,8 +98,10 @@ BT::NodeStatus ClearCostmapAroundRobotService::on_completion( { if (response->success) { return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(node_->get_logger(), "ClearCostmapAroundRobot: Failed to clear costmap layers"); + return BT::NodeStatus::FAILURE; } - return BT::NodeStatus::FAILURE; } ClearCostmapAroundPoseService::ClearCostmapAroundPoseService( @@ -124,8 +128,10 @@ BT::NodeStatus ClearCostmapAroundPoseService::on_completion( { if (response->success) { return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(node_->get_logger(), "ClearCostmapAroundPose: Failed to clear costmap layers"); + return BT::NodeStatus::FAILURE; } - return BT::NodeStatus::FAILURE; } } // namespace nav2_behavior_tree From a1b6ecad2b89fcc3606843f29c2a6bc1acc70e85 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Mon, 29 Sep 2025 22:32:40 +0300 Subject: [PATCH 22/26] improve logic Signed-off-by: BCKSELFDRIVEWORLD --- nav2_costmap_2d/src/clear_costmap_service.cpp | 41 +++++++++++++++++-- 1 file changed, 37 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 9401afbaeb..a951ae31c5 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -147,12 +147,22 @@ bool ClearCostmapService::clearAroundPose( auto layers = costmap_.getLayeredCostmap()->getPlugins(); bool any_plugin_cleared = false; + bool any_clearable_plugin_failed = false; for (auto & layer : *layers) { 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; + } } } @@ -160,7 +170,8 @@ bool ClearCostmapService::clearAroundPose( RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearAroundPose"); } - return any_plugin_cleared; + // 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::clearRegion( @@ -178,12 +189,22 @@ bool ClearCostmapService::clearRegion( auto layers = costmap_.getLayeredCostmap()->getPlugins(); bool any_plugin_cleared = false; + bool any_clearable_plugin_failed = false; for (auto & layer : *layers) { 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; + } } } @@ -193,8 +214,9 @@ bool ClearCostmapService::clearRegion( if (!any_plugin_cleared) { RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearRegion"); } - - return any_plugin_cleared; + + // 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( @@ -232,6 +254,7 @@ bool ClearCostmapService::clearEntirely(const std::vector & 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()); @@ -239,6 +262,15 @@ bool ClearCostmapService::clearEntirely(const std::vector & plugins 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) { @@ -249,7 +281,8 @@ bool ClearCostmapService::clearEntirely(const std::vector & plugins } else { RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearEntirely"); } - return any_plugin_cleared; + // 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; } } From 7cc125a3bb6a9370d2c44f26dc670d71fe831fe3 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Mon, 29 Sep 2025 22:37:05 +0300 Subject: [PATCH 23/26] fix typo Signed-off-by: BCKSELFDRIVEWORLD --- nav2_costmap_2d/src/clear_costmap_service.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index a951ae31c5..a157c3debc 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -214,7 +214,7 @@ bool ClearCostmapService::clearRegion( 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; } From b8b88af53b95353719747929f055a532df901262 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Wed, 29 Oct 2025 15:51:02 +0300 Subject: [PATCH 24/26] added dummy_Server Signed-off-by: BCKSELFDRIVEWORLD --- .../test/utils/test_service.hpp | 13 +++++++++++- .../src/behavior_tree/server_handler.cpp | 4 ++-- .../src/behavior_tree/server_handler.hpp | 20 +++++++++++++++++-- 3 files changed, 32 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/test/utils/test_service.hpp b/nav2_behavior_tree/test/utils/test_service.hpp index e9abad8c0c..fb4301486b 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_system_tests/src/behavior_tree/server_handler.cpp b/nav2_system_tests/src/behavior_tree/server_handler.cpp index 9eed165cab..276ab1c017 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 2b852cd085..93843e9178 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; From 20bc1ca83ae463ddcca7804c6cadff543c1363a2 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Wed, 29 Oct 2025 17:22:09 +0300 Subject: [PATCH 25/26] ClearCostmapService to introduce plugin validation and categorization methods Signed-off-by: BCKSELFDRIVEWORLD --- .../nav2_costmap_2d/clear_costmap_service.hpp | 28 +++ nav2_costmap_2d/src/clear_costmap_service.cpp | 200 +++++++++++------- 2 files changed, 156 insertions(+), 72 deletions(-) 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 0e5a7819ce..811ef31d2d 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" @@ -139,6 +140,33 @@ class ClearCostmapService */ 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 */ diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index a157c3debc..4b78f835ed 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -146,32 +146,27 @@ bool ClearCostmapService::clearAroundPose( 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 (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 (!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; } - - 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; } bool ClearCostmapService::clearRegion( @@ -188,35 +183,27 @@ bool ClearCostmapService::clearRegion( } auto layers = costmap_.getLayeredCostmap()->getPlugins(); - bool any_plugin_cleared = false; - bool any_clearable_plugin_failed = false; - - for (auto & layer : *layers) { - 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; + + 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; } } + + return any_plugin_cleared; } - - // 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( @@ -253,39 +240,108 @@ bool ClearCostmapService::clearEntirely(const std::vector & plugins // 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)) { + + bool result = validateAndClearPlugins( + plugins, layers, + [this](std::shared_ptr & layer) { 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) { + 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 { - RCLCPP_ERROR(logger_, "No requested plugins were cleared in clearEntirely"); + valid_plugins.push_back(requested_plugin); } - // 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::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 From 28696b4ec19a4aa8898bd9a7eb5a2e203ca2f5a5 Mon Sep 17 00:00:00 2001 From: BCKSELFDRIVEWORLD Date: Wed, 29 Oct 2025 17:35:07 +0300 Subject: [PATCH 26/26] fix typo Signed-off-by: BCKSELFDRIVEWORLD --- .../test/utils/test_service.hpp | 2 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 40 ++++++++++--------- 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/nav2_behavior_tree/test/utils/test_service.hpp b/nav2_behavior_tree/test/utils/test_service.hpp index fb4301486b..e018cc6cfa 100644 --- a/nav2_behavior_tree/test/utils/test_service.hpp +++ b/nav2_behavior_tree/test/utils/test_service.hpp @@ -58,7 +58,7 @@ class TestService : public rclcpp::Node { response->success = true; } - + template void setSuccessIfExists(T) { diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 4b78f835ed..03548ff003 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -146,7 +146,7 @@ bool ClearCostmapService::clearAroundPose( y = global_pose.pose.position.y; auto layers = costmap_.getLayeredCostmap()->getPlugins(); - + if (!plugins.empty()) { return validateAndClearPlugins( plugins, layers, @@ -164,7 +164,7 @@ bool ClearCostmapService::clearAroundPose( any_plugin_cleared = true; } } - + return any_plugin_cleared; } } @@ -183,7 +183,7 @@ bool ClearCostmapService::clearRegion( } auto layers = costmap_.getLayeredCostmap()->getPlugins(); - + if (!plugins.empty()) { return validateAndClearPlugins( plugins, layers, @@ -201,7 +201,7 @@ bool ClearCostmapService::clearRegion( any_plugin_cleared = true; } } - + return any_plugin_cleared; } } @@ -240,7 +240,7 @@ bool ClearCostmapService::clearEntirely(const std::vector & plugins // 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) { @@ -248,14 +248,14 @@ bool ClearCostmapService::clearEntirely(const std::vector & plugins 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; } } @@ -268,11 +268,11 @@ void ClearCostmapService::validateAndCategorizePlugins( { 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; @@ -280,7 +280,7 @@ void ClearCostmapService::validateAndCategorizePlugins( break; } } - + if (!found) { invalid_plugins.push_back(requested_plugin + " (not found)"); } else if (!clearable) { @@ -299,10 +299,10 @@ bool ClearCostmapService::validateAndClearPlugins( { 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: "; @@ -314,31 +314,33 @@ bool ClearCostmapService::validateAndClearPlugins( } 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()) { + 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_, + 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; }