diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml index a015765c79..be70c69ec8 100644 --- a/controller_manager/doc/parameters_context.yaml +++ b/controller_manager/doc/parameters_context.yaml @@ -15,7 +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: | - 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 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 4e9e72018d..1125b2d121 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3562,7 +3562,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, @@ -3662,12 +3664,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; @@ -3677,31 +3688,166 @@ 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_)) { - if (!atleast_one_hw_active) + *params_ = cm_param_listener_->get_params(); + } + + 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(); + }; + + // 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, 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) + { + 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")); + const bool publish_periodicity_stats = + is_async || (comp_info.rw_rate != this->get_update_rate()); + if (publish_periodicity_stats) + { + 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, "\nHigh execution jitter or mean error : [ " + 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, + "\nHigh periodicity jitter or mean error : [ " + 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..4ebe007574 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 in Hz. 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 in Hz. 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 in Hz. 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 in Hz. 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 in microseconds. 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 in microseconds. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index e35cc8d5da..4322b22655 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -168,6 +168,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 ************ diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index e1e125a064..8d79cfd97b 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" @@ -85,6 +86,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); @@ -98,6 +103,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 833e150653..f3e2bce9cf 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" @@ -73,6 +74,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; } @@ -84,6 +87,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 591ac39435..c5c452681e 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" @@ -85,6 +86,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); @@ -98,6 +103,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 2a13633ada..b7f4a779a2 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -155,6 +155,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(); @@ -314,6 +316,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())) @@ -330,7 +342,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; @@ -352,7 +377,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 a1c96b849e..eb67f9fec1 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -160,6 +160,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)); @@ -2147,6 +2154,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); @@ -2249,6 +2264,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 603db06135..7d3a318117 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -152,6 +152,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())) @@ -267,6 +268,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())) @@ -283,7 +289,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 640426975d..fc401bb0cc 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -153,6 +153,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(); @@ -312,6 +314,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())) @@ -328,7 +340,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; @@ -350,7 +375,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 d26db656bf..8ee4afa101 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -116,6 +116,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) { @@ -147,6 +152,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 8975684b11..dbbc2183fc 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -108,6 +108,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) { @@ -132,6 +137,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 4237a5e6b7..f252478002 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1745,10 +1745,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 @@ -1773,6 +1779,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])); @@ -1847,11 +1856,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(); } @@ -1860,6 +1933,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; @@ -1873,7 +1947,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); } @@ -1882,7 +1965,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( @@ -1910,7 +2021,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( @@ -1938,7 +2077,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( @@ -1966,7 +2133,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}; @@ -2028,6 +2195,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(), @@ -2051,21 +2219,66 @@ 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); } @@ -2101,7 +2314,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) { @@ -2117,10 +2329,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 efaa6a2cc5..b92957c1c6 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 @@ -793,6 +793,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"(