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,6 +479,10 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg:
   }
 
   for (std::shared_ptr<Polygon> polygon : polygons_) {
+    if (!enabled_) {
+      break;
+    }
+
     if (!polygon->getEnabled()) {
       continue;
     }
@@ -651,13 +662,31 @@ void CollisionMonitor::notifyActionState(
 
 void CollisionMonitor::publishPolygons() const
 {
+  if (!enabled_) {
+    return;
+  }
+
   for (std::shared_ptr<Polygon> polygon : polygons_) {
     if (polygon->getEnabled()) {
       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_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