Merge branch 'master' into sdf-compatibility

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