Add a parameter to set initial enabled state for the collision monitor (#5554)

* Added a new parameter to set initial enabled state for the collision monitor

Signed-off-by: Abhishekh Reddy <[email protected]>

* Fixed enabled_at_start parameter default value

Signed-off-by: Abhishekh Reddy <[email protected]>

* Updated toggle service unit test for code coverage

Signed-off-by: Abhishekh Reddy <[email protected]>

* Updated enabled state log message on startup

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Abhishekh Reddy <[email protected]>

* Updated enabled state log message on startup

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: Abhishekh Reddy <[email protected]>

* Renamed parameter to 'enabled'

Signed-off-by: Abhishekh Reddy <[email protected]>

---------

Signed-off-by: Abhishekh Reddy <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Author: armgits

nav2_bringup/params/nav2_params.yaml

@@ -485,6 +485,7 @@ velocity_smoother:
 
 collision_monitor:
   ros__parameters:
+    enabled: True
     base_frame_id: "base_footprint"
     odom_frame_id: "odom"
     cmd_vel_in_topic: "cmd_vel_smoothed"

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -1,5 +1,6 @@
 collision_monitor:
   ros__parameters:
+    enabled: True
     base_frame_id: "base_footprint"
     odom_frame_id: "odom"
     cmd_vel_in_topic: "cmd_vel_smoothed"

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -102,6 +102,15 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
     }
   }
 
+  nav2::declare_parameter_if_not_declared(node, "enabled", rclcpp::ParameterValue(true));
+  node->get_parameter("enabled", enabled_);
+
+  if (!enabled_) {
+    RCLCPP_WARN(get_logger(), "Collision monitor is disabled at startup.");
+  } else {
+    RCLCPP_INFO(get_logger(), "Collision monitor is enabled at startup.");
+  }
+
   return nav2::CallbackReturn::SUCCESS;
 }
 

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -302,6 +302,8 @@ Tester::~Tester()
 
 void Tester::setCommonParameters()
 {
+  cm_->declare_parameter("enabled", rclcpp::ParameterValue(true));
+
   cm_->declare_parameter(
     "cmd_vel_in_topic", rclcpp::ParameterValue(CMD_VEL_IN_TOPIC));
   cm_->set_parameter(
@@ -841,26 +843,30 @@ TEST_F(Tester, testToggleService)
   addSource(SCAN_NAME, SCAN);
   setVectors({"Stop"}, {SCAN_NAME});
 
+  // Test the parameter in disabled state
+  cm_->set_parameter(rclcpp::Parameter("enabled", false));
+
   // Start collision monitor node
   cm_->start();
+  ASSERT_FALSE(cm_->isEnabled());
 
   auto request = std::make_shared<nav2_msgs::srv::Toggle::Request>();
 
-  // Disable test
-  request->enable = false;
+  // Enable test
+  request->enable = true;
   {
     auto result_future = toggle_client_->async_call(request);
     ASSERT_TRUE(waitToggle(result_future, 2s));
   }
-  ASSERT_FALSE(cm_->isEnabled());
+  ASSERT_TRUE(cm_->isEnabled());
 
-  // Enable test
-  request->enable = true;
+  // Disable test
+  request->enable = false;
   {
     auto result_future = toggle_client_->async_call(request);
     ASSERT_TRUE(waitToggle(result_future, 2s));
   }
-  ASSERT_TRUE(cm_->isEnabled());
+  ASSERT_FALSE(cm_->isEnabled());
 
   // Stop the collision monitor
   cm_->stop();