Added async thread to log outside of update()

Author: VinDp

ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp

@@ -38,12 +38,11 @@
 
 #pragma once
 
-#include <realtime_tools/realtime_buffer.h>
-#include <realtime_tools/realtime_server_goal_handle.h>
-
 #include <memory>
 #include <string>
 #include <vector>
+#include <thread>
+#include <mutex>
 
 #include <controller_interface/controller_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -109,6 +108,15 @@ class FreedriveModeController : public controller_interface::ControllerInterface
   void start_timer();
   void timeout_callback();
 
+  std::thread logging_thread_;
+  std::atomic<bool> logging_thread_running_;
+  std::atomic<bool> logging_requested_;
+  std::condition_variable logging_condition_;
+  std::mutex log_mutex_;
+  void log_task();
+  void start_logging_thread();
+  void stop_logging_thread();
+
   static constexpr double ASYNC_WAITING = 2.0;
   /**
    * @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries

ur_controllers/src/freedrive_mode_controller.cpp

@@ -104,6 +104,8 @@ ur_controllers::FreedriveModeController::on_configure(const rclcpp_lifecycle::St
   // Get parameters from the listener in case they were updated
   freedrive_params_ = freedrive_param_listener_->get_params();
 
+  start_logging_thread();
+
   return ControllerInterface::on_configure(previous_state);
 }
 
@@ -112,9 +114,12 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta
 {
   change_requested_ = false;
   freedrive_active_ = false;
-  first_log_ = false;
   async_state_ = std::numeric_limits<double>::quiet_NaN();
 
+  first_log_ = false;
+  logging_thread_running_ = false;
+  logging_requested_ = false;
+
   {
     const std::string interface_name = freedrive_params_.tf_prefix + "freedrive_mode/"
                                                                        "async_success";
@@ -162,12 +167,10 @@ controller_interface::CallbackReturn
 ur_controllers::FreedriveModeController::on_deactivate(const rclcpp_lifecycle::State&)
 {
   abort_command_interface_->get().set_value(1.0);
-
-  // Set enable value to false, so in the update
-  // we can deactivate the freedrive mode
-  // Old comment?
   freedrive_active_ = false;
 
+  stop_logging_thread();
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -208,12 +211,12 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat
   }
 
   if((async_state_ == 1.0) && (first_log_)){
-    if(freedrive_active_){
-      RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully.");
-    } else {
-      RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully.");
-    }
     first_log_ = false;
+    logging_requested_= true;
+
+    // Notify logging thread
+    logging_condition_.notify_one();
+
   }
   return controller_interface::return_type::OK;
 }
@@ -232,7 +235,6 @@ void FreedriveModeController::readFreedriveModeCmd(const std_msgs::msg::Bool::Sh
     if((freedrive_active_) && (!change_requested_)){
       freedrive_active_ = false;
       change_requested_ = true;
-      //start_timer();
     }
   }
 
@@ -268,6 +270,46 @@ void FreedriveModeController::timeout_callback()
   timer_started_ = false;
 }
 
+  void FreedriveModeController::start_logging_thread() {
+    if (!logging_thread_running_) {
+      logging_thread_running_ = true;
+      logging_thread_ = std::thread(&FreedriveModeController::log_task, this);
+    }
+  }
+
+  void FreedriveModeController::stop_logging_thread() {
+    logging_thread_running_ = false;
+    if (logging_thread_.joinable()) {
+      logging_thread_.join();
+    }
+  }
+
+  void FreedriveModeController::log_task() {
+    while (logging_thread_running_) {
+
+      std::unique_lock<std::mutex> lock(log_mutex_);
+
+      auto condition = [this] {
+            return !logging_thread_running_ || logging_requested_;
+      };
+
+      // Wait for the condition
+      logging_condition_.wait(lock, condition);
+
+
+      if (!logging_thread_running_) break;
+
+      if(freedrive_active_){
+        RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been enabled successfully.");
+      } else {
+        RCLCPP_INFO(get_node()->get_logger(), "Freedrive mode has been disabled successfully.");
+      }
+
+      // Reset to log only once
+      logging_requested_ = false;
+    }
+  }
+
 bool FreedriveModeController::waitForAsyncCommand(std::function<double(void)> get_value)
 {
   const auto maximum_retries = freedrive_params_.check_io_successful_retries;