async components

controller_manager/src/controller_manager.cpp

@@ -135,7 +135,8 @@ ControllerManager::ControllerManager(
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
   const std::string & namespace_, const rclcpp::NodeOptions & options)
 : rclcpp::Node(manager_node_name, namespace_, options),
-  resource_manager_(std::make_unique<hardware_interface::ResourceManager>()),
+  resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
+    this->get_node_clock_interface(), update_rate_)),
   diagnostics_updater_(this),
   executor_(executor),
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
@@ -234,6 +235,9 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       "future anymore. Use hardware_spawner instead.");
     resource_manager_->activate_all_components();
   }
+
+  // At this point all components are supposed to be stored in the resource storage's vector;
+  resource_manager_->allocate_threads_for_async_components();
 }
 
 void ControllerManager::init_services()

hardware_interface/include/hardware_interface/async_components.hpp

@@ -0,0 +1,103 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_
+#define HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_
+
+#include <atomic>
+#include <thread>
+#include <type_traits>
+
+#include "hardware_interface/actuator.hpp"
+#include "hardware_interface/sensor.hpp"
+#include "hardware_interface/system.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "rclcpp/duration.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/time.hpp"
+
+namespace hardware_interface
+{
+
+template <typename HardwareT>
+class AsyncComponentThread
+{
+public:
+  static_assert(
+    std::is_same<hardware_interface::Actuator, HardwareT>::value ||
+      std::is_same<hardware_interface::System, HardwareT>::value ||
+      std::is_same<hardware_interface::Sensor, HardwareT>::value,
+    "Async component has to have a valid hardware type.");
+
+  explicit AsyncComponentThread(
+    HardwareT & component, unsigned int update_rate,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+  : async_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface)
+  {
+  }
+
+  AsyncComponentThread(const AsyncComponentThread & t) = delete;
+  AsyncComponentThread(AsyncComponentThread && t) = default;
+
+  ~AsyncComponentThread()
+  {
+    terminated_.store(true, std::memory_order_seq_cst);
+    if (write_and_read_.joinable())
+    {
+      write_and_read_.join();
+    }
+  }
+
+  void start() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); }
+
+  void write_and_read()
+  {
+    using TimePoint = std::chrono::system_clock::time_point;
+
+    auto previous_time = clock_interface_->get_clock()->now();
+    while (!terminated_.load(std::memory_order_relaxed))
+    {
+      auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_);
+      TimePoint next_iteration_time =
+        TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds()));
+
+      if (async_component_.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+      {
+        auto current_time = clock_interface_->get_clock()->now();
+        auto measured_period = current_time - previous_time;
+        previous_time = current_time;
+
+        if constexpr (!std::is_same_v<hardware_interface::Sensor, HardwareT>)
+        {
+          // write
+        }
+        // read
+      }
+      next_iteration_time += period;
+      std::this_thread::sleep_until(next_iteration_time);
+    }
+  }
+
+private:
+  std::atomic<bool> terminated_{false};
+  HardwareT & async_component_;
+  std::thread write_and_read_{};
+
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
+  unsigned int cm_update_rate_;
+};
+
+};  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -16,25 +16,28 @@
 #define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
 
 #include <memory>
-#include <mutex>
 #include <string>
 #include <unordered_map>
 #include <vector>
 
+#include "hardware_interface/actuator.hpp"
 #include "hardware_interface/hardware_component_info.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
+#include "hardware_interface/sensor.hpp"
+#include "hardware_interface/system.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/lifecycle_state_names.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/node.hpp"
 #include "rclcpp/time.hpp"
 
 namespace hardware_interface
 {
-class ActuatorInterface;
-class SensorInterface;
-class SystemInterface;
 class ResourceStorage;
+class ControllerManager;
 
 struct HardwareReadWriteStatus
 {
@@ -46,7 +49,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 {
 public:
   /// Default constructor for the Resource Manager.
-  ResourceManager();
+  ResourceManager(
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
+    int update_rate = 100);
 
   /// Constructor for the Resource Manager.
   /**
@@ -65,7 +70,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * "autostart_components" and "autoconfigure_components" instead.
    */
   explicit ResourceManager(
-    const std::string & urdf, bool validate_interfaces = true, bool activate_all = false);
+    const std::string & urdf, bool validate_interfaces = true, bool activate_all = false,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
 
   ResourceManager(const ResourceManager &) = delete;
 
@@ -390,6 +396,14 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   bool state_interface_exists(const std::string & key) const;
 
+  /// Creates the background threads for all async components.
+  /**
+   * The threads get stored in the respective map based on the underlying component's type.
+   * Even though the callback is registered, and the thread is running by default, the read and write methods
+   * aren't called until the components get into active state.
+   */
+  void allocate_threads_for_async_components();
+
 private:
   void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
 
@@ -405,6 +419,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 
   // Structure to store read and write status so it is not initialized in the real-time loop
   HardwareReadWriteStatus read_write_status;
+
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_ = nullptr;
+  int cm_update_rate_;
 };
 
 }  // namespace hardware_interface