Publish controller manager statistics to better introspect the timings (#2449)

(cherry picked from commit ddb7fa9)

Author: saikishor

controller_manager/include/controller_manager/controller_manager.hpp

@@ -674,6 +674,21 @@ class ControllerManager : public rclcpp::Node
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
   rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
 
+  struct ControllerManagerExecutionTime
+  {
+    double read_time = 0.0;
+    double update_time = 0.0;
+    double write_time = 0.0;
+    double switch_time = 0.0;
+    double total_time = 0.0;
+    double switch_chained_mode_time = 0.0;
+    double switch_perform_mode_time = 0.0;
+    double deactivation_time = 0.0;
+    double activation_time = 0.0;
+  };
+
+  ControllerManagerExecutionTime execution_time_;
+
   controller_manager::MovingAverageStatistics periodicity_stats_;
 
   struct SwitchParams

controller_manager/include/controller_manager/controller_spec.hpp

@@ -24,13 +24,12 @@
 #include <vector>
 #include "controller_interface/controller_interface_base.hpp"
 #include "hardware_interface/controller_info.hpp"
-#include "libstatistics_collector/moving_average_statistics/moving_average.hpp"
+#include "hardware_interface/types/statistics_types.hpp"
 
 namespace controller_manager
 {
 
-using MovingAverageStatistics =
-  libstatistics_collector::moving_average_statistics::MovingAverageStatistics;
+using MovingAverageStatistics = ros2_control::MovingAverageStatistics;
 /// Controller Specification
 /**
  * This struct contains both a pointer to a given controller, \ref c, as well

controller_manager/src/controller_manager.cpp

@@ -912,21 +912,21 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
       testing::AllOf(testing::Ge(0.7 / cm_update_rate), testing::Lt((1.6 / cm_update_rate))));
     ASSERT_EQ(
       test_controller->internal_counter,
-      cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount());
+      cm_->get_loaded_controllers()[0].execution_time_statistics->get_count());
     ASSERT_EQ(
       test_controller->internal_counter - 1,
-      cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount())
+      cm_->get_loaded_controllers()[0].periodicity_statistics->get_count())
       << "The first update is not counted in periodicity statistics";
     EXPECT_THAT(
-      cm_->get_loaded_controllers()[0].periodicity_statistics->Average(),
+      cm_->get_loaded_controllers()[0].periodicity_statistics->get_average(),
       testing::AllOf(
         testing::Ge(0.90 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate()))));
     EXPECT_THAT(
-      cm_->get_loaded_controllers()[0].periodicity_statistics->Min(),
+      cm_->get_loaded_controllers()[0].periodicity_statistics->get_min(),
       testing::AllOf(
         testing::Ge(0.70 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate()))));
     EXPECT_THAT(
-      cm_->get_loaded_controllers()[0].periodicity_statistics->Max(),
+      cm_->get_loaded_controllers()[0].periodicity_statistics->get_max(),
       testing::AllOf(
         testing::Ge(0.75 * cm_->get_update_rate()), testing::Lt((2.0 * cm_->get_update_rate()))));
     loop_rate.sleep();
@@ -1085,22 +1085,22 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
         actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1)));
       ASSERT_EQ(
         test_controller->internal_counter,
-        cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount());
+        cm_->get_loaded_controllers()[0].execution_time_statistics->get_count());
       ASSERT_EQ(
         test_controller->internal_counter - 1,
-        cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount())
+        cm_->get_loaded_controllers()[0].periodicity_statistics->get_count())
         << "The first update is not counted in periodicity statistics";
       EXPECT_THAT(
-        cm_->get_loaded_controllers()[0].periodicity_statistics->Average(),
+        cm_->get_loaded_controllers()[0].periodicity_statistics->get_average(),
         testing::AllOf(testing::Ge(0.92 * exp_periodicity), testing::Lt((1.05 * exp_periodicity))));
       EXPECT_THAT(
-        cm_->get_loaded_controllers()[0].periodicity_statistics->Min(),
+        cm_->get_loaded_controllers()[0].periodicity_statistics->get_min(),
         testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
       EXPECT_THAT(
-        cm_->get_loaded_controllers()[0].periodicity_statistics->Max(),
+        cm_->get_loaded_controllers()[0].periodicity_statistics->get_max(),
         testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((2.0 * exp_periodicity))));
       EXPECT_LT(
-        cm_->get_loaded_controllers()[0].execution_time_statistics->Average(),
+        cm_->get_loaded_controllers()[0].execution_time_statistics->get_average(),
         50.0);  // 50 microseconds
     }
   }
@@ -1231,22 +1231,22 @@ TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_an
       EXPECT_THAT(
         actual_counter, testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter + 1)));
       EXPECT_THAT(
-        cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount(),
+        cm_->get_loaded_controllers()[0].execution_time_statistics->get_count(),
         testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter)));
       EXPECT_THAT(
-        cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount(),
+        cm_->get_loaded_controllers()[0].periodicity_statistics->get_count(),
         testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter)));
       EXPECT_THAT(
-        cm_->get_loaded_controllers()[0].periodicity_statistics->Average(),
+        cm_->get_loaded_controllers()[0].periodicity_statistics->get_average(),
         testing::AllOf(testing::Ge(0.9 * exp_periodicity), testing::Lt((1.1 * exp_periodicity))));
       EXPECT_THAT(
-        cm_->get_loaded_controllers()[0].periodicity_statistics->Min(),
+        cm_->get_loaded_controllers()[0].periodicity_statistics->get_min(),
         testing::AllOf(testing::Ge(0.5 * exp_periodicity), testing::Lt((1.2 * exp_periodicity))));
       EXPECT_THAT(
-        cm_->get_loaded_controllers()[0].periodicity_statistics->Max(),
+        cm_->get_loaded_controllers()[0].periodicity_statistics->get_max(),
         testing::AllOf(testing::Ge(0.75 * exp_periodicity), testing::Lt((2.0 * exp_periodicity))));
       EXPECT_LT(
-        cm_->get_loaded_controllers()[0].execution_time_statistics->Average(),
+        cm_->get_loaded_controllers()[0].execution_time_statistics->get_average(),
         12000);  // more or less 12 milliseconds considering the waittime in the controller
     }
   }

controller_manager/test/test_controller_manager.cpp

@@ -19,6 +19,15 @@ The topic ``~/introspection_data/full`` can be used to integrate with your custo
 .. note::
   If you have a high frequency of data, it is recommended to use the ``~/introspection_data/names`` and ``~/introspection_data/values`` topic. So, that the data transferred and stored is minimized.
 
+Along with the above introspection data, the ``controller_manager`` also publishes the statistics of the execution time and periodicity of the read and write cycles of the hardware components and the update cycle of the controllers. This is done by registering the statistics of these variables and publishing them on the ``~/statistics`` topic.
+
+All the registered variables are published over 3 topics: ``~/statistics/full``, ``~/statistics/names``, and ``~/statistics/values``.
+- The ``~/statistics/full`` topic publishes the full introspection data along with names and values in a single message. This can be useful to track or view variables and information from command line.
+- The ``~/statistics/names`` topic publishes the names of the registered variables. This topic contains the names of the variables registered. This is only published every time a a variables is registered and unregistered.
+- The ``~/statistics/values`` topic publishes the values of the registered variables. This topic contains the values of the variables registered.
+
+This topic is mainly used to introspect the behaviour of the realtime loops, this is very crucial for hardware that need to meet strict deadlines and also to understand the which component of the ecosystem is consuming more time in the realtime loop.
+
 How to introspect internal variables of controllers and hardware components
 ============================================================================
 

doc/introspection.rst

@@ -95,6 +95,7 @@ controller_manager
 * The controller manager will use a monotonic clock for triggering read-update-write cycles, but when the ``use_sim_time`` parameter is set to true, it will use the ROS Clock for triggering. When monotonic clock is being used, all the hardware components will receive the monotonic time in their read and write method, instead the controllers will always receive the ROS time in their update method irrespective of the clock being used. (`#2046 <https://github.com/ros-controls/ros2_control/pull/2046>`_).
 * The default strictness of the ``switch_controllers`` can now we be chosen using ROS 2 parameters. The default behaviour is still left to ``BEST_EFFORT`` (`#2168 <https://github.com/ros-controls/ros2_control/pull/2168>`_).
 * Parameter ``shutdown_on_initial_state_failure`` was added to avoid shutting down on hardware initial state failure  (`#2230 <https://github.com/ros-controls/ros2_control/pull/2230>`_).
+* The controller manager now publishes ``~/statistics/names`` and ``~/statistics/values`` topics to introspect the execution time and periodicity of the different entities running in the realtime loop (`#2449 <https://github.com/ros-controls/ros2_control/pull/2449>`_).
 
 hardware_interface
 ******************

doc/release_notes.rst

@@ -17,13 +17,37 @@
 #ifndef HARDWARE_INTERFACE__INTROSPECTION_HPP_
 #define HARDWARE_INTERFACE__INTROSPECTION_HPP_
 
+#include <string>
+
+#include "hardware_interface/types/statistics_types.hpp"
 #include "pal_statistics/pal_statistics_macros.hpp"
 #include "pal_statistics/pal_statistics_utils.hpp"
 
+namespace pal_statistics
+{
+template <>
+inline IdType customRegister(
+  StatisticsRegistry & registry, const std::string & name,
+  const libstatistics_collector::moving_average_statistics::StatisticData * variable,
+  RegistrationsRAII * bookkeeping, bool enabled)
+{
+  registry.registerVariable(name + "/max", &variable->max, bookkeeping, enabled);
+  registry.registerVariable(name + "/min", &variable->min, bookkeeping, enabled);
+  registry.registerVariable(name + "/average", &variable->average, bookkeeping, enabled);
+  registry.registerVariable(
+    name + "/standard_deviation", &variable->standard_deviation, bookkeeping, enabled);
+  std::function<double()> sample_func = [variable]
+  { return static_cast<double>(variable->sample_count); };
+  return registry.registerFunction(name + "/sample_count", sample_func, bookkeeping, enabled);
+}
+}  // namespace pal_statistics
+
 namespace hardware_interface
 {
 constexpr char DEFAULT_REGISTRY_KEY[] = "ros2_control";
 constexpr char DEFAULT_INTROSPECTION_TOPIC[] = "~/introspection_data";
+constexpr char CM_STATISTICS_KEY[] = "cm_execution_statistics";
+constexpr char CM_STATISTICS_TOPIC[] = "~/statistics";
 
 #define REGISTER_ROS2_CONTROL_INTROSPECTION(ID, ENTITY)                      \
   REGISTER_ENTITY(                                                           \