Async Function Handler for Controllers

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"
@@ -153,6 +155,21 @@ 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 return_type::OK if update is successfully, otherwise return_type::ERROR.
+ */
+ 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();
 
@@ -268,12 +285,26 @@ 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_ = "";

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>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"
@@ -36,6 +35,7 @@ return_type ControllerInterfaceBase::init(
  {
  auto_declare<int>("update_rate", cm_update_rate);
  auto_declare<bool>("is_async", false);
+ auto_declare<int>("thread_priority", 50);
  }
  catch (const std::exception & e)
  {
@@ -56,7 +56,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));
@@ -88,6 +95,20 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
  update_rate_ = static_cast<unsigned int>(get_node()->get_parameter("update_rate").as_int());
  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_STREAM(
+ get_node()->get_logger(),
+ "Starting async handler with scheduler priority: " << 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();
+ }
 
  return get_node()->configure();
 }
@@ -111,6 +132,19 @@ 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)
+{
+ if (is_async())
+ {
+ return async_handler_->trigger_async_callback(time, period);
+ }
+ else
+ {
+ return std::make_pair(true, update(time, period));
+ }
+}
+
 std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node()
 {
  if (!node_.get())
@@ -135,4 +169,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"
@@ -606,9 +605,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