From 15b337728d7c3bd5c58020549d7ed848168fe36f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 25 Sep 2024 17:00:36 +0200 Subject: [PATCH 01/42] 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 b6eb1d4785ac0acec2c2e43ceb75cc7f400f8edd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 23:40:50 +0100 Subject: [PATCH 02/42] 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 c25866e6e7ba15f2b3e06af9b51567e2babf29e3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 23:42:20 +0100 Subject: [PATCH 03/42] 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 66135e6643f98787e84a387700bf7b22fed3ab08 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 23:43:44 +0100 Subject: [PATCH 04/42] 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 6531ab24ae321c5e4543219be11198597b01764a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 6 Nov 2024 23:44:45 +0100 Subject: [PATCH 05/42] 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 348b590305c335cbe48ed44ca62f39541c8fc526 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 7 Nov 2024 08:28:19 +0100 Subject: [PATCH 06/42] 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 8129cf0a46f0864ce5285e84e6e64ec776188529 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 7 Nov 2024 08:39:15 +0100 Subject: [PATCH 07/42] 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 e873706849581c9cbc4f69db747598c81ec6bb12 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 00:42:18 +0100 Subject: [PATCH 08/42] 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 1a57fdf4ef34d4dd65ce2bbde69770ac986d5504 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 16:46:05 +0100 Subject: [PATCH 09/42] 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 eae02c1ea25067bdb6e525faa786faa5d5e33c59 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 16:46:41 +0100 Subject: [PATCH 10/42] 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 2856a29e4c857ca2e341af00764cfc5e2e0d39f5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 16:54:41 +0100 Subject: [PATCH 11/42] 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 be6414169a5f2562a2211c116f5d672259c720ec Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 8 Nov 2024 16:59:52 +0100 Subject: [PATCH 12/42] 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 2c77713c861dc393d9e3a034162bef2d9b4b2677 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 9 Nov 2024 00:08:45 +0100 Subject: [PATCH 13/42] 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 b1ad07e71045a19bb20aabe3cd77c74146f84354 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:34:27 +0100 Subject: [PATCH 14/42] 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 9e45ce04f990c187dc9ae8cb94c70ad1cd66101a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:36:21 +0100 Subject: [PATCH 15/42] 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 62f583238234d2195bf4b1799005a09728a06aa5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:36:47 +0100 Subject: [PATCH 16/42] 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 6e9be7d9613a3272c0619a013155503743d27ca0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:38:26 +0100 Subject: [PATCH 17/42] 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 02eb08dedb25904e8fe5b200748c6061302676cd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:54:16 +0100 Subject: [PATCH 18/42] 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 f13289aa4e33220b46aebe9987b05536a8d43f99 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 00:54:27 +0100 Subject: [PATCH 19/42] 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 186b5ff73a4ad4b7a2b297f6b9929b88c3a18032 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 01:07:24 +0100 Subject: [PATCH 20/42] 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 0d5b4e8f0975c587284cfa5b54c301e57dad0a4e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 01:09:56 +0100 Subject: [PATCH 21/42] 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 a9fa9ab229875b769434729e8ef1e0fa48ee1692 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 12:39:36 +0100 Subject: [PATCH 22/42] 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 cad7a742195231caface793bcda69c5633a47e4c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 12:40:24 +0100 Subject: [PATCH 23/42] 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 3f9f9eb7c1f78085be193333bbb6d89b58e9b407 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 12:41:14 +0100 Subject: [PATCH 24/42] 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 5969df7571039c450a83ebc8e89077b61423b692 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 12:42:00 +0100 Subject: [PATCH 25/42] 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 9ba1f50f4c81f943d1e9a6e4e14f94d87938bde9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 16:59:36 +0100 Subject: [PATCH 26/42] 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 3f5faaded5e00fdfb4ee1b9973b836d6f80c3d66 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 17:21:13 +0100 Subject: [PATCH 27/42] 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 d22e206fbf00cf66f94d8f571b70c57b9bb98c19 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 17:21:53 +0100 Subject: [PATCH 28/42] 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 853abe7dd421c03d829f22d2c820b40fcce7020a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 17:32:32 +0100 Subject: [PATCH 29/42] 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 615ccc6752c5aa4e8f2c06afc376873dbd3fab05 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 17:38:42 +0100 Subject: [PATCH 30/42] 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 cb00998a7f1dbc31b346dbec9aa677922aa65ca8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 11 Nov 2024 19:22:33 +0100 Subject: [PATCH 31/42] 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 aee43bd32e33d18946404b627327714ba8824a2d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 11 Nov 2024 20:02:59 +0100 Subject: [PATCH 32/42] 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From 973decc51d8f2e6269ae50e00cd582824639e612 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 12 Nov 2024 23:08:18 +0100 Subject: [PATCH 33/42] Add first version of the GPL configuration --- controller_manager/CMakeLists.txt | 10 ++- .../controller_manager/controller_manager.hpp | 3 + controller_manager/package.xml | 1 + .../src/controller_manager_parameters.yaml | 81 +++++++++++++++++++ 4 files changed, 94 insertions(+), 1 deletion(-) create mode 100644 controller_manager/src/controller_manager_parameters.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index ccf4755770..5035f17ec0 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -17,6 +17,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS realtime_tools std_msgs libstatistics_collector + generate_parameter_library ) find_package(ament_cmake REQUIRED) @@ -28,6 +29,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(controller_manager_parameters + src/controller_manager_parameters.yaml +) + add_library(controller_manager SHARED src/controller_manager.cpp ) @@ -36,6 +41,9 @@ target_include_directories(controller_manager PUBLIC $ $ ) +target_link_libraries(controller_manager PUBLIC + controller_manager_parameters +) ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, @@ -234,7 +242,7 @@ install( DESTINATION include/controller_manager ) install( - TARGETS controller_manager + TARGETS controller_manager controller_manager_parameters EXPORT export_controller_manager RUNTIME DESTINATION bin LIBRARY DESTINATION lib diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 0595e69a68..80bd37d6dc 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -48,6 +48,8 @@ #include "rclcpp/node.hpp" #include "std_msgs/msg/string.hpp" +#include "controller_manager_parameters.hpp" + namespace controller_manager { using ControllersListIterator = std::vector::const_iterator; @@ -473,6 +475,7 @@ class ControllerManager : public rclcpp::Node */ rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const; + std::shared_ptr cm_param_listener_; diagnostic_updater::Updater diagnostics_updater_; std::shared_ptr executor_; diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c89923257c..42211e4e40 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -29,6 +29,7 @@ ros2run std_msgs libstatistics_collector + generate_parameter_library ament_cmake_gmock ament_cmake_pytest diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml new file mode 100644 index 0000000000..71ed5d2816 --- /dev/null +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -0,0 +1,81 @@ +controller_manager: + update_rate: { + type: int, + default_value: 100, + read_only: true, + description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controller and writes commands to hardware." + } + + diagnostics: + threshold: + controller_manager: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "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." + } + error: { + type: double, + default_value: 10.0, + description: "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." + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "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." + } + error: { + type: double, + default_value: 10.0, + description: "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." + } + controllers: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "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. This 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." + } + error: { + type: double, + default_value: 10.0, + description: "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. This 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." + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "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. This 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." + } + error: { + type: double, + default_value: 10.0, + description: "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. This 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." + } + execution_time: + mean_error: + warn: { + type: double, + default_value: 1000.0, + description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. 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" + } + error: { + type: double, + default_value: 2000.0, + description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. 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" + } + standard_deviation: + warn: { + type: double, + default_value: 100.0, + description: "The warning threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published." + } + error: { + type: double, + default_value: 200.0, + description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published." + } From 8d65eb3123798993f347fa925ff96f0971d4a8ff Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 13 Nov 2024 08:36:16 +0100 Subject: [PATCH 34/42] Use forward declaration for ParamListener --- .../include/controller_manager/controller_manager.hpp | 3 +-- controller_manager/src/controller_manager.cpp | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 80bd37d6dc..321c655c6e 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -48,10 +48,9 @@ #include "rclcpp/node.hpp" #include "std_msgs/msg/string.hpp" -#include "controller_manager_parameters.hpp" - namespace controller_manager { +class ParamListener; using ControllersListIterator = std::vector::const_iterator; CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index be6ab2fee3..cca88cef8c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -28,6 +28,8 @@ #include "rclcpp/version.h" #include "rclcpp_lifecycle/state.hpp" +#include "controller_manager_parameters.hpp" + namespace // utility { static constexpr const char * kControllerInterfaceNamespace = "controller_interface"; From 8d2a9c0f409ac75b7013e940f2ebfa30d32e582d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 14 Nov 2024 00:03:27 +0100 Subject: [PATCH 35/42] Use parameters from the GPL library --- .../controller_manager/controller_manager.hpp | 6 +- controller_manager/src/controller_manager.cpp | 111 +++++++++--------- 2 files changed, 58 insertions(+), 59 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 321c655c6e..332d565238 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -51,6 +51,7 @@ namespace controller_manager { class ParamListener; +class Params; using ControllersListIterator = std::vector::const_iterator; CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); @@ -347,7 +348,7 @@ class ControllerManager : public rclcpp::Node // Per controller update rate support unsigned int update_loop_counter_ = 0; - unsigned int update_rate_ = 100; + unsigned int update_rate_; std::vector> chained_controllers_configuration_; std::unique_ptr resource_manager_; @@ -358,6 +359,8 @@ class ControllerManager : public rclcpp::Node const std::string & command_interface); void init_controller_manager(); + void initialize_parameters(); + /** * Clear request lists used when switching controllers. The lists are shared between "callback" * and "control loop" threads. @@ -475,6 +478,7 @@ class ControllerManager : public rclcpp::Node rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const; std::shared_ptr cm_param_listener_; + std::shared_ptr params_; diagnostic_updater::Updater diagnostics_updater_; std::shared_ptr executor_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index cca88cef8c..d1dc439199 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -239,6 +239,7 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); init_controller_manager(); } @@ -247,10 +248,6 @@ ControllerManager::ControllerManager( bool activate_all_hw_components, const std::string & manager_node_name, const std::string & node_namespace, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, node_namespace, options), - update_rate_(get_parameter_or("update_rate", 100)), - resource_manager_(std::make_unique( - urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), - activate_all_hw_components, update_rate_)), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( @@ -260,6 +257,10 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); + resource_manager_ = std::make_unique( + urdf, this->get_node_clock_interface(), this->get_node_logging_interface(), + activate_all_hw_components, params_->update_rate); init_controller_manager(); } @@ -278,18 +279,13 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), cm_node_options_(options) { + initialize_parameters(); init_controller_manager(); } void ControllerManager::init_controller_manager() { // Get parameters needed for RT "update" loop to work - if (!get_parameter("update_rate", update_rate_)) - { - RCLCPP_WARN( - get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); - } - if (is_resource_manager_initialized()) { init_services(); @@ -327,6 +323,25 @@ void ControllerManager::init_controller_manager() &ControllerManager::controller_manager_diagnostic_callback); } +void ControllerManager::initialize_parameters() +{ + // Initialize parameters + try + { + cm_param_listener_ = std::make_shared( + this->get_node_parameters_interface(), this->get_logger()); + params_ = std::make_shared(cm_param_listener_->get_params()); + update_rate_ = static_cast(params_->update_rate); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + this->get_logger(), + "Exception thrown while initializing controller manager parameters: %s \n", e.what()); + throw e; + } +} + void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) { RCLCPP_INFO(get_logger(), "Received robot description from topic."); @@ -3072,31 +3087,14 @@ 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_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_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 exec_time_mean_warn_threshold = - 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_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 = - this->get_parameter_or(param_prefix + exec_time_suffix + std_dev_suffix + ".error", 200.0); + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } auto make_stats_string = [](const auto & statistics_data, const std::string & measurement_unit) -> std::string @@ -3138,15 +3136,19 @@ void ControllerManager::controller_activity_diagnostic_callback( const double periodicity_error = std::abs( periodicity_stats.average - static_cast(controllers[i].c->get_update_rate())); if ( - periodicity_error > periodicity_mean_error_threshold || - periodicity_stats.standard_deviation > periodicity_std_dev_error_threshold) + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.error || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.error) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; add_element_to_list(bad_periodicity_async_controllers, controllers[i].info.name); } else if ( - periodicity_error > periodicity_mean_warn_threshold || - periodicity_stats.standard_deviation > periodicity_std_dev_warn_threshold) + periodicity_error > + params_->diagnostics.threshold.controllers.periodicity.mean_error.warn || + periodicity_stats.standard_deviation > + params_->diagnostics.threshold.controllers.periodicity.standard_deviation.warn) { if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) { @@ -3157,15 +3159,19 @@ void ControllerManager::controller_activity_diagnostic_callback( } 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 || - exec_time_stats.standard_deviation > exec_time_std_dev_error_threshold) + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.error || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.error) { 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 || - exec_time_stats.standard_deviation > exec_time_std_dev_warn_threshold) + (exec_time_stats.average - max_exp_exec_time) > + params_->diagnostics.threshold.controllers.execution_time.mean_error.warn || + exec_time_stats.standard_deviation > + params_->diagnostics.threshold.controllers.execution_time.standard_deviation.warn) { if (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) { @@ -3294,34 +3300,23 @@ void ControllerManager::controller_manager_diagnostic_callback( } } - 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(cm_stats.average - get_update_rate()); const std::string diag_summary = "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 || - cm_stats.standard_deviation > periodicity_std_dev_error_threshold) + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.error || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.error) { stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, diag_summary); } else if ( - periodicity_error > periodicity_mean_warn_threshold || - cm_stats.standard_deviation > periodicity_std_dev_warn_threshold) + periodicity_error > + params_->diagnostics.threshold.controller_manager.periodicity.mean_error.warn || + cm_stats.standard_deviation > + params_->diagnostics.threshold.controller_manager.periodicity.standard_deviation.warn) { stat.mergeSummary(diagnostic_msgs::msg::DiagnosticStatus::WARN, diag_summary); } From a125ba8dfa0bc7122adf646d60e5df15e0926d0b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 17 Nov 2024 18:40:46 +0100 Subject: [PATCH 36/42] Add validators to the parameters --- .../src/controller_manager_parameters.yaml | 60 +++++++++++++++---- 1 file changed, 48 insertions(+), 12 deletions(-) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 71ed5d2816..67c175e808 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -14,23 +14,35 @@ controller_manager: warn: { type: double, default_value: 5.0, - description: "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." + description: "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.", + validation: { + gt<>: 0.0, + } } error: { type: double, default_value: 10.0, - description: "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." + description: "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.", + validation: { + gt<>: 0.0, + } } standard_deviation: warn: { type: double, default_value: 5.0, - description: "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." + description: "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.", + validation: { + gt<>: 0.0, + } } error: { type: double, default_value: 10.0, - description: "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." + description: "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.", + validation: { + gt<>: 0.0, + } } controllers: periodicity: @@ -38,44 +50,68 @@ controller_manager: warn: { type: double, default_value: 5.0, - description: "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. This 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." + description: "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. This 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.", + validation: { + gt<>: 0.0, + } } error: { type: double, default_value: 10.0, - description: "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. This 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." + description: "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. This 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.", + validation: { + gt<>: 0.0, + } } standard_deviation: warn: { type: double, default_value: 5.0, - description: "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. This 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." + description: "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. This 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.", + validation: { + gt<>: 0.0, + } } error: { type: double, default_value: 10.0, - description: "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. This 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." + description: "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. This 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.", + validation: { + gt<>: 0.0, + } } execution_time: mean_error: warn: { type: double, default_value: 1000.0, - description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. 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" + description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. 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", + validation: { + gt<>: 0.0, + } } error: { type: double, default_value: 2000.0, - description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. 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" + description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. 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", + validation: { + gt<>: 0.0, + } } standard_deviation: warn: { type: double, default_value: 100.0, - description: "The warning threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published." + description: "The warning threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } } error: { type: double, default_value: 200.0, - description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published." + description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } } From ccf482c350e0171f2ab88b25e567210b6d6d58b6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 17 Nov 2024 18:41:06 +0100 Subject: [PATCH 37/42] Add hardware_components_initial_state parameters to GPL --- .../src/controller_manager_parameters.yaml | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 67c175e808..b5f9dfdc0a 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -6,6 +6,27 @@ controller_manager: description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controller and writes commands to hardware." } + hardware_components_initial_state: + unconfigured: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be only loaded immediately when controller manager is started.", + read_only: true, + validation: { + unique<>: null, + } + } + + inactive: { + type: string_array, + default_value: [], + description: "Defines which hardware components will be configured immediately when controller manager is started.", + read_only: true, + validation: { + unique<>: null, + } + } + diagnostics: threshold: controller_manager: From 33d531f2de3e42a3ab0ce71815b970fea069cadc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 17 Nov 2024 18:46:50 +0100 Subject: [PATCH 38/42] Use hardware_components_initial_state parameters from the GPL directly --- controller_manager/src/controller_manager.cpp | 54 +++++++++---------- 1 file changed, 25 insertions(+), 29 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d1dc439199..6ffb154cb4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -383,51 +383,47 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript using lifecycle_msgs::msg::State; auto set_components_to_state = - [&](const std::string & parameter_name, rclcpp_lifecycle::State state) + [&](const std::vector & components_to_set, rclcpp_lifecycle::State state) { - std::vector components_to_set = std::vector({}); - if (get_parameter(parameter_name, components_to_set)) + for (const auto & component : components_to_set) { - for (const auto & component : components_to_set) + if (component.empty()) { - if (component.empty()) - { - continue; - } - if (components_to_activate.find(component) == components_to_activate.end()) - { - RCLCPP_WARN( - get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", - component.c_str(), state.label().c_str()); - } - else + continue; + } + if (components_to_activate.find(component) == components_to_activate.end()) + { + RCLCPP_WARN( + get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", + component.c_str(), state.label().c_str()); + } + else + { + RCLCPP_INFO( + get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), + state.label().c_str()); + if ( + resource_manager_->set_component_state(component, state) == + hardware_interface::return_type::ERROR) { - RCLCPP_INFO( - get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), - state.label().c_str()); - if ( - resource_manager_->set_component_state(component, state) == - hardware_interface::return_type::ERROR) - { - throw std::runtime_error( - "Failed to set the initial state of the component : " + component + " to " + - state.label()); - } - components_to_activate.erase(component); + throw std::runtime_error( + "Failed to set the initial state of the component : " + component + " to " + + state.label()); } + components_to_activate.erase(component); } } }; // unconfigured (loaded only) set_components_to_state( - "hardware_components_initial_state.unconfigured", + params_->hardware_components_initial_state.unconfigured, rclcpp_lifecycle::State( State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); // inactive (configured) set_components_to_state( - "hardware_components_initial_state.inactive", + params_->hardware_components_initial_state.inactive, rclcpp_lifecycle::State( State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); From 2af19ea9b032ecdaa7e1a3e4f1e667e2e031144f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 17 Nov 2024 18:55:47 +0100 Subject: [PATCH 39/42] Add parameters_context.yaml and move parameters documentation --- .../doc/parameters_context.yaml | 21 +++++ controller_manager/doc/userdoc.rst | 81 ++----------------- 2 files changed, 27 insertions(+), 75 deletions(-) create mode 100644 controller_manager/doc/parameters_context.yaml diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml new file mode 100644 index 0000000000..f5df997661 --- /dev/null +++ b/controller_manager/doc/parameters_context.yaml @@ -0,0 +1,21 @@ +hardware_components_initial_state: | + Map of parameters for controlled lifecycle management of hardware components. + The names of the components are defined as attribute of ````-tag in ``robot_description``. + Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. + Detailed explanation of each parameter is given below. + The full structure of the map is given in the following example: + + .. code-block:: yaml + + hardware_components_initial_state: + unconfigured: + - "arm1" + - "arm2" + inactive: + - "base3" + +diagnostics.threshold.controllers.periodicity: | + The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity. + +diagnostics.threshold.controllers.execution_time: | + The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle. diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index f5b340cfb4..9285d3ecd8 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -57,32 +57,6 @@ robot_description [std_msgs::msg::String] Parameters ----------- -hardware_components_initial_state - Map of parameters for controlled lifecycle management of hardware components. - The names of the components are defined as attribute of ````-tag in ``robot_description``. - Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. - Detailed explanation of each parameter is given below. - The full structure of the map is given in the following example: - -.. code-block:: yaml - - hardware_components_initial_state: - unconfigured: - - "arm1" - - "arm2" - inactive: - - "base3" - -hardware_components_initial_state.unconfigured (optional; list; default: empty) - Defines which hardware components will be only loaded immediately when controller manager is started. - -hardware_components_initial_state.inactive (optional; list; default: empty) - Defines which hardware components will be configured immediately when controller manager is started. - -update_rate (mandatory; integer) - The frequency of controller manager's real-time update loop. - This loop reads states from hardware, updates controller and writes commands to hardware. - .type Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. @@ -99,58 +73,15 @@ update_rate (mandatory; integer) The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation. It is recommended to test the fallback strategy in simulation before deploying it on the real robot. -diagnostics.threshold.controller_manager.periodicity.mean_error.warn - The warning threshold for the mean error of the controller manager's periodicity. - If the mean error exceeds this threshold, a warning diagnostic will be published. - -diagnostics.threshold.controller_manager.periodicity.mean_error.error - The error threshold for the mean error of the controller manager's periodicity. - If the mean error exceeds this threshold, an error diagnostic will be published. - -diagnostics.threshold.controller_manager.periodicity.standard_deviation.warn - The warning threshold for the standard deviation of the controller manager's periodicity. - If the standard deviation exceeds this threshold, a warning diagnostic will be published. - -diagnostics.threshold.controller_manager.periodicity.standard_deviation.error - The error threshold for the standard deviation of the controller manager's periodicity. - If the standard deviation exceeds this threshold, an error diagnostic will be published. - -diagnostics.threshold.controllers.periodicity.mean_error.warn - The warning threshold for the mean error of the controller update loop. - If the mean error exceeds this threshold, a warning diagnostic will be published. - -diagnostics.threshold.controllers.periodicity.mean_error.error - The error threshold for the mean error of the controller update loop. - If the mean error exceeds this threshold, an error diagnostic will be published. - -diagnostics.threshold.controllers.periodicity.standard_deviation.warn - The warning threshold for the standard deviation of the controller update loop. - If the standard deviation exceeds this threshold, a warning diagnostic will be published. - -diagnostics.threshold.controllers.periodicity.standard_deviation.error - The error threshold for the standard deviation of the controller update loop. - If the standard deviation exceeds this threshold, an error diagnostic will be published. - -diagnostics.threshold.controllers.execution_time.mean_error.warn - The warning threshold for the mean error of the controller execution time. - If the mean error exceeds this threshold, a warning diagnostic will be published. - -diagnostics.threshold.controllers.execution_time.mean_error.error - The error threshold for the mean error of the controller execution time. - If the mean error exceeds this threshold, an error diagnostic will be published. - -diagnostics.threshold.controllers.execution_time.standard_deviation.warn - The warning threshold for the standard deviation of the controller execution time. - If the standard deviation exceeds this threshold, a warning diagnostic will be published. +.. generate_parameter_library_details:: + ../src/controller_manager_parameters.yaml + parameters_context.yaml -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. +**An example parameter file:** -.. 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. +.. generate_parameter_library_default:: + ../src/controller_manager_parameters.yaml - 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 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From faf9c7596c3b5683bdb9a3c4cc762395f5395751 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 17 Nov 2024 20:42:06 +0100 Subject: [PATCH 40/42] Fix hardware management service tests --- controller_manager/doc/parameters_context.yaml | 14 +++++++------- controller_manager/src/controller_manager.cpp | 5 +++++ .../src/controller_manager_parameters.yaml | 2 -- .../test/test_hardware_management_srvs.cpp | 4 ++-- 4 files changed, 14 insertions(+), 11 deletions(-) diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml index f5df997661..a015765c79 100644 --- a/controller_manager/doc/parameters_context.yaml +++ b/controller_manager/doc/parameters_context.yaml @@ -5,14 +5,14 @@ hardware_components_initial_state: | Detailed explanation of each parameter is given below. The full structure of the map is given in the following example: - .. code-block:: yaml + .. code-block:: yaml - hardware_components_initial_state: - unconfigured: - - "arm1" - - "arm2" - inactive: - - "base3" + hardware_components_initial_state: + unconfigured: + - "arm1" + - "arm2" + inactive: + - "base3" diagnostics.threshold.controllers.periodicity: | The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6ffb154cb4..d9de54ea38 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -415,6 +415,11 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript } }; + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + // unconfigured (loaded only) set_components_to_state( params_->hardware_components_initial_state.unconfigured, diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index b5f9dfdc0a..dd4bf1bc1b 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -11,7 +11,6 @@ controller_manager: type: string_array, default_value: [], description: "Defines which hardware components will be only loaded immediately when controller manager is started.", - read_only: true, validation: { unique<>: null, } @@ -21,7 +20,6 @@ controller_manager: type: string_array, default_value: [], description: "Defines which hardware components will be configured immediately when controller manager is started.", - read_only: true, validation: { unique<>: null, } diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index c7b67e0cfe..bdd48f15ae 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -65,6 +65,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs cm_ = std::make_shared(executor_, TEST_CM_NAME); run_updater_ = false; + SetUpSrvsCMExecutor(); cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured", std::vector({TEST_SYSTEM_HARDWARE_NAME}))); @@ -72,11 +73,10 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "hardware_components_initial_state.inactive", std::vector({TEST_SENSOR_HARDWARE_NAME}))); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); - - SetUpSrvsCMExecutor(); } void check_component_fileds( From 91f549a7f16aed0cac09215578310c32e1e7eeeb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 2 Dec 2024 21:04:40 +0100 Subject: [PATCH 41/42] Add suggestions from code review Co-authored-by: Bence Magyar --- controller_manager/src/controller_manager_parameters.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index dd4bf1bc1b..1bb9b152b1 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -3,14 +3,14 @@ controller_manager: type: int, default_value: 100, read_only: true, - description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controller and writes commands to hardware." + description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware." } hardware_components_initial_state: unconfigured: { type: string_array, default_value: [], - description: "Defines which hardware components will be only loaded immediately when controller manager is started.", + description: "Defines which hardware components will be only loaded when controller manager is started. These hardware components will need to be configured and activated manually or via a hardware spawner.", validation: { unique<>: null, } @@ -19,7 +19,7 @@ controller_manager: inactive: { type: string_array, default_value: [], - description: "Defines which hardware components will be configured immediately when controller manager is started.", + description: "Defines which hardware components will be configured when controller manager is started. These hardware components will need to be activated manually or via a hardware spawner.", validation: { unique<>: null, } From 18e67f60c0d0a01c7665cb15d0fdb1fcc692ce72 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Dec 2024 15:19:01 +0100 Subject: [PATCH 42/42] Rename the variable `ok` to `successful` and add documentation --- .../controller_interface_base.hpp | 12 +++++++++++- .../src/controller_interface_base.cpp | 4 ++-- controller_manager/src/controller_manager.cpp | 2 +- 3 files changed, 14 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 bff7e204ad..e41779cf7b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -72,9 +72,19 @@ struct ControllerUpdateStats unsigned int failed_triggers; }; +/** + * Struct to store the status of the controller update method. + * The status contains information if the update was triggered successfully, the result of the + * update method and the execution duration of the update method. The status is used to provide + * feedback to the controller_manager. + * @var successful: true if the update was triggered successfully, false if not. + * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR. + * @var execution_time: duration of the execution of the update method. + * @var period: period of the update method. + */ struct ControllerUpdateStatus { - bool ok = true; + bool successful = 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 3f1cb73d13..065fe77061 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -176,7 +176,7 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( "The controller missed %u update cycles out of %u total triggers.", trigger_stats_.failed_triggers, trigger_stats_.total_triggers); } - status.ok = result.first; + status.successful = result.first; status.result = result.second; const auto execution_time = async_handler_->get_last_execution_time(); if (execution_time.count() > 0) @@ -191,7 +191,7 @@ ControllerUpdateStatus ControllerInterfaceBase::trigger_update( else { const auto start_time = std::chrono::steady_clock::now(); - status.ok = true; + status.successful = true; status.result = update(time, period); status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d469346c22..56e95c9f7b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2432,7 +2432,7 @@ controller_interface::return_type ControllerManager::update( { const auto trigger_result = loaded_controller.c->trigger_update(current_time, controller_actual_period); - trigger_status = trigger_result.ok; + trigger_status = trigger_result.successful; controller_ret = trigger_result.result; if (trigger_status && trigger_result.execution_time.has_value()) {