Collision monitor toggle bt plugin

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

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 <string>
+
+#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<nav2_msgs::srv::Toggle>
+{
+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<bool>(
+          "enable", true,
+          "Whether to enable or disable collision monitoring"),
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -311,6 +311,12 @@
       <output_port name="pose">Stamped extracted pose</output_port>
     </Action>
 
+    <Action ID="ToggleCollisionMonitor">
+      <input_port name="enable">Whether to enable collision monitoring</input_port>
+      <input_port name="service_name">Service name</input_port>
+      <input_port name="server_timeout">Server timeout</input_port>
+    </Action>
+
     <Action ID="GetCurrentPose">
       <input_port name="global_frame">Global reference frame</input_port>
       <input_port name="robot_base_frame">robot base frame</input_port>

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 <string>
+#include <memory>
+
+#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<nav2_msgs::srv::Toggle>(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<nav2_behavior_tree::ToggleCollisionMonitorService>(
+    "ToggleCollisionMonitor");
+}

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)

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 <gtest/gtest.h>
+#include <memory>
+#include <set>
+#include <string>
+
+#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<nav2_msgs::srv::Toggle>
+{
+public:
+  ToggleCollisionMonitorService()
+  : TestService("toggle_collision_monitor")
+  {}
+};
+
+class ToggleCollisionMonitorTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<nav2::LifecycleNode>("toggle_collision_monitor_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    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<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(20));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
+    config_->blackboard->set("initial_pose_received", false);
+
+    factory_->registerNodeType<nav2_behavior_tree::ToggleCollisionMonitorService>(
+      "ToggleCollisionMonitor");
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    server_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<ToggleCollisionMonitorService> server_;
+
+protected:
+  static nav2::LifecycleNode::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+nav2::LifecycleNode::SharedPtr ToggleCollisionMonitorTestFixture::node_ = nullptr;
+std::shared_ptr<ToggleCollisionMonitorService>
+ToggleCollisionMonitorTestFixture::server_ = nullptr;
+BT::NodeConfiguration * ToggleCollisionMonitorTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory>
+ToggleCollisionMonitorTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> ToggleCollisionMonitorTestFixture::tree_ = nullptr;
+
+class ToggleParamTest
+  : public ToggleCollisionMonitorTestFixture,
+  public ::testing::WithParamInterface<bool> {};
+
+TEST_P(ToggleParamTest, test_tick)
+{
+  const bool enable = GetParam();
+
+  std::string xml_txt =
+    std::string(
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <ToggleCollisionMonitor service_name="toggle_collision_monitor" enable=")")
+    +
+    std::string(enable ? "true" : "false") +
+    R"(" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(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<ToggleCollisionMonitorService>();
+  std::thread server_thread([]() {
+      rclcpp::spin(ToggleCollisionMonitorTestFixture::server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}