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 ec653b7af5f..4d4752d5e39 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" @@ -48,6 +50,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") + }); + } }; /** @@ -74,6 +97,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 @@ -84,7 +115,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") }); } }; @@ -112,6 +145,14 @@ class ClearCostmapAroundRobotService : public BtServiceNode response) override; + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -122,7 +163,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") }); } }; @@ -150,6 +193,14 @@ class ClearCostmapAroundPoseService : public BtServiceNode response) override; + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -162,7 +213,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 fdb1520d9ef..01a75661f95 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -72,18 +72,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. @@ -91,6 +94,7 @@ Server timeout Pose around which to clear the costmap Distance from the pose under which obstacles are cleared + List of specific plugins to clear. diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index d637571c740..2b52c9a3f1b 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp" @@ -29,9 +30,23 @@ ClearEntireCostmapService::ClearEntireCostmapService( void ClearEntireCostmapService::on_tick() { + if (!getInput("plugins", request_->plugins)) { + request_->plugins.clear(); + } increment_recovery_count(); } +BT::NodeStatus ClearEntireCostmapService::on_completion( + std::shared_ptr response) +{ + if (response->success) { + return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(node_->get_logger(), "ClearEntireCostmap: Failed to clear costmap layers"); + return BT::NodeStatus::FAILURE; + } +} + ClearCostmapExceptRegionService::ClearCostmapExceptRegionService( const std::string & service_node_name, const BT::NodeConfiguration & conf) @@ -42,9 +57,24 @@ ClearCostmapExceptRegionService::ClearCostmapExceptRegionService( void ClearCostmapExceptRegionService::on_tick() { getInput("reset_distance", request_->reset_distance); + if (!getInput("plugins", request_->plugins)) { + request_->plugins.clear(); + } + increment_recovery_count(); } +BT::NodeStatus ClearCostmapExceptRegionService::on_completion( + std::shared_ptr response) +{ + if (response->success) { + return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(node_->get_logger(), "ClearCostmapExceptRegion: Failed to clear costmap layers"); + return BT::NodeStatus::FAILURE; + } +} + ClearCostmapAroundRobotService::ClearCostmapAroundRobotService( const std::string & service_node_name, const BT::NodeConfiguration & conf) @@ -55,9 +85,25 @@ ClearCostmapAroundRobotService::ClearCostmapAroundRobotService( void ClearCostmapAroundRobotService::on_tick() { getInput("reset_distance", request_->reset_distance); + + if (!getInput("plugins", request_->plugins)) { + request_->plugins.clear(); + } + increment_recovery_count(); } +BT::NodeStatus ClearCostmapAroundRobotService::on_completion( + std::shared_ptr response) +{ + if (response->success) { + return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(node_->get_logger(), "ClearCostmapAroundRobot: Failed to clear costmap layers"); + return BT::NodeStatus::FAILURE; + } +} + ClearCostmapAroundPoseService::ClearCostmapAroundPoseService( const std::string & service_node_name, const BT::NodeConfiguration & conf) @@ -69,9 +115,25 @@ void ClearCostmapAroundPoseService::on_tick() { getInput("pose", request_->pose); getInput("reset_distance", request_->reset_distance); + + if (!getInput("plugins", request_->plugins)) { + request_->plugins.clear(); + } + increment_recovery_count(); } +BT::NodeStatus ClearCostmapAroundPoseService::on_completion( + std::shared_ptr response) +{ + if (response->success) { + return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(node_->get_logger(), "ClearCostmapAroundPose: Failed to clear costmap layers"); + return BT::NodeStatus::FAILURE; + } +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index f639e9975fa..274dd7c2032 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -112,6 +112,47 @@ TEST_F(ClearEntireCostmapServiceTestFixture, test_tick) EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); } +TEST_F(ClearEntireCostmapServiceTestFixture, test_tick_with_plugins) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + + auto request = server_->getCurrentRequest(); + ASSERT_NE(request, nullptr); + EXPECT_EQ(request->plugins.size(), 2u); + EXPECT_EQ(request->plugins[0], "obstacle_layer"); + EXPECT_EQ(request->plugins[1], "inflation_layer"); +} + +TEST_F(ClearEntireCostmapServiceTestFixture, test_tick_with_single_plugin) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + + auto request = server_->getCurrentRequest(); + ASSERT_NE(request, nullptr); + EXPECT_EQ(request->plugins.size(), 1u); + EXPECT_EQ(request->plugins[0], "obstacle_layer"); +} + class ClearCostmapExceptRegionService : public TestService { public: @@ -206,6 +247,27 @@ TEST_F(ClearCostmapExceptRegionServiceTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); } + +TEST_F(ClearCostmapExceptRegionServiceTestFixture, test_tick_with_plugins) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + + auto request = server_->getCurrentRequest(); + ASSERT_NE(request, nullptr); + EXPECT_EQ(request->plugins.size(), 1u); + EXPECT_EQ(request->plugins[0], "obstacle_layer"); + EXPECT_DOUBLE_EQ(request->reset_distance, 2.0); +} //****************************************** class ClearCostmapAroundRobotService : public TestService { @@ -302,6 +364,28 @@ TEST_F(ClearCostmapAroundRobotServiceTestFixture, test_tick) EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); } +TEST_F(ClearCostmapAroundRobotServiceTestFixture, test_tick_with_plugins) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + + auto request = server_->getCurrentRequest(); + ASSERT_NE(request, nullptr); + EXPECT_EQ(request->plugins.size(), 2u); + EXPECT_EQ(request->plugins[0], "obstacle_layer"); + EXPECT_EQ(request->plugins[1], "voxel_layer"); + EXPECT_DOUBLE_EQ(request->reset_distance, 1.5); +} + class ClearCostmapAroundPoseService : public TestService { public: @@ -397,6 +481,27 @@ TEST_F(ClearCostmapAroundPoseServiceTestFixture, test_tick) EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); } +TEST_F(ClearCostmapAroundPoseServiceTestFixture, test_tick_with_plugins) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + + auto request = server_->getCurrentRequest(); + ASSERT_NE(request, nullptr); + EXPECT_EQ(request->plugins.size(), 1u); + EXPECT_EQ(request->plugins[0], "obstacle_layer"); + EXPECT_DOUBLE_EQ(request->reset_distance, 2.5); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/utils/test_service.hpp b/nav2_behavior_tree/test/utils/test_service.hpp index e9abad8c0c9..e018cc6cfa8 100644 --- a/nav2_behavior_tree/test/utils/test_service.hpp +++ b/nav2_behavior_tree/test/utils/test_service.hpp @@ -48,11 +48,22 @@ class TestService : public rclcpp::Node const std::shared_ptr response) { (void)request_header; - (void)response; current_request_ = request; + setSuccessIfExists(response); } private: + template + auto setSuccessIfExists(std::shared_ptr response) -> decltype(response->success, void()) + { + response->success = true; + } + + template + void setSuccessIfExists(T) + { + } + typename rclcpp::Service::SharedPtr server_; std::shared_ptr current_request_; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 93d3b310dc2..544877bf57b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_msgs/srv/clear_costmap_except_region.hpp" @@ -58,18 +59,23 @@ class ClearCostmapService /** * @brief Clears the region outside of a user-specified area reverting to the static map + * @return true if at least one layer was successfully cleared, false otherwise */ - void clearRegion(double reset_distance, bool invert); + bool clearRegion(double reset_distance, bool invert, const std::vector & plugins); /** * @brief Clears the region around a specific pose + * @return true if at least one layer was successfully cleared, false otherwise */ - void clearAroundPose(const geometry_msgs::msg::PoseStamped & pose, double reset_distance); + bool clearAroundPose( + const geometry_msgs::msg::PoseStamped & pose, double reset_distance, + const std::vector & plugins); /** * @brief Clears all layers + * @return true if at least one layer was successfully cleared, false otherwise */ - void clearEntirely(); + bool clearEntirely(const std::vector & plugins); private: // The Logger object for logging @@ -126,13 +132,38 @@ class ClearCostmapService * @brief Function used to clear a given costmap layer */ void clearLayerRegion( - std::shared_ptr & costmap, double pose_x, double pose_y, double reset_distance, - bool invert); + std::shared_ptr & costmap, + double pose_x, double pose_y, double reset_distance, bool invert); /** * @brief Get the robot's position in the costmap using the master costmap */ bool getPosition(double & x, double & y) const; + + /** + * @brief Validates requested plugins and returns list of invalid plugins + * @param requested_plugins List of plugin names to validate + * @param layers Pointer to all available layers + * @param invalid_plugins Output: list of invalid plugins with reasons + */ + void validatePlugins( + const std::vector & requested_plugins, + const std::vector> * layers, + 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; }; } // 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..66a7d921842 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -78,50 +78,51 @@ ClearCostmapService::~ClearCostmapService() void ClearCostmapService::clearExceptRegionCallback( const shared_ptr/*request_header*/, const shared_ptr request, - const shared_ptr/*response*/) + const shared_ptr response) { RCLCPP_INFO( logger_, "%s", ("Received request to clear except a region the " + costmap_.getName()).c_str()); - clearRegion(request->reset_distance, true); + response->success = clearRegion(request->reset_distance, true, request->plugins); } void ClearCostmapService::clearAroundRobotCallback( const shared_ptr/*request_header*/, const shared_ptr request, - const shared_ptr/*response*/) + const shared_ptr response) { - clearRegion(request->reset_distance, false); + response->success = clearRegion(request->reset_distance, false, request->plugins); } void ClearCostmapService::clearAroundPoseCallback( const shared_ptr/*request_header*/, const shared_ptr request, - const shared_ptr/*response*/) + const shared_ptr response) { RCLCPP_INFO( logger_, "%s", ("Received request to clear around pose for " + costmap_.getName()).c_str()); - clearAroundPose(request->pose, request->reset_distance); + response->success = clearAroundPose(request->pose, request->reset_distance, request->plugins); } void ClearCostmapService::clearEntireCallback( const std::shared_ptr/*request_header*/, - const std::shared_ptr/*request*/, - const std::shared_ptr/*response*/) + const std::shared_ptr request, + const std::shared_ptr response) { RCLCPP_INFO( logger_, "%s", ("Received request to clear entirely the " + costmap_.getName()).c_str()); - clearEntirely(); + response->success = clearEntirely(request->plugins); } -void ClearCostmapService::clearAroundPose( +bool ClearCostmapService::clearAroundPose( const geometry_msgs::msg::PoseStamped & pose, - const double reset_distance) + const double reset_distance, + const std::vector & plugins) { double x, y; @@ -138,7 +139,7 @@ void ClearCostmapService::clearAroundPose( logger_, "Cannot clear map around pose because pose cannot be transformed to costmap frame: %s", ex.what()); - return; + return false; } x = global_pose.pose.position.x; @@ -146,15 +147,28 @@ void ClearCostmapService::clearAroundPose( auto layers = costmap_.getLayeredCostmap()->getPlugins(); - for (auto & layer : *layers) { - if (layer->isClearable()) { - auto costmap_layer = std::static_pointer_cast(layer); - clearLayerRegion(costmap_layer, x, y, reset_distance, false); + if (!plugins.empty()) { + return validateAndClearPlugins( + plugins, layers, + [this, x, y, reset_distance](std::shared_ptr & layer) { + clearLayerRegion(layer, x, y, reset_distance, false); + }, + "clear costmap around pose"); + } else { + // Clear all clearable layers (default behavior) + for (auto & layer : *layers) { + if (layer->isClearable()) { + auto costmap_layer = std::static_pointer_cast(layer); + clearLayerRegion(costmap_layer, x, y, reset_distance, false); + } } + return true; } } -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 +176,28 @@ void ClearCostmapService::clearRegion(const double reset_distance, bool invert) RCLCPP_ERROR( logger_, "%s", "Cannot clear map because robot pose cannot be retrieved."); - return; + return false; } auto layers = costmap_.getLayeredCostmap()->getPlugins(); - for (auto & layer : *layers) { - if (layer->isClearable()) { - auto costmap_layer = std::static_pointer_cast(layer); - clearLayerRegion(costmap_layer, x, y, reset_distance, invert); + if (!plugins.empty()) { + return validateAndClearPlugins( + plugins, layers, + [this, x, y, reset_distance, invert](std::shared_ptr & layer) { + clearLayerRegion(layer, x, y, reset_distance, invert); + }, + "clear costmap region"); + } else { + // Clear all clearable layers (default behavior) + for (auto & layer : *layers) { + if (layer->isClearable()) { + auto costmap_layer = std::static_pointer_cast(layer); + clearLayerRegion(costmap_layer, x, y, reset_distance, invert); + } } + return true; } - - // AlexeyMerzlyakov: No need to clear layer region for costmap filters - // as they are always supposed to be not clearable. } void ClearCostmapService::clearLayerRegion( @@ -200,10 +222,101 @@ void ClearCostmapService::clearLayerRegion( costmap->addExtraBounds(ox, oy, ox + width, oy + height); } -void ClearCostmapService::clearEntirely() +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; + } else { + // Clear only specified plugins + std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); + auto layers = costmap_.getLayeredCostmap()->getPlugins(); + + bool result = validateAndClearPlugins( + plugins, layers, + [this](std::shared_ptr & layer) { + RCLCPP_INFO(logger_, "Clearing entire layer: %s", layer->getName().c_str()); + layer->resetMap(0, 0, layer->getSizeInCellsX(), layer->getSizeInCellsY()); + }, + "clear costmap entirely"); + + if (result) { + RCLCPP_INFO(logger_, "Resetting master costmap after plugin clearing"); + costmap_.getCostmap()->resetMap(0, 0, + costmap_.getCostmap()->getSizeInCellsX(), + costmap_.getCostmap()->getSizeInCellsY()); + } + + return result; + } +} + +void ClearCostmapService::validatePlugins( + const std::vector & requested_plugins, + const std::vector> * layers, + std::vector & invalid_plugins) const +{ + 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)"); + } + } +} + +bool ClearCostmapService::validateAndClearPlugins( + const std::vector & plugins, + const std::vector> * layers, + std::function &)> clear_callback, + const std::string & operation_name) const { - std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); - costmap_.resetLayers(); + std::vector invalid_plugins; + + validatePlugins(plugins, layers, invalid_plugins); + + 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()); + RCLCPP_ERROR( + logger_, + "Failed to %s: %zu invalid plugin(s) out of %zu requested. No layers were cleared.", + operation_name.c_str(), invalid_plugins.size(), plugins.size()); + return false; + } + + for (auto & layer : *layers) { + if (std::find(plugins.begin(), plugins.end(), + layer->getName()) != plugins.end()) + { + auto costmap_layer = std::static_pointer_cast(layer); + clear_callback(costmap_layer); + } + } + + return true; } bool ClearCostmapService::getPosition(double & x, double & y) const diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 621b4e8b119..924ef03491a 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -13,6 +13,11 @@ target_link_libraries(costmap_cost_service_test nav2_costmap_2d_core ) +nav2_add_gtest(clear_costmap_service_test clear_costmap_service_test.cpp) +target_link_libraries(clear_costmap_service_test + nav2_costmap_2d_core +) + nav2_add_gtest(declare_parameter_test declare_parameter_test.cpp) target_link_libraries(declare_parameter_test nav2_costmap_2d_core diff --git a/nav2_costmap_2d/test/unit/clear_costmap_service_test.cpp b/nav2_costmap_2d/test/unit/clear_costmap_service_test.cpp new file mode 100644 index 00000000000..e0d712cc907 --- /dev/null +++ b/nav2_costmap_2d/test/unit/clear_costmap_service_test.cpp @@ -0,0 +1,106 @@ +// Copyright (c) 2024 BCK +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include +#include "nav2_msgs/srv/clear_entire_costmap.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +using namespace std::chrono_literals; + +class ClearCostmapServiceTest : public ::testing::Test +{ +protected: + void SetUp() override + { + costmap_ = std::make_shared("costmap"); + clear_entire_client_ = costmap_->create_client( + "/costmap/clear_entirely_costmap"); + costmap_->on_configure(rclcpp_lifecycle::State()); + ASSERT_TRUE(clear_entire_client_->wait_for_service(10s)); + } + + std::shared_ptr costmap_; + nav2::ServiceClient::SharedPtr clear_entire_client_; +}; + +TEST_F(ClearCostmapServiceTest, ClearEntireDefaultBehavior) +{ + auto request = std::make_shared(); + + auto result_future = clear_entire_client_->async_call(request); + if (rclcpp::spin_until_future_complete( + costmap_, + result_future) == rclcpp::FutureReturnCode::SUCCESS) + { + auto response = result_future.get(); + EXPECT_TRUE(response->success) << "Default clear entire should succeed"; + } else { + FAIL() << "Failed to call ClearEntireCostmap service"; + } +} + +TEST_F(ClearCostmapServiceTest, ClearEntireWithInvalidPlugin) +{ + auto request = std::make_shared(); + request->plugins.push_back("nonexistent_layer"); + + auto result_future = clear_entire_client_->async_call(request); + if (rclcpp::spin_until_future_complete( + costmap_, + result_future) == rclcpp::FutureReturnCode::SUCCESS) + { + auto response = result_future.get(); + EXPECT_FALSE(response->success) << "Clear with invalid plugin should fail"; + } else { + FAIL() << "Failed to call ClearEntireCostmap service"; + } +} + +TEST_F(ClearCostmapServiceTest, ClearEntireWithMultipleInvalidPlugins) +{ + auto request = std::make_shared(); + request->plugins.push_back("nonexistent_layer_1"); + request->plugins.push_back("nonexistent_layer_2"); + + auto result_future = clear_entire_client_->async_call(request); + if (rclcpp::spin_until_future_complete( + costmap_, + result_future) == rclcpp::FutureReturnCode::SUCCESS) + { + auto response = result_future.get(); + EXPECT_FALSE(response->success) << "Clear with multiple invalid plugins should fail"; + } else { + FAIL() << "Failed to call ClearEntireCostmap service"; + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(0, nullptr); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} diff --git a/nav2_msgs/srv/ClearCostmapAroundPose.srv b/nav2_msgs/srv/ClearCostmapAroundPose.srv index 5d244279f67..e255b825a1d 100644 --- a/nav2_msgs/srv/ClearCostmapAroundPose.srv +++ b/nav2_msgs/srv/ClearCostmapAroundPose.srv @@ -2,5 +2,6 @@ geometry_msgs/PoseStamped pose float64 reset_distance +string[] plugins --- -std_msgs/Empty response +bool success diff --git a/nav2_msgs/srv/ClearCostmapAroundRobot.srv b/nav2_msgs/srv/ClearCostmapAroundRobot.srv index e3b41bf584e..ff8e8870245 100644 --- a/nav2_msgs/srv/ClearCostmapAroundRobot.srv +++ b/nav2_msgs/srv/ClearCostmapAroundRobot.srv @@ -1,5 +1,6 @@ # Clears the costmap within a distance float32 reset_distance +string[] plugins --- -std_msgs/Empty response +bool success diff --git a/nav2_msgs/srv/ClearCostmapExceptRegion.srv b/nav2_msgs/srv/ClearCostmapExceptRegion.srv index 56bb2f4938e..0c0d75c686a 100644 --- a/nav2_msgs/srv/ClearCostmapExceptRegion.srv +++ b/nav2_msgs/srv/ClearCostmapExceptRegion.srv @@ -1,5 +1,6 @@ # Clears the costmap except a rectangular region specified by reset_distance float32 reset_distance +string[] plugins --- -std_msgs/Empty response +bool success diff --git a/nav2_msgs/srv/ClearEntireCostmap.srv b/nav2_msgs/srv/ClearEntireCostmap.srv index ed1b675f2d4..e7e95281ba7 100644 --- a/nav2_msgs/srv/ClearEntireCostmap.srv +++ b/nav2_msgs/srv/ClearEntireCostmap.srv @@ -1,5 +1,6 @@ # Clears all layers on the costmap std_msgs/Empty request +string[] plugins --- -std_msgs/Empty response +bool success diff --git a/nav2_system_tests/src/behavior_tree/server_handler.cpp b/nav2_system_tests/src/behavior_tree/server_handler.cpp index 9eed165cab3..0287b5e8e4d 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.cpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.cpp @@ -31,10 +31,16 @@ 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"); + clear_costmap_around_robot_server = std::make_unique( + node_, "local_costmap/clear_around_local_costmap"); + clear_costmap_except_region_server = std::make_unique( + node_, "local_costmap/clear_except_local_costmap"); + clear_costmap_around_pose_server = std::make_unique( + node_, "local_costmap/clear_around_pose_local_costmap"); compute_path_to_pose_server = std::make_unique(node_); follow_path_server = std::make_unique(node_); spin_server = std::make_unique>( @@ -89,6 +95,9 @@ void ServerHandler::reset() const { clear_global_costmap_server->reset(); clear_local_costmap_server->reset(); + clear_costmap_around_robot_server->reset(); + clear_costmap_except_region_server->reset(); + clear_costmap_around_pose_server->reset(); compute_path_to_pose_server->reset(); follow_path_server->reset(); spin_server->reset(); diff --git a/nav2_system_tests/src/behavior_tree/server_handler.hpp b/nav2_system_tests/src/behavior_tree/server_handler.hpp index 2b852cd085f..09f071dc7a8 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.hpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.hpp @@ -24,6 +24,9 @@ #include #include "nav2_msgs/srv/clear_entire_costmap.hpp" +#include "nav2_msgs/srv/clear_costmap_around_robot.hpp" +#include "nav2_msgs/srv/clear_costmap_except_region.hpp" +#include "nav2_msgs/srv/clear_costmap_around_pose.hpp" #include "nav2_msgs/action/compute_path_to_pose.hpp" #include "nav2_msgs/action/follow_path.hpp" #include "nav2_msgs/action/spin.hpp" @@ -95,6 +98,76 @@ class DummyFollowPathActionServer : public DummyActionServer +{ +public: + explicit DummyClearEntireCostmapService( + 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 DummyClearCostmapAroundRobotService + : public DummyService +{ +public: + explicit DummyClearCostmapAroundRobotService( + 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 DummyClearCostmapExceptRegionService + : public DummyService +{ +public: + explicit DummyClearCostmapExceptRegionService( + 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 DummyClearCostmapAroundPoseService + : public DummyService +{ +public: + explicit DummyClearCostmapAroundPoseService( + 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 +187,11 @@ 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 clear_costmap_around_robot_server; + std::unique_ptr clear_costmap_except_region_server; + std::unique_ptr clear_costmap_around_pose_server; std::unique_ptr compute_path_to_pose_server; std::unique_ptr follow_path_server; std::unique_ptr> spin_server;