From 4c9e7e0b1b8eede26ea8d6dc92367bac1cd23237 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 1 Mar 2025 22:15:30 +0100 Subject: [PATCH 1/9] Add diagnostics of execution time and periodicity of the hardware components --- .../doc/parameters_context.yaml | 10 +- controller_manager/src/controller_manager.cpp | 171 ++++++++++- .../src/controller_manager_parameters.yaml | 71 +++++ .../include/hardware_interface/actuator.hpp | 8 + .../hardware_component_info.hpp | 15 +- .../include/hardware_interface/sensor.hpp | 5 + .../include/hardware_interface/system.hpp | 8 + .../types/statistics_types.hpp | 141 +++++++++ hardware_interface/src/actuator.cpp | 42 ++- hardware_interface/src/resource_manager.cpp | 23 ++ hardware_interface/src/sensor.cpp | 21 +- hardware_interface/src/system.cpp | 42 ++- .../test/test_components/test_actuator.cpp | 10 + .../test/test_components/test_system.cpp | 10 + .../test/test_resource_manager.cpp | 287 +++++++++++++++++- .../ros2_control_test_assets/descriptions.hpp | 49 +++ 16 files changed, 876 insertions(+), 37 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/types/statistics_types.hpp diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml index a015765c79..c9e95f6396 100644 --- a/controller_manager/doc/parameters_context.yaml +++ b/controller_manager/doc/parameters_context.yaml @@ -17,5 +17,11 @@ hardware_components_initial_state: | 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. +diagnostics.threshold.controllers.execution_time.mean_error: | + 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 to execute its update cycle. + +diagnostics.threshold.hardware_components.periodicity: | + The ``periodicity`` diagnostics will be published only for the asynchronous hardware components, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity. + +diagnostics.threshold.hardware_components.execution_time.mean_error: | + The ``execution_time`` diagnostics will be published for all hardware components. The ``mean_error`` for a synchronous hardware component will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous hardware component will be computed against its desired read/write period, as the hardware component can take a maximum of the desired period to execute the read/write cycle. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f89fe11a4b..399fcb6dd7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3388,12 +3388,21 @@ void ControllerManager::controller_activity_diagnostic_callback( void ControllerManager::hardware_components_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { + if (!is_resource_manager_initialized()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Resource manager is not yet initialized!"); + return; + } + bool all_active = true; bool atleast_one_hw_active = false; + const std::string read_cycle_suffix = ".read_cycle"; + const std::string write_cycle_suffix = ".write_cycle"; + const std::string state_suffix = ".state"; const auto & hw_components_info = resource_manager_->get_components_status(); for (const auto & [component_name, component_info] : hw_components_info) { - stat.add(component_name, component_info.state.label()); if (component_info.state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { all_active = false; @@ -3403,31 +3412,165 @@ void ControllerManager::hardware_components_diagnostic_callback( atleast_one_hw_active = true; } } - if (!is_resource_manager_initialized()) + if (hw_components_info.empty()) { stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Resource manager is not yet initialized!"); + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are loaded!"); + return; } - else if (hw_components_info.empty()) + else if (!atleast_one_hw_active) { stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are loaded!"); + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No hardware components are currently active"); + return; } - else + + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All hardware components are active" : "Not all hardware components are active"); + + 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 { - if (!atleast_one_hw_active) + 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(); + }; + + // Variable to define the overall status of the controller diagnostics + auto level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + std::vector high_exec_time_hw; + std::vector bad_periodicity_async_hw; + + for (const auto & [component_name, component_info] : hw_components_info) + { + stat.add(component_name + state_suffix, component_info.state.label()); + if (component_info.state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, - "No hardware components are currently active"); + all_active = false; } else { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::OK, all_active - ? "All hardware components are active" - : "Not all hardware components are active"); + atleast_one_hw_active = true; } + if (component_info.state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + auto update_stats = + [&bad_periodicity_async_hw, &high_exec_time_hw, &stat, &make_stats_string]( + const std::string & comp_name, const auto & statistics, + const std::string & statistics_type_suffix, auto & diag_level, const auto & comp_info, + const auto & params) + { + if (!statistics) + { + return; + } + const bool is_async = comp_info.is_async; + const std::string periodicity_suffix = ".periodicity"; + const std::string exec_time_suffix = ".execution_time"; + const auto periodicity_stats = statistics->periodicity.get_statistics(); + const auto exec_time_stats = statistics->execution_time.get_statistics(); + stat.add( + comp_name + statistics_type_suffix + exec_time_suffix, + make_stats_string(exec_time_stats, "us")); + if (is_async) + { + stat.add( + comp_name + statistics_type_suffix + periodicity_suffix, + make_stats_string(periodicity_stats, "Hz") + + " -> Desired : " + std::to_string(comp_info.rw_rate) + " Hz"); + const double periodicity_error = + std::abs(periodicity_stats.average - static_cast(comp_info.rw_rate)); + if ( + periodicity_error > + params->diagnostics.threshold.hardware_components.periodicity.mean_error.error || + periodicity_stats.standard_deviation > params->diagnostics.threshold.hardware_components + .periodicity.standard_deviation.error) + { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + add_element_to_list(bad_periodicity_async_hw, comp_name); + } + else if ( + periodicity_error > + params->diagnostics.threshold.hardware_components.periodicity.mean_error.warn || + periodicity_stats.standard_deviation > + params->diagnostics.threshold.hardware_components.periodicity.standard_deviation.warn) + { + if (diag_level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + add_element_to_list(bad_periodicity_async_hw, comp_name); + } + } + const double max_exp_exec_time = + is_async ? 1.e6 / static_cast(comp_info.rw_rate) : 0.0; + if ( + (exec_time_stats.average - max_exp_exec_time) > + params->diagnostics.threshold.hardware_components.execution_time.mean_error.error || + exec_time_stats.standard_deviation > params->diagnostics.threshold.hardware_components + .execution_time.standard_deviation.error) + { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + high_exec_time_hw.push_back(comp_name); + } + else if ( + (exec_time_stats.average - max_exp_exec_time) > + params->diagnostics.threshold.hardware_components.execution_time.mean_error.warn || + exec_time_stats.standard_deviation > params->diagnostics.threshold.hardware_components + .execution_time.standard_deviation.warn) + { + if (diag_level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + high_exec_time_hw.push_back(comp_name); + } + }; + + // For components : {actuator, sensor and system} + update_stats( + component_name, component_info.read_statistics, read_cycle_suffix, level, component_info, + params_); + // For components : {actuator and system} + update_stats( + component_name, component_info.write_statistics, write_cycle_suffix, level, component_info, + params_); + } + } + + if (!high_exec_time_hw.empty()) + { + std::string high_exec_time_hw_string; + for (const auto & hw_comp : high_exec_time_hw) + { + high_exec_time_hw_string.append(hw_comp); + high_exec_time_hw_string.append(" "); + } + stat.mergeSummary( + level, + "\nHardware Components with high execution time : [ " + high_exec_time_hw_string + "]"); + } + if (!bad_periodicity_async_hw.empty()) + { + std::string bad_periodicity_async_hw_string; + for (const auto & hw_comp : bad_periodicity_async_hw) + { + bad_periodicity_async_hw_string.append(hw_comp); + bad_periodicity_async_hw_string.append(" "); + } + stat.mergeSummary( + level, + "\nHardware Components with bad periodicity : [ " + bad_periodicity_async_hw_string + "]"); } } diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 9041e883fb..a2e2476449 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -134,3 +134,74 @@ controller_manager: gt<>: 0.0, } } + hardware_components: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the hardware component's read/write loop. 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 hardware component's read/write loop. 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 hardware component's read/write loop. 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 hardware component's read/write loop. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + 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 hardware component's read/write cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 2000.0, + description: "The error threshold for the mean error of the hardware component's read/write cycle execution time in microseconds. If the mean error exceeds this threshold, a error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 100.0, + description: "The warning threshold for the standard deviation of the hardware component's read/write 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 hardware component's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index aab829089a..5ac67c1c36 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/statistics_types.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" @@ -82,6 +83,10 @@ class Actuator final const rclcpp::Time & get_last_write_time() const; + const HardwareComponentStatisticsCollector & get_read_statistics() const; + + const HardwareComponentStatisticsCollector & get_write_statistics() const; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -95,6 +100,9 @@ class Actuator final rclcpp::Time last_read_cycle_time_; // Last write cycle time rclcpp::Time last_write_cycle_time_; + // Component statistics + HardwareComponentStatisticsCollector read_statistics_; + HardwareComponentStatisticsCollector write_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 70a0482811..a7861ac93a 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -19,14 +19,21 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ +#include #include #include -#include +#include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "hardware_interface/types/statistics_types.hpp" namespace hardware_interface { +struct HardwareComponentStatisticsData +{ + ros2_control::MovingAverageStatisticsData execution_time; + ros2_control::MovingAverageStatisticsData periodicity; +}; /// Hardware Component Information /** * This struct contains information about a given hardware component. @@ -59,6 +66,12 @@ struct HardwareComponentInfo /// List of provided command interfaces by the component. std::vector command_interfaces; + + /// Read cycle statistics of the component. + std::shared_ptr read_statistics = nullptr; + + /// Write cycle statistics of the component. + std::shared_ptr write_statistics = nullptr; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index e47e34c0d7..4cba2e761f 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/statistics_types.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" @@ -70,6 +71,8 @@ class Sensor final const rclcpp::Time & get_last_read_time() const; + const HardwareComponentStatisticsCollector & get_read_statistics() const; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } @@ -81,6 +84,8 @@ class Sensor final mutable std::recursive_mutex sensors_mutex_; // Last read cycle time rclcpp::Time last_read_cycle_time_; + // Component statistics + HardwareComponentStatisticsCollector read_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 750cbb301a..134ce40d9d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/statistics_types.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" @@ -82,6 +83,10 @@ class System final const rclcpp::Time & get_last_write_time() const; + const HardwareComponentStatisticsCollector & get_read_statistics() const; + + const HardwareComponentStatisticsCollector & get_write_statistics() const; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -95,6 +100,9 @@ class System final rclcpp::Time last_read_cycle_time_; // Last write cycle time rclcpp::Time last_write_cycle_time_; + // Component statistics + HardwareComponentStatisticsCollector read_statistics_; + HardwareComponentStatisticsCollector write_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/types/statistics_types.hpp b/hardware_interface/include/hardware_interface/types/statistics_types.hpp new file mode 100644 index 0000000000..ba22a47c99 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/statistics_types.hpp @@ -0,0 +1,141 @@ +// Copyright 2025 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sai Kishor Kothakota + +#ifndef HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ +#define HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ + +#include +#include + +#include "libstatistics_collector/moving_average_statistics/moving_average.hpp" +#include "libstatistics_collector/moving_average_statistics/types.hpp" +#include "realtime_tools/mutex.hpp" + +namespace ros2_control +{ +/** + * @brief Data structure to store the statistics of a moving average. The data is protected by a + * mutex and the data can be updated and retrieved. + */ +class MovingAverageStatisticsData +{ + using MovingAverageStatisticsCollector = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; + using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; + +public: + MovingAverageStatisticsData() + { + reset(); + reset_statistics_sample_count_ = std::numeric_limits::max(); + } + + /** + * @brief Update the statistics data with the new statistics data. + * @param statistics statistics collector to update the current statistics data. + */ + void update_statistics(const std::shared_ptr & statistics) + { + std::unique_lock lock(mutex_); + if (statistics->GetCount() > 0) + { + statistics_data.average = statistics->Average(); + statistics_data.min = statistics->Min(); + statistics_data.max = statistics->Max(); + statistics_data.standard_deviation = statistics->StandardDeviation(); + statistics_data.sample_count = statistics->GetCount(); + statistics_data = statistics->GetStatistics(); + } + if (statistics->GetCount() >= reset_statistics_sample_count_) + { + statistics->Reset(); + } + } + + /** + * @brief Set the number of samples to reset the statistics. + * @param reset_sample_count number of samples to reset the statistics. + */ + void set_reset_statistics_sample_count(unsigned int reset_sample_count) + { + std::unique_lock lock(mutex_); + reset_statistics_sample_count_ = reset_sample_count; + } + + void reset() + { + statistics_data.average = std::numeric_limits::quiet_NaN(); + statistics_data.min = std::numeric_limits::quiet_NaN(); + statistics_data.max = std::numeric_limits::quiet_NaN(); + statistics_data.standard_deviation = std::numeric_limits::quiet_NaN(); + statistics_data.sample_count = 0; + } + + /** + * @brief Get the statistics data. + * @return statistics data. + */ + const StatisticData & get_statistics() const + { + std::unique_lock lock(mutex_); + return statistics_data; + } + +private: + /// Statistics data + StatisticData statistics_data; + /// Number of samples to reset the statistics + unsigned int reset_statistics_sample_count_ = std::numeric_limits::max(); + /// Mutex to protect the statistics data + mutable realtime_tools::prio_inherit_mutex mutex_; +}; +} // namespace ros2_control + +namespace hardware_interface +{ +/** + * @brief Data structure with two moving average statistics collectors. One for the execution time + * and the other for the periodicity. + */ +struct HardwareComponentStatisticsCollector +{ + HardwareComponentStatisticsCollector() + { + execution_time = std::make_shared(); + periodicity = std::make_shared(); + } + + using MovingAverageStatisticsCollector = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; + + /** + * @brief Resets the internal statistics of the execution time and periodicity statistics + * collectors. + */ + void reset_statistics() + { + execution_time->Reset(); + periodicity->Reset(); + } + + /// Execution time statistics collector + std::shared_ptr execution_time = nullptr; + /// Periodicity statistics collector + std::shared_ptr periodicity = nullptr; +}; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 0312ee5fc9..d30602f73b 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -143,6 +143,8 @@ const rclcpp_lifecycle::State & Actuator::activate() std::unique_lock lock(actuators_mutex_); last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); + read_statistics_.reset_statistics(); + write_statistics_.reset_statistics(); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { impl_->prepare_for_activation(); @@ -296,6 +298,16 @@ const rclcpp::Time & Actuator::get_last_read_time() const { return last_read_cyc const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_cycle_time_; } +const HardwareComponentStatisticsCollector & Actuator::get_read_statistics() const +{ + return read_statistics_; +} + +const HardwareComponentStatisticsCollector & Actuator::get_write_statistics() const +{ + return write_statistics_; +} + return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) @@ -312,7 +324,20 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p { error(); } - last_read_cycle_time_ = time; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + read_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_read_cycle_time_).seconds()); + } + last_read_cycle_time_ = time; + } return trigger_result.result; } return return_type::OK; @@ -334,7 +359,20 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & { error(); } - last_write_cycle_time_ = time; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + write_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + write_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_write_cycle_time_).seconds()); + } + last_write_cycle_time_ = time; + } return trigger_result.result; } return return_type::OK; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 80f50fa73d..4278e6ba66 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -144,6 +144,13 @@ class ResourceStorage component_info.rw_rate = hardware_info.rw_rate; component_info.plugin_name = hardware_info.hardware_plugin_name; component_info.is_async = hardware_info.is_async; + component_info.read_statistics = std::make_shared(); + + // if the type of the hardware is sensor then don't initialize the write statistics + if (hardware_info.type != "sensor") + { + component_info.write_statistics = std::make_shared(); + } hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); hw_group_state_.insert(std::make_pair(component_info.group, return_type::OK)); @@ -1790,6 +1797,14 @@ HardwareReadWriteStatus ResourceManager::read( ret_val = component.read(current_time, actual_period); } } + if (hardware_component_info.read_statistics) + { + const auto & read_statistics_collector = component.get_read_statistics(); + hardware_component_info.read_statistics->execution_time.update_statistics( + read_statistics_collector.execution_time); + hardware_component_info.read_statistics->periodicity.update_statistics( + read_statistics_collector.periodicity); + } const auto component_group = component.get_group_name(); ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); @@ -1879,6 +1894,14 @@ HardwareReadWriteStatus ResourceManager::write( ret_val = component.write(current_time, actual_period); } } + if (hardware_component_info.write_statistics) + { + const auto & write_statistics_collector = component.get_write_statistics(); + hardware_component_info.write_statistics->execution_time.update_statistics( + write_statistics_collector.execution_time); + hardware_component_info.write_statistics->periodicity.update_statistics( + write_statistics_collector.periodicity); + } const auto component_group = component.get_group_name(); ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 50ef9f687e..ebbbd54c15 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -140,6 +140,7 @@ const rclcpp_lifecycle::State & Sensor::activate() { std::unique_lock lock(sensors_mutex_); last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); + read_statistics_.reset_statistics(); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_lifecycle_state())) @@ -249,6 +250,11 @@ const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle_time_; } +const HardwareComponentStatisticsCollector & Sensor::get_read_statistics() const +{ + return read_statistics_; +} + return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) @@ -265,7 +271,20 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per { error(); } - last_read_cycle_time_ = time; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + read_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_read_cycle_time_).seconds()); + } + last_read_cycle_time_ = time; + } return trigger_result.result; } return return_type::OK; diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 10914798c2..1cb34eaef9 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -141,6 +141,8 @@ const rclcpp_lifecycle::State & System::activate() std::unique_lock lock(system_mutex_); last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); + read_statistics_.reset_statistics(); + write_statistics_.reset_statistics(); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { impl_->prepare_for_activation(); @@ -294,6 +296,16 @@ const rclcpp::Time & System::get_last_read_time() const { return last_read_cycle const rclcpp::Time & System::get_last_write_time() const { return last_write_cycle_time_; } +const HardwareComponentStatisticsCollector & System::get_read_statistics() const +{ + return read_statistics_; +} + +const HardwareComponentStatisticsCollector & System::get_write_statistics() const +{ + return write_statistics_; +} + return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) @@ -310,7 +322,20 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per { error(); } - last_read_cycle_time_ = time; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + read_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_read_cycle_time_).seconds()); + } + last_read_cycle_time_ = time; + } return trigger_result.result; } return return_type::OK; @@ -332,7 +357,20 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe { error(); } - last_write_cycle_time_ = time; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + write_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + write_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_write_cycle_time_).seconds()); + } + last_write_cycle_time_ = time; + } return trigger_result.result; } return return_type::OK; diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 630cb9f27c..01171fe0fa 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -111,6 +111,11 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (get_hardware_info().is_async) + { + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); + } // simulate error on read if (velocity_command_ == test_constants::READ_FAIL_VALUE) { @@ -135,6 +140,11 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (get_hardware_info().is_async) + { + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate))); + } // simulate error on write if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) { diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 395935e314..a9db764306 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -100,6 +100,11 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (get_hardware_info().is_async) + { + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); + } // simulate error on read if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) { @@ -124,6 +129,11 @@ class TestSystem : public SystemInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (get_hardware_info().is_async) + { + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate))); + } // simulate error on write if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) { diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 75ce7fe5f7..803a51da9e 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1739,10 +1739,16 @@ TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManagerTest { public: - void setup_resource_manager_and_do_initial_checks() + void setup_resource_manager_and_do_initial_checks(bool async_components) { - rm = std::make_shared( - node_, ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate, false); + const auto minimal_robot_urdf_with_different_hw_rw_rate_with_async = + std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_with_different_rw_rates_with_async) + + std::string(ros2_control_test_assets::urdf_tail); + const std::string urdf = + async_components ? minimal_robot_urdf_with_different_hw_rw_rate_with_async + : ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate; + rm = std::make_shared(node_, urdf, false); activate_components(*rm); cm_update_rate_ = 100u; // The default value inside @@ -1767,6 +1773,9 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage actuator_rw_rate_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate; system_rw_rate_ = status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate; + actuator_is_async_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].is_async; + system_is_async_ = status_map[TEST_SYSTEM_HARDWARE_NAME].is_async; + claimed_itfs.push_back( rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); @@ -1841,11 +1850,75 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage } // Even though we skip some read and write iterations, the state interfaces should be the same // as previous updated one until the next cycle - ASSERT_EQ(state_itfs[0].get_optional().value(), prev_act_state_value); - ASSERT_EQ(state_itfs[1].get_optional().value(), prev_system_state_value); + if (actuator_is_async_) + { + // check it is either the previous value or the new one + EXPECT_THAT( + state_itfs[0].get_optional().value(), testing::AnyOf( + testing::DoubleEq(prev_act_state_value), + testing::DoubleEq(prev_act_state_value + 5.0))); + } + else + { + ASSERT_EQ(state_itfs[0].get_optional().value(), prev_act_state_value); + } + if (system_is_async_) + { + // check it is either the previous value or the new one + EXPECT_THAT( + state_itfs[1].get_optional().value(), + testing::AnyOf( + testing::DoubleEq(prev_system_state_value), + testing::DoubleEq(prev_system_state_value + 10.0))); + } + else + { + ASSERT_EQ(state_itfs[1].get_optional().value(), prev_system_state_value); + } auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); EXPECT_TRUE(ok_write); EXPECT_TRUE(failed_hardware_names_write.empty()); + + if (test_for_changing_values) + { + auto status_map = rm->get_components_status(); + auto check_periodicity = [&](const std::string & component_name, unsigned int rate) + { + if (i > (cm_update_rate_ / rate)) + { + const double expec_read_execution_time = (1.e6 / (3 * rate)) + 200.0; + const double expec_write_execution_time = (1.e6 / (6 * rate)) + 200.0; + EXPECT_LT( + status_map[component_name].read_statistics->execution_time.get_statistics().average, + expec_read_execution_time); + EXPECT_LT( + status_map[component_name].read_statistics->periodicity.get_statistics().average, + 1.2 * rate); + EXPECT_THAT( + status_map[component_name].read_statistics->periodicity.get_statistics().min, + testing::AllOf(testing::Ge(0.5 * rate), testing::Lt((1.2 * rate)))); + EXPECT_THAT( + status_map[component_name].read_statistics->periodicity.get_statistics().max, + testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate)))); + + EXPECT_LT( + status_map[component_name].write_statistics->execution_time.get_statistics().average, + expec_write_execution_time); + EXPECT_LT( + status_map[component_name].write_statistics->periodicity.get_statistics().average, + 1.2 * rate); + EXPECT_THAT( + status_map[component_name].write_statistics->periodicity.get_statistics().min, + testing::AllOf(testing::Ge(0.5 * rate), testing::Lt((1.2 * rate)))); + EXPECT_THAT( + status_map[component_name].write_statistics->periodicity.get_statistics().max, + testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate)))); + } + }; + + check_periodicity(TEST_ACTUATOR_HARDWARE_NAME, actuator_rw_rate_); + check_periodicity(TEST_SYSTEM_HARDWARE_NAME, system_rw_rate_); + } node_.get_clock()->sleep_until(time + duration); time = node_.get_clock()->now(); } @@ -1854,6 +1927,7 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage public: std::shared_ptr rm; unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + bool actuator_is_async_, system_is_async_; std::vector claimed_itfs; std::vector state_itfs; @@ -1867,7 +1941,16 @@ TEST_F( ResourceManagerTestReadWriteDifferentReadWriteRate, test_components_with_different_read_write_freq_on_activate) { - setup_resource_manager_and_do_initial_checks(); + setup_resource_manager_and_do_initial_checks(false); + + check_read_and_write_cycles(true); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_activate_with_async) +{ + setup_resource_manager_and_do_initial_checks(true); check_read_and_write_cycles(true); } @@ -1876,7 +1959,35 @@ TEST_F( ResourceManagerTestReadWriteDifferentReadWriteRate, test_components_with_different_read_write_freq_on_deactivate) { - setup_resource_manager_and_do_initial_checks(); + setup_resource_manager_and_do_initial_checks(false); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_inactive( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + check_read_and_write_cycles(true); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_deactivate_with_async) +{ + setup_resource_manager_and_do_initial_checks(true); // Now deactivate all the components and test the same as above rclcpp_lifecycle::State state_inactive( @@ -1904,7 +2015,35 @@ TEST_F( ResourceManagerTestReadWriteDifferentReadWriteRate, test_components_with_different_read_write_freq_on_unconfigured) { - setup_resource_manager_and_do_initial_checks(); + setup_resource_manager_and_do_initial_checks(false); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_unconfigured( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + hardware_interface::lifecycle_state_names::UNCONFIGURED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + check_read_and_write_cycles(false); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_unconfigured_with_async) +{ + setup_resource_manager_and_do_initial_checks(true); // Now deactivate all the components and test the same as above rclcpp_lifecycle::State state_unconfigured( @@ -1932,7 +2071,35 @@ TEST_F( ResourceManagerTestReadWriteDifferentReadWriteRate, test_components_with_different_read_write_freq_on_finalized) { - setup_resource_manager_and_do_initial_checks(); + setup_resource_manager_and_do_initial_checks(false); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_finalized( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + hardware_interface::lifecycle_state_names::FINALIZED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + check_read_and_write_cycles(false); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_finalized_with_async) +{ + setup_resource_manager_and_do_initial_checks(true); // Now deactivate all the components and test the same as above rclcpp_lifecycle::State state_finalized( @@ -1968,6 +2135,7 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest rm = std::make_shared(node_, minimal_robot_urdf_async, false); activate_components(*rm); + time = node_.get_clock()->now(); auto status_map = rm->get_components_status(); EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), @@ -1991,21 +2159,58 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1])); state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1])); + auto check_statistics_for_nan = [&](const std::string & component_name) + { + EXPECT_TRUE(std::isnan( + status_map[component_name].read_statistics->periodicity.get_statistics().average)); + EXPECT_TRUE(std::isnan( + status_map[component_name].write_statistics->periodicity.get_statistics().average)); + + EXPECT_TRUE( + std::isnan(status_map[component_name].read_statistics->periodicity.get_statistics().min)); + EXPECT_TRUE( + std::isnan(status_map[component_name].write_statistics->periodicity.get_statistics().min)); + + EXPECT_TRUE( + std::isnan(status_map[component_name].read_statistics->periodicity.get_statistics().max)); + EXPECT_TRUE( + std::isnan(status_map[component_name].write_statistics->periodicity.get_statistics().max)); + + EXPECT_TRUE(std::isnan( + status_map[component_name].read_statistics->execution_time.get_statistics().average)); + EXPECT_TRUE(std::isnan( + status_map[component_name].write_statistics->execution_time.get_statistics().average)); + + EXPECT_TRUE(std::isnan( + status_map[component_name].read_statistics->execution_time.get_statistics().min)); + EXPECT_TRUE(std::isnan( + status_map[component_name].write_statistics->execution_time.get_statistics().min)); + + EXPECT_TRUE(std::isnan( + status_map[component_name].read_statistics->execution_time.get_statistics().max)); + EXPECT_TRUE(std::isnan( + status_map[component_name].write_statistics->execution_time.get_statistics().max)); + }; + + check_statistics_for_nan(TEST_ACTUATOR_HARDWARE_NAME); + check_statistics_for_nan(TEST_SYSTEM_HARDWARE_NAME); + check_if_interface_available(true, true); // with default values read and write should run without any problems { auto [ok, failed_hardware_names] = rm->read(time, duration); EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); + EXPECT_THAT(failed_hardware_names, testing::IsEmpty()); } { // claimed_itfs[0].set_value(10.0); // claimed_itfs[1].set_value(20.0); auto [ok, failed_hardware_names] = rm->write(time, duration); EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); + EXPECT_THAT(failed_hardware_names, testing::IsEmpty()); } - time = time + duration; + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); check_if_interface_available(true, true); } @@ -2041,7 +2246,6 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest auto [ok, failed_hardware_names] = rm->read(time, duration); EXPECT_TRUE(ok); EXPECT_TRUE(failed_hardware_names.empty()); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); // The values are computations exactly within the test_components if (check_for_updated_values) { @@ -2057,10 +2261,63 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest ASSERT_NEAR( state_itfs[1].get_optional().value(), prev_system_state_value, system_increment / 2.0); auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); EXPECT_TRUE(ok_write); EXPECT_TRUE(failed_hardware_names_write.empty()); - time = time + duration; + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + + auto status_map = rm->get_components_status(); + auto check_periodicity = [&](const std::string & component_name, unsigned int rate) + { + EXPECT_LT( + status_map[component_name].read_statistics->periodicity.get_statistics().average, + 1.2 * rate); + EXPECT_THAT( + status_map[component_name].read_statistics->periodicity.get_statistics().min, + testing::AllOf(testing::Ge(0.4 * rate), testing::Lt((1.2 * rate)))); + EXPECT_THAT( + status_map[component_name].read_statistics->periodicity.get_statistics().max, + testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate)))); + + EXPECT_LT( + status_map[component_name].write_statistics->periodicity.get_statistics().average, + 1.2 * rate); + EXPECT_THAT( + status_map[component_name].write_statistics->periodicity.get_statistics().min, + testing::AllOf(testing::Ge(0.4 * rate), testing::Lt((1.2 * rate)))); + EXPECT_THAT( + status_map[component_name].write_statistics->periodicity.get_statistics().max, + testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate)))); + }; + + if (check_for_updated_values) + { + const unsigned int rw_rate = 100u; + const double expec_read_execution_time = (1.e6 / (3 * rw_rate)) + 200.0; + const double expec_write_execution_time = (1.e6 / (6 * rw_rate)) + 200.0; + check_periodicity(TEST_ACTUATOR_HARDWARE_NAME, rw_rate); + check_periodicity(TEST_SYSTEM_HARDWARE_NAME, rw_rate); + EXPECT_LT( + status_map[TEST_ACTUATOR_HARDWARE_NAME] + .read_statistics->execution_time.get_statistics() + .average, + expec_read_execution_time); + EXPECT_LT( + status_map[TEST_ACTUATOR_HARDWARE_NAME] + .write_statistics->execution_time.get_statistics() + .average, + expec_write_execution_time); + EXPECT_LT( + status_map[TEST_SYSTEM_HARDWARE_NAME] + .read_statistics->execution_time.get_statistics() + .average, + expec_read_execution_time); + EXPECT_LT( + status_map[TEST_SYSTEM_HARDWARE_NAME] + .write_statistics->execution_time.get_statistics() + .average, + expec_write_execution_time); } } diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 0be4ed369b..5c0f4dcbb7 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -758,6 +758,55 @@ const auto hardware_resources_with_different_rw_rates = )"; +const auto hardware_resources_with_different_rw_rates_with_async = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + const auto hardware_resources_with_negative_rw_rates = R"( From b572ef18a29eab2581f1420eb7e22278c0e55cba Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 1 Mar 2025 22:25:26 +0100 Subject: [PATCH 2/9] Update the description of the parameters --- .../src/controller_manager_parameters.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index a2e2476449..4ebe007574 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -140,7 +140,7 @@ controller_manager: warn: { type: double, default_value: 5.0, - description: "The warning threshold for the mean error of the hardware component's read/write loop. If the mean error exceeds this threshold, a warning diagnostic will be published.", + description: "The warning threshold for the mean error of the hardware component's read/write loop in Hz. If the mean error exceeds this threshold, a warning diagnostic will be published.", validation: { gt<>: 0.0, } @@ -148,7 +148,7 @@ controller_manager: error: { type: double, default_value: 10.0, - description: "The error threshold for the mean error of the hardware component's read/write loop. If the mean error exceeds this threshold, an error diagnostic will be published.", + description: "The error threshold for the mean error of the hardware component's read/write loop in Hz. If the mean error exceeds this threshold, an error diagnostic will be published.", validation: { gt<>: 0.0, } @@ -157,7 +157,7 @@ controller_manager: warn: { type: double, default_value: 5.0, - description: "The warning threshold for the standard deviation of the hardware component's read/write loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + description: "The warning threshold for the standard deviation of the hardware component's read/write loop in Hz. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", validation: { gt<>: 0.0, } @@ -165,7 +165,7 @@ controller_manager: error: { type: double, default_value: 10.0, - description: "The error threshold for the standard deviation of the hardware component's read/write loop. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + description: "The error threshold for the standard deviation of the hardware component's read/write loop in Hz. If the standard deviation exceeds this threshold, an error diagnostic will be published.", validation: { gt<>: 0.0, } @@ -192,7 +192,7 @@ controller_manager: warn: { type: double, default_value: 100.0, - description: "The warning threshold for the standard deviation of the hardware component's read/write 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 hardware component's read/write cycle execution time in microseconds. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", validation: { gt<>: 0.0, } @@ -200,7 +200,7 @@ controller_manager: error: { type: double, default_value: 200.0, - description: "The error threshold for the standard deviation of the hardware component'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 hardware component's update cycle execution time in microseconds. If the standard deviation exceeds this threshold, an error diagnostic will be published.", validation: { gt<>: 0.0, } From 7bd16c99d4b8723ccbd5350411a4f427fc983213 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 1 Mar 2025 23:04:54 +0100 Subject: [PATCH 3/9] Publish periodicity stats for controllers and hardware running at different frequencies --- controller_manager/doc/parameters_context.yaml | 4 ++-- controller_manager/src/controller_manager.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml index c9e95f6396..be70c69ec8 100644 --- a/controller_manager/doc/parameters_context.yaml +++ b/controller_manager/doc/parameters_context.yaml @@ -15,13 +15,13 @@ hardware_components_initial_state: | - "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. + The ``periodicity`` diagnostics will be published for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity. It is also published for the synchronous controllers that have a different update rate than the controller manager update rate. diagnostics.threshold.controllers.execution_time.mean_error: | 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 to execute its update cycle. diagnostics.threshold.hardware_components.periodicity: | - The ``periodicity`` diagnostics will be published only for the asynchronous hardware components, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity. + The ``periodicity`` diagnostics will be published for the asynchronous hardware components, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity. It is also published for the synchronous hardware components that have a different read/write rate than the controller manager update rate. diagnostics.threshold.hardware_components.execution_time.mean_error: | The ``execution_time`` diagnostics will be published for all hardware components. The ``mean_error`` for a synchronous hardware component will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous hardware component will be computed against its desired read/write period, as the hardware component can take a maximum of the desired period to execute the read/write cycle. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 399fcb6dd7..9fc4f212bd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3288,7 +3288,9 @@ void ControllerManager::controller_activity_diagnostic_callback( const auto exec_time_stats = controllers[i].execution_time_statistics->GetStatistics(); stat.add( controllers[i].info.name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); - if (is_async) + const bool publish_periodicity_stats = + is_async || (controllers[i].c->get_update_rate() != this->get_update_rate()); + if (publish_periodicity_stats) { stat.add( controllers[i].info.name + periodicity_suffix, @@ -3482,7 +3484,9 @@ void ControllerManager::hardware_components_diagnostic_callback( stat.add( comp_name + statistics_type_suffix + exec_time_suffix, make_stats_string(exec_time_stats, "us")); - if (is_async) + const bool publish_periodicity_stats = + is_async || (controllers[i].c->get_update_rate() != this->get_update_rate()); + if (publish_periodicity_stats) { stat.add( comp_name + statistics_type_suffix + periodicity_suffix, From 75425447de0049f9c13f2f5679320445bc669a5d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 1 Mar 2025 22:52:14 +0100 Subject: [PATCH 4/9] Update the summary of hardware components --- controller_manager/src/controller_manager.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9fc4f212bd..6b0e7c775e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3561,8 +3561,7 @@ void ControllerManager::hardware_components_diagnostic_callback( high_exec_time_hw_string.append(" "); } stat.mergeSummary( - level, - "\nHardware Components with high execution time : [ " + high_exec_time_hw_string + "]"); + level, "\nHigh execution jitter or mean error : [ " + high_exec_time_hw_string + "]"); } if (!bad_periodicity_async_hw.empty()) { @@ -3574,7 +3573,7 @@ void ControllerManager::hardware_components_diagnostic_callback( } stat.mergeSummary( level, - "\nHardware Components with bad periodicity : [ " + bad_periodicity_async_hw_string + "]"); + "\nHigh periodicity jitter or mean error : [ " + bad_periodicity_async_hw_string + "]"); } } From e0a21850acbfb221086a4c575c6aa093f21a47f5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 1 Mar 2025 23:11:29 +0100 Subject: [PATCH 5/9] Update release notes --- doc/release_notes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a5b87cd7f5..28a35f0f6d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -165,6 +165,7 @@ hardware_interface * With (`#1567 `_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components. * The hardware components can be easily introspect the internal member variables using the macro ``REGISTER_ROS2_CONTROL_INTROSPECTION`` (`#1918 `_) * Added new ``get_optional`` method that returns ``std::optional`` of the templated type, and this can be used to check if the value is available or not (`#1976 `_ and `#2061 `_) +* Added hardware components execution time and periodicity statistics diagnostics (`#2086 `_) joint_limits ************ From 4cd545ecc53b3383528000f20022edad7478ad29 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 1 Mar 2025 23:43:54 +0100 Subject: [PATCH 6/9] Update controller_manager/src/controller_manager.cpp --- 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 6b0e7c775e..505c4c2caa 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3485,7 +3485,7 @@ void ControllerManager::hardware_components_diagnostic_callback( comp_name + statistics_type_suffix + exec_time_suffix, make_stats_string(exec_time_stats, "us")); const bool publish_periodicity_stats = - is_async || (controllers[i].c->get_update_rate() != this->get_update_rate()); + is_async || comp_info.rw_rate != this->get_update_rate()); if (publish_periodicity_stats) { stat.add( From e45355ce8add266db81f6c76b7265f6f290293bf Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 2 Mar 2025 08:47:06 +0000 Subject: [PATCH 7/9] Fix compilation --- controller_manager/src/controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 505c4c2caa..52289d5e99 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3467,7 +3467,7 @@ void ControllerManager::hardware_components_diagnostic_callback( if (component_info.state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto update_stats = - [&bad_periodicity_async_hw, &high_exec_time_hw, &stat, &make_stats_string]( + [&bad_periodicity_async_hw, &high_exec_time_hw, &stat, &make_stats_string, this]( const std::string & comp_name, const auto & statistics, const std::string & statistics_type_suffix, auto & diag_level, const auto & comp_info, const auto & params) @@ -3485,7 +3485,7 @@ void ControllerManager::hardware_components_diagnostic_callback( comp_name + statistics_type_suffix + exec_time_suffix, make_stats_string(exec_time_stats, "us")); const bool publish_periodicity_stats = - is_async || comp_info.rw_rate != this->get_update_rate()); + is_async || (comp_info.rw_rate != this->get_update_rate()); if (publish_periodicity_stats) { stat.add( From ca2efadc7441d3610d0edc7d3dd0119e1abc9f71 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Mar 2025 18:30:29 +0100 Subject: [PATCH 8/9] add missing false arg in newly added test --- hardware_interface_testing/test/test_resource_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 7ccdf2f333..7a9dc6cf33 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -2127,7 +2127,7 @@ TEST_F( ResourceManagerTestReadWriteDifferentReadWriteRate, test_components_with_different_read_write_freq_not_exact_timing) { - setup_resource_manager_and_do_initial_checks(); + setup_resource_manager_and_do_initial_checks(false); const auto test_jitter = std::chrono::milliseconds{1}; From c217baa2c3c62e3ff9de9db97313fd5f91ea9b8a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 4 Apr 2025 12:29:29 +0000 Subject: [PATCH 9/9] Fix pre-commit --- .../test/test_resource_manager.cpp | 40 +++++++++++-------- 1 file changed, 24 insertions(+), 16 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index ede29812be..b54474c401 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -2221,10 +2221,12 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest auto check_statistics_for_nan = [&](const std::string & component_name) { - EXPECT_TRUE(std::isnan( - status_map[component_name].read_statistics->periodicity.get_statistics().average)); - EXPECT_TRUE(std::isnan( - status_map[component_name].write_statistics->periodicity.get_statistics().average)); + EXPECT_TRUE( + std::isnan( + status_map[component_name].read_statistics->periodicity.get_statistics().average)); + EXPECT_TRUE( + std::isnan( + status_map[component_name].write_statistics->periodicity.get_statistics().average)); EXPECT_TRUE( std::isnan(status_map[component_name].read_statistics->periodicity.get_statistics().min)); @@ -2236,20 +2238,26 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest EXPECT_TRUE( std::isnan(status_map[component_name].write_statistics->periodicity.get_statistics().max)); - EXPECT_TRUE(std::isnan( - status_map[component_name].read_statistics->execution_time.get_statistics().average)); - EXPECT_TRUE(std::isnan( - status_map[component_name].write_statistics->execution_time.get_statistics().average)); + EXPECT_TRUE( + std::isnan( + status_map[component_name].read_statistics->execution_time.get_statistics().average)); + EXPECT_TRUE( + std::isnan( + status_map[component_name].write_statistics->execution_time.get_statistics().average)); - EXPECT_TRUE(std::isnan( - status_map[component_name].read_statistics->execution_time.get_statistics().min)); - EXPECT_TRUE(std::isnan( - status_map[component_name].write_statistics->execution_time.get_statistics().min)); + EXPECT_TRUE( + std::isnan( + status_map[component_name].read_statistics->execution_time.get_statistics().min)); + EXPECT_TRUE( + std::isnan( + status_map[component_name].write_statistics->execution_time.get_statistics().min)); - EXPECT_TRUE(std::isnan( - status_map[component_name].read_statistics->execution_time.get_statistics().max)); - EXPECT_TRUE(std::isnan( - status_map[component_name].write_statistics->execution_time.get_statistics().max)); + EXPECT_TRUE( + std::isnan( + status_map[component_name].read_statistics->execution_time.get_statistics().max)); + EXPECT_TRUE( + std::isnan( + status_map[component_name].write_statistics->execution_time.get_statistics().max)); }; check_statistics_for_nan(TEST_ACTUATOR_HARDWARE_NAME);