diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 88a749f078..7584f1b228 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -674,6 +674,21 @@ class ControllerManager : public rclcpp::Node rclcpp::Subscription::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 diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 9cc508772d..cb7a31cfe9 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -24,13 +24,12 @@ #include #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 diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 18a22caad9..2dc90aaec7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -110,7 +110,7 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const * \return true if interface has a controller name as prefix, false otherwise. */ bool is_interface_a_chained_interface( - const std::string & interface_name, + const std::string interface_name, const std::vector & controllers, controller_manager::ControllersListIterator & following_controller_it) { @@ -316,7 +316,7 @@ controller_interface::return_type evaluate_switch_result( { std::string list = std::accumulate( std::next(deactivate_list.begin()), deactivate_list.end(), deactivate_list.front(), - [](const std::string & a, const std::string & b) { return a + " " + b; }); + [](std::string a, std::string b) { return a + " " + b; }); const std::string info_msg = fmt::format(FMT_COMPILE("Deactivated controllers: [ {} ]"), list); message += "\n" + info_msg; @@ -326,7 +326,7 @@ controller_interface::return_type evaluate_switch_result( { std::string list = std::accumulate( std::next(activate_list.begin()), activate_list.end(), activate_list.front(), - [](const std::string & a, const std::string & b) { return a + " " + b; }); + [](std::string a, std::string b) { return a + " " + b; }); const std::string info_msg = fmt::format(FMT_COMPILE("Activated controllers: [ {} ]"), list); message += "\n" + info_msg; @@ -356,6 +356,23 @@ void get_controller_list_command_interfaces( } } } + +void register_controller_manager_statistics( + const std::string & name, + const libstatistics_collector::moving_average_statistics::StatisticData * variable) +{ + REGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name, variable); +} + +void unregister_controller_manager_statistics(const std::string & name) +{ + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/max"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/min"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/average"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/standard_deviation"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/sample_count"); + UNREGISTER_ENTITY(hardware_interface::CM_STATISTICS_KEY, name + "/current_value"); +} } // namespace namespace controller_manager @@ -410,14 +427,8 @@ ControllerManager::ControllerManager( robot_description_(urdf) { initialize_parameters(); - hardware_interface::ResourceManagerParams params; - params.robot_description = urdf; - params.clock = trigger_clock_; - params.logger = this->get_logger(); - params.activate_all = activate_all_hw_components; - params.update_rate = static_cast(params_->update_rate); - params.executor = executor_; - resource_manager_ = std::make_unique(params, true); + resource_manager_ = std::make_unique( + urdf, trigger_clock_, this->get_logger(), activate_all_hw_components, params_->update_rate); init_controller_manager(); } @@ -523,7 +534,7 @@ void ControllerManager::init_controller_manager() robot_description_subscription_->get_topic_name()); // Setup diagnostics - periodicity_stats_.Reset(); + periodicity_stats_.reset(); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); @@ -538,6 +549,9 @@ void ControllerManager::init_controller_manager() this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC, hardware_interface::DEFAULT_REGISTRY_KEY); START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY); + INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY( + this, hardware_interface::CM_STATISTICS_TOPIC, hardware_interface::CM_STATISTICS_KEY); + START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::CM_STATISTICS_KEY); // Add on_shutdown callback to stop the controller manager rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context(); @@ -726,6 +740,61 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript } } robot_description_notification_timer_->cancel(); + + auto hw_components_info = resource_manager_->get_components_status(); + + for (const auto & [component_name, component_info] : hw_components_info) + { + if (component_name.empty()) + { + RCLCPP_WARN( + get_logger(), "Component name is empty, skipping statistics registration for it."); + continue; + } + if (!component_info.read_statistics && !component_info.write_statistics) + { + RCLCPP_WARN( + get_logger(), + "Component '%s' does not have read or write statistics initialized, skipping registration.", + component_name.c_str()); + continue; + } + RCLCPP_INFO(get_logger(), "Registering statistics for : %s", component_name.c_str()); + const std::string read_cycle_exec_time_prefix = + component_name + ".stats/read_cycle/execution_time"; + const std::string read_cycle_periodicity_prefix = + component_name + ".stats/read_cycle/periodicity"; + register_controller_manager_statistics( + read_cycle_exec_time_prefix, + &component_info.read_statistics->execution_time.get_statistics()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, read_cycle_exec_time_prefix + "/current_value", + &component_info.read_statistics->execution_time.get_current_data()); + register_controller_manager_statistics( + read_cycle_periodicity_prefix, &component_info.read_statistics->periodicity.get_statistics()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, read_cycle_periodicity_prefix + "/current_value", + &component_info.read_statistics->periodicity.get_current_data()); + if (component_info.write_statistics) + { + const std::string write_cycle_exec_time_prefix = + component_name + ".stats/write_cycle/execution_time"; + const std::string write_cycle_periodicity_prefix = + component_name + ".stats/write_cycle/periodicity"; + register_controller_manager_statistics( + write_cycle_exec_time_prefix, + &component_info.write_statistics->execution_time.get_statistics()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, write_cycle_exec_time_prefix + "/current_value", + &component_info.write_statistics->execution_time.get_current_data()); + register_controller_manager_statistics( + write_cycle_periodicity_prefix, + &component_info.write_statistics->periodicity.get_statistics()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, write_cycle_periodicity_prefix + "/current_value", + &component_info.write_statistics->periodicity.get_current_data()); + } + } } void ControllerManager::init_services() @@ -780,6 +849,30 @@ void ControllerManager::init_services() "~/set_hardware_component_state", std::bind(&ControllerManager::set_hardware_component_state_srv_cb, this, _1, _2), qos_services, best_effort_callback_group_); + + const std::string cm_name = get_name(); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".update_time", &execution_time_.update_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".read_time", &execution_time_.read_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".write_time", &execution_time_.write_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".total_time", &execution_time_.total_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".switch_time", &execution_time_.switch_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".switch_chained_mode_time", + &execution_time_.switch_chained_mode_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".switch_perform_mode_time", + &execution_time_.switch_perform_mode_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".deactivation_time", + &execution_time_.deactivation_time); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, cm_name + ".activation_time", + &execution_time_.activation_time); } controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( @@ -845,6 +938,20 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c std::make_shared(0, 0, this->get_trigger_clock()->get_clock_type()); controller_spec.execution_time_statistics = std::make_shared(); controller_spec.periodicity_statistics = std::make_shared(); + const std::string controller_exec_time_prefix = controller_name + ".stats/execution_time"; + const std::string controller_periodicity_prefix = controller_name + ".stats/periodicity"; + register_controller_manager_statistics( + controller_exec_time_prefix, + &controller_spec.execution_time_statistics->get_statistics_const_ptr()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, controller_exec_time_prefix + "/current_value", + &controller_spec.execution_time_statistics->get_current_measurement_const_ptr()); + register_controller_manager_statistics( + controller_periodicity_prefix, + &controller_spec.periodicity_statistics->get_statistics_const_ptr()); + REGISTER_ENTITY( + hardware_interface::CM_STATISTICS_KEY, controller_periodicity_prefix + "/current_value", + &controller_spec.periodicity_statistics->get_current_measurement_const_ptr()); // We have to fetch the parameters_file at the time of loading the controller, because this way we // can load them at the creation of the LifeCycleNode and this helps in using the features such as @@ -984,6 +1091,8 @@ controller_interface::return_type ControllerManager::unload_controller( get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str()); shutdown_controller(controller); } + unregister_controller_manager_statistics(controller_name + ".stats/execution_time"); + unregister_controller_manager_statistics(controller_name + ".stats/periodicity"); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -1195,7 +1304,7 @@ controller_interface::return_type ControllerManager::configure_controller( { std::string cmd_itfs_str = std::accumulate( std::next(cmd_itfs.begin()), cmd_itfs.end(), cmd_itfs.front(), - [](const std::string & a, const std::string & b) { return a + ", " + b; }); + [](std::string a, std::string b) { return a + ", " + b; }); RCLCPP_ERROR( get_logger(), "The command interfaces of the controller '%s' are not unique. Please make sure that the " @@ -1209,7 +1318,7 @@ controller_interface::return_type ControllerManager::configure_controller( { std::string state_itfs_str = std::accumulate( std::next(state_itfs.begin()), state_itfs.end(), state_itfs.front(), - [](const std::string & a, const std::string & b) { return a + ", " + b; }); + [](std::string a, std::string b) { return a + ", " + b; }); RCLCPP_ERROR( get_logger(), "The state interfaces of the controller '%s' are not unique. Please make sure that the state " @@ -1852,10 +1961,9 @@ controller_interface::return_type ControllerManager::switch_controller_cb( switch_params_.timeout = timeout.to_chrono(); } switch_params_.do_switch = true; - // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - std::unique_lock switch_params_guard(switch_params_.mutex); + std::unique_lock switch_params_guard(switch_params_.mutex, std::defer_lock); if (!switch_params_.cv.wait_for( switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; })) { @@ -2182,8 +2290,8 @@ void ControllerManager::activate_controllers( try { - found_it->periodicity_statistics->Reset(); - found_it->execution_time_statistics->Reset(); + found_it->periodicity_statistics->reset(); + found_it->execution_time_statistics->reset(); const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -2675,7 +2783,8 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - periodicity_stats_.AddMeasurement(1.0 / period.seconds()); + periodicity_stats_.add_measurement(1.0 / period.seconds()); + const auto start_time = std::chrono::steady_clock::now(); auto [result, failed_hardware_names] = resource_manager_->read(time, period); if (result != hardware_interface::return_type::OK) @@ -2699,12 +2808,14 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str()); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - - // As the hardware is in UNCONFIGURED state with error call, no need to prepare or perform - // command mode switch + perform_hardware_command_mode_change( + rt_controller_list, {}, rt_buffer_.deactivate_controllers_list, "read"); deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); // TODO(destogl): do auto-start of broadcasters } + execution_time_.read_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); } void ControllerManager::manage_switch() @@ -2715,22 +2826,39 @@ void ControllerManager::manage_switch() RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle."); return; } + const auto start_time = std::chrono::steady_clock::now(); // Ask hardware interfaces to change mode if (!resource_manager_->perform_command_mode_switch( activate_command_interface_request_, deactivate_command_interface_request_)) { RCLCPP_ERROR(get_logger(), "Error while performing mode switch."); } + execution_time_.switch_perform_mode_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + const auto deact_start_time = std::chrono::steady_clock::now(); deactivate_controllers(rt_controller_list, deactivate_request_); + execution_time_.deactivation_time = + std::chrono::duration(std::chrono::steady_clock::now() - deact_start_time) + .count(); + const auto chain_start_time = std::chrono::steady_clock::now(); switch_chained_mode(to_chained_mode_request_, true); switch_chained_mode(from_chained_mode_request_, false); + RCLCPP_DEBUG( + get_logger(), + "Switching %lu controllers to chained mode and %lu controllers from chained mode", + to_chained_mode_request_.size(), from_chained_mode_request_.size()); + execution_time_.switch_chained_mode_time = + std::chrono::duration(std::chrono::steady_clock::now() - chain_start_time) + .count(); // activate controllers once the switch is fully complete + const auto act_start_time = std::chrono::steady_clock::now(); if (!switch_params_.activate_asap) { activate_controllers(rt_controller_list, activate_request_); @@ -2740,15 +2868,27 @@ void ControllerManager::manage_switch() // activate controllers as soon as their required joints are done switching activate_controllers_asap(rt_controller_list, activate_request_); } + execution_time_.activation_time = + std::chrono::duration(std::chrono::steady_clock::now() - act_start_time) + .count(); // All controllers switched --> switching done switch_params_.do_switch = false; switch_params_.cv.notify_all(); + execution_time_.switch_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); } controller_interface::return_type ControllerManager::update( const rclcpp::Time & time, const rclcpp::Duration & period) { + const auto start_time = std::chrono::steady_clock::now(); + execution_time_.switch_time = 0.0; + execution_time_.switch_chained_mode_time = 0.0; + execution_time_.activation_time = 0.0; + execution_time_.deactivation_time = 0.0; + execution_time_.switch_perform_mode_time = 0.0; std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); @@ -2844,12 +2984,12 @@ controller_interface::return_type ControllerManager::update( controller_ret = trigger_result.result; if (trigger_status && trigger_result.execution_time.has_value()) { - loaded_controller.execution_time_statistics->AddMeasurement( + loaded_controller.execution_time_statistics->add_measurement( static_cast(trigger_result.execution_time.value().count()) / 1.e3); } if (!first_update_cycle && trigger_status && trigger_result.period.has_value()) { - loaded_controller.periodicity_statistics->AddMeasurement( + loaded_controller.periodicity_statistics->add_measurement( 1.0 / trigger_result.period.value().seconds()); } } @@ -2940,12 +3080,18 @@ controller_interface::return_type ControllerManager::update( } PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::DEFAULT_REGISTRY_KEY); + PUBLISH_ROS2_CONTROL_INTROSPECTION_DATA_ASYNC(hardware_interface::CM_STATISTICS_KEY); + + execution_time_.update_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); return ret; } void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + const auto start_time = std::chrono::steady_clock::now(); auto [result, failed_hardware_names] = resource_manager_->write(time, period); if (result == hardware_interface::return_type::ERROR) @@ -2971,8 +3117,8 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - // As the hardware is in UNCONFIGURED state with error call, no need to prepare or perform - // command mode switch + perform_hardware_command_mode_change( + rt_controller_list, {}, rt_buffer_.deactivate_controllers_list, "write"); deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); // TODO(destogl): do auto-start of broadcasters } @@ -3021,6 +3167,26 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration rt_controller_list, {}, rt_buffer_.deactivate_controllers_list, "write"); deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); } + execution_time_.write_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time) + .count(); + execution_time_.total_time = + execution_time_.write_time + execution_time_.update_time + execution_time_.read_time; + const double expected_cycle_time = 1.e6 / static_cast(get_update_rate()); + if (execution_time_.total_time > expected_cycle_time) + { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, + "Overrun might occur, Total time : %.3f us (Expected < %.3f us) --> Read time : %.3f us, " + "Update time : %.3f us (Switch time : %.3f us (Switch chained mode time : %.3f us, perform " + "mode change time : %.3f us, Activation time : %.3f us, Deactivation time : %.3f us)), Write " + "time : %.3f us", + execution_time_.total_time, expected_cycle_time, execution_time_.read_time, + execution_time_.update_time, execution_time_.switch_time, + execution_time_.switch_chained_mode_time, execution_time_.switch_perform_mode_time, + execution_time_.activation_time, execution_time_.deactivation_time, + execution_time_.write_time); + } } std::vector & @@ -3707,8 +3873,8 @@ void ControllerManager::controller_activity_diagnostic_callback( controllers[i].info.name + state_suffix, controllers[i].c->get_lifecycle_state().label()); if (is_controller_active(controllers[i].c)) { - const auto periodicity_stats = controllers[i].periodicity_statistics->GetStatistics(); - const auto exec_time_stats = controllers[i].execution_time_statistics->GetStatistics(); + const auto periodicity_stats = controllers[i].periodicity_statistics->get_statistics(); + const auto exec_time_stats = controllers[i].execution_time_statistics->get_statistics(); stat.add( controllers[i].info.name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); const bool publish_periodicity_stats = @@ -4004,7 +4170,7 @@ void ControllerManager::controller_manager_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { const std::string periodicity_stat_name = "periodicity"; - const auto cm_stats = periodicity_stats_.GetStatistics(); + const auto cm_stats = periodicity_stats_.get_statistics(); stat.add("update_rate", std::to_string(get_update_rate())); stat.add(periodicity_stat_name + ".average", std::to_string(cm_stats.average)); stat.add( diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 06b82ed3d8..fded54186b 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_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 } } diff --git a/doc/introspection.rst b/doc/introspection.rst index 5be7d66807..35fe77ee89 100644 --- a/doc/introspection.rst +++ b/doc/introspection.rst @@ -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 ============================================================================ diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 5ae2ad3480..3acc4a0591 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -9,6 +9,7 @@ controller_manager ****************** * 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 `_). * Parameter ``shutdown_on_initial_state_failure`` was added to avoid shutting down on hardware initial state failure (`#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 `_). hardware_interface ****************** diff --git a/hardware_interface/include/hardware_interface/introspection.hpp b/hardware_interface/include/hardware_interface/introspection.hpp index d25b71c2d8..dff4ee6b13 100644 --- a/hardware_interface/include/hardware_interface/introspection.hpp +++ b/hardware_interface/include/hardware_interface/introspection.hpp @@ -17,13 +17,37 @@ #ifndef HARDWARE_INTERFACE__INTROSPECTION_HPP_ #define HARDWARE_INTERFACE__INTROSPECTION_HPP_ +#include + +#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 sample_func = [variable] + { return static_cast(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( \ diff --git a/hardware_interface/include/hardware_interface/types/statistics_types.hpp b/hardware_interface/include/hardware_interface/types/statistics_types.hpp index 8083b66f73..c764a1866a 100644 --- a/hardware_interface/include/hardware_interface/types/statistics_types.hpp +++ b/hardware_interface/include/hardware_interface/types/statistics_types.hpp @@ -17,9 +17,11 @@ #ifndef HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ #define HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ +#include #include #include +#include "hardware_interface/introspection.hpp" #include "libstatistics_collector/moving_average_statistics/moving_average.hpp" #include "libstatistics_collector/moving_average_statistics/types.hpp" #if !defined(_WIN32) && !defined(__APPLE__) @@ -31,14 +33,188 @@ namespace ros2_control { +/** + * A class for calculating moving average statistics. This operates in constant memory and constant + * time. Note: reset() must be called manually in order to start a new measurement window. + * + * The statistics calculated are average, maximum, minimum, and standard deviation (population). + * All are calculated online without storing the observation data. Specifically, the average is a + * running sum and the variance is obtained by Welford's online algorithm (reference: + * https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford%27s_online_algorithm) + * for standard deviation. + * + * When statistics are not available, e.g. no observations have been made, NaNs are returned. + */ +class MovingAverageStatistics +{ +public: + using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; + MovingAverageStatistics() = default; + + ~MovingAverageStatistics() = default; + + /** + * Returns the arithmetic mean of all data recorded. If no observations have been made, returns + * NaN. + * + * @return The arithmetic mean of all data recorded, or NaN if the sample count is 0. + */ + double get_average() const + { + std::lock_guard lock(mutex_); + return statistics_data_.average; + } + + /** + * Returns the maximum value recorded. If size of list is zero, returns NaN. + * + * @return The maximum value recorded, or NaN if size of data is zero. + */ + double get_max() const + { + std::lock_guard lock(mutex_); + return statistics_data_.max; + } + + /** + * Returns the minimum value recorded. If size of list is zero, returns NaN. + * + * @return The minimum value recorded, or NaN if size of data is zero. + */ + double get_min() const + { + std::lock_guard lock(mutex_); + return statistics_data_.min; + } + + /** + * Returns the standard deviation (population) of all data recorded. If size of list is zero, + * returns NaN. + * + * Variance is obtained by Welford's online algorithm, + * see + * https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford%27s_online_algorithm + * + * @return The standard deviation (population) of all data recorded, or NaN if size of data is + * zero. + */ + double get_standard_deviation() const + { + std::lock_guard lock(mutex_); + return statistics_data_.standard_deviation; + } + + /** + * Return a StatisticData object, containing average, minimum, maximum, standard deviation + * (population), and sample count. For the case of no observations, the average, min, max, and + * standard deviation are NaN. + * + * @return StatisticData object, containing average, minimum, maximum, standard deviation + * (population), and sample count. + */ + const StatisticData & get_statistics_const_ptr() const + { + std::lock_guard lock(mutex_); + return statistics_data_; + } + + StatisticData get_statistics() const + { + std::lock_guard lock(mutex_); + return statistics_data_; + } + + /** + * Get the current measurement value. + * This is the last value added to the statistics collector. + * + * @return The current measurement value, or NaN if no measurements have been made. + */ + const double & get_current_measurement_const_ptr() const + { + std::lock_guard lock(mutex_); + return current_measurement_; + } + + double get_current_measurement() const + { + std::lock_guard lock(mutex_); + return current_measurement_; + } + + /** + * Reset all calculated values. Equivalent to a new window for a moving average. + */ + void reset() + { + std::lock_guard lock(mutex_); + statistics_data_.average = 0.0; + statistics_data_.min = std::numeric_limits::max(); + statistics_data_.max = std::numeric_limits::lowest(); + statistics_data_.standard_deviation = 0.0; + statistics_data_.sample_count = 0; + current_measurement_ = std::numeric_limits::quiet_NaN(); + sum_of_square_diff_from_mean_ = 0; + } + + void reset_current_measurement() + { + std::lock_guard lock(mutex_); + current_measurement_ = 0.0; + } + + /** + * Observe a sample for the given window. The input item is used to calculate statistics. + * Note: any input values of NaN will be discarded and not added as a measurement. + * + * @param item The item that was observed + */ + virtual void add_measurement(const double item) + { + std::lock_guard guard{mutex_}; + + current_measurement_ = item; + if (std::isfinite(item)) + { + statistics_data_.sample_count = statistics_data_.sample_count + 1; + const double previous_average = statistics_data_.average; + statistics_data_.average = + previous_average + (current_measurement_ - previous_average) / + static_cast(statistics_data_.sample_count); + statistics_data_.min = std::min(statistics_data_.min, current_measurement_); + statistics_data_.max = std::max(statistics_data_.max, current_measurement_); + sum_of_square_diff_from_mean_ = + sum_of_square_diff_from_mean_ + (current_measurement_ - previous_average) * + (current_measurement_ - statistics_data_.average); + statistics_data_.standard_deviation = std::sqrt( + sum_of_square_diff_from_mean_ / static_cast(statistics_data_.sample_count)); + } + } + + /** + * Return the number of samples observed + * + * @return the number of samples observed + */ + uint64_t get_count() const + { + std::lock_guard lock(mutex_); + return statistics_data_.sample_count; + } + +private: + mutable DEFAULT_MUTEX mutex_; + StatisticData statistics_data_; + double current_measurement_ = std::numeric_limits::quiet_NaN(); + double sum_of_square_diff_from_mean_ = 0.0; +}; + /** * @brief Data structure to store the statistics of a moving average. The data is protected by a * mutex and the data can be updated and retrieved. */ class MovingAverageStatisticsData { - using MovingAverageStatisticsCollector = - libstatistics_collector::moving_average_statistics::MovingAverageStatistics; using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; public: @@ -52,21 +228,22 @@ class MovingAverageStatisticsData * @brief Update the statistics data with the new statistics data. * @param statistics statistics collector to update the current statistics data. */ - void update_statistics(const std::shared_ptr & statistics) + void update_statistics(const std::shared_ptr & statistics) { std::unique_lock lock(mutex_); - if (statistics->GetCount() > 0) + if (statistics->get_count() > 0) { - statistics_data.average = statistics->Average(); - statistics_data.min = statistics->Min(); - statistics_data.max = statistics->Max(); - statistics_data.standard_deviation = statistics->StandardDeviation(); - statistics_data.sample_count = statistics->GetCount(); - statistics_data = statistics->GetStatistics(); + statistics_data_.average = statistics->get_average(); + statistics_data_.min = statistics->get_min(); + statistics_data_.max = statistics->get_max(); + statistics_data_.standard_deviation = statistics->get_standard_deviation(); + statistics_data_.sample_count = statistics->get_count(); + statistics_data_ = statistics->get_statistics(); + current_data_ = statistics->get_current_measurement(); } - if (statistics->GetCount() >= reset_statistics_sample_count_) + if (statistics->get_count() >= reset_statistics_sample_count_) { - statistics->Reset(); + statistics->reset(); } } @@ -82,11 +259,11 @@ class MovingAverageStatisticsData void reset() { - statistics_data.average = std::numeric_limits::quiet_NaN(); - statistics_data.min = std::numeric_limits::quiet_NaN(); - statistics_data.max = std::numeric_limits::quiet_NaN(); - statistics_data.standard_deviation = std::numeric_limits::quiet_NaN(); - statistics_data.sample_count = 0; + statistics_data_.average = std::numeric_limits::quiet_NaN(); + statistics_data_.min = std::numeric_limits::quiet_NaN(); + statistics_data_.max = std::numeric_limits::quiet_NaN(); + statistics_data_.standard_deviation = std::numeric_limits::quiet_NaN(); + statistics_data_.sample_count = 0; } /** @@ -96,16 +273,24 @@ class MovingAverageStatisticsData const StatisticData & get_statistics() const { std::unique_lock lock(mutex_); - return statistics_data; + return statistics_data_; + } + + const double & get_current_data() const + { + std::unique_lock lock(mutex_); + return current_data_; } private: + /// Mutex to protect the statistics data + mutable DEFAULT_MUTEX mutex_; /// Statistics data - StatisticData statistics_data; + StatisticData statistics_data_; + /// Current data value, used to calculate the statistics + double current_data_ = std::numeric_limits::quiet_NaN(); /// Number of samples to reset the statistics unsigned int reset_statistics_sample_count_ = std::numeric_limits::max(); - /// Mutex to protect the statistics data - mutable DEFAULT_MUTEX mutex_; }; } // namespace ros2_control @@ -119,27 +304,24 @@ struct HardwareComponentStatisticsCollector { HardwareComponentStatisticsCollector() { - execution_time = std::make_shared(); - periodicity = std::make_shared(); + execution_time = std::make_shared(); + periodicity = std::make_shared(); } - using MovingAverageStatisticsCollector = - libstatistics_collector::moving_average_statistics::MovingAverageStatistics; - /** * @brief Resets the internal statistics of the execution time and periodicity statistics * collectors. */ void reset_statistics() { - execution_time->Reset(); - periodicity->Reset(); + execution_time->reset(); + periodicity->reset(); } /// Execution time statistics collector - std::shared_ptr execution_time = nullptr; + std::shared_ptr execution_time = nullptr; /// Periodicity statistics collector - std::shared_ptr periodicity = nullptr; + std::shared_ptr periodicity = nullptr; }; } // namespace hardware_interface diff --git a/hardware_interface/src/hardware_component.cpp b/hardware_interface/src/hardware_component.cpp index 7e4651fea0..9a9f0f0f5f 100644 --- a/hardware_interface/src/hardware_component.cpp +++ b/hardware_interface/src/hardware_component.cpp @@ -373,12 +373,12 @@ return_type HardwareComponent::read(const rclcpp::Time & time, const rclcpp::Dur { if (trigger_result.execution_time.has_value()) { - read_statistics_.execution_time->AddMeasurement( + read_statistics_.execution_time->add_measurement( static_cast(trigger_result.execution_time.value().count()) / 1.e3); } if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) { - read_statistics_.periodicity->AddMeasurement( + read_statistics_.periodicity->add_measurement( 1.0 / (time - last_read_cycle_time_).seconds()); } last_read_cycle_time_ = time; @@ -412,12 +412,12 @@ return_type HardwareComponent::write(const rclcpp::Time & time, const rclcpp::Du { if (trigger_result.execution_time.has_value()) { - write_statistics_.execution_time->AddMeasurement( + write_statistics_.execution_time->add_measurement( static_cast(trigger_result.execution_time.value().count()) / 1.e3); } if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) { - write_statistics_.periodicity->AddMeasurement( + write_statistics_.periodicity->add_measurement( 1.0 / (time - last_write_cycle_time_).seconds()); } last_write_cycle_time_ = time;