[CM] Async Function Handler for Controllers (#1489)

---------

Co-authored-by: Jordan Palacios <[email protected]>
Co-authored-by: atzaros <[email protected]>
Co-authored-by: Bence Magyar <[email protected]>
Co-authored-by: Dr. Denis <[email protected]>

controller_interface/CMakeLists.txt

@@ -9,6 +9,7 @@ endif()
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   hardware_interface
   rclcpp_lifecycle
+  realtime_tools
 )
 
 find_package(ament_cmake REQUIRED)

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -17,9 +17,11 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "controller_interface/visibility_control.h"
+#include "realtime_tools/async_function_handler.hpp"
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
@@ -58,6 +60,17 @@ struct InterfaceConfiguration
   std::vector<std::string> names = {};
 };
 
+struct ControllerUpdateStats
+{
+  void reset()
+  {
+    total_triggers = 0;
+    failed_triggers = 0;
+  }
+
+  unsigned int total_triggers;
+  unsigned int failed_triggers;
+};
 /**
  * Base interface class  for an controller. The interface may not be used to implement a controller.
  * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface`
@@ -153,6 +166,23 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
+  /**
+   * Trigger update method. This method is used by the controller_manager to trigger the update
+   * method of the controller.
+   * The method is used to trigger the update method of the controller synchronously or
+   * asynchronously, based on the controller configuration.
+   * **The method called in the (real-time) control 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
+   * \returns A pair with the first element being a boolean indicating if the async callback method
+   * was triggered and the second element being the last return value of the async function. For
+   * more details check the AsyncFunctionHandler implementation in `realtime_tools` package.
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  std::pair<bool, return_type> trigger_update(
+    const rclcpp::Time & time, const rclcpp::Duration & period);
+
   CONTROLLER_INTERFACE_PUBLIC
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();
 
@@ -270,15 +300,30 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual bool is_in_chained_mode() const = 0;
 
+  /**
+   * Method to wait for any running async update cycle to finish after finishing the current cycle.
+   * This is needed to be called before deactivating the controller by the controller_manager, so
+   * that the interfaces still exist when the controller finishes its cycle and then it's exits.
+   *
+   * \note **The method is not real-time safe and shouldn't be called in the control loop.**
+   *
+   * If the controller is running in async mode, the method will wait for the current async update
+   * to finish. If the controller is not running in async mode, the method will do nothing.
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  void wait_for_trigger_update_to_finish();
+
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
 
 private:
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
+  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
   unsigned int update_rate_ = 0;
   bool is_async_ = false;
   std::string urdf_ = "";
+  ControllerUpdateStats trigger_stats_;
 };
 
 using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;

controller_interface/package.xml

@@ -12,12 +12,16 @@
   <buildtool_depend>ament_cmake_gen_version_h</buildtool_depend>
 
   <build_depend>hardware_interface</build_depend>
+  <build_depend>realtime_tools</build_depend>
   <build_depend>rclcpp_lifecycle</build_depend>
   <build_depend>sensor_msgs</build_depend>
 
   <build_export_depend>hardware_interface</build_export_depend>
+  <build_export_depend>realtime_tools</build_export_depend>
   <build_export_depend>rclcpp_lifecycle</build_export_depend>
 
+  <exec_depend>realtime_tools</exec_depend>
+
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>geometry_msgs</test_depend>
   <test_depend>sensor_msgs</test_depend>

controller_interface/src/controller_interface_base.cpp

@@ -16,7 +16,6 @@
 
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
 #include "lifecycle_msgs/msg/state.hpp"
@@ -37,6 +36,7 @@ return_type ControllerInterfaceBase::init(
   {
     auto_declare<int>("update_rate", update_rate_);
     auto_declare<bool>("is_async", false);
+    auto_declare<int>("thread_priority", 50);
   }
   catch (const std::exception & e)
   {
@@ -57,7 +57,14 @@ return_type ControllerInterfaceBase::init(
     std::bind(&ControllerInterfaceBase::on_configure, this, std::placeholders::_1));
 
   node_->register_on_cleanup(
-    std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      if (is_async() && async_handler_ && async_handler_->is_running())
+      {
+        async_handler_->stop_thread();
+      }
+      return on_cleanup(previous_state);
+    });
 
   node_->register_on_activate(
     std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1));
@@ -106,6 +113,21 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
     }
     is_async_ = get_node()->get_parameter("is_async").as_bool();
   }
+  if (is_async_)
+  {
+    const unsigned int thread_priority =
+      static_cast<unsigned int>(get_node()->get_parameter("thread_priority").as_int());
+    RCLCPP_INFO(
+      get_node()->get_logger(), "Starting async handler with scheduler priority: %d",
+      thread_priority);
+    async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
+    async_handler_->init(
+      std::bind(
+        &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2),
+      thread_priority);
+    async_handler_->start_thread();
+  }
+  trigger_stats_.reset();
 
   return get_node()->configure();
 }
@@ -129,6 +151,29 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c
   return node_->get_current_state();
 }
 
+std::pair<bool, return_type> ControllerInterfaceBase::trigger_update(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
+{
+  trigger_stats_.total_triggers++;
+  if (is_async())
+  {
+    const auto result = async_handler_->trigger_async_callback(time, period);
+    if (!result.first)
+    {
+      trigger_stats_.failed_triggers++;
+      RCLCPP_WARN_THROTTLE(
+        get_node()->get_logger(), *get_node()->get_clock(), 20000,
+        "The controller missed %u update cycles out of %u total triggers.",
+        trigger_stats_.failed_triggers, trigger_stats_.total_triggers);
+    }
+    return result;
+  }
+  else
+  {
+    return std::make_pair(true, update(time, period));
+  }
+}
+
 std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node()
 {
   if (!node_.get())
@@ -153,4 +198,11 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; }
 
 const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; }
 
+void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
+{
+  if (is_async() && async_handler_ && async_handler_->is_running())
+  {
+    async_handler_->wait_for_trigger_cycle_to_finish();
+  }
+}
 }  // namespace controller_interface

controller_manager/include/controller_manager/controller_manager.hpp

@@ -22,7 +22,6 @@
 #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"
@@ -628,9 +627,6 @@ class ControllerManager : public rclcpp::Node
   };
 
   SwitchParams switch_params_;
-
-  std::unordered_map<std::string, std::unique_ptr<controller_interface::AsyncControllerThread>>
-    async_controller_threads_;
 };
 
 }  // namespace controller_manager