Working async controllers and components [not synchronized] (#1041)

(cherry picked from commit 2cbe470)

# Conflicts:
#	controller_manager/include/controller_manager/controller_manager.hpp
#	controller_manager/src/controller_manager.cpp
#	hardware_interface/include/hardware_interface/async_components.hpp
#	hardware_interface/src/resource_manager.cpp

controller_interface/include/controller_interface/async_controller.hpp

@@ -0,0 +1,111 @@
+// Copyright 2024 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 CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_
+#define CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_
+
+#include <atomic>
+#include <memory>
+#include <thread>
+
+#include "controller_interface_base.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+
+namespace controller_interface
+{
+
+class AsyncControllerThread
+{
+public:
+  /// Constructor for the AsyncControllerThread object.
+  /**
+   *
+   * \param[in] controller shared pointer to a controller.
+   * \param[in] cm_update_rate the controller manager's update rate.
+   */
+  AsyncControllerThread(
+    std::shared_ptr<controller_interface::ControllerInterfaceBase> & controller, int cm_update_rate)
+  : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate)
+  {
+  }
+
+  AsyncControllerThread(const AsyncControllerThread & t) = delete;
+  AsyncControllerThread(AsyncControllerThread && t) = delete;
+
+  // Destructor, called when the component is erased from its map.
+  ~AsyncControllerThread()
+  {
+    terminated_.store(true, std::memory_order_seq_cst);
+    if (thread_.joinable())
+    {
+      thread_.join();
+    }
+  }
+
+  /// Creates the controller's thread.
+  /**
+   * Called when the controller is activated.
+   *
+   */
+  void activate()
+  {
+    thread_ = std::thread(&AsyncControllerThread::controller_update_callback, this);
+  }
+
+  /// Periodically execute the controller's update method.
+  /**
+   * Callback of the async controller's thread.
+   * **Not synchronized with the controller manager's write and read currently**
+   *
+   */
+  void controller_update_callback()
+  {
+    using TimePoint = std::chrono::system_clock::time_point;
+    unsigned int used_update_rate =
+      controller_->get_update_rate() == 0 ? cm_update_rate_ : controller_->get_update_rate();
+
+    auto previous_time = controller_->get_node()->now();
+    while (!terminated_.load(std::memory_order_relaxed))
+    {
+      auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate);
+      TimePoint next_iteration_time =
+        TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds()));
+
+      if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+      {
+        auto const current_time = controller_->get_node()->now();
+        auto const measured_period = current_time - previous_time;
+        previous_time = current_time;
+        controller_->update(
+          controller_->get_node()->now(),
+          (controller_->get_update_rate() != cm_update_rate_ && controller_->get_update_rate() != 0)
+            ? rclcpp::Duration::from_seconds(1.0 / controller_->get_update_rate())
+            : measured_period);
+      }
+
+      next_iteration_time += period;
+      std::this_thread::sleep_until(next_iteration_time);
+    }
+  }
+
+private:
+  std::atomic<bool> terminated_;
+  std::shared_ptr<controller_interface::ControllerInterfaceBase> controller_;
+  std::thread thread_;
+  unsigned int cm_update_rate_;
+};
+
+}  // namespace controller_interface
+
+#endif  // CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_

controller_manager/include/controller_manager/controller_manager.hpp

@@ -23,6 +23,7 @@
 #include <utility>
 #include <vector>
 
+#include "controller_interface/async_controller.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "controller_interface/controller_interface.hpp"
 #include "controller_interface/controller_interface_base.hpp"
@@ -194,6 +195,15 @@ 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();
@@ -520,6 +530,12 @@ class ControllerManager : public rclcpp::Node
   };
 
   SwitchParams switch_params_;
+<<<<<<< HEAD
+=======
+
+  std::unordered_map<std::string, std::unique_ptr<controller_interface::AsyncControllerThread>>
+    async_controller_threads_;
+>>>>>>> 2cbe470 (Working async controllers and components [not synchronized] (#1041))
 };
 
 }  // namespace controller_manager

controller_manager/src/controller_manager.cpp

@@ -744,6 +744,24 @@ controller_interface::return_type ControllerManager::configure_controller(
       controller_name.c_str(), new_state.label().c_str());
     return controller_interface::return_type::ERROR;
   }
+<<<<<<< HEAD
+=======
+  catch (...)
+  {
+    RCLCPP_ERROR(
+      get_logger(), "Caught unknown exception while configuring controller '%s'",
+      controller_name.c_str());
+    return controller_interface::return_type::ERROR;
+  }
+
+  // ASYNCHRONOUS CONTROLLERS: Start background thread for update
+  if (controller->is_async())
+  {
+    async_controller_threads_.emplace(
+      controller_name,
+      std::make_unique<controller_interface::AsyncControllerThread>(controller, update_rate_));
+  }
+>>>>>>> 2cbe470 (Working async controllers and components [not synchronized] (#1041))
 
   const auto controller_update_rate = controller->get_update_rate();
   const auto cm_update_rate = get_update_rate();
@@ -2135,6 +2153,13 @@ 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()
+{
+  async_controller_threads_.erase(
+    async_controller_threads_.begin(), async_controller_threads_.end());
+  resource_manager_->shutdown_async_components();
+}
+
 void ControllerManager::propagate_deactivation_of_chained_mode(
   const std::vector<ControllerSpec> & controllers)
 {

controller_manager/src/ros2_control_node.cpp

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

hardware_interface/include/hardware_interface/async_components.hpp

@@ -0,0 +1,122 @@
+// 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 <type_traits>
+#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(
+    unsigned int update_rate,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+  : cm_update_rate_(update_rate), clock_interface_(clock_interface)
+  {
+  }
+
+  // Fills the internal variant with the desired component.
+  template <typename T>
+  void register_component(T * component)
+  {
+    hardware_component_ = component;
+  }
+
+  AsyncComponentThread(const AsyncComponentThread & t) = delete;
+  AsyncComponentThread(AsyncComponentThread && t) = delete;
+
+  // Destructor, called when the component is erased from its map.
+  ~AsyncComponentThread()
+  {
+    terminated_.store(true, std::memory_order_seq_cst);
+    if (write_and_read_.joinable())
+    {
+      write_and_read_.join();
+    }
+  }
+  /// Creates the component's thread.
+  /**
+   * Called when the component is activated.
+   *
+   */
+  void activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); }
+
+  /// Periodically execute the component's write and read methods.
+  /**
+   * Callback of the async component's thread.
+   * **Not synchronized with the controller manager's update currently**
+   *
+   */
+  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;
+
+            if (!first_iteration)
+            {
+              component->write(clock_interface_->get_clock()->now(), measured_period);
+            }
+            component->read(clock_interface_->get_clock()->now(), measured_period);
+            first_iteration = false;
+          }
+          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_;
+  bool first_iteration = true;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
+};
+
+};  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -369,6 +369,12 @@ 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/src/resource_manager.cpp

@@ -193,6 +193,18 @@ class ResourceStorage
             hardware.get_name().c_str(), interface.c_str());
         }
       }
+<<<<<<< HEAD
+=======
+
+      if (hardware_info_map_[hardware.get_name()].is_async)
+      {
+        async_component_threads_.emplace(
+          std::piecewise_construct, std::forward_as_tuple(hardware.get_name()),
+          std::forward_as_tuple(cm_update_rate_, clock_interface_));
+
+        async_component_threads_.at(hardware.get_name()).register_component(&hardware);
+      }
+>>>>>>> 2cbe470 (Working async controllers and components [not synchronized] (#1041))
     }
     return result;
   }
@@ -1228,6 +1240,13 @@ return_type ResourceManager::set_component_state(
   return result;
 }
 
+void ResourceManager::shutdown_async_components()
+{
+  resource_storage_->async_component_threads_.erase(
+    resource_storage_->async_component_threads_.begin(),
+    resource_storage_->async_component_threads_.end());
+}
+
 // CM API: Called in "update"-thread
 HardwareReadWriteStatus ResourceManager::read(
   const rclcpp::Time & time, const rclcpp::Duration & period)