Fix to start timer only for relevant msgs

Author: VinDp

ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp

@@ -109,6 +109,7 @@ class FreedriveModeController : public controller_interface::ControllerInterface
   std::atomic<double> first_log_;
   std::atomic<double> timer_started_;
 
+  void start_timer();
   void timeout_callback();
 
   static constexpr double ASYNC_WAITING = 2.0;

ur_controllers/src/freedrive_mode_controller.cpp

@@ -220,28 +220,20 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat
 
 void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg)
 {
-  if (!timer_started_)
-  {
-    // Start the timer only after the first message is received
-    freedrive_sub_timer_ = get_node()->create_wall_timer(
-        timeout_interval_,
-        std::bind(&FreedriveModeController::timeout_callback, this));
-    timer_started_ = true;
-
-    RCLCPP_INFO(get_node()->get_logger(), "Timer started after receiving first command.");
-  }
 
   // Process the freedrive_mode command.
   if(msg->data)
   {
     if((!freedrive_active_) && (!change_requested_)){
       freedrive_active_ = true;
       change_requested_ = true;
+      start_timer();
     }
   } else{
     if((freedrive_active_) && (!change_requested_)){
       freedrive_active_ = false;
       change_requested_ = true;
+      start_timer();
     }
   }
 
@@ -251,6 +243,20 @@ void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::Sh
   }
 }
 
+void FreedriveModeController::start_timer()
+{
+  if (!timer_started_)
+  {
+    // Start the timer only after the first message is received
+    freedrive_sub_timer_ = get_node()->create_wall_timer(
+        timeout_interval_,
+        std::bind(&FreedriveModeController::timeout_callback, this));
+    timer_started_ = true;
+
+    RCLCPP_INFO(get_node()->get_logger(), "Timer started after receiving first command.");
+  }
+}
+
 void FreedriveModeController::timeout_callback()
 {