Async Hardware Components

controller_manager/include/controller_manager/controller_manager.hpp

@@ -222,15 +222,6 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   unsigned int get_update_rate() const;
 
-  /// Deletes all async controllers and components.
-  /**
-   * Needed to join the threads immediately after the control loop is ended
-   * to avoid unnecessary iterations. Otherwise
-   * the threads will be joined only when the controller manager gets destroyed.
-   */
-  CONTROLLER_MANAGER_PUBLIC
-  void shutdown_async_controllers_and_components();
-
 protected:
   CONTROLLER_MANAGER_PUBLIC
   void init_services();

controller_manager/src/controller_manager.cpp

@@ -2681,11 +2681,6 @@ std::pair<std::string, std::string> ControllerManager::split_command_interface(
 
 unsigned int ControllerManager::get_update_rate() const { return update_rate_; }
 
-void ControllerManager::shutdown_async_controllers_and_components()
-{
-  resource_manager_->shutdown_async_components();
-}
-
 void ControllerManager::propagate_deactivation_of_chained_mode(
   const std::vector<ControllerSpec> & controllers)
 {

controller_manager/src/ros2_control_node.cpp

@@ -147,8 +147,6 @@ int main(int argc, char ** argv)
           std::this_thread::sleep_until(next_iteration_time);
         }
       }
-
-      cm->shutdown_async_controllers_and_components();
     });
 
   executor->add_node(cm);

hardware_interface/CMakeLists.txt

@@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp_lifecycle
   rcpputils
   rcutils
+  realtime_tools
   TinyXML2
   tinyxml2_vendor
   joint_limits

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -34,6 +34,7 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/async_function_handler.hpp"
 
 namespace hardware_interface
 {
@@ -111,6 +112,30 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
     clock_interface_ = clock_interface;
     actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name);
     info_ = hardware_info;
+    if (info_.is_async)
+    {
+      RCLCPP_INFO_STREAM(
+        get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
+      async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
+      async_handler_->init(
+        [this](const rclcpp::Time & time, const rclcpp::Duration & period)
+        {
+          if (next_trigger_ == TriggerType::READ)
+          {
+            const auto ret = read(time, period);
+            next_trigger_ = TriggerType::WRITE;
+            return ret;
+          }
+          else
+          {
+            const auto ret = write(time, period);
+            next_trigger_ = TriggerType::READ;
+            return ret;
+          }
+        },
+        info_.thread_priority);
+      async_handler_->start_thread();
+    }
     return on_init(hardware_info);
   };
 
@@ -321,6 +346,50 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
     return return_type::OK;
   }
 
+  /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo
+  /**
+   * The data readings from the physical hardware has to be updated
+   * and reflected accordingly in the exported state interfaces.
+   * That is, the data pointed by the interfaces shall be updated.
+   * The method is called in the resource_manager's read loop
+   *
+   * \param[in] time The time at the start of this control loop iteration
+   * \param[in] period The measured time taken by the last control loop iteration
+   * \return return_type::OK if the read was successful, return_type::ERROR otherwise.
+   */
+  return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period)
+  {
+    return_type result = return_type::ERROR;
+    if (info_.is_async)
+    {
+      bool trigger_status = true;
+      if (next_trigger_ == TriggerType::WRITE)
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "Trigger read called while write async handler call is still pending for hardware "
+          "interface : '%s'. Skipping read cycle and will wait for a write cycle!",
+          info_.name.c_str());
+        return return_type::OK;
+      }
+      std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
+      if (!trigger_status)
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "Trigger read called while write async trigger is still in progress for hardware "
+          "interface : '%s'. Failed to trigger read cycle!",
+          info_.name.c_str());
+        return return_type::OK;
+      }
+    }
+    else
+    {
+      result = read(time, period);
+    }
+    return result;
+  }
+
   /// Read the current state values from the actuator.
   /**
    * The data readings from the physical hardware has to be updated
@@ -333,6 +402,49 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
+  /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo
+  /**
+   * The physical hardware shall be updated with the latest value from
+   * the exported command interfaces.
+   * The method is called in the resource_manager's write loop
+   *
+   * \param[in] time The time at the start of this control loop iteration
+   * \param[in] period The measured time taken by the last control loop iteration
+   * \return return_type::OK if the read was successful, return_type::ERROR otherwise.
+   */
+  return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period)
+  {
+    return_type result = return_type::ERROR;
+    if (info_.is_async)
+    {
+      bool trigger_status = true;
+      if (next_trigger_ == TriggerType::READ)
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "Trigger write called while read async handler call is still pending for hardware "
+          "interface : '%s'. Skipping write cycle and will wait for a read cycle!",
+          info_.name.c_str());
+        return return_type::OK;
+      }
+      std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
+      if (!trigger_status)
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "Trigger write called while read async trigger is still in progress for hardware "
+          "interface : '%s'. Failed to trigger write cycle!",
+          info_.name.c_str());
+        return return_type::OK;
+      }
+    }
+    else
+    {
+      result = write(time, period);
+    }
+    return result;
+  }
+
   /// Write the current command values to the actuator.
   /**
    * The physical hardware shall be updated with the latest value from
@@ -426,13 +538,20 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   std::vector<CommandInterface::SharedPtr> unlisted_commands_;
 
   rclcpp_lifecycle::State lifecycle_state_;
+  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
 
 private:
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
   rclcpp::Logger actuator_logger_;
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;
+  enum class TriggerType
+  {
+    READ,
+    WRITE
+  };
+  std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -178,6 +178,8 @@ struct HardwareInfo
   unsigned int rw_rate;
   /// Component is async
   bool is_async;
+  /// Async thread priority
+  int thread_priority;
   /// Name of the pluginlib plugin of the hardware that will be loaded.
   std::string hardware_plugin_name;
   /// (Optional) Key-value pairs for hardware parameters.

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -426,12 +426,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   return_type set_component_state(
     const std::string & component_name, rclcpp_lifecycle::State & target_state);
 
-  /// Deletes all async components from the resource storage
-  /**
-   * Needed to join the threads immediately after the control loop is ended.
-   */
-  void shutdown_async_components();
-
   /// Reads all loaded hardware components.
   /**
    * Reads from all active hardware components.

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -34,6 +34,7 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/async_function_handler.hpp"
 
 namespace hardware_interface
 {
@@ -111,6 +112,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     clock_interface_ = clock_interface;
     sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name);
     info_ = hardware_info;
+    if (info_.is_async)
+    {
+      RCLCPP_INFO_STREAM(
+        get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority);
+      read_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
+      read_async_handler_->init(
+        std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2),
+        info_.thread_priority);
+      read_async_handler_->start_thread();
+    }
     return on_init(hardware_info);
   };
 
@@ -214,6 +225,40 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     return state_interfaces;
   }
 
+  /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo
+  /**
+   * The data readings from the physical hardware has to be updated
+   * and reflected accordingly in the exported state interfaces.
+   * That is, the data pointed by the interfaces shall be updated.
+   *
+   * \param[in] time The time at the start of this control loop iteration
+   * \param[in] period The measured time taken by the last control loop iteration
+   * \return return_type::OK if the read was successful, return_type::ERROR otherwise.
+   */
+  return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period)
+  {
+    return_type result = return_type::ERROR;
+    if (info_.is_async)
+    {
+      bool trigger_status = true;
+      std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period);
+      if (!trigger_status)
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "Trigger read called while read async handler is still in progress for hardware "
+          "interface : '%s'. Failed to trigger read cycle!",
+          info_.name.c_str());
+        return return_type::OK;
+      }
+    }
+    else
+    {
+      result = read(time, period);
+    }
+    return result;
+  }
+
   /// Read the current state values from the actuator.
   /**
    * The data readings from the physical hardware has to be updated
@@ -294,6 +339,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   std::vector<StateInterface::SharedPtr> unlisted_states_;
 
   rclcpp_lifecycle::State lifecycle_state_;
+  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> read_async_handler_;
 
 private:
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;