[Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -71,6 +71,15 @@ struct ControllerUpdateStats
   unsigned int total_triggers;
   unsigned int failed_triggers;
 };
+
+struct ControllerUpdateStatus
+{
+  bool ok = true;
+  return_type result = return_type::OK;
+  std::optional<std::chrono::nanoseconds> execution_time = std::nullopt;
+  std::optional<rclcpp::Duration> period = std::nullopt;
+};
+
 /**
  * Base interface class  for an controller. The interface may not be used to implement a controller.
  * The class provides definitions for `ControllerInterface` and `ChainableControllerInterface`
@@ -175,13 +184,11 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    *
    * \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.
+   * \returns ControllerUpdateStatus. The status contains information if the update was triggered
+   * successfully, the result of the update method and the execution duration of the update method.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  std::pair<bool, return_type> trigger_update(
-    const rclcpp::Time & time, const rclcpp::Duration & period);
+  ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period);
 
   CONTROLLER_INTERFACE_PUBLIC
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();

controller_interface/src/controller_interface_base.cpp

@@ -159,12 +159,14 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c
   return node_->get_current_state();
 }
 
-std::pair<bool, return_type> ControllerInterfaceBase::trigger_update(
+ControllerUpdateStatus ControllerInterfaceBase::trigger_update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  ControllerUpdateStatus status;
   trigger_stats_.total_triggers++;
   if (is_async())
   {
+    const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time();
     const auto result = async_handler_->trigger_async_callback(time, period);
     if (!result.first)
     {
@@ -174,12 +176,28 @@ std::pair<bool, return_type> ControllerInterfaceBase::trigger_update(
         "The controller missed %u update cycles out of %u total triggers.",
         trigger_stats_.failed_triggers, trigger_stats_.total_triggers);
     }
-    return result;
+    status.ok = result.first;
+    status.result = result.second;
+    const auto execution_time = async_handler_->get_last_execution_time();
+    if (execution_time.count() > 0)
+    {
+      status.execution_time = execution_time;
+    }
+    if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED)
+    {
+      status.period = time - last_trigger_time;
+    }
   }
   else
   {
-    return std::make_pair(true, update(time, period));
+    const auto start_time = std::chrono::steady_clock::now();
+    status.ok = true;
+    status.result = update(time, period);
+    status.execution_time = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      std::chrono::steady_clock::now() - start_time);
+    status.period = period;
   }
+  return status;
 }
 
 std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node()

controller_manager/CMakeLists.txt

@@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
   realtime_tools
   std_msgs
+  libstatistics_collector
 )
 
 find_package(ament_cmake REQUIRED)

controller_manager/doc/userdoc.rst

@@ -99,6 +99,59 @@ update_rate (mandatory; integer)
   The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation.
   It is recommended to test the fallback strategy in simulation before deploying it on the real robot.
 
+diagnostics.threshold.controller_manager.periodicity.mean_error.warn
+  The warning threshold for the mean error of the controller manager's periodicity.
+  If the mean error exceeds this threshold, a warning diagnostic will be published.
+
+diagnostics.threshold.controller_manager.periodicity.mean_error.error
+  The error threshold for the mean error of the controller manager's periodicity.
+  If the mean error exceeds this threshold, an error diagnostic will be published.
+
+diagnostics.threshold.controller_manager.periodicity.standard_deviation.warn
+  The warning threshold for the standard deviation of the controller manager's periodicity.
+  If the standard deviation exceeds this threshold, a warning diagnostic will be published.
+
+diagnostics.threshold.controller_manager.periodicity.standard_deviation.error
+  The error threshold for the standard deviation of the controller manager's periodicity.
+  If the standard deviation exceeds this threshold, an error diagnostic will be published.
+
+diagnostics.threshold.controllers.periodicity.mean_error.warn
+  The warning threshold for the mean error of the controller update loop.
+  If the mean error exceeds this threshold, a warning diagnostic will be published.
+
+diagnostics.threshold.controllers.periodicity.mean_error.error
+  The error threshold for the mean error of the controller update loop.
+  If the mean error exceeds this threshold, an error diagnostic will be published.
+
+diagnostics.threshold.controllers.periodicity.standard_deviation.warn
+  The warning threshold for the standard deviation of the controller update loop.
+  If the standard deviation exceeds this threshold, a warning diagnostic will be published.
+
+diagnostics.threshold.controllers.periodicity.standard_deviation.error
+  The error threshold for the standard deviation of the controller update loop.
+  If the standard deviation exceeds this threshold, an error diagnostic will be published.
+
+diagnostics.threshold.controllers.execution_time.mean_error.warn
+  The warning threshold for the mean error of the controller execution time.
+  If the mean error exceeds this threshold, a warning diagnostic will be published.
+
+diagnostics.threshold.controllers.execution_time.mean_error.error
+  The error threshold for the mean error of the controller execution time.
+  If the mean error exceeds this threshold, an error diagnostic will be published.
+
+diagnostics.threshold.controllers.execution_time.standard_deviation.warn
+  The warning threshold for the standard deviation of the controller execution time.
+  If the standard deviation exceeds this threshold, a warning diagnostic will be published.
+
+diagnostics.threshold.controllers.execution_time.standard_deviation.error
+  The error threshold for the standard deviation of the controller execution time.
+  If the standard deviation exceeds this threshold, an error diagnostic will be published.
+
+.. note::
+  The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.
+
+  The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle.
+
 Handling Multiple Controller Managers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 

controller_manager/include/controller_manager/controller_manager.hpp

@@ -603,6 +603,8 @@ class ControllerManager : public rclcpp::Node
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
   rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
 
+  controller_manager::MovingAverageStatistics periodicity_stats_;
+
   struct SwitchParams
   {
     void reset()

controller_manager/include/controller_manager/controller_spec.hpp

@@ -24,9 +24,13 @@
 #include <vector>
 #include "controller_interface/controller_interface_base.hpp"
 #include "hardware_interface/controller_info.hpp"
+#include "libstatistics_collector/moving_average_statistics/moving_average.hpp"
 
 namespace controller_manager
 {
+
+using MovingAverageStatistics =
+  libstatistics_collector::moving_average_statistics::MovingAverageStatistics;
 /// Controller Specification
 /**
  * This struct contains both a pointer to a given controller, \ref c, as well
@@ -35,9 +39,18 @@ namespace controller_manager
  */
 struct ControllerSpec
 {
+  ControllerSpec()
+  {
+    last_update_cycle_time = std::make_shared<rclcpp::Time>(0, 0, RCL_CLOCK_UNINITIALIZED);
+    execution_time_statistics = std::make_shared<MovingAverageStatistics>();
+    periodicity_statistics = std::make_shared<MovingAverageStatistics>();
+  }
+
   hardware_interface::ControllerInfo info;
   controller_interface::ControllerInterfaceBaseSharedPtr c;
   std::shared_ptr<rclcpp::Time> last_update_cycle_time;
+  std::shared_ptr<MovingAverageStatistics> execution_time_statistics;
+  std::shared_ptr<MovingAverageStatistics> periodicity_statistics;
 };
 
 struct ControllerChainSpec

controller_manager/package.xml

@@ -28,6 +28,7 @@
   <depend>ros2param</depend>
   <depend>ros2run</depend>
   <depend>std_msgs</depend>
+  <depend>libstatistics_collector</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>