ros-navigation · SteveMacenski · Sep 20, 2025 · Aug 26, 2025 · Aug 27, 2025 · Sep 3, 2025
diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>Nav2 behavior tree wrappers, nodes, and utilities</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>

diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>Nav2 behavior server</description>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>Nav2 BT Navigator Server</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
@@ -33,6 +33,7 @@
 #include "nav2_util/twist_publisher.hpp"
 #include "nav2_util/twist_subscriber.hpp"
 #include "nav2_msgs/msg/collision_monitor_state.hpp"
+#include "nav2_msgs/srv/toggle.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/polygon.hpp"
@@ -200,6 +201,16 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    */
   void publishPolygons() const;
 
+  /**
+   * @brief Enable/disable collision monitor service callback
+   * @param request Service request
+   * @param response Service response
+   */
+  void toggleCMServiceCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::Toggle::Request> request,
+    std::shared_ptr<nav2_msgs::srv::Toggle::Response> response);
+
   // ----- Variables -----
 
   /// @brief TF buffer
@@ -227,6 +238,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     collision_points_marker_pub_;
 
+  /// @brief Enable/disable collision monitor service
+  rclcpp::Service<nav2_msgs::srv::Toggle>::SharedPtr toggle_cm_service_;
+
+  /// @brief Whether collision monitor is enabled
+  bool enabled_;
+
   /// @brief Whether main routine is active
   bool process_active_;
 

diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_collision_monitor</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>Collision Monitor</description>
   <maintainer email="[email protected]">Alexey Merzlyakov</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -25,12 +25,14 @@
 
 #include "nav2_collision_monitor/kinematics.hpp"
 
+using namespace std::placeholders;
+
 namespace nav2_collision_monitor
 {
 
 CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options)
 : nav2_util::LifecycleNode("collision_monitor", "", options),
-  process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""},
+  enabled_{true}, process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}, ""},
   stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0)
 {
 }
@@ -82,6 +84,11 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
   collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
     "~/collision_points_marker", 1);
 
+  // Toggle service initialization
+  toggle_cm_service_ = create_service<nav2_msgs::srv::Toggle>(
+    "~/toggle",
+    std::bind(&CollisionMonitor::toggleCMServiceCallback, this, _1, _2, _3));
+
   nav2_util::declare_parameter_if_not_declared(
     node, "use_realtime_priority", rclcpp::ParameterValue(false));
   bool use_realtime_priority = false;
@@ -473,7 +480,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
   }
 
   for (std::shared_ptr<Polygon> polygon : polygons_) {
-    if (!polygon->getEnabled()) {
+    if (!polygon->getEnabled() || !enabled_) {
       continue;
     }
     if (robot_action.action_type == STOP) {
@@ -500,7 +507,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
     }
   }
 
-  if (robot_action.polygon_name != robot_action_prev_.polygon_name) {
+  if ((robot_action.polygon_name != robot_action_prev_.polygon_name) && enabled_) {
     // Report changed robot behavior
     notifyActionState(robot_action, action_polygon);
   }
@@ -653,12 +660,26 @@ void CollisionMonitor::notifyActionState(
 void CollisionMonitor::publishPolygons() const
 {
   for (std::shared_ptr<Polygon> polygon : polygons_) {
-    if (polygon->getEnabled()) {
+    if (polygon->getEnabled() || !enabled_) {
       polygon->publish();
     }
   }
 }
 
+void CollisionMonitor::toggleCMServiceCallback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::Toggle::Request> request,
+  std::shared_ptr<nav2_msgs::srv::Toggle::Response> response)
+{
+  enabled_ = request->enable;
+
+  std::stringstream message;
+  message << "Collision monitor toggled " << (enabled_ ? "on" : "off") << " successfully";
+
+  response->success = true;
+  response->message = message.str();
+}
+
 }  // namespace nav2_collision_monitor
 
 #include "rclcpp_components/register_node_macro.hpp"

diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -131,6 +131,11 @@ class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor
     }
     return false;
   }
+
+  bool isEnabled() const
+  {
+    return enabled_;
+  }
 };  // CollisionMonitorWrapper
 
 class Tester : public ::testing::Test
@@ -180,6 +185,9 @@ class Tester : public ::testing::Test
     const std::chrono::nanoseconds & timeout);
   bool waitActionState(const std::chrono::nanoseconds & timeout);
   bool waitCollisionPointsMarker(const std::chrono::nanoseconds & timeout);
+  bool waitToggle(
+    rclcpp::Client<nav2_msgs::srv::Toggle>::SharedFuture result_future,
+    const std::chrono::nanoseconds & timeout);
 
 protected:
   void cmdVelOutCallback(geometry_msgs::msg::Twist::SharedPtr msg);
@@ -215,6 +223,9 @@ class Tester : public ::testing::Test
 
   // Service client for setting CollisionMonitor parameters
   rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr parameters_client_;
+
+  // Service client for toggling collision monitor
+  rclcpp::Client<nav2_msgs::srv::Toggle>::SharedPtr toggle_client_;
 };  // Tester
 
 Tester::Tester()
@@ -249,6 +260,8 @@ Tester::Tester()
     cm_->create_client<rcl_interfaces::srv::SetParameters>(
     std::string(
       cm_->get_name()) + "/set_parameters");
+
+  toggle_client_ = cm_->create_client<nav2_msgs::srv::Toggle>("~/toggle");
 }
 
 Tester::~Tester()
@@ -745,6 +758,22 @@ bool Tester::waitFuture(
   return false;
 }
 
+bool Tester::waitToggle(
+  rclcpp::Client<nav2_msgs::srv::Toggle>::SharedFuture result_future,
+  const std::chrono::nanoseconds & timeout)
+{
+  rclcpp::Time start_time = cm_->now();
+  while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) {
+    std::future_status status = result_future.wait_for(10ms);
+    if (status == std::future_status::ready) {
+      return true;
+    }
+    rclcpp::spin_some(cm_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+  return false;
+}
+
 bool Tester::waitActionState(const std::chrono::nanoseconds & timeout)
 {
   rclcpp::Time start_time = cm_->now();
@@ -786,6 +815,39 @@ void Tester::collisionPointsMarkerCallback(visualization_msgs::msg::MarkerArray:
   collision_points_marker_msg_ = msg;
 }
 
+TEST_F(Tester, testToggleService)
+{
+  // Set parameters for collision monitor
+  setCommonParameters();
+  addPolygon("Stop", POLYGON, 1.0, "stop");
+  addSource(SCAN_NAME, SCAN);
+  setVectors({"Stop"}, {SCAN_NAME});
+
+  // Start collision monitor node
+  cm_->start();
+
+  auto request = std::make_shared<nav2_msgs::srv::Toggle::Request>();
+
+  // Disable test
+  request->enable = false;
+  {
+    auto result_future = toggle_client_->async_send_request(request);
+    ASSERT_TRUE(waitToggle(result_future.share(), 2s));
+  }
+  ASSERT_FALSE(cm_->isEnabled());
+
+  // Enable test
+  request->enable = true;
+  {
+    auto result_future = toggle_client_->async_send_request(request);
+    ASSERT_TRUE(waitToggle(result_future.share(), 2s));
+  }
+  ASSERT_TRUE(cm_->isEnabled());
+
+  // Stop the collision monitor
+  cm_->stop();
+}
+
 TEST_F(Tester, testProcessStopSlowdownLimit)
 {
   rclcpp::Time curr_time = cm_->now();

diff --git a/nav2_common/package.xml b/nav2_common/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_common</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_constrained_smoother</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="[email protected]">Matej Vargovcik</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_controller</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>Controller action interface</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_core/package.xml b/nav2_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_core</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">Carl Delsey</maintainer>

diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format ="3">
   <name>nav2_costmap_2d</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>
     This package provides an implementation of a 2D costmap that takes in sensor
     data from the world, builds a 2D or 3D occupancy grid of the data (depending

diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>opennav_docking</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>A Task Server for robot charger docking</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>opennav_docking_bt</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>A set of BT nodes and XMLs for docking</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>opennav_docking_core</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>A set of headers for plugins core to the opennav docking framework</description>
   <maintainer email="[email protected]">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>The costmap_queue package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_core</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>DWB core interfaces package</description>
   <maintainer email="[email protected]">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>The dwb_critics package</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_msgs</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="[email protected]">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>
       Standard implementations of the GoalChecker
       and TrajectoryGenerators for dwb_core

diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_dwb_controller</name>
-  <version>1.3.8</version>
+  <version>1.3.9</version>
   <description>
     ROS2 controller (DWB) metapackage
   </description>