Add a service for enabling/disabling the collision monitor

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::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::LifecycleNode
   nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     collision_points_marker_pub_;
 
+  /// @brief Enable/disable collision monitor service
+  nav2::ServiceServer<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_;
 

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::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)
 {
 }
@@ -81,6 +83,11 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
   collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
     "~/collision_points_marker");
 
+  // Toggle service initialization
+  toggle_cm_service_ = create_service<nav2_msgs::srv::Toggle>(
+    "~/toggle",
+    std::bind(&CollisionMonitor::toggleCMServiceCallback, this, _1, _2, _3));
+
   nav2::declare_parameter_if_not_declared(
     node, "use_realtime_priority", rclcpp::ParameterValue(false));
   bool use_realtime_priority = false;
@@ -472,7 +479,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) {
@@ -499,7 +506,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);
   }
@@ -652,12 +659,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"

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);
@@ -217,6 +225,9 @@ class Tester : public ::testing::Test
 
   // Service client for setting CollisionMonitor parameters
   nav2::ServiceClient<rcl_interfaces::srv::SetParameters>::SharedPtr parameters_client_;
+
+  // Service client for toggling collision monitor
+  nav2::ServiceClient<nav2_msgs::srv::Toggle>::SharedPtr toggle_client_;
 };  // Tester
 
 Tester::Tester()
@@ -258,6 +269,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()
@@ -759,6 +772,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();
@@ -800,6 +829,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_call(request);
+    ASSERT_TRUE(waitToggle(result_future, 2s));
+  }
+  ASSERT_FALSE(cm_->isEnabled());
+
+  // Enable test
+  request->enable = true;
+  {
+    auto result_future = toggle_client_->async_call(request);
+    ASSERT_TRUE(waitToggle(result_future, 2s));
+  }
+  ASSERT_TRUE(cm_->isEnabled());
+
+  // Stop the collision monitor
+  cm_->stop();
+}
+
 TEST_F(Tester, testProcessStopSlowdownLimit)
 {
   rclcpp::Time curr_time = cm_->now();

nav2_msgs/CMakeLists.txt

@@ -53,6 +53,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/ReloadDockDatabase.srv"
   "srv/SetRouteGraph.srv"
   "srv/DynamicEdges.srv"
+  "srv/Toggle.srv"
   "action/AssistedTeleop.action"
   "action/BackUp.action"
   "action/ComputePathToPose.action"

nav2_msgs/srv/Toggle.srv

@@ -0,0 +1,4 @@
+bool enable
+---
+bool success
+string message