Add class for thread management of async hw interfaces (#981)

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>(
+    update_rate_, this->get_node_clock_interface())),
   diagnostics_updater_(this),
   executor_(executor),
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(

hardware_interface/include/hardware_interface/async_components.hpp

@@ -0,0 +1,112 @@
+// Copyright 2023 ros2_control development team
+//
+// 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 <variant>
+
+#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
+{
+
+class AsyncComponentThread
+{
+public:
+  explicit AsyncComponentThread(
+    Actuator * component, unsigned int update_rate,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+  : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface)
+  {
+  }
+
+  explicit AsyncComponentThread(
+    System * component, unsigned int update_rate,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+  : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface)
+  {
+  }
+
+  explicit AsyncComponentThread(
+    Sensor * component, unsigned int update_rate,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+  : hardware_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 activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); }
+
+  void write_and_read()
+  {
+    using TimePoint = std::chrono::system_clock::time_point;
+
+    std::visit(
+      [this](auto & component)
+      {
+        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 (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;
+
+            // write
+            // read
+          }
+          next_iteration_time += period;
+          std::this_thread::sleep_until(next_iteration_time);
+        }
+      },
+      hardware_component_);
+  }
+
+private:
+  std::atomic<bool> terminated_{false};
+  std::variant<Actuator *, System *, Sensor *> hardware_component_;
+  std::thread write_and_read_{};
+
+  unsigned int cm_update_rate_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
+};
+
+};  // 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(
+    unsigned int update_rate = 100,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
 
   /// Constructor for the Resource Manager.
   /**
@@ -65,7 +70,9 @@ 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,
+    unsigned int update_rate = 100,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
 
   ResourceManager(const ResourceManager &) = delete;
 

hardware_interface/include/hardware_interface/sensor.hpp

@@ -77,6 +77,9 @@ class Sensor final
   HARDWARE_INTERFACE_PUBLIC
   return_type read(const rclcpp::Time & time, const rclcpp::Duration & period);
 
+  HARDWARE_INTERFACE_PUBLIC
+  return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; }
+
 private:
   std::unique_ptr<SensorInterface> impl_;
 };