First version with timer

Author: VinDp

ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp

@@ -96,8 +96,8 @@ class FreedriveModeController : public controller_interface::ControllerInterface
 
   std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Bool>> enable_freedrive_mode_sub_;
 
-  rclcpp::TimerBase::SharedPtr goal_handle_timer_;  ///< Timer to frequently check on the running goal
-  rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);
+  rclcpp::TimerBase::SharedPtr freedrive_sub_timer_;  ///< Timer to check on the topic
+  std::chrono::seconds timeout_interval_ = std::chrono::seconds(2);
   void readFreedriveModeCmd(const std_msgs::msg::Bool::SharedPtr msg);
 
   std::shared_ptr<freedrive_mode_controller::ParamListener> freedrive_param_listener_;
@@ -107,6 +107,9 @@ class FreedriveModeController : public controller_interface::ControllerInterface
   std::atomic<bool> change_requested_;
   std::atomic<double> async_state_;
   std::atomic<double> first_log_;
+  std::atomic<double> timer_started_;
+
+  void timeout_callback();
 
   static constexpr double ASYNC_WAITING = 2.0;
   /**

ur_controllers/src/freedrive_mode_controller.cpp

@@ -88,6 +88,8 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St
       "~/freedrive_mode_active", 10,
       std::bind(&FreedriveModeController::readFreedriveModeCmd, this, std::placeholders::_1));
 
+  timer_started_ = false;
+
   const auto logger = get_node()->get_logger();
 
   if (!freedrive_param_listener_) {
@@ -218,6 +220,17 @@ 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)
   {
@@ -231,6 +244,24 @@ void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::Sh
       change_requested_ = true;
     }
   }
+
+  if (freedrive_sub_timer_)
+  {
+    freedrive_sub_timer_->reset();
+  }
+}
+
+void FreedriveModeController::timeout_callback()
+{
+
+  if(timer_started_){
+    RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode will be deactivated since client is not reachable.");
+
+    freedrive_active_ = false;
+    change_requested_ = true;
+  }
+
+  timer_started_ = false;
 }
 
 // Timeout handling for the topic