From 56277965125e5856f6c64eb8d0bee8c2ff60e895 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 25 Sep 2024 17:00:36 +0200 Subject: [PATCH 01/32] add MovingAverageStatistics to ControllerSpec --- controller_manager/CMakeLists.txt | 1 + .../include/controller_manager/controller_spec.hpp | 3 +++ controller_manager/package.xml | 1 + controller_manager/src/controller_manager.cpp | 5 +++++ 4 files changed, 10 insertions(+) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1bb84eb32c..ccf4755770 100644 --- a/controller_manager/CMakeLists.txt +++ b/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) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 0f44867814..09b670f290 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -24,6 +24,7 @@ #include #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 { @@ -38,6 +39,8 @@ struct ControllerSpec hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr last_update_cycle_time; + std::shared_ptr + statistics; }; struct ControllerChainSpec diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 18189d5d16..c89923257c 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -28,6 +28,7 @@ ros2param ros2run std_msgs + libstatistics_collector ament_cmake_gmock ament_cmake_pytest diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d7e924a863..c2f8dbb6b2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -545,6 +545,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.type = controller_type; controller_spec.last_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); + controller_spec.statistics = + std::make_shared(); // 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 @@ -1762,6 +1764,8 @@ void ControllerManager::activate_controllers( try { + found_it->statistics->Reset(); + found_it->statistics->AddMeasurement(1.0 / controller->get_update_rate()); const auto new_state = controller->get_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -2387,6 +2391,7 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { + loaded_controller.statistics->AddMeasurement(controller_actual_period.seconds()); auto controller_ret = controller_interface::return_type::OK; bool trigger_status = true; // Catch exceptions thrown by the controller update function From d7e9bbd24bc9e7e9dea59a31350d025279c7022c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 23:40:50 +0100 Subject: [PATCH 02/32] Use `ControllerUpdateStatus` object to return trigger_update status with more information --- .../controller_interface_base.hpp | 16 +++++++++++----- .../src/controller_interface_base.cpp | 14 +++++++++++--- controller_manager/src/controller_manager.cpp | 4 +++- 3 files changed, 25 insertions(+), 9 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index ae3804919c..14f33219ad 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -71,6 +71,14 @@ struct ControllerUpdateStats unsigned int total_triggers; unsigned int failed_triggers; }; + +struct ControllerUpdateStatus +{ + bool ok = true; + return_type result = return_type::OK; + std::chrono::nanoseconds execution_time = std::chrono::nanoseconds(0); +}; + /** * 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 +183,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 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 get_node(); diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 0713ec3c25..fe30db351a 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -159,9 +159,10 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } -std::pair ControllerInterfaceBase::trigger_update( +ControllerUpdateStatus ControllerInterfaceBase::trigger_update( const rclcpp::Time & time, const rclcpp::Duration & period) { + ControllerUpdateStatus status; trigger_stats_.total_triggers++; if (is_async()) { @@ -174,12 +175,19 @@ std::pair 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; + status.execution_time = async_handler_->get_last_execution_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::steady_clock::now() - start_time); } + return status; } std::shared_ptr ControllerInterfaceBase::get_node() diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c2f8dbb6b2..464cba2b34 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2397,8 +2397,10 @@ controller_interface::return_type ControllerManager::update( // Catch exceptions thrown by the controller update function try { - std::tie(trigger_status, controller_ret) = + const auto trigger_result = loaded_controller.c->trigger_update(time, controller_actual_period); + trigger_status = trigger_result.ok; + controller_ret = trigger_result.result; } catch (const std::exception & e) { From 1bcc7be7d34d1ccab0cfbbf56021f3feb79c25d8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 23:42:20 +0100 Subject: [PATCH 03/32] add stats of execution time and the periodicity of the controllers --- .../controller_manager/controller_spec.hpp | 7 +++++-- controller_manager/src/controller_manager.cpp | 17 ++++++++++++----- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 09b670f290..b8a947b113 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -28,6 +28,9 @@ 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 @@ -39,8 +42,8 @@ struct ControllerSpec hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr last_update_cycle_time; - std::shared_ptr - statistics; + std::shared_ptr execution_time_statistics; + std::shared_ptr periodicity_statistics; }; struct ControllerChainSpec diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 464cba2b34..d3c7f9bffd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -545,8 +545,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.type = controller_type; controller_spec.last_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); - controller_spec.statistics = - std::make_shared(); + controller_spec.execution_time_statistics = std::make_shared(); + controller_spec.periodicity_statistics = std::make_shared(); // 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 @@ -1764,8 +1764,9 @@ void ControllerManager::activate_controllers( try { - found_it->statistics->Reset(); - found_it->statistics->AddMeasurement(1.0 / controller->get_update_rate()); + found_it->periodicity_statistics->Reset(); + found_it->periodicity_statistics->AddMeasurement(controller->get_update_rate()); + 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) { @@ -2391,7 +2392,8 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - loaded_controller.statistics->AddMeasurement(controller_actual_period.seconds()); + loaded_controller.periodicity_statistics->AddMeasurement( + 1.0 / controller_actual_period.seconds()); auto controller_ret = controller_interface::return_type::OK; bool trigger_status = true; // Catch exceptions thrown by the controller update function @@ -2401,6 +2403,11 @@ controller_interface::return_type ControllerManager::update( loaded_controller.c->trigger_update(time, controller_actual_period); trigger_status = trigger_result.ok; controller_ret = trigger_result.result; + if (trigger_status) + { + loaded_controller.execution_time_statistics->AddMeasurement( + static_cast(trigger_result.execution_time.count()) / 1.e3); + } } catch (const std::exception & e) { From 1d8f32accf628a184b7a61822bd1ee6de8a52e6f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 23:43:44 +0100 Subject: [PATCH 04/32] Add periodicity stats to the controller manager --- .../include/controller_manager/controller_manager.hpp | 2 ++ controller_manager/src/controller_manager.cpp | 2 ++ controller_manager/src/ros2_control_node.cpp | 2 +- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 068eefc1f9..0595e69a68 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -603,6 +603,8 @@ class ControllerManager : public rclcpp::Node rclcpp::Subscription::SharedPtr robot_description_subscription_; rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; + controller_manager::MovingAverageStatistics periodicity_stats_; + struct SwitchParams { void reset() diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d3c7f9bffd..fd95a78577 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -313,6 +313,7 @@ void ControllerManager::init_controller_manager() robot_description_subscription_->get_topic_name()); // Setup diagnostics + periodicity_stats_.Reset(); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); @@ -2248,6 +2249,7 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + periodicity_stats_.AddMeasurement(period.seconds()); auto [ok, failed_hardware_names] = resource_manager_->read(time, period); if (!ok) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index f8347df952..7e6f4e3260 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -109,7 +109,7 @@ int main(int argc, char ** argv) next_iteration_time{cm_now}; // for calculating the measured period of the loop - rclcpp::Time previous_time = cm->now(); + rclcpp::Time previous_time = cm->now() - period; while (rclcpp::ok()) { From c5c8776ba979342f3e84f5e49d5bc5db44bfac6c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 23:44:45 +0100 Subject: [PATCH 05/32] update the diagnostics of the controller manager with periodicity information --- controller_manager/src/controller_manager.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fd95a78577..da4d2d203b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3146,7 +3146,14 @@ void ControllerManager::hardware_components_diagnostic_callback( void ControllerManager::controller_manager_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { + const std::string periodicity_stat_name = "periodicity"; stat.add("update_rate", std::to_string(get_update_rate())); + stat.add(periodicity_stat_name + ".average", std::to_string(periodicity_stats_.Average())); + stat.add( + periodicity_stat_name + ".standard_deviation", + std::to_string(periodicity_stats_.StandardDeviation())); + stat.add(periodicity_stat_name + ".min", std::to_string(periodicity_stats_.Min())); + stat.add(periodicity_stat_name + ".max", std::to_string(periodicity_stats_.Max())); if (is_resource_manager_initialized()) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running"); From 6da2bb0839d205892a4d0dfff0e4e24fccb33de0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 7 Nov 2024 08:28:19 +0100 Subject: [PATCH 06/32] make execution time in the ControllerUpdateStatus optional for async first operation --- .../controller_interface/controller_interface_base.hpp | 2 +- controller_interface/src/controller_interface_base.cpp | 6 +++++- controller_manager/src/controller_manager.cpp | 4 ++-- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 14f33219ad..2299d59107 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -76,7 +76,7 @@ struct ControllerUpdateStatus { bool ok = true; return_type result = return_type::OK; - std::chrono::nanoseconds execution_time = std::chrono::nanoseconds(0); + std::optional execution_time = std::nullopt; }; /** diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index fe30db351a..e02ff23b82 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -177,7 +177,11 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( } status.ok = result.first; status.result = result.second; - status.execution_time = async_handler_->get_last_execution_time(); + const auto execution_time = async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } } else { diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index da4d2d203b..7e432e5617 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2405,10 +2405,10 @@ controller_interface::return_type ControllerManager::update( loaded_controller.c->trigger_update(time, controller_actual_period); trigger_status = trigger_result.ok; controller_ret = trigger_result.result; - if (trigger_status) + if (trigger_status && trigger_result.execution_time.has_value()) { loaded_controller.execution_time_statistics->AddMeasurement( - static_cast(trigger_result.execution_time.count()) / 1.e3); + static_cast(trigger_result.execution_time.value().count()) / 1.e3); } } catch (const std::exception & e) From e8b362d90f776a92e25b923dfb3d4ac5668fa36c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 7 Nov 2024 08:39:15 +0100 Subject: [PATCH 07/32] adapt periodicity statistics for async controllers --- controller_manager/src/controller_manager.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 7e432e5617..4419df2730 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2394,8 +2394,6 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - loaded_controller.periodicity_statistics->AddMeasurement( - 1.0 / controller_actual_period.seconds()); auto controller_ret = controller_interface::return_type::OK; bool trigger_status = true; // Catch exceptions thrown by the controller update function @@ -2405,10 +2403,22 @@ controller_interface::return_type ControllerManager::update( loaded_controller.c->trigger_update(time, controller_actual_period); trigger_status = trigger_result.ok; controller_ret = trigger_result.result; - if (trigger_status && trigger_result.execution_time.has_value()) + if (trigger_status) { - loaded_controller.execution_time_statistics->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); + loaded_controller.periodicity_statistics->AddMeasurement( + 1.0 / controller_actual_period.seconds()); + if (trigger_result.execution_time.has_value()) + { + loaded_controller.execution_time_statistics->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + } + else + { + // If an async controller misses it's trigger cycle, then it has to wait for next + // trigger cycle + loaded_controller.periodicity_statistics->AddMeasurement( + 1.0 / (controller_actual_period + controller_period).seconds()); } } catch (const std::exception & e) From 71dbd0397f80901fa0c3f82499037119575474d4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 00:42:18 +0100 Subject: [PATCH 08/32] Add controller statistics diagnostics --- controller_manager/src/controller_manager.cpp | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 4419df2730..95cb6308ee 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3076,13 +3076,32 @@ void ControllerManager::controller_activity_diagnostic_callback( std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); bool all_active = true; + const std::string periodicity_suffix = ".periodicity"; + const std::string state_suffix = ".state"; + auto make_stats_string = + [](const auto & statistics_data, const std::string & measurement_unit) -> std::string + { + std::ostringstream oss; + oss << std::fixed << std::setprecision(2); + oss << "Avg: " << statistics_data.average << " [" << statistics_data.min << " - " + << statistics_data.max << "]" << " " << measurement_unit + << ", StdDev: " << statistics_data.standard_deviation; + return oss.str(); + }; for (size_t i = 0; i < controllers.size(); ++i) { if (!is_controller_active(controllers[i].c)) { all_active = false; } - stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); + stat.add( + controllers[i].info.name + state_suffix, controllers[i].c->get_lifecycle_state().label()); + stat.add( + controllers[i].info.name + periodicity_suffix, + make_stats_string(controllers[i].execution_time_statistics->GetStatistics(), "us")); + stat.add( + controllers[i].info.name + periodicity_suffix, + make_stats_string(controllers[i].periodicity_statistics->GetStatistics(), "Hz")); } if (!atleast_one_hw_active) From bb4a93cb2746e8a4e469ede299f9fbac70013fe1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 16:46:05 +0100 Subject: [PATCH 09/32] use current trigger time from AsyncFunctionHandler to calculate the right periodicity --- .../controller_interface/controller_interface_base.hpp | 1 + controller_interface/src/controller_interface_base.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2299d59107..bff7e204ad 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -77,6 +77,7 @@ struct ControllerUpdateStatus bool ok = true; return_type result = return_type::OK; std::optional execution_time = std::nullopt; + std::optional period = std::nullopt; }; /** diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index e02ff23b82..492a7ee1bc 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -166,6 +166,7 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( 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) { @@ -182,6 +183,10 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( { status.execution_time = execution_time; } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } } else { @@ -190,6 +195,7 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( status.result = update(time, period); status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); + status.period = period; } return status; } From 986bbd0b81fdcc6281502fed53e858723557c00a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 16:46:41 +0100 Subject: [PATCH 10/32] Update the logic to use the period from ControllerUpdateStatus --- controller_manager/src/controller_manager.cpp | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 95cb6308ee..75dcf04dfc 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2403,22 +2403,15 @@ controller_interface::return_type ControllerManager::update( loaded_controller.c->trigger_update(time, controller_actual_period); trigger_status = trigger_result.ok; controller_ret = trigger_result.result; - if (trigger_status) + if (trigger_status && trigger_result.execution_time.has_value()) { - loaded_controller.periodicity_statistics->AddMeasurement( - 1.0 / controller_actual_period.seconds()); - if (trigger_result.execution_time.has_value()) - { - loaded_controller.execution_time_statistics->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); - } + loaded_controller.execution_time_statistics->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); } - else + if (trigger_status && trigger_result.period.has_value()) { - // If an async controller misses it's trigger cycle, then it has to wait for next - // trigger cycle loaded_controller.periodicity_statistics->AddMeasurement( - 1.0 / (controller_actual_period + controller_period).seconds()); + 1.0 / trigger_result.period.value().seconds()); } } catch (const std::exception & e) From 46016ae1c79fa10ba97144eed586a34b92eded35 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 16:54:41 +0100 Subject: [PATCH 11/32] Only publish the diagnostics stats for the active controllers --- controller_manager/src/controller_manager.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 75dcf04dfc..b895d7a67f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3070,6 +3070,7 @@ void ControllerManager::controller_activity_diagnostic_callback( const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); bool all_active = true; const std::string periodicity_suffix = ".periodicity"; + const std::string exec_time_suffix = ".execution_time"; const std::string state_suffix = ".state"; auto make_stats_string = [](const auto & statistics_data, const std::string & measurement_unit) -> std::string @@ -3089,12 +3090,15 @@ void ControllerManager::controller_activity_diagnostic_callback( } stat.add( controllers[i].info.name + state_suffix, controllers[i].c->get_lifecycle_state().label()); - stat.add( - controllers[i].info.name + periodicity_suffix, - make_stats_string(controllers[i].execution_time_statistics->GetStatistics(), "us")); - stat.add( - controllers[i].info.name + periodicity_suffix, - make_stats_string(controllers[i].periodicity_statistics->GetStatistics(), "Hz")); + if (is_controller_active(controllers[i].c)) + { + stat.add( + controllers[i].info.name + exec_time_suffix, + make_stats_string(controllers[i].execution_time_statistics->GetStatistics(), "us")); + stat.add( + controllers[i].info.name + periodicity_suffix, + make_stats_string(controllers[i].periodicity_statistics->GetStatistics(), "Hz")); + } } if (!atleast_one_hw_active) From 1d4e4b35bd254ca61854a238d8eaff2f3c2f1282 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 16:59:52 +0100 Subject: [PATCH 12/32] fix CM periodicity stat --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b895d7a67f..9e3ee598a0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2249,7 +2249,7 @@ std::vector ControllerManager::get_controller_names() void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) { - periodicity_stats_.AddMeasurement(period.seconds()); + periodicity_stats_.AddMeasurement(1.0 / period.seconds()); auto [ok, failed_hardware_names] = resource_manager_->read(time, period); if (!ok) From 4759316decc78582e24aa34a6ff11000e24e0b39 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 9 Nov 2024 00:08:45 +0100 Subject: [PATCH 13/32] Fix the tests by initializing the pointers on construction --- .../include/controller_manager/controller_spec.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index b8a947b113..9cc508772d 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -39,6 +39,13 @@ using MovingAverageStatistics = */ struct ControllerSpec { + ControllerSpec() + { + last_update_cycle_time = std::make_shared(0, 0, RCL_CLOCK_UNINITIALIZED); + execution_time_statistics = std::make_shared(); + periodicity_statistics = std::make_shared(); + } + hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr last_update_cycle_time; From 21796d2c42a58830337f06552a921eca088e2b0d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:34:27 +0100 Subject: [PATCH 14/32] Don't add the initial periodicity upon activation --- controller_manager/src/controller_manager.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9e3ee598a0..456df1c822 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1766,7 +1766,6 @@ void ControllerManager::activate_controllers( try { found_it->periodicity_statistics->Reset(); - found_it->periodicity_statistics->AddMeasurement(controller->get_update_rate()); 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) From cd56f82135a869e0f1a34ac23e4bd9f4c78643f6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:36:21 +0100 Subject: [PATCH 15/32] set the first update period to zero and do not count for periodicity --- controller_manager/src/controller_manager.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 456df1c822..f4e8e15e81 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2361,12 +2361,14 @@ controller_interface::return_type ControllerManager::update( : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); const rclcpp::Time current_time = get_clock()->now(); + bool first_update_cycle = false; if ( *loaded_controller.last_update_cycle_time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { + first_update_cycle = true; // it is zero after activation - *loaded_controller.last_update_cycle_time = current_time - controller_period; + *loaded_controller.last_update_cycle_time = current_time; RCLCPP_DEBUG( get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s", loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str()); @@ -2384,7 +2386,7 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate || (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || - (controller_actual_period.seconds() * controller_update_rate >= 0.99); + (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle; RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2407,7 +2409,7 @@ controller_interface::return_type ControllerManager::update( loaded_controller.execution_time_statistics->AddMeasurement( static_cast(trigger_result.execution_time.value().count()) / 1.e3); } - if (trigger_status && trigger_result.period.has_value()) + if (!first_update_cycle && trigger_status && trigger_result.period.has_value()) { loaded_controller.periodicity_statistics->AddMeasurement( 1.0 / trigger_result.period.value().seconds()); From b4ff424daa201b8156295d5e7e3e8a22714ffce0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:36:47 +0100 Subject: [PATCH 16/32] pass current_time to the trigger_update method --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f4e8e15e81..303cd8ccce 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2401,7 +2401,7 @@ controller_interface::return_type ControllerManager::update( try { const auto trigger_result = - loaded_controller.c->trigger_update(time, controller_actual_period); + loaded_controller.c->trigger_update(current_time, controller_actual_period); trigger_status = trigger_result.ok; controller_ret = trigger_result.result; if (trigger_status && trigger_result.execution_time.has_value()) From f4701293539c9ef36f418338e5c0075f5984a063 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:38:26 +0100 Subject: [PATCH 17/32] Fix the test and add tests of the statistics --- .../test/test_controller_manager.cpp | 54 ++++++++++++++----- 1 file changed, 40 insertions(+), 14 deletions(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 0c4e51985f..0a934c6185 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -683,6 +683,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) const auto initial_counter = test_controller->internal_counter; // don't start with zero to check if the period is correct if controller is activated anytime rclcpp::Time time = time_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { rclcpp::Time old_time = time; @@ -691,19 +693,36 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); - // In case of a non perfect divisor, the update period should respect the rule - // [controller_update_rate, 2*controller_update_rate) - EXPECT_THAT( - test_controller->update_period_.seconds(), - testing::AllOf( - testing::Gt(0.99 * controller_period), - testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) - << "update_counter: " << update_counter << " desired controller period: " << controller_period - << " actual controller period: " << test_controller->update_period_.seconds(); - if (update_counter % cm_update_rate == 0) + if (test_controller->internal_counter - initial_counter > 0) + { + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf( + testing::Gt(0.99 * controller_period), + testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + << "update_counter: " << update_counter + << " desired controller period: " << controller_period + << " actual controller period: " << test_controller->update_period_.seconds(); + } + else + { + // Check that the first cycle update period is zero + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) { const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it // is clearly tracking, so adding 1 here won't affect the final count. @@ -711,10 +730,17 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 // cycles it will have 369 instead of 370. EXPECT_THAT( - test_controller->internal_counter - initial_counter, - testing::AnyOf( - testing::Ge((controller_update_rate - 1) * no_of_secs_passed), - testing::Lt((controller_update_rate * no_of_secs_passed)))); + 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()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); } } } From abdfe36371f8abb4ad7398b3c1ddb1a795184e17 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:54:16 +0100 Subject: [PATCH 18/32] Add some reasonable checks about the execution time --- controller_manager/test/test_controller_manager.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 0a934c6185..3b64c7693d 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -741,6 +741,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_THAT( cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 50.0); // 50 microseconds } } } From bb1b97526d7fedd774cc64d7d54c0e502480366a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:54:27 +0100 Subject: [PATCH 19/32] Add more statistics tests --- controller_manager/test/test_controller_manager.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 3b64c7693d..f8541b3932 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -577,6 +577,17 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_THAT( test_controller->update_period_.seconds(), testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate)))); + ASSERT_EQ( + test_controller->internal_counter, + cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); + ASSERT_EQ( + test_controller->internal_counter - 1, + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount()) + << "The first update is not counted in periodicity statistics"; + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf( + testing::Ge(0.95 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate())))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be From c161017720cd0050c815b8edc22cee7c46b87b92 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 01:07:24 +0100 Subject: [PATCH 20/32] Publish periodicity only if it is an async controller --- controller_manager/src/controller_manager.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 303cd8ccce..5d6a73d8f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3096,9 +3096,12 @@ void ControllerManager::controller_activity_diagnostic_callback( stat.add( controllers[i].info.name + exec_time_suffix, make_stats_string(controllers[i].execution_time_statistics->GetStatistics(), "us")); - stat.add( - controllers[i].info.name + periodicity_suffix, - make_stats_string(controllers[i].periodicity_statistics->GetStatistics(), "Hz")); + if (controllers[i].c->is_async()) + { + stat.add( + controllers[i].info.name + periodicity_suffix, + make_stats_string(controllers[i].periodicity_statistics->GetStatistics(), "Hz")); + } } } From 719d1f9ba6bf83cc4d34a6342549d2c2424c502d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 01:09:56 +0100 Subject: [PATCH 21/32] Add an initial way to retrieve threshold parameters from paramserver --- controller_manager/src/controller_manager.cpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5d6a73d8f3..971dbb4607 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3070,9 +3070,32 @@ void ControllerManager::controller_activity_diagnostic_callback( std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); bool all_active = true; + const std::string param_prefix = "diagnostics.threshold.controllers"; const std::string periodicity_suffix = ".periodicity"; const std::string exec_time_suffix = ".execution_time"; + const std::string mean_suffix = ".mean"; + const std::string std_dev_suffix = ".standard_deviation"; const std::string state_suffix = ".state"; + + // Get threshold values from param server + const double periodicity_mean_warn_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + mean_suffix + ".warn", 5.0); + const double periodicity_mean_error_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + mean_suffix + ".error", 10.0); + const double periodicity_std_dev_warn_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".warn", 5.0); + const double periodicity_std_dev_error_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".error", 10.0); + + const double exec_time_mean_warn_threshold = + this->get_parameter_or(param_prefix + exec_time_suffix + mean_suffix + ".warn", 1000.0); + const double exec_time_mean_error_threshold = + this->get_parameter_or(param_prefix + exec_time_suffix + mean_suffix + ".error", 2000.0); + const double exec_time_std_dev_warn_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".warn", 5.0); + const double exec_time_std_dev_error_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".error", 10.0); + auto make_stats_string = [](const auto & statistics_data, const std::string & measurement_unit) -> std::string { From 9b1b95f477026aed63a64ecfd6a0f45693a8bc8c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 12:39:36 +0100 Subject: [PATCH 22/32] Don't use const reference as it is going to be changed in different thread --- controller_interface/src/controller_interface_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 492a7ee1bc..3f1cb73d13 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -166,7 +166,7 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( trigger_stats_.total_triggers++; if (is_async()) { - const rclcpp::Time & last_trigger_time = async_handler_->get_current_callback_time(); + 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) { From a570ca8ca30f24947d19419eee0092a426d97c55 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 12:40:24 +0100 Subject: [PATCH 23/32] Update async test to check if the first period is set to zero or not --- .../test/test_controller_manager.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index f8541b3932..622b770a5d 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -366,6 +366,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + time_ = cm_->get_clock()->now(); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -373,6 +374,21 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) std::this_thread::sleep_for( std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); EXPECT_EQ(test_controller->internal_counter, 1u); + EXPECT_EQ(test_controller->update_period_.seconds(), 0.0) + << "The first trigger cycle should have zero period"; + + const double exp_period = (cm_->get_clock()->now() - time_).seconds(); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(test_controller->internal_counter, 1u); + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(test_controller->internal_counter, 2u); + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period)))); size_t last_internal_counter = test_controller->internal_counter; // Stop controller, will take effect at the end of the update function From 22da2b70368ceb935f05048227ef291a771e05b7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 12:41:14 +0100 Subject: [PATCH 24/32] Update the tests to also check for the Min and Max of the periodicity --- .../test/test_controller_manager.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 622b770a5d..7343c8a39f 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -592,7 +592,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // [cm_update_rate, 2*cm_update_rate) EXPECT_THAT( test_controller->update_period_.seconds(), - testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate)))); + testing::AllOf(testing::Ge(0.85 / cm_update_rate), testing::Lt((1.15 / cm_update_rate)))); ASSERT_EQ( test_controller->internal_counter, cm_->get_loaded_controllers()[0].execution_time_statistics->GetCount()); @@ -604,6 +604,14 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), testing::AllOf( testing::Ge(0.95 * cm_->get_update_rate()), testing::Lt((1.05 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf( + testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf( + testing::Ge(0.85 * cm_->get_update_rate()), testing::Lt((1.2 * cm_->get_update_rate())))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -768,6 +776,12 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_THAT( cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); EXPECT_LT( cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), 50.0); // 50 microseconds From 0c3fecdb5d3e73980facd24f9582f000817a8c36 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 12:42:00 +0100 Subject: [PATCH 25/32] Add tests to check for the async update with different rate with stats --- .../test/test_controller_manager.cpp | 142 ++++++++++++++++++ 1 file changed, 142 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 7343c8a39f..b715bc23dc 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -793,6 +793,148 @@ INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90)); +class TestAsyncControllerUpdateRates +: public ControllerManagerFixture +{ +}; + +TEST_F(TestAsyncControllerUpdateRates, check_the_async_controller_update_rate_and_stats) +{ + const unsigned int ctrl_update_rate = cm_->get_update_rate() / 2; + auto test_controller = std::make_shared(); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); + test_controller->get_node()->set_parameter({"is_async", true}); + // configure controller + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } + ASSERT_TRUE(test_controller->is_async()); + time_ = test_controller->get_node()->now(); // set to something nonzero + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + cm_->get_clock()->sleep_until(time_ + PERIOD); + time_ = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + const auto cm_update_rate = cm_->get_update_rate(); + const auto controller_update_rate = test_controller->get_update_rate(); + const double controller_period = 1.0 / controller_update_rate; + + const auto initial_counter = test_controller->internal_counter; + // don't start with zero to check if the period is correct if controller is activated anytime + rclcpp::Time time = time_; + const auto exp_periodicity = + cm_update_rate / std::ceil(static_cast(cm_update_rate) / controller_update_rate); + for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) + { + rclcpp::Time old_time = time; + cm_->get_clock()->sleep_until(old_time + PERIOD); + time = cm_->get_clock()->now(); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + + // the async controllers will have to wait for one cycle to have the correct update period in + // the controller + if (test_controller->internal_counter - initial_counter > 1) + { + EXPECT_THAT( + test_controller->update_period_.seconds(), + testing::AllOf( + testing::Gt(0.99 * controller_period), + testing::Lt((1.05 * controller_period) + PERIOD.seconds()))) + << "update_counter: " << update_counter + << " desired controller period: " << controller_period + << " actual controller period: " << test_controller->update_period_.seconds(); + } + // else + // { + // // Check that the first cycle update period is zero + // EXPECT_EQ(test_controller->update_period_.seconds(), 0.0); + // } + + if (update_counter > 0 && update_counter % cm_update_rate == 0) + { + const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; + const auto actual_counter = test_controller->internal_counter - initial_counter; + const unsigned int exp_counter = + static_cast(exp_periodicity * no_of_secs_passed); + SCOPED_TRACE( + "The internal counter is : " + std::to_string(actual_counter) + " [" + + std::to_string(exp_counter - 1) + ", " + std::to_string(exp_counter + 1) + + "] and number of seconds passed : " + std::to_string(no_of_secs_passed)); + // NOTE: here EXPECT_THAT is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + 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(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->GetCount(), + testing::AnyOf(testing::Ge(exp_counter - 1), testing::Le(exp_counter))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Average(), + testing::AllOf(testing::Ge(0.95 * exp_periodicity), testing::Lt((1.05 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Min(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_THAT( + cm_->get_loaded_controllers()[0].periodicity_statistics->Max(), + testing::AllOf(testing::Ge(0.85 * exp_periodicity), testing::Lt((1.2 * exp_periodicity)))); + EXPECT_LT( + cm_->get_loaded_controllers()[0].execution_time_statistics->Average(), + 12000); // more or less 12 milliseconds considering the waittime in the controller + } + } +} + class TestControllerManagerFallbackControllers : public ControllerManagerFixture, public testing::WithParamInterface From 7a6236607dd1c84b9b9685a297a38a9a086bfd12 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 16:59:36 +0100 Subject: [PATCH 26/32] Add first version of the statistics information --- controller_manager/src/controller_manager.cpp | 106 +++++++++++++++--- 1 file changed, 89 insertions(+), 17 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 971dbb4607..77ed533207 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3106,8 +3106,15 @@ void ControllerManager::controller_activity_diagnostic_callback( << ", StdDev: " << statistics_data.standard_deviation; return oss.str(); }; + + // Variable to define the overall status of the controller diagnostics + auto level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + std::vector high_exec_time_controllers; + std::vector bad_periodicity_async_controllers; for (size_t i = 0; i < controllers.size(); ++i) { + const bool is_async = controllers[i].c->is_async(); if (!is_controller_active(controllers[i].c)) { all_active = false; @@ -3116,37 +3123,102 @@ 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(); stat.add( - controllers[i].info.name + exec_time_suffix, - make_stats_string(controllers[i].execution_time_statistics->GetStatistics(), "us")); - if (controllers[i].c->is_async()) + controllers[i].info.name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); + if (is_async) { stat.add( controllers[i].info.name + periodicity_suffix, - make_stats_string(controllers[i].periodicity_statistics->GetStatistics(), "Hz")); + make_stats_string(periodicity_stats, "Hz")); + const double periodicity_percentage = + (periodicity_stats.average / static_cast(controllers[i].c->get_update_rate())) * + 100; + const double periodicity_std_dev_percentage = + (periodicity_stats.standard_deviation / periodicity_stats.average) * 100.0; + if ( + periodicity_percentage < periodicity_mean_error_threshold || + periodicity_std_dev_percentage > periodicity_std_dev_error_threshold) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } + else if ( + periodicity_percentage < periodicity_mean_warn_threshold || + periodicity_std_dev_percentage > periodicity_std_dev_warn_threshold) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); + } } + const double max_exp_exec_time = + is_async ? 1.e6 / controllers[i].c->get_update_rate() : 0.0; + if ( + (exec_time_stats.average - max_exp_exec_time) > exec_time_mean_error_threshold || + (max_exp_exec_time > 0.0 && + (exec_time_stats.standard_deviation / exec_time_stats.average) * 100.0 > + exec_time_std_dev_error_threshold)) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + high_exec_time_controllers.push_back(controllers[i].info.name); + } + else if ( + (exec_time_stats.average - max_exp_exec_time) > exec_time_mean_warn_threshold || + (max_exp_exec_time > 0.0 && + (exec_time_stats.standard_deviation / exec_time_stats.average) * 100.0 > + exec_time_std_dev_warn_threshold)) + { + if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + high_exec_time_controllers.push_back(controllers[i].info.name); + } + } + } + + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All controllers are active" : "Not all controllers are active"); + + if (!high_exec_time_controllers.empty()) + { + std::string high_exec_time_controllers_string; + for (const auto & controller : high_exec_time_controllers) + { + high_exec_time_controllers_string.append(controller); + high_exec_time_controllers_string.append(" "); + } + stat.mergeSummary( + level, "\nControllers with high execution time : [ " + high_exec_time_controllers_string + "]"); + } + if (!bad_periodicity_async_controllers.empty()) + { + std::string bad_periodicity_async_controllers_string; + for (const auto & controller : bad_periodicity_async_controllers) + { + bad_periodicity_async_controllers_string.append(controller); + bad_periodicity_async_controllers_string.append(" "); } + stat.mergeSummary( + level, + "\nControllers with bad periodicity : [ " + bad_periodicity_async_controllers_string + "]"); } if (!atleast_one_hw_active) { - stat.summary( + stat.mergeSummary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are currently active to activate controllers"); } - else + else if (controllers.empty()) { - if (controllers.empty()) - { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); - } - else - { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::OK, - all_active ? "All controllers are active" : "Not all controllers are active"); - } + stat.mergeSummary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No controllers are currently loaded"); } } From 24605bfd31e9d906ef0a132fe06cbe00d81b7fdb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 17:21:13 +0100 Subject: [PATCH 27/32] use the direct error and std dev instead of the percentage way --- controller_manager/src/controller_manager.cpp | 38 ++++++++----------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 77ed533207..01fd0306e7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3092,9 +3092,9 @@ void ControllerManager::controller_activity_diagnostic_callback( const double exec_time_mean_error_threshold = this->get_parameter_or(param_prefix + exec_time_suffix + mean_suffix + ".error", 2000.0); const double exec_time_std_dev_warn_threshold = - this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".warn", 5.0); + this->get_parameter_or(param_prefix + exec_time_suffix + std_dev_suffix + ".warn", 100.0); const double exec_time_std_dev_error_threshold = - this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".error", 10.0); + this->get_parameter_or(param_prefix + exec_time_suffix + std_dev_suffix + ".error", 200.0); auto make_stats_string = [](const auto & statistics_data, const std::string & measurement_unit) -> std::string @@ -3102,7 +3102,7 @@ void ControllerManager::controller_activity_diagnostic_callback( std::ostringstream oss; oss << std::fixed << std::setprecision(2); oss << "Avg: " << statistics_data.average << " [" << statistics_data.min << " - " - << statistics_data.max << "]" << " " << measurement_unit + << statistics_data.max << "] " << measurement_unit << ", StdDev: " << statistics_data.standard_deviation; return oss.str(); }; @@ -3131,22 +3131,20 @@ void ControllerManager::controller_activity_diagnostic_callback( { stat.add( controllers[i].info.name + periodicity_suffix, - make_stats_string(periodicity_stats, "Hz")); - const double periodicity_percentage = - (periodicity_stats.average / static_cast(controllers[i].c->get_update_rate())) * - 100; - const double periodicity_std_dev_percentage = - (periodicity_stats.standard_deviation / periodicity_stats.average) * 100.0; + make_stats_string(periodicity_stats, "Hz") + + " -> Desired : " + std::to_string(controllers[i].c->get_update_rate()) + " Hz"); + const double periodicity_error = std::abs( + periodicity_stats.average - static_cast(controllers[i].c->get_update_rate())); if ( - periodicity_percentage < periodicity_mean_error_threshold || - periodicity_std_dev_percentage > periodicity_std_dev_error_threshold) + periodicity_error > periodicity_mean_error_threshold || + periodicity_stats.standard_deviation > periodicity_std_dev_error_threshold) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); } else if ( - periodicity_percentage < periodicity_mean_warn_threshold || - periodicity_std_dev_percentage > periodicity_std_dev_warn_threshold) + periodicity_error > periodicity_mean_warn_threshold || + periodicity_stats.standard_deviation > periodicity_std_dev_warn_threshold) { if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) { @@ -3155,22 +3153,17 @@ void ControllerManager::controller_activity_diagnostic_callback( add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); } } - const double max_exp_exec_time = - is_async ? 1.e6 / controllers[i].c->get_update_rate() : 0.0; + const double max_exp_exec_time = is_async ? 1.e6 / controllers[i].c->get_update_rate() : 0.0; if ( (exec_time_stats.average - max_exp_exec_time) > exec_time_mean_error_threshold || - (max_exp_exec_time > 0.0 && - (exec_time_stats.standard_deviation / exec_time_stats.average) * 100.0 > - exec_time_std_dev_error_threshold)) + exec_time_stats.standard_deviation > exec_time_std_dev_error_threshold) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; high_exec_time_controllers.push_back(controllers[i].info.name); } else if ( (exec_time_stats.average - max_exp_exec_time) > exec_time_mean_warn_threshold || - (max_exp_exec_time > 0.0 && - (exec_time_stats.standard_deviation / exec_time_stats.average) * 100.0 > - exec_time_std_dev_warn_threshold)) + exec_time_stats.standard_deviation > exec_time_std_dev_warn_threshold) { if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) { @@ -3194,7 +3187,8 @@ void ControllerManager::controller_activity_diagnostic_callback( high_exec_time_controllers_string.append(" "); } stat.mergeSummary( - level, "\nControllers with high execution time : [ " + high_exec_time_controllers_string + "]"); + level, + "\nControllers with high execution time : [ " + high_exec_time_controllers_string + "]"); } if (!bad_periodicity_async_controllers.empty()) { From b5175a452c165bbc338b37301b0b0b04eac98dfa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 17:21:53 +0100 Subject: [PATCH 28/32] Change the parameter to mean_error --- controller_manager/src/controller_manager.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 01fd0306e7..0484bc0bcf 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3073,24 +3073,24 @@ void ControllerManager::controller_activity_diagnostic_callback( const std::string param_prefix = "diagnostics.threshold.controllers"; const std::string periodicity_suffix = ".periodicity"; const std::string exec_time_suffix = ".execution_time"; - const std::string mean_suffix = ".mean"; + const std::string mean_error_suffix = ".mean_error"; const std::string std_dev_suffix = ".standard_deviation"; const std::string state_suffix = ".state"; // Get threshold values from param server const double periodicity_mean_warn_threshold = - this->get_parameter_or(param_prefix + periodicity_suffix + mean_suffix + ".warn", 5.0); + this->get_parameter_or(param_prefix + periodicity_suffix + mean_error_suffix + ".warn", 5.0); const double periodicity_mean_error_threshold = - this->get_parameter_or(param_prefix + periodicity_suffix + mean_suffix + ".error", 10.0); + this->get_parameter_or(param_prefix + periodicity_suffix + mean_error_suffix + ".error", 10.0); const double periodicity_std_dev_warn_threshold = this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".warn", 5.0); const double periodicity_std_dev_error_threshold = this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".error", 10.0); const double exec_time_mean_warn_threshold = - this->get_parameter_or(param_prefix + exec_time_suffix + mean_suffix + ".warn", 1000.0); + this->get_parameter_or(param_prefix + exec_time_suffix + mean_error_suffix + ".warn", 1000.0); const double exec_time_mean_error_threshold = - this->get_parameter_or(param_prefix + exec_time_suffix + mean_suffix + ".error", 2000.0); + this->get_parameter_or(param_prefix + exec_time_suffix + mean_error_suffix + ".error", 2000.0); const double exec_time_std_dev_warn_threshold = this->get_parameter_or(param_prefix + exec_time_suffix + std_dev_suffix + ".warn", 100.0); const double exec_time_std_dev_error_threshold = From 3d7e24405aa6ddc81f1f05ab0dd2419f3b9bcc3a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 17:32:32 +0100 Subject: [PATCH 29/32] Apply the statistics to the controller manager periodicity --- controller_manager/src/controller_manager.cpp | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0484bc0bcf..7aef45dcdb 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3291,6 +3291,38 @@ void ControllerManager::controller_manager_diagnostic_callback( "Resource Manager is not initialized properly!"); } } + + const std::string param_prefix = "diagnostics.threshold.controller_manager"; + const std::string periodicity_suffix = ".periodicity"; + const std::string mean_error_suffix = ".mean_error"; + const std::string std_dev_suffix = ".standard_deviation"; + + // Get threshold values from param server + const double periodicity_mean_warn_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + mean_error_suffix + ".warn", 5.0); + const double periodicity_mean_error_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + mean_error_suffix + ".error", 10.0); + const double periodicity_std_dev_warn_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".warn", 5.0); + const double periodicity_std_dev_error_threshold = + this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".error", 10.0); + + const double periodicity_error = std::abs(periodicity_stats_.Average() - get_update_rate()); + const std::string diag_summary = + "Controller Manager has bad periodicity : " + std::to_string(periodicity_stats_.Average()) + + " Hz. Expected consistent " + std::to_string(get_update_rate()) + " Hz"; + if ( + periodicity_error > periodicity_mean_error_threshold || + periodicity_stats_.StandardDeviation() > periodicity_std_dev_error_threshold) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, diag_summary); + } + else if ( + periodicity_error > periodicity_mean_warn_threshold || + periodicity_stats_.StandardDeviation() > periodicity_std_dev_warn_threshold) + { + stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_summary); + } } void ControllerManager::update_list_with_controller_chain( From 0f2af8dff486cc89d1f267fb2cf7f090b4d388c6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 17:38:42 +0100 Subject: [PATCH 30/32] Use from a variable instead of calling methods everytime --- controller_manager/src/controller_manager.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 7aef45dcdb..be6ab2fee3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3266,13 +3266,13 @@ void ControllerManager::controller_manager_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { const std::string periodicity_stat_name = "periodicity"; + const auto cm_stats = periodicity_stats_.GetStatistics(); stat.add("update_rate", std::to_string(get_update_rate())); - stat.add(periodicity_stat_name + ".average", std::to_string(periodicity_stats_.Average())); + stat.add(periodicity_stat_name + ".average", std::to_string(cm_stats.average)); stat.add( - periodicity_stat_name + ".standard_deviation", - std::to_string(periodicity_stats_.StandardDeviation())); - stat.add(periodicity_stat_name + ".min", std::to_string(periodicity_stats_.Min())); - stat.add(periodicity_stat_name + ".max", std::to_string(periodicity_stats_.Max())); + periodicity_stat_name + ".standard_deviation", std::to_string(cm_stats.standard_deviation)); + stat.add(periodicity_stat_name + ".min", std::to_string(cm_stats.min)); + stat.add(periodicity_stat_name + ".max", std::to_string(cm_stats.max)); if (is_resource_manager_initialized()) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Controller Manager is running"); @@ -3307,19 +3307,19 @@ void ControllerManager::controller_manager_diagnostic_callback( const double periodicity_std_dev_error_threshold = this->get_parameter_or(param_prefix + periodicity_suffix + std_dev_suffix + ".error", 10.0); - const double periodicity_error = std::abs(periodicity_stats_.Average() - get_update_rate()); + const double periodicity_error = std::abs(cm_stats.average - get_update_rate()); const std::string diag_summary = - "Controller Manager has bad periodicity : " + std::to_string(periodicity_stats_.Average()) + + "Controller Manager has bad periodicity : " + std::to_string(cm_stats.average) + " Hz. Expected consistent " + std::to_string(get_update_rate()) + " Hz"; if ( periodicity_error > periodicity_mean_error_threshold || - periodicity_stats_.StandardDeviation() > periodicity_std_dev_error_threshold) + cm_stats.standard_deviation > periodicity_std_dev_error_threshold) { stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, diag_summary); } else if ( periodicity_error > periodicity_mean_warn_threshold || - periodicity_stats_.StandardDeviation() > periodicity_std_dev_warn_threshold) + cm_stats.standard_deviation > periodicity_std_dev_warn_threshold) { stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_summary); } From 32d747575780ca693c88de1331c9aaefee382ec7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 11 Nov 2024 19:22:33 +0100 Subject: [PATCH 31/32] add first version of the documentation --- controller_manager/doc/userdoc.rst | 48 ++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index fa673cff0a..259c371ff8 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -99,6 +99,54 @@ 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 update loop. + 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 update loop. + 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 update loop. + 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 update loop. + 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. + Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From 12efdef0cb32ecdc6e72c9644872a39a6024b709 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 11 Nov 2024 20:02:59 +0100 Subject: [PATCH 32/32] Update documentation --- controller_manager/doc/userdoc.rst | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 259c371ff8..f5b340cfb4 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -100,19 +100,19 @@ update_rate (mandatory; integer) 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 update loop. + 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 update loop. + 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 update loop. + 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 update loop. + 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 @@ -147,6 +147,11 @@ 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^