[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,25 @@ struct ControllerUpdateStats
   unsigned int total_triggers;
   unsigned int failed_triggers;
 };
+
+/**
+ * Struct to store the status of the controller update method.
+ * The status contains information if the update was triggered successfully, the result of the
+ * update method and the execution duration of the update method. The status is used to provide
+ * feedback to the controller_manager.
+ * @var successful: true if the update was triggered successfully, false if not.
+ * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR.
+ * @var execution_time: duration of the execution of the update method.
+ * @var period: period of the update method.
+ */
+struct ControllerUpdateStatus
+{
+  bool successful = 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 +194,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.successful = 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.successful = 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,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
   realtime_tools
   std_msgs
+  libstatistics_collector
+  generate_parameter_library
 )
 
 find_package(ament_cmake REQUIRED)
@@ -27,6 +29,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
+generate_parameter_library(controller_manager_parameters
+  src/controller_manager_parameters.yaml
+)
+
 add_library(controller_manager SHARED
   src/controller_manager.cpp
 )
@@ -35,6 +41,9 @@ target_include_directories(controller_manager PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/controller_manager>
 )
+target_link_libraries(controller_manager PUBLIC
+  controller_manager_parameters
+)
 ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 # Causes the visibility macros to use dllexport rather than dllimport,
@@ -236,7 +245,7 @@ install(
   DESTINATION include/controller_manager
 )
 install(
-  TARGETS controller_manager
+  TARGETS controller_manager controller_manager_parameters
   EXPORT export_controller_manager
   RUNTIME DESTINATION bin
   LIBRARY DESTINATION lib

controller_manager/doc/parameters_context.yaml

@@ -0,0 +1,21 @@
+hardware_components_initial_state: |
+    Map of parameters for controlled lifecycle management of hardware components.
+    The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
+    Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated.
+    Detailed explanation of each parameter is given below.
+    The full structure of the map is given in the following example:
+
+    .. code-block:: yaml
+
+        hardware_components_initial_state:
+          unconfigured:
+            - "arm1"
+            - "arm2"
+          inactive:
+            - "base3"
+
+diagnostics.threshold.controllers.periodicity: |
+  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.
+
+diagnostics.threshold.controllers.execution_time: |
+  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.

controller_manager/doc/userdoc.rst

@@ -57,32 +57,6 @@ robot_description [std_msgs::msg::String]
 Parameters
 -----------
 
-hardware_components_initial_state
-  Map of parameters for controlled lifecycle management of hardware components.
-  The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
-  Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated.
-  Detailed explanation of each parameter is given below.
-  The full structure of the map is given in the following example:
-
-.. code-block:: yaml
-
-    hardware_components_initial_state:
-      unconfigured:
-        - "arm1"
-        - "arm2"
-      inactive:
-        - "base3"
-
-hardware_components_initial_state.unconfigured (optional; list<string>; default: empty)
-  Defines which hardware components will be only loaded immediately when controller manager is started.
-
-hardware_components_initial_state.inactive (optional; list<string>; default: empty)
-  Defines which hardware components will be configured immediately when controller manager is started.
-
-update_rate (mandatory; integer)
-  The frequency of controller manager's real-time update loop.
-  This loop reads states from hardware, updates controller and writes commands to hardware.
-
 <controller_name>.type
   Name of a plugin exported using ``pluginlib`` for a controller.
   This is a class from which controller's instance with name "``controller_name``" is created.
@@ -99,6 +73,16 @@ 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.
 
+.. generate_parameter_library_details::
+  ../src/controller_manager_parameters.yaml
+  parameters_context.yaml
+
+**An example parameter file:**
+
+.. generate_parameter_library_default::
+  ../src/controller_manager_parameters.yaml
+
+
 Handling Multiple Controller Managers
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 

controller_manager/include/controller_manager/controller_manager.hpp

@@ -50,6 +50,8 @@
 
 namespace controller_manager
 {
+class ParamListener;
+class Params;
 using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
 
 CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options();
@@ -346,7 +348,7 @@ class ControllerManager : public rclcpp::Node
 
   // Per controller update rate support
   unsigned int update_loop_counter_ = 0;
-  unsigned int update_rate_ = 100;
+  unsigned int update_rate_;
   std::vector<std::vector<std::string>> chained_controllers_configuration_;
 
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
@@ -357,6 +359,8 @@ class ControllerManager : public rclcpp::Node
     const std::string & command_interface);
   void init_controller_manager();
 
+  void initialize_parameters();
+
   /**
    * Clear request lists used when switching controllers. The lists are shared between "callback"
    * and "control loop" threads.
@@ -473,6 +477,8 @@ class ControllerManager : public rclcpp::Node
    */
   rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const;
 
+  std::shared_ptr<controller_manager::ParamListener> cm_param_listener_;
+  std::shared_ptr<controller_manager::Params> params_;
   diagnostic_updater::Updater diagnostics_updater_;
 
   std::shared_ptr<rclcpp::Executor> executor_;
@@ -603,6 +609,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,8 @@
   <depend>ros2param</depend>
   <depend>ros2run</depend>
   <depend>std_msgs</depend>
+  <depend>libstatistics_collector</depend>
+  <depend>generate_parameter_library</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>