diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 2004b5c62b3..f2daaaeeeba 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -255,6 +255,9 @@ list(APPEND plugin_libs nav2_compute_and_track_route_bt_node) add_library(nav2_compute_route_bt_node SHARED plugins/action/compute_route_action.cpp) list(APPEND plugin_libs nav2_compute_route_bt_node) +add_library(nav2_toggle_collision_monitor_service_bt_node SHARED plugins/action/toggle_collision_monitor_service.cpp) +list(APPEND plugin_libs nav2_toggle_collision_monitor_service_bt_node) + foreach(bt_plugin ${plugin_libs}) target_include_directories(${bt_plugin} PRIVATE diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp new file mode 100644 index 00000000000..a06b841d1c3 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2025 David Grbac +// +// 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. + +#pragma once + +#include + +#include "nav2_behavior_tree/bt_service_node.hpp" +#include "nav2_msgs/srv/toggle.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::Toggle + * @note This is an Asynchronous (long-running) node which may return a RUNNING state while executing. + * It will re-initialize when halted. + */ +class ToggleCollisionMonitorService : public BtServiceNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ToggleCollisionMonitorService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ + ToggleCollisionMonitorService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf); + + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ + void on_tick() 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( + "enable", true, + "Whether to enable or disable collision monitoring"), + }); + } +}; + +} // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index eabba3a4b09..d555f0df948 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -311,6 +311,12 @@ Stamped extracted pose + + Whether to enable collision monitoring + Service name + Server timeout + + Global reference frame robot base frame diff --git a/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.cpp b/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.cpp new file mode 100644 index 00000000000..02553ac2207 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2025 David Grbac +// +// 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 "nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp" + +namespace nav2_behavior_tree +{ + +ToggleCollisionMonitorService::ToggleCollisionMonitorService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf) +: BtServiceNode(service_node_name, conf) +{ +} + +void ToggleCollisionMonitorService::on_tick() +{ + getInput("enable", request_->enable); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType( + "ToggleCollisionMonitor"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 742b6487fcf..5bd9e2c69e6 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -90,3 +90,7 @@ plugin_add_test(test_smoother_selector_node test_smoother_selector_node.cpp nav2 plugin_add_test(test_goal_checker_selector_node test_goal_checker_selector_node.cpp nav2_goal_checker_selector_bt_node) plugin_add_test(test_progress_checker_selector_node test_progress_checker_selector_node.cpp nav2_progress_checker_selector_bt_node) + +plugin_add_test(test_toggle_collision_monitor_service + test_toggle_collision_monitor_service.cpp + nav2_toggle_collision_monitor_service_bt_node) diff --git a/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp b/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp new file mode 100644 index 00000000000..01ff34c968c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp @@ -0,0 +1,142 @@ +// Copyright (c) 2025 David Grbac +// +// 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 "behaviortree_cpp/bt_factory.h" + +#include "utils/test_service.hpp" +#include "nav2_behavior_tree/plugins/action/toggle_collision_monitor_service.hpp" + +class ToggleCollisionMonitorService : public TestService +{ +public: + ToggleCollisionMonitorService() + : TestService("toggle_collision_monitor") + {} +}; + +class ToggleCollisionMonitorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("toggle_collision_monitor_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); + config_->blackboard->set("initial_pose_received", false); + + factory_->registerNodeType( + "ToggleCollisionMonitor"); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr server_; + +protected: + static nav2::LifecycleNode::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +nav2::LifecycleNode::SharedPtr ToggleCollisionMonitorTestFixture::node_ = nullptr; +std::shared_ptr +ToggleCollisionMonitorTestFixture::server_ = nullptr; +BT::NodeConfiguration * ToggleCollisionMonitorTestFixture::config_ = nullptr; +std::shared_ptr +ToggleCollisionMonitorTestFixture::factory_ = nullptr; +std::shared_ptr ToggleCollisionMonitorTestFixture::tree_ = nullptr; + +class ToggleParamTest + : public ToggleCollisionMonitorTestFixture, + public ::testing::WithParamInterface {}; + +TEST_P(ToggleParamTest, test_tick) +{ + const bool enable = GetParam(); + + std::string xml_txt = + std::string( + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); +} + +INSTANTIATE_TEST_SUITE_P(EnableDisable, ToggleParamTest, ::testing::Values(true, false)); + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize service and spin on new thread + ToggleCollisionMonitorTestFixture::server_ = + std::make_shared(); + std::thread server_thread([]() { + rclcpp::spin(ToggleCollisionMonitorTestFixture::server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +}