From 73dd839fdc80356a130e3dd49e43911cb702c0bb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 00:00:34 +0100 Subject: [PATCH 01/38] Add MovingAverageStatistics struct --- .../hardware_component_info.hpp | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 70a0482811..113b1495aa 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -19,14 +19,57 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ +#include #include #include #include +#include "libstatistics_collector/moving_average_statistics/moving_average.hpp" +#include "libstatistics_collector/moving_average_statistics/types.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/mutex.hpp" namespace hardware_interface { +using MovingAverageStatistics = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; +using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; + +struct MovingAverageStatisticsData +{ +public: + MovingAverageStatisticsData() { statistics = std::make_unique(); } + + void add_measurement(double measurement) + { + std::unique_lock lock(mutex_); + statistics->AddMeasurement(measurement); + statistics_data = statistics->GetStatistics(); + } + + void reset() + { + statistics->Reset(); + statistics_data = StatisticData(); + } + + const StatisticData & get_statistics() const + { + std::unique_lock lock(mutex_); + return statistics_data; + } + +private: + std::unique_ptr statistics = nullptr; + StatisticData statistics_data; + mutable realtime_tools::prio_inherit_recursive_mutex mutex_; +}; + +struct HardwareComponentStatistics +{ + MovingAverageStatisticsData execution_time; + MovingAverageStatisticsData periodicity; +}; /// Hardware Component Information /** * This struct contains information about a given hardware component. @@ -59,6 +102,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 From 556b4725e44211a6ec5853cbfb8326b8c753b0cc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 00:00:59 +0100 Subject: [PATCH 02/38] Add HardwareComponentCycleStatus return type --- .../hardware_interface_return_values.hpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index d39dee2c64..fbd508a929 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -15,7 +15,11 @@ #ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_HPP_ #define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_HPP_ +#include #include +#include + +#include namespace hardware_interface { @@ -26,6 +30,24 @@ enum class return_type : std::uint8_t DEACTIVATE = 2, }; +/** + * Struct to store the status of the Hardware read or write methods return state. + * The status contains information if the cycle was triggered successfully, the result of the + * cycle method and the execution duration of the method. The status is used to provide + * feedback to the controller_manager. + * @var successful: true if it was triggered successfully, false if not. + * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR. + * @var execution_time: duration of the execution of the update method. + * @var period: period of the update method. + */ +struct HardwareComponentCycleStatus +{ + bool successful = true; + return_type result = return_type::OK; + std::optional execution_time = std::nullopt; + std::optional period = std::nullopt; +}; + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_HPP_ From 04180e7c773b4aa57335016e4e0ce0c658b488b1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 00:02:14 +0100 Subject: [PATCH 03/38] initialize the statistics --- hardware_interface/src/resource_manager.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e43c650d5d..0fac897dc2 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)); From 2ad2d0d79b38b116fb3420dfa17dc5c1e01aeec3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 00:02:53 +0100 Subject: [PATCH 04/38] Integrate HardwareComponentCycleStatus on the trigger read and write methods --- .../hardware_interface/actuator_interface.hpp | 38 ++++++++++++------- .../hardware_interface/sensor_interface.hpp | 16 +++++--- .../hardware_interface/system_interface.hpp | 38 ++++++++++++------- hardware_interface/src/actuator.cpp | 4 +- hardware_interface/src/sensor.cpp | 2 +- hardware_interface/src/system.cpp | 4 +- 6 files changed, 63 insertions(+), 39 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1f693d1d34..12e9261c4f 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -359,9 +359,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \param[in] period The measured time taken by the last control loop iteration * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ - return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + HardwareComponentCycleStatus trigger_read( + const rclcpp::Time & time, const rclcpp::Duration & period) { - return_type result = return_type::ERROR; + HardwareComponentCycleStatus status; + status.result = return_type::ERROR; if (info_.is_async) { bool trigger_status = true; @@ -372,9 +374,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod "Trigger read called while write async handler call is still pending for hardware " "interface : '%s'. Skipping read cycle and will wait for a write cycle!", info_.name.c_str()); - return return_type::OK; + status.result = return_type::OK; + return status; } - std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + std::tie(trigger_status, status.result) = + async_handler_->trigger_async_callback(time, period); if (!trigger_status) { RCLCPP_WARN( @@ -382,14 +386,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod "Trigger read called while write async trigger is still in progress for hardware " "interface : '%s'. Failed to trigger read cycle!", info_.name.c_str()); - return return_type::OK; + status.result = return_type::OK; + return status; } } else { - result = read(time, period); + status.result = read(time, period); } - return result; + return status; } /// Read the current state values from the actuator. @@ -414,9 +419,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \param[in] period The measured time taken by the last control loop iteration * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ - return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + HardwareComponentCycleStatus trigger_write( + const rclcpp::Time & time, const rclcpp::Duration & period) { - return_type result = return_type::ERROR; + HardwareComponentCycleStatus status; + status.result = return_type::ERROR; if (info_.is_async) { bool trigger_status = true; @@ -427,9 +434,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod "Trigger write called while read async handler call is still pending for hardware " "interface : '%s'. Skipping write cycle and will wait for a read cycle!", info_.name.c_str()); - return return_type::OK; + status.result = return_type::OK; + return status; } - std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + std::tie(trigger_status, status.result) = + async_handler_->trigger_async_callback(time, period); if (!trigger_status) { RCLCPP_WARN( @@ -437,14 +446,15 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod "Trigger write called while read async trigger is still in progress for hardware " "interface : '%s'. Failed to trigger write cycle!", info_.name.c_str()); - return return_type::OK; + status.result = return_type::OK; + return status; } } else { - result = write(time, period); + status.result = write(time, period); } - return result; + return status; } /// Write the current command values to the actuator. diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 58a8b4790a..2c2e00a392 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -236,13 +236,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \param[in] period The measured time taken by the last control loop iteration * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ - return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + HardwareComponentCycleStatus trigger_read( + const rclcpp::Time & time, const rclcpp::Duration & period) { - return_type result = return_type::ERROR; + HardwareComponentCycleStatus status; + status.result = return_type::ERROR; if (info_.is_async) { bool trigger_status = true; - std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period); + std::tie(trigger_status, status.result) = + read_async_handler_->trigger_async_callback(time, period); if (!trigger_status) { RCLCPP_WARN( @@ -250,14 +253,15 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI "Trigger read called while read async handler is still in progress for hardware " "interface : '%s'. Failed to trigger read cycle!", info_.name.c_str()); - return return_type::OK; + status.result = return_type::OK; + return status; } } else { - result = read(time, period); + status.result = read(time, period); } - return result; + return status; } /// Read the current state values from the actuator. diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 7577d0ebdc..2542c47cb5 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -388,9 +388,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \param[in] period The measured time taken by the last control loop iteration * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ - return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + HardwareComponentCycleStatus trigger_read( + const rclcpp::Time & time, const rclcpp::Duration & period) { - return_type result = return_type::ERROR; + HardwareComponentCycleStatus status; + status.result = return_type::ERROR; if (info_.is_async) { bool trigger_status = true; @@ -401,9 +403,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI "Trigger read called while write async handler call is still pending for hardware " "interface : '%s'. Skipping read cycle and will wait for a write cycle!", info_.name.c_str()); - return return_type::OK; + status.result = return_type::OK; + return status; } - std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + std::tie(trigger_status, status.result) = + async_handler_->trigger_async_callback(time, period); if (!trigger_status) { RCLCPP_WARN( @@ -411,14 +415,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI "Trigger read called while write async trigger is still in progress for hardware " "interface : '%s'. Failed to trigger read cycle!", info_.name.c_str()); - return return_type::OK; + status.result = return_type::OK; + return status; } } else { - result = read(time, period); + status.result = read(time, period); } - return result; + return status; } /// Read the current state values from the actuator. @@ -443,9 +448,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \param[in] period The measured time taken by the last control loop iteration * \return return_type::OK if the read was successful, return_type::ERROR otherwise. */ - return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + HardwareComponentCycleStatus trigger_write( + const rclcpp::Time & time, const rclcpp::Duration & period) { - return_type result = return_type::ERROR; + HardwareComponentCycleStatus status; + status.result = return_type::ERROR; if (info_.is_async) { bool trigger_status = true; @@ -456,9 +463,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI "Trigger write called while read async handler call is still pending for hardware " "interface : '%s'. Skipping write cycle and will wait for a read cycle!", info_.name.c_str()); - return return_type::OK; + status.result = return_type::OK; + return status; } - std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + std::tie(trigger_status, status.result) = + async_handler_->trigger_async_callback(time, period); if (!trigger_status) { RCLCPP_WARN( @@ -466,14 +475,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI "Trigger write called while read async trigger is still in progress for hardware " "interface : '%s'. Failed to trigger write cycle!", info_.name.c_str()); - return return_type::OK; + status.result = return_type::OK; + return status; } } else { - result = write(time, period); + status.result = write(time, period); } - return result; + return status; } /// Write the current command values to the actuator. diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 65b337aa62..e0df076af6 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -307,7 +307,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_read(time, period); + result = impl_->trigger_read(time, period).result; if (result == return_type::ERROR) { error(); @@ -329,7 +329,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_write(time, period); + result = impl_->trigger_write(time, period).result; if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 87ea42790e..aa5501d6ac 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -261,7 +261,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_read(time, period); + result = impl_->trigger_read(time, period).result; if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 8f626f7c8d..c295fbb09e 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -305,7 +305,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_read(time, period); + result = impl_->trigger_read(time, period).result; if (result == return_type::ERROR) { error(); @@ -327,7 +327,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_write(time, period); + result = impl_->trigger_write(time, period).result; if (result == return_type::ERROR) { error(); From b11fc8eb4179ae2af774c90ccbade66417df8bd6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 12:29:07 +0100 Subject: [PATCH 05/38] Add statistics_types.hpp header --- .../types/statistics_types.hpp | 94 +++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100644 hardware_interface/include/hardware_interface/types/statistics_types.hpp 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..25f6f5bbdc --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/statistics_types.hpp @@ -0,0 +1,94 @@ +// 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 +{ +struct MovingAverageStatisticsData +{ + using MovingAverageStatistics = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; + using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; + +public: + MovingAverageStatisticsData() { reset(); } + + void update_statistics(std::shared_ptr & statistics) + { + std::unique_lock lock(mutex_); + 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(); + } + + 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; + } + + const StatisticData & get_statistics() const + { + std::unique_lock lock(mutex_); + return statistics_data; + } + +private: + StatisticData statistics_data; + mutable realtime_tools::prio_inherit_recursive_mutex mutex_; +}; +} // namespace ros2_control + +namespace hardware_interface +{ +struct HardwareComponentStatisticsCollector +{ + HardwareComponentStatisticsCollector() + { + execution_time = std::make_shared(); + periodicity = std::make_shared(); + } + + using MovingAverageStatistics = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; + + void reset_statistics() + { + execution_time->Reset(); + periodicity->Reset(); + } + + std::shared_ptr execution_time = nullptr; + std::shared_ptr periodicity = nullptr; +}; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ From d19fcde77ba177f90ef5784f7c2208483e3a2d22 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 12:30:28 +0100 Subject: [PATCH 06/38] add HardwareComponentStatisticsCollector --- .../include/hardware_interface/actuator.hpp | 4 ++ .../hardware_component_info.hpp | 50 +++---------------- .../include/hardware_interface/sensor.hpp | 3 ++ .../include/hardware_interface/system.hpp | 4 ++ hardware_interface/src/resource_manager.cpp | 4 +- 5 files changed, 20 insertions(+), 45 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index aab829089a..2aafa1037a 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" @@ -95,6 +96,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 113b1495aa..a7861ac93a 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -23,52 +23,16 @@ #include #include -#include -#include "libstatistics_collector/moving_average_statistics/moving_average.hpp" -#include "libstatistics_collector/moving_average_statistics/types.hpp" +#include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/mutex.hpp" +#include "hardware_interface/types/statistics_types.hpp" namespace hardware_interface { -using MovingAverageStatistics = - libstatistics_collector::moving_average_statistics::MovingAverageStatistics; -using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; - -struct MovingAverageStatisticsData -{ -public: - MovingAverageStatisticsData() { statistics = std::make_unique(); } - - void add_measurement(double measurement) - { - std::unique_lock lock(mutex_); - statistics->AddMeasurement(measurement); - statistics_data = statistics->GetStatistics(); - } - - void reset() - { - statistics->Reset(); - statistics_data = StatisticData(); - } - - const StatisticData & get_statistics() const - { - std::unique_lock lock(mutex_); - return statistics_data; - } - -private: - std::unique_ptr statistics = nullptr; - StatisticData statistics_data; - mutable realtime_tools::prio_inherit_recursive_mutex mutex_; -}; - -struct HardwareComponentStatistics +struct HardwareComponentStatisticsData { - MovingAverageStatisticsData execution_time; - MovingAverageStatisticsData periodicity; + ros2_control::MovingAverageStatisticsData execution_time; + ros2_control::MovingAverageStatisticsData periodicity; }; /// Hardware Component Information /** @@ -104,10 +68,10 @@ struct HardwareComponentInfo std::vector command_interfaces; /// Read cycle statistics of the component. - std::shared_ptr read_statistics = nullptr; + std::shared_ptr read_statistics = nullptr; /// Write cycle statistics of the component. - std::shared_ptr write_statistics = nullptr; + 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..8433623e3c 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" @@ -81,6 +82,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..98845ce738 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" @@ -95,6 +96,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/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 0fac897dc2..24e4fa8853 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -144,12 +144,12 @@ 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(); + 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(); + component_info.write_statistics = std::make_shared(); } hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); From 3a12da6273817908cad3a2c6900375eb408da2d3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 12:31:31 +0100 Subject: [PATCH 07/38] Change to prio_inherit_mutex instead of recursive one --- .../include/hardware_interface/types/statistics_types.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/types/statistics_types.hpp b/hardware_interface/include/hardware_interface/types/statistics_types.hpp index 25f6f5bbdc..2d0fede058 100644 --- a/hardware_interface/include/hardware_interface/types/statistics_types.hpp +++ b/hardware_interface/include/hardware_interface/types/statistics_types.hpp @@ -37,7 +37,7 @@ struct MovingAverageStatisticsData void update_statistics(std::shared_ptr & statistics) { - std::unique_lock lock(mutex_); + std::unique_lock lock(mutex_); statistics_data.average = statistics->Average(); statistics_data.min = statistics->Min(); statistics_data.max = statistics->Max(); @@ -57,13 +57,13 @@ struct MovingAverageStatisticsData const StatisticData & get_statistics() const { - std::unique_lock lock(mutex_); + std::unique_lock lock(mutex_); return statistics_data; } private: StatisticData statistics_data; - mutable realtime_tools::prio_inherit_recursive_mutex mutex_; + mutable realtime_tools::prio_inherit_mutex mutex_; }; } // namespace ros2_control From a1e89d907844dd97a6726092c9d82f4596da68e4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 14:03:05 +0100 Subject: [PATCH 08/38] Fill in the HardwareComponentCycleStatus from read and write cycle information --- .../hardware_interface/actuator_interface.hpp | 46 +++++++++++++++---- .../hardware_interface/sensor_interface.hpp | 23 ++++++++-- .../hardware_interface/system_interface.hpp | 46 +++++++++++++++---- 3 files changed, 95 insertions(+), 20 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 12e9261c4f..a9e02b4fd9 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -366,7 +366,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::ERROR; if (info_.is_async) { - bool trigger_status = true; if (next_trigger_ == TriggerType::WRITE) { RCLCPP_WARN( @@ -377,9 +376,20 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::OK; return status; } - std::tie(trigger_status, status.result) = - async_handler_->trigger_async_callback(time, period); - if (!trigger_status) + const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); + const auto result = async_handler_->trigger_async_callback(time, period); + status.successful = result.first; + status.result = result.second; + const auto execution_time = async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } + if (!status.successful) { RCLCPP_WARN( get_logger(), @@ -392,7 +402,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod } else { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; status.result = read(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + status.period = period; } return status; } @@ -426,7 +441,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::ERROR; if (info_.is_async) { - bool trigger_status = true; if (next_trigger_ == TriggerType::READ) { RCLCPP_WARN( @@ -437,9 +451,20 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::OK; return status; } - std::tie(trigger_status, status.result) = - async_handler_->trigger_async_callback(time, period); - if (!trigger_status) + const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); + const auto result = async_handler_->trigger_async_callback(time, period); + status.successful = result.first; + status.result = result.second; + const auto execution_time = async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } + if (!status.successful) { RCLCPP_WARN( get_logger(), @@ -452,7 +477,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod } else { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; status.result = write(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + status.period = period; } return status; } diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2c2e00a392..2f4affaac0 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -243,10 +243,20 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::ERROR; if (info_.is_async) { - bool trigger_status = true; - std::tie(trigger_status, status.result) = - read_async_handler_->trigger_async_callback(time, period); - if (!trigger_status) + const rclcpp::Time last_trigger_time = read_async_handler_->get_current_callback_time(); + const auto result = read_async_handler_->trigger_async_callback(time, period); + status.successful = result.first; + status.result = result.second; + const auto execution_time = read_async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } + if (!status.successful) { RCLCPP_WARN( get_logger(), @@ -259,7 +269,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } else { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; status.result = read(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + status.period = period; } return status; } diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 2542c47cb5..16f43a3b36 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -395,7 +395,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::ERROR; if (info_.is_async) { - bool trigger_status = true; if (next_trigger_ == TriggerType::WRITE) { RCLCPP_WARN( @@ -406,9 +405,20 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::OK; return status; } - std::tie(trigger_status, status.result) = - async_handler_->trigger_async_callback(time, period); - if (!trigger_status) + const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); + const auto result = async_handler_->trigger_async_callback(time, period); + status.successful = result.first; + status.result = result.second; + const auto execution_time = async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } + if (!status.successful) { RCLCPP_WARN( get_logger(), @@ -421,7 +431,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } else { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; status.result = read(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + status.period = period; } return status; } @@ -455,7 +470,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::ERROR; if (info_.is_async) { - bool trigger_status = true; if (next_trigger_ == TriggerType::READ) { RCLCPP_WARN( @@ -466,9 +480,20 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::OK; return status; } - std::tie(trigger_status, status.result) = - async_handler_->trigger_async_callback(time, period); - if (!trigger_status) + const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); + const auto result = async_handler_->trigger_async_callback(time, period); + status.successful = result.first; + status.result = result.second; + const auto execution_time = async_handler_->get_last_execution_time(); + if (execution_time.count() > 0) + { + status.execution_time = execution_time; + } + if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + status.period = time - last_trigger_time; + } + if (!status.successful) { RCLCPP_WARN( get_logger(), @@ -481,7 +506,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } else { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; status.result = write(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + status.period = period; } return status; } From 85a33b69d34a345a5c56caf35db52bff27fe4e7b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 20:02:44 +0100 Subject: [PATCH 09/38] Return trigger result for system --- hardware_interface/src/system.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index c295fbb09e..2d4efa92e1 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -159,6 +159,8 @@ const rclcpp_lifecycle::State & System::activate() break; } } + read_statistics_->reset_statistics(); + write_statistics_->reset_statistics(); return impl_->get_lifecycle_state(); } @@ -300,19 +302,18 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per last_read_cycle_time_ = time; return return_type::OK; } - return_type result = return_type::ERROR; if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_read(time, period).result; - if (result == return_type::ERROR) + const auto trigger_result = impl_->trigger_read(time, period); + if (trigger_result.result == return_type::ERROR) { error(); } last_read_cycle_time_ = time; } - return result; + return trigger_result.result; } return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) @@ -322,19 +323,18 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe last_write_cycle_time_ = time; return return_type::OK; } - return_type result = return_type::ERROR; if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_write(time, period).result; - if (result == return_type::ERROR) + const auto trigger_result = impl_->trigger_write(time, period); + if (trigger_result.result == return_type::ERROR) { error(); } last_write_cycle_time_ = time; } - return result; + return trigger_result.result; } std::recursive_mutex & System::get_mutex() { return system_mutex_; } From 3286be2060613978c7e32cc63df4cacd94affa36 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 22:33:07 +0100 Subject: [PATCH 10/38] add logic to update the statistics information --- hardware_interface/src/actuator.cpp | 34 ++++++++++++++++++++++------- hardware_interface/src/sensor.cpp | 17 +++++++++++---- hardware_interface/src/system.cpp | 28 ++++++++++++++++++++---- 3 files changed, 63 insertions(+), 16 deletions(-) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index e0df076af6..66e9923b9f 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -302,19 +302,28 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p last_read_cycle_time_ = time; return return_type::OK; } - return_type result = return_type::ERROR; if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_read(time, period).result; - if (result == return_type::ERROR) + const auto trigger_result = impl_->trigger_read(time, period); + if (trigger_result.result == return_type::ERROR) { error(); } + if (trigger_result.successful && trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (trigger_result.successful && trigger_result.period.has_value()) + { + read_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds()); + } last_read_cycle_time_ = time; + return trigger_result.result; } - return result; + return return_type::OK; } return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) @@ -324,19 +333,28 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & last_write_cycle_time_ = time; return return_type::OK; } - return_type result = return_type::ERROR; if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_write(time, period).result; - if (result == return_type::ERROR) + const auto trigger_result = impl_->trigger_write(time, period); + if (trigger_result.result == return_type::ERROR) { error(); } + if (trigger_result.successful && trigger_result.execution_time.has_value()) + { + write_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (trigger_result.successful && trigger_result.period.has_value()) + { + write_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds()); + } last_write_cycle_time_ = time; + return trigger_result.result; } - return result; + return return_type::OK; } std::recursive_mutex & Actuator::get_mutex() { return actuators_mutex_; } diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index aa5501d6ac..6efa47eac2 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -256,19 +256,28 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per last_read_cycle_time_ = time; return return_type::OK; } - return_type result = return_type::ERROR; if ( impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->trigger_read(time, period).result; - if (result == return_type::ERROR) + const auto trigger_result = impl_->trigger_read(time, period); + if (trigger_result.result == return_type::ERROR) { error(); } + if (trigger_result.successful && trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (trigger_result.successful && trigger_result.period.has_value()) + { + read_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds()); + } last_read_cycle_time_ = time; + return trigger_result.result; } - return result; + return return_type::OK; } std::recursive_mutex & Sensor::get_mutex() { return sensors_mutex_; } diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 2d4efa92e1..07990dbfb3 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -141,6 +141,8 @@ const rclcpp_lifecycle::State & System::shutdown() const rclcpp_lifecycle::State & System::activate() { std::unique_lock lock(system_mutex_); + read_statistics_.reset_statistics(); + write_statistics_.reset_statistics(); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_lifecycle_state())) @@ -159,8 +161,6 @@ const rclcpp_lifecycle::State & System::activate() break; } } - read_statistics_->reset_statistics(); - write_statistics_->reset_statistics(); return impl_->get_lifecycle_state(); } @@ -311,9 +311,19 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per { error(); } + if (trigger_result.successful && trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (trigger_result.successful && trigger_result.period.has_value()) + { + read_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds()); + } last_read_cycle_time_ = time; + return trigger_result.result; } - return trigger_result.result; + return return_type::OK; } return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) @@ -332,9 +342,19 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe { error(); } + if (trigger_result.successful && trigger_result.execution_time.has_value()) + { + write_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (trigger_result.successful && trigger_result.period.has_value()) + { + write_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds()); + } last_write_cycle_time_ = time; + return trigger_result.result; } - return trigger_result.result; + return return_type::OK; } std::recursive_mutex & System::get_mutex() { return system_mutex_; } From 6aaf2e20a9bc3fcc360f7f696cd95008c19c0c51 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 22:35:42 +0100 Subject: [PATCH 11/38] Add missing reset_statistics on activate --- hardware_interface/src/actuator.cpp | 2 ++ hardware_interface/src/sensor.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 66e9923b9f..8345f7eadc 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -143,6 +143,8 @@ const rclcpp_lifecycle::State & Actuator::shutdown() const rclcpp_lifecycle::State & Actuator::activate() { std::unique_lock lock(actuators_mutex_); + read_statistics_.reset_statistics(); + write_statistics_.reset_statistics(); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_lifecycle_state())) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 6efa47eac2..0868c91ed5 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -140,6 +140,7 @@ const rclcpp_lifecycle::State & Sensor::shutdown() const rclcpp_lifecycle::State & Sensor::activate() { std::unique_lock lock(sensors_mutex_); + 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())) From cd4dc021f5b050f8b2304904c04c0d4ef59d3068 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:06:34 +0100 Subject: [PATCH 12/38] Add get_read_statistics and get_write_statistics to the Hardware Components --- .../include/hardware_interface/actuator.hpp | 4 ++++ .../include/hardware_interface/sensor.hpp | 2 ++ .../include/hardware_interface/system.hpp | 4 ++++ hardware_interface/src/actuator.cpp | 10 ++++++++++ hardware_interface/src/sensor.cpp | 5 +++++ hardware_interface/src/system.cpp | 10 ++++++++++ 6 files changed, 35 insertions(+) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 2aafa1037a..5ac67c1c36 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -83,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); diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 8433623e3c..4cba2e761f 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -71,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; } diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 98845ce738..134ce40d9d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -83,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); diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 8345f7eadc..50b0cfa042 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -297,6 +297,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())) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 0868c91ed5..a420944339 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -250,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())) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 07990dbfb3..34159448dd 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -295,6 +295,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())) From f7f4656a5eed660b0412307e95b41012812cded5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:07:04 +0100 Subject: [PATCH 13/38] Only update_statistics when the sample count is higher --- .../types/statistics_types.hpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/hardware_interface/include/hardware_interface/types/statistics_types.hpp b/hardware_interface/include/hardware_interface/types/statistics_types.hpp index 2d0fede058..9dd9f29b57 100644 --- a/hardware_interface/include/hardware_interface/types/statistics_types.hpp +++ b/hardware_interface/include/hardware_interface/types/statistics_types.hpp @@ -35,15 +35,18 @@ struct MovingAverageStatisticsData public: MovingAverageStatisticsData() { reset(); } - void update_statistics(std::shared_ptr & statistics) + void update_statistics(const std::shared_ptr & statistics) { std::unique_lock lock(mutex_); - 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() > 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(); + } } void reset() From 5f5d7d4cb3aaf37ee57ca59b61789d3b123b9ecf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:09:34 +0100 Subject: [PATCH 14/38] Update read and write statistics data of hardware component info --- hardware_interface/src/resource_manager.cpp | 30 ++++++++++++++------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 24e4fa8853..46bb43d87e 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1775,17 +1775,17 @@ HardwareReadWriteStatus ResourceManager::read( auto ret_val = return_type::OK; try { + auto & hardware_component_info = + resource_storage_->hardware_info_map_[component.get_name()]; if ( - resource_storage_->hardware_info_map_[component.get_name()].rw_rate == 0 || - resource_storage_->hardware_info_map_[component.get_name()].rw_rate == - resource_storage_->cm_update_rate_) + hardware_component_info.rw_rate == 0 || + hardware_component_info.rw_rate == resource_storage_->cm_update_rate_) { ret_val = component.read(time, period); } else { - const double read_rate = - resource_storage_->hardware_info_map_[component.get_name()].rw_rate; + const double read_rate = hardware_component_info.rw_rate; const auto current_time = resource_storage_->get_clock()->now(); const rclcpp::Duration actual_period = current_time - component.get_last_read_time(); if (actual_period.seconds() * read_rate >= 0.99) @@ -1793,6 +1793,11 @@ HardwareReadWriteStatus ResourceManager::read( ret_val = component.read(current_time, actual_period); } } + 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); @@ -1861,17 +1866,17 @@ HardwareReadWriteStatus ResourceManager::write( auto ret_val = return_type::OK; try { + auto & hardware_component_info = + resource_storage_->hardware_info_map_[component.get_name()]; if ( - resource_storage_->hardware_info_map_[component.get_name()].rw_rate == 0 || - resource_storage_->hardware_info_map_[component.get_name()].rw_rate == - resource_storage_->cm_update_rate_) + hardware_component_info.rw_rate == 0 || + hardware_component_info.rw_rate == resource_storage_->cm_update_rate_) { ret_val = component.write(time, period); } else { - const double write_rate = - resource_storage_->hardware_info_map_[component.get_name()].rw_rate; + const double write_rate = hardware_component_info.rw_rate; const auto current_time = resource_storage_->get_clock()->now(); const rclcpp::Duration actual_period = current_time - component.get_last_write_time(); if (actual_period.seconds() * write_rate >= 0.99) @@ -1879,6 +1884,11 @@ HardwareReadWriteStatus ResourceManager::write( ret_val = component.write(current_time, actual_period); } } + 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); From af1ddc994817f9f6c8762c114245c99e7b955755 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:17:29 +0100 Subject: [PATCH 15/38] return a const reference from `get_components_status` method --- controller_manager/src/controller_manager.cpp | 4 ++-- .../include/hardware_interface/resource_manager.hpp | 2 +- hardware_interface/src/resource_manager.cpp | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 07af09c7ac..98c2662a3f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3270,7 +3270,7 @@ void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { bool atleast_one_hw_active = false; - const auto hw_components_info = resource_manager_->get_components_status(); + const auto & hw_components_info = resource_manager_->get_components_status(); for (const auto & [component_name, component_info] : hw_components_info) { if (component_info.state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) @@ -3425,7 +3425,7 @@ void ControllerManager::hardware_components_diagnostic_callback( { bool all_active = true; bool atleast_one_hw_active = false; - const auto hw_components_info = resource_manager_->get_components_status(); + 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()); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 06b8f51952..d5d25044dd 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -377,7 +377,7 @@ class ResourceManager /** * \return map of hardware names and their status. */ - std::unordered_map get_components_status(); + const std::unordered_map & get_components_status(); /// Prepare the hardware components for a new command interface mode /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 46bb43d87e..6f7405933f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1468,7 +1468,8 @@ void ResourceManager::import_component( } // CM API: Called in "callback/slow"-thread -std::unordered_map ResourceManager::get_components_status() +const std::unordered_map & +ResourceManager::get_components_status() { auto loop_and_get_state = [&](auto & container) { From 916661e2c394bcef9b51527d6861e72041623471 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:22:55 +0100 Subject: [PATCH 16/38] Add hardware_components statistics GPL information --- .../src/controller_manager_parameters.yaml | 71 +++++++++++++++++++ 1 file changed, 71 insertions(+) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 1bb9b152b1..1d41504092 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 read/write loop. If the mean error exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the hardware component read/write loop. If the mean error exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the hardware component read/write loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the hardware component read/write loop. If the standard deviation exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + validation: { + gt<>: 0.0, + } + } + execution_time: + mean_error: + warn: { + type: double, + default_value: 1000.0, + description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 2000.0, + description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 100.0, + description: "The warning threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 200.0, + description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } From f444a05ad36c029531332475e7e9742409ec80ce Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 16:43:57 +0100 Subject: [PATCH 17/38] Add first version of the hardware component diagnostics --- controller_manager/src/controller_manager.cpp | 123 ++++++++++++++++-- 1 file changed, 110 insertions(+), 13 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 98c2662a3f..8d2295e82f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3423,8 +3423,20 @@ 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 periodicity_suffix = ".periodicity"; + const std::string exec_time_suffix = ".execution_time"; + 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) { @@ -3438,30 +3450,115 @@ 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 + { + 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) { - if (!atleast_one_hw_active) + stat.add(component_name + state_suffix, component_info.state.label()); + const bool is_async = component_info.is_async; + 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) + { + const auto periodicity_stats = component_info.read_statistics->periodicity.get_statistics(); + const auto exec_time_stats = component_info.read_statistics->execution_time.get_statistics(); + stat.add(component_name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); + if (is_async) + { + stat.add( + component_name + periodicity_suffix, + make_stats_string(periodicity_stats, "Hz") + + " -> Desired : " + std::to_string(component_info.rw_rate) + " Hz"); + const double periodicity_error = + std::abs(periodicity_stats.average - static_cast(component_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) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + add_element_to_list(bad_periodicity_async_hw, component_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 (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + add_element_to_list(bad_periodicity_async_hw, component_name); + } + } + const double max_exp_exec_time = + is_async ? 1.e6 / static_cast(component_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) + { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + high_exec_time_hw.push_back(component_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 (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + high_exec_time_hw.push_back(component_name); + } } } } From 0f8f973e929eb0f6452260089ea07fa457f6e2d0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 22:07:17 +0100 Subject: [PATCH 18/38] Add logic to the hardware components diagnostics --- controller_manager/src/controller_manager.cpp | 142 +++++++++++------- 1 file changed, 91 insertions(+), 51 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8d2295e82f..41cfb9bab6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3434,13 +3434,10 @@ void ControllerManager::hardware_components_diagnostic_callback( 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 periodicity_suffix = ".periodicity"; - const std::string exec_time_suffix = ".execution_time"; 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; @@ -3492,7 +3489,6 @@ void ControllerManager::hardware_components_diagnostic_callback( for (const auto & [component_name, component_info] : hw_components_info) { stat.add(component_name + state_suffix, component_info.state.label()); - const bool is_async = component_info.is_async; if (component_info.state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { all_active = false; @@ -3503,64 +3499,108 @@ void ControllerManager::hardware_components_diagnostic_callback( } if (component_info.state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - const auto periodicity_stats = component_info.read_statistics->periodicity.get_statistics(); - const auto exec_time_stats = component_info.read_statistics->execution_time.get_statistics(); - stat.add(component_name + exec_time_suffix, make_stats_string(exec_time_stats, "us")); - if (is_async) - { + 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) + { + 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( - component_name + periodicity_suffix, - make_stats_string(periodicity_stats, "Hz") + - " -> Desired : " + std::to_string(component_info.rw_rate) + " Hz"); - const double periodicity_error = - std::abs(periodicity_stats.average - static_cast(component_info.rw_rate)); + 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 ( - periodicity_error > - params_->diagnostics.threshold.hardware_components.periodicity.mean_error.error || - periodicity_stats.standard_deviation > - params_->diagnostics.threshold.hardware_components.periodicity.standard_deviation.error) + (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) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - add_element_to_list(bad_periodicity_async_hw, component_name); + diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + high_exec_time_hw.push_back(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) + (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 (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + if (diag_level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; } - add_element_to_list(bad_periodicity_async_hw, component_name); - } - } - const double max_exp_exec_time = - is_async ? 1.e6 / static_cast(component_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) - { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - high_exec_time_hw.push_back(component_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 (level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) - { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + high_exec_time_hw.push_back(comp_name); } - high_exec_time_hw.push_back(component_name); - } + }; + + update_stats( + component_name, component_info.read_statistics, read_cycle_suffix, level, component_info, + params_); + 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 + "]"); + } } void ControllerManager::controller_manager_diagnostic_callback( From cee7a3791cd40799000bd9aa9dcdc31e078074b9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 22:12:42 +0100 Subject: [PATCH 19/38] add set_reset_statistics_sample_count method to reset statistics based on samples --- .../types/statistics_types.hpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/types/statistics_types.hpp b/hardware_interface/include/hardware_interface/types/statistics_types.hpp index 9dd9f29b57..19b3650af9 100644 --- a/hardware_interface/include/hardware_interface/types/statistics_types.hpp +++ b/hardware_interface/include/hardware_interface/types/statistics_types.hpp @@ -33,7 +33,11 @@ struct MovingAverageStatisticsData using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; public: - MovingAverageStatisticsData() { reset(); } + MovingAverageStatisticsData() + { + reset(); + reset_statistics_sample_count_ = std::numeric_limits::max(); + } void update_statistics(const std::shared_ptr & statistics) { @@ -47,6 +51,16 @@ struct MovingAverageStatisticsData statistics_data.sample_count = statistics->GetCount(); statistics_data = statistics->GetStatistics(); } + if (statistics->GetCount() >= reset_statistics_sample_count_) + { + statistics->Reset(); + } + } + + 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() @@ -66,6 +80,7 @@ struct MovingAverageStatisticsData private: StatisticData statistics_data; + unsigned int reset_statistics_sample_count_ = std::numeric_limits::max(); mutable realtime_tools::prio_inherit_mutex mutex_; }; } // namespace ros2_control From 0fd53055a246e533247badd6b6f7efbad07bb313 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 23:36:06 +0100 Subject: [PATCH 20/38] add test on the hardware statistics --- .../test/test_resource_manager.cpp | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 65735e23f7..418568214b 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1846,6 +1846,45 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage 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)) + { + EXPECT_LT( + status_map[component_name].read_statistics->execution_time.get_statistics().average, + 100); + 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, + 100); + 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(); } From bbc18e2e10aa85eb722e9618c31f9a7d8c378d0f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 23:51:36 +0100 Subject: [PATCH 21/38] Update parameter description --- .../src/controller_manager_parameters.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 1d41504092..f316b81055 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 read/write loop. If the mean error exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + 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. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", 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 read/write loop. If the mean error exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + 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. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", 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 read/write loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + 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. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", 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 read/write loop. If the standard deviation exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + 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. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", validation: { gt<>: 0.0, } @@ -175,7 +175,7 @@ controller_manager: warn: { type: double, default_value: 1000.0, - description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + description: "The warning threshold for the mean error of the hardware component's read/write cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. 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 cycle to execute it's cycle", validation: { gt<>: 0.0, } @@ -183,7 +183,7 @@ controller_manager: error: { type: double, default_value: 2000.0, - description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + description: "The error threshold for the mean error of the hardware component's read/write cycle execution time in microseconds. If the mean error exceeds this threshold, a error diagnostic will be published. 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 cycle to execute it's cycle", 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 controller's update cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + description: "The warning threshold for the standard deviation of the 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, } @@ -200,7 +200,7 @@ controller_manager: error: { type: double, default_value: 200.0, - description: "The error threshold for the standard deviation of the controller's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + description: "The error threshold for the standard deviation of the hardware component's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", validation: { gt<>: 0.0, } From 38ded1c933f5c9e7d76007d9dd004f8a2145bd1a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 17 Jan 2025 00:28:42 +0100 Subject: [PATCH 22/38] fix the logic for async hardware components --- hardware_interface/src/actuator.cpp | 4 ++-- hardware_interface/src/system.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 50b0cfa042..443539179e 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -299,12 +299,12 @@ const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_c const HardwareComponentStatisticsCollector & Actuator::get_read_statistics() const { - return read_statistics_; + return !impl_->get_hardware_info().is_async ? read_statistics_ : write_statistics_; } const HardwareComponentStatisticsCollector & Actuator::get_write_statistics() const { - return write_statistics_; + return !impl_->get_hardware_info().is_async ? write_statistics_ : read_statistics_; } return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 34159448dd..7862965a44 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -297,12 +297,12 @@ const rclcpp::Time & System::get_last_write_time() const { return last_write_cyc const HardwareComponentStatisticsCollector & System::get_read_statistics() const { - return read_statistics_; + return !impl_->get_hardware_info().is_async ? read_statistics_ : write_statistics_; } const HardwareComponentStatisticsCollector & System::get_write_statistics() const { - return write_statistics_; + return !impl_->get_hardware_info().is_async ? write_statistics_ : read_statistics_; } return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) From 2c3bce6ce4b56ed6c453bd05793f0e24915d3399 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 17 Jan 2025 00:40:42 +0100 Subject: [PATCH 23/38] update write cycle statistics only if it is valid --- controller_manager/src/controller_manager.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 41cfb9bab6..2cff12bc3e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3568,12 +3568,17 @@ void ControllerManager::hardware_components_diagnostic_callback( } }; + // For components : {actuator, sensor and system} update_stats( component_name, component_info.read_statistics, read_cycle_suffix, level, component_info, params_); - update_stats( - component_name, component_info.write_statistics, write_cycle_suffix, level, component_info, - params_); + // For components : {actuator and system} + if (component_info.write_statistics) + { + update_stats( + component_name, component_info.write_statistics, write_cycle_suffix, level, + component_info, params_); + } } } From e66427c9fb48839de944839c5d91f59fd2e78c44 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 18 Jan 2025 11:37:55 +0100 Subject: [PATCH 24/38] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- controller_manager/src/controller_manager_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index f316b81055..c755ee45c3 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -175,7 +175,7 @@ controller_manager: 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. 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 cycle to execute it's cycle", + 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. 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 cycle to execute its cycle", validation: { gt<>: 0.0, } From 7abd0537b8539ae1f8d9a96705d961cea8245728 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 18 Jan 2025 11:38:32 +0100 Subject: [PATCH 25/38] Update controller_manager/src/controller_manager_parameters.yaml --- controller_manager/src/controller_manager_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index c755ee45c3..56c1a583b2 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -183,7 +183,7 @@ controller_manager: 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. 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 cycle to execute it's cycle", + 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. 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 cycle to execute its cycle", validation: { gt<>: 0.0, } From a387d769d305c11e70f2d1beac389442568b1d08 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 18 Jan 2025 13:16:31 +0100 Subject: [PATCH 26/38] Add conditioning to protect the hardware statistics --- controller_manager/src/controller_manager.cpp | 13 +++++----- hardware_interface/src/resource_manager.cpp | 26 ++++++++++++------- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2cff12bc3e..208068d6ad 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3505,6 +3505,10 @@ void ControllerManager::hardware_components_diagnostic_callback( 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"; @@ -3573,12 +3577,9 @@ void ControllerManager::hardware_components_diagnostic_callback( component_name, component_info.read_statistics, read_cycle_suffix, level, component_info, params_); // For components : {actuator and system} - if (component_info.write_statistics) - { - update_stats( - component_name, component_info.write_statistics, write_cycle_suffix, level, - component_info, params_); - } + update_stats( + component_name, component_info.write_statistics, write_cycle_suffix, level, component_info, + params_); } } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 6f7405933f..3f56cc94ac 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1794,11 +1794,14 @@ HardwareReadWriteStatus ResourceManager::read( ret_val = component.read(current_time, actual_period); } } - 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); + 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); @@ -1885,11 +1888,14 @@ HardwareReadWriteStatus ResourceManager::write( ret_val = component.write(current_time, actual_period); } } - 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); + 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); From c3af405ea9ecbe000413c10ebe65e9ccf95f2138 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 22:15:09 +0100 Subject: [PATCH 27/38] Fix the logic in the hardware components periodicity and proper cycles --- hardware_interface/src/actuator.cpp | 48 ++++++++++++--------- hardware_interface/src/resource_manager.cpp | 21 ++++++--- hardware_interface/src/sensor.cpp | 24 ++++++----- hardware_interface/src/system.cpp | 48 ++++++++++++--------- 4 files changed, 85 insertions(+), 56 deletions(-) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 443539179e..dbaf01bf5f 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -41,8 +41,8 @@ Actuator::Actuator(Actuator && other) noexcept { std::lock_guard lock(other.actuators_mutex_); impl_ = std::move(other.impl_); - last_read_cycle_time_ = other.last_read_cycle_time_; - last_write_cycle_time_ = other.last_write_cycle_time_; + last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); + last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); } const rclcpp_lifecycle::State & Actuator::initialize( @@ -55,8 +55,6 @@ const rclcpp_lifecycle::State & Actuator::initialize( switch (impl_->init(actuator_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - last_read_cycle_time_ = clock_interface->get_clock()->now(); - last_write_cycle_time_ = clock_interface->get_clock()->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); @@ -143,6 +141,8 @@ const rclcpp_lifecycle::State & Actuator::shutdown() 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) @@ -323,16 +323,20 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p { error(); } - if (trigger_result.successful && trigger_result.execution_time.has_value()) + if (trigger_result.successful) { - read_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); + 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; } - if (trigger_result.successful && trigger_result.period.has_value()) - { - read_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds()); - } - last_read_cycle_time_ = time; return trigger_result.result; } return return_type::OK; @@ -354,16 +358,20 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & { error(); } - if (trigger_result.successful && trigger_result.execution_time.has_value()) + if (trigger_result.successful) { - write_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); + 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; } - if (trigger_result.successful && trigger_result.period.has_value()) - { - write_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().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 3f56cc94ac..f6fa90d317 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1778,17 +1778,22 @@ HardwareReadWriteStatus ResourceManager::read( { auto & hardware_component_info = resource_storage_->hardware_info_map_[component.get_name()]; + const auto current_time = resource_storage_->get_clock()->now(); + const rclcpp::Duration actual_period = + component.get_last_read_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED + ? current_time - component.get_last_read_time() + : rclcpp::Duration::from_seconds( + 1.0 / static_cast(hardware_component_info.rw_rate)); + // const rclcpp::Duration actual_period = current_time - component.get_last_read_time(); if ( hardware_component_info.rw_rate == 0 || hardware_component_info.rw_rate == resource_storage_->cm_update_rate_) { - ret_val = component.read(time, period); + ret_val = component.read(current_time, actual_period); } else { const double read_rate = hardware_component_info.rw_rate; - const auto current_time = resource_storage_->get_clock()->now(); - const rclcpp::Duration actual_period = current_time - component.get_last_read_time(); if (actual_period.seconds() * read_rate >= 0.99) { ret_val = component.read(current_time, actual_period); @@ -1872,17 +1877,21 @@ HardwareReadWriteStatus ResourceManager::write( { auto & hardware_component_info = resource_storage_->hardware_info_map_[component.get_name()]; + const auto current_time = resource_storage_->get_clock()->now(); + const rclcpp::Duration actual_period = + component.get_last_write_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED + ? current_time - component.get_last_write_time() + : rclcpp::Duration::from_seconds( + 1.0 / static_cast(hardware_component_info.rw_rate)); if ( hardware_component_info.rw_rate == 0 || hardware_component_info.rw_rate == resource_storage_->cm_update_rate_) { - ret_val = component.write(time, period); + ret_val = component.write(current_time, actual_period); } else { const double write_rate = hardware_component_info.rw_rate; - const auto current_time = resource_storage_->get_clock()->now(); - const rclcpp::Duration actual_period = current_time - component.get_last_write_time(); if (actual_period.seconds() * write_rate >= 0.99) { ret_val = component.write(current_time, actual_period); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index a420944339..ebbbd54c15 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -40,7 +40,7 @@ Sensor::Sensor(Sensor && other) noexcept { std::lock_guard lock(other.sensors_mutex_); impl_ = std::move(other.impl_); - last_read_cycle_time_ = other.last_read_cycle_time_; + last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); } const rclcpp_lifecycle::State & Sensor::initialize( @@ -53,7 +53,6 @@ const rclcpp_lifecycle::State & Sensor::initialize( switch (impl_->init(sensor_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - last_read_cycle_time_ = clock_interface->get_clock()->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); @@ -140,6 +139,7 @@ const rclcpp_lifecycle::State & Sensor::shutdown() 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) { @@ -271,16 +271,20 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per { error(); } - if (trigger_result.successful && trigger_result.execution_time.has_value()) + if (trigger_result.successful) { - read_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); + 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; } - if (trigger_result.successful && trigger_result.period.has_value()) - { - read_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().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 7862965a44..6bc0db6464 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -39,8 +39,8 @@ System::System(System && other) noexcept { std::lock_guard lock(other.system_mutex_); impl_ = std::move(other.impl_); - last_read_cycle_time_ = other.last_read_cycle_time_; - last_write_cycle_time_ = other.last_write_cycle_time_; + last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); + last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); } const rclcpp_lifecycle::State & System::initialize( @@ -53,8 +53,6 @@ const rclcpp_lifecycle::State & System::initialize( switch (impl_->init(system_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - last_read_cycle_time_ = clock_interface->get_clock()->now(); - last_write_cycle_time_ = clock_interface->get_clock()->now(); impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); @@ -141,6 +139,8 @@ const rclcpp_lifecycle::State & System::shutdown() 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) @@ -321,16 +321,20 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per { error(); } - if (trigger_result.successful && trigger_result.execution_time.has_value()) + if (trigger_result.successful) { - read_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); + 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; } - if (trigger_result.successful && trigger_result.period.has_value()) - { - read_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds()); - } - last_read_cycle_time_ = time; return trigger_result.result; } return return_type::OK; @@ -352,16 +356,20 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe { error(); } - if (trigger_result.successful && trigger_result.execution_time.has_value()) + if (trigger_result.successful) { - write_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); + 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; } - if (trigger_result.successful && trigger_result.period.has_value()) - { - write_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds()); - } - last_write_cycle_time_ = time; return trigger_result.result; } return return_type::OK; From 09b2270384e339351e27ce5b531843e4bb20c91b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 22:16:16 +0100 Subject: [PATCH 28/38] Add proper assertions for the async periodicity tests --- .../test/test_resource_manager.cpp | 93 ++++++++++++++++++- 1 file changed, 91 insertions(+), 2 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 418568214b..b1099732c4 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -2007,6 +2007,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(), @@ -2030,6 +2031,42 @@ 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 { @@ -2044,7 +2081,8 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest EXPECT_TRUE(ok); EXPECT_TRUE(failed_hardware_names.empty()); } - time = time + duration; + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); check_if_interface_available(true, true); } @@ -2097,7 +2135,58 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest 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.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->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)))); + }; + + if (check_for_updated_values) + { + check_periodicity(TEST_ACTUATOR_HARDWARE_NAME, 100u); + check_periodicity(TEST_SYSTEM_HARDWARE_NAME, 100u); + EXPECT_LT( + status_map[TEST_ACTUATOR_HARDWARE_NAME] + .read_statistics->execution_time.get_statistics() + .average, + 100); + EXPECT_LT( + status_map[TEST_ACTUATOR_HARDWARE_NAME] + .write_statistics->execution_time.get_statistics() + .average, + 100); + EXPECT_LT( + status_map[TEST_SYSTEM_HARDWARE_NAME] + .read_statistics->execution_time.get_statistics() + .average, + 100); + EXPECT_LT( + status_map[TEST_SYSTEM_HARDWARE_NAME] + .write_statistics->execution_time.get_statistics() + .average, + 100); } } From 1828107abcfafb95b4f0d63a1a29694e1e60c7a7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 22:41:30 +0100 Subject: [PATCH 29/38] Fix the parameters_context for the controllers diagnostics --- controller_manager/doc/parameters_context.yaml | 4 ++-- .../src/controller_manager_parameters.yaml | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml index a015765c79..2b0d46f48b 100644 --- a/controller_manager/doc/parameters_context.yaml +++ b/controller_manager/doc/parameters_context.yaml @@ -17,5 +17,5 @@ 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 cycle to execute its update cycle. diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 56c1a583b2..a413fb7a77 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -69,7 +69,7 @@ controller_manager: warn: { type: double, default_value: 5.0, - description: "The warning threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + description: "The warning threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, a warning diagnostic will be published.", validation: { gt<>: 0.0, } @@ -77,7 +77,7 @@ controller_manager: error: { type: double, default_value: 10.0, - description: "The error threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + description: "The error threshold for the mean error of the controller update loop. If the mean error exceeds this threshold, an error diagnostic will be published.", validation: { gt<>: 0.0, } @@ -86,7 +86,7 @@ controller_manager: warn: { type: double, default_value: 5.0, - description: "The warning threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + description: "The warning threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", validation: { gt<>: 0.0, } @@ -94,7 +94,7 @@ controller_manager: error: { type: double, default_value: 10.0, - description: "The error threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, an error diagnostic will be published. This diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.", + description: "The error threshold for the standard deviation of the controller update loop. If the standard deviation exceeds this threshold, an error diagnostic will be published.", validation: { gt<>: 0.0, } @@ -104,7 +104,7 @@ controller_manager: warn: { type: double, default_value: 1000.0, - description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + description: "The warning threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published.", validation: { gt<>: 0.0, } @@ -112,7 +112,7 @@ controller_manager: error: { type: double, default_value: 2000.0, - description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle", + description: "The error threshold for the mean error of the controller's update cycle execution time in microseconds. If the mean error exceeds this threshold, an error diagnostic will be published.", validation: { gt<>: 0.0, } From ead9307f1c17cfa74bb7e719dbaf0dc6291b0b41 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 22:50:02 +0100 Subject: [PATCH 30/38] Use parameters_context.yaml for common documentation of the parameters --- controller_manager/doc/parameters_context.yaml | 8 +++++++- .../src/controller_manager_parameters.yaml | 12 ++++++------ 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml index 2b0d46f48b..c9e95f6396 100644 --- a/controller_manager/doc/parameters_context.yaml +++ b/controller_manager/doc/parameters_context.yaml @@ -18,4 +18,10 @@ 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.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 cycle to execute its update cycle. + 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_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index a413fb7a77..049c76fc9f 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. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + 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, } @@ -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. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + 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, } @@ -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. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + 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, } @@ -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. This diagnostics will be published only for the asynchronous hardware component, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity.", + 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, } @@ -175,7 +175,7 @@ controller_manager: 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. 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 cycle to execute its cycle", + 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, } @@ -183,7 +183,7 @@ controller_manager: 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. 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 cycle to execute its cycle", + 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, } From e63792ada33bca49b3a61c9ef058e9741fe99fd5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 23:20:45 +0100 Subject: [PATCH 31/38] fix the logic back for failing tests --- hardware_interface/src/resource_manager.cpp | 22 ++++++++++----------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index f6fa90d317..b4c215a9c0 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1779,21 +1779,20 @@ HardwareReadWriteStatus ResourceManager::read( auto & hardware_component_info = resource_storage_->hardware_info_map_[component.get_name()]; const auto current_time = resource_storage_->get_clock()->now(); - const rclcpp::Duration actual_period = - component.get_last_read_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED - ? current_time - component.get_last_read_time() - : rclcpp::Duration::from_seconds( - 1.0 / static_cast(hardware_component_info.rw_rate)); // const rclcpp::Duration actual_period = current_time - component.get_last_read_time(); if ( hardware_component_info.rw_rate == 0 || hardware_component_info.rw_rate == resource_storage_->cm_update_rate_) { - ret_val = component.read(current_time, actual_period); + ret_val = component.read(current_time, period); } else { const double read_rate = hardware_component_info.rw_rate; + const rclcpp::Duration actual_period = + component.get_last_read_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED + ? current_time - component.get_last_read_time() + : rclcpp::Duration::from_seconds(1.0 / static_cast(read_rate)); if (actual_period.seconds() * read_rate >= 0.99) { ret_val = component.read(current_time, actual_period); @@ -1878,20 +1877,19 @@ HardwareReadWriteStatus ResourceManager::write( auto & hardware_component_info = resource_storage_->hardware_info_map_[component.get_name()]; const auto current_time = resource_storage_->get_clock()->now(); - const rclcpp::Duration actual_period = - component.get_last_write_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED - ? current_time - component.get_last_write_time() - : rclcpp::Duration::from_seconds( - 1.0 / static_cast(hardware_component_info.rw_rate)); if ( hardware_component_info.rw_rate == 0 || hardware_component_info.rw_rate == resource_storage_->cm_update_rate_) { - ret_val = component.write(current_time, actual_period); + ret_val = component.write(current_time, period); } else { const double write_rate = hardware_component_info.rw_rate; + const rclcpp::Duration actual_period = + component.get_last_write_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED + ? current_time - component.get_last_write_time() + : rclcpp::Duration::from_seconds(1.0 / static_cast(write_rate)); if (actual_period.seconds() * write_rate >= 0.99) { ret_val = component.write(current_time, actual_period); From e1b3cb121f22dfd8b72199f379b49f2df65b141c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Jan 2025 10:17:41 +0100 Subject: [PATCH 32/38] Remove the period variable from the Return status --- .../hardware_interface/actuator_interface.hpp | 12 ------------ .../include/hardware_interface/sensor_interface.hpp | 6 ------ .../include/hardware_interface/system_interface.hpp | 12 ------------ .../types/hardware_interface_return_values.hpp | 2 -- 4 files changed, 32 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index a9e02b4fd9..4916a38065 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -376,7 +376,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::OK; return status; } - const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); const auto result = async_handler_->trigger_async_callback(time, period); status.successful = result.first; status.result = result.second; @@ -385,10 +384,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { status.execution_time = execution_time; } - if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - status.period = time - last_trigger_time; - } if (!status.successful) { RCLCPP_WARN( @@ -407,7 +402,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = read(time, period); status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); - status.period = period; } return status; } @@ -451,7 +445,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::OK; return status; } - const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); const auto result = async_handler_->trigger_async_callback(time, period); status.successful = result.first; status.result = result.second; @@ -460,10 +453,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod { status.execution_time = execution_time; } - if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - status.period = time - last_trigger_time; - } if (!status.successful) { RCLCPP_WARN( @@ -482,7 +471,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = write(time, period); status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); - status.period = period; } return status; } diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2f4affaac0..f117e6a7d5 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -243,7 +243,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::ERROR; if (info_.is_async) { - const rclcpp::Time last_trigger_time = read_async_handler_->get_current_callback_time(); const auto result = read_async_handler_->trigger_async_callback(time, period); status.successful = result.first; status.result = result.second; @@ -252,10 +251,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { status.execution_time = execution_time; } - if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - status.period = time - last_trigger_time; - } if (!status.successful) { RCLCPP_WARN( @@ -274,7 +269,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = read(time, period); status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); - status.period = period; } return status; } diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 16f43a3b36..71ba717f0a 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -405,7 +405,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::OK; return status; } - const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); const auto result = async_handler_->trigger_async_callback(time, period); status.successful = result.first; status.result = result.second; @@ -414,10 +413,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { status.execution_time = execution_time; } - if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - status.period = time - last_trigger_time; - } if (!status.successful) { RCLCPP_WARN( @@ -436,7 +431,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = read(time, period); status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); - status.period = period; } return status; } @@ -480,7 +474,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::OK; return status; } - const rclcpp::Time last_trigger_time = async_handler_->get_current_callback_time(); const auto result = async_handler_->trigger_async_callback(time, period); status.successful = result.first; status.result = result.second; @@ -489,10 +482,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { status.execution_time = execution_time; } - if (last_trigger_time.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - status.period = time - last_trigger_time; - } if (!status.successful) { RCLCPP_WARN( @@ -511,7 +500,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = write(time, period); status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); - status.period = period; } return status; } diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index fbd508a929..3be99be527 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -38,14 +38,12 @@ enum class return_type : std::uint8_t * @var successful: true if it was triggered successfully, false if not. * @var result: return_type::OK if update is successfully, otherwise return_type::ERROR. * @var execution_time: duration of the execution of the update method. - * @var period: period of the update method. */ struct HardwareComponentCycleStatus { bool successful = true; return_type result = return_type::OK; std::optional execution_time = std::nullopt; - std::optional period = std::nullopt; }; } // namespace hardware_interface From 0d4c705eff94208d880aed20aeeee3f81ff54c23 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Jan 2025 11:36:28 +0100 Subject: [PATCH 33/38] Add documentation to the newly added structs --- .../types/statistics_types.hpp | 45 +++++++++++++++---- 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/hardware_interface/include/hardware_interface/types/statistics_types.hpp b/hardware_interface/include/hardware_interface/types/statistics_types.hpp index 19b3650af9..ba22a47c99 100644 --- a/hardware_interface/include/hardware_interface/types/statistics_types.hpp +++ b/hardware_interface/include/hardware_interface/types/statistics_types.hpp @@ -26,9 +26,13 @@ namespace ros2_control { -struct MovingAverageStatisticsData +/** + * @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 MovingAverageStatistics = + using MovingAverageStatisticsCollector = libstatistics_collector::moving_average_statistics::MovingAverageStatistics; using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; @@ -39,7 +43,11 @@ struct MovingAverageStatisticsData reset_statistics_sample_count_ = std::numeric_limits::max(); } - void update_statistics(const std::shared_ptr & statistics) + /** + * @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) @@ -57,6 +65,10 @@ struct MovingAverageStatisticsData } } + /** + * @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_); @@ -72,6 +84,10 @@ struct MovingAverageStatisticsData statistics_data.sample_count = 0; } + /** + * @brief Get the statistics data. + * @return statistics data. + */ const StatisticData & get_statistics() const { std::unique_lock lock(mutex_); @@ -79,33 +95,46 @@ struct MovingAverageStatisticsData } 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(); + execution_time = std::make_shared(); + periodicity = std::make_shared(); } - using MovingAverageStatistics = + 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(); } - std::shared_ptr execution_time = nullptr; - std::shared_ptr periodicity = nullptr; + /// Execution time statistics collector + std::shared_ptr execution_time = nullptr; + /// Periodicity statistics collector + std::shared_ptr periodicity = nullptr; }; } // namespace hardware_interface From cfe2ffd66f69ab3b1f918eb86875f1dafd8d8c86 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 28 Jan 2025 15:58:29 +0100 Subject: [PATCH 34/38] Use the memory ordering for better performance --- .../include/hardware_interface/actuator_interface.hpp | 8 ++++---- .../include/hardware_interface/system_interface.hpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 4916a38065..15ea8d6eec 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -125,13 +125,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod if (next_trigger_ == TriggerType::READ) { const auto ret = read(time, period); - next_trigger_ = TriggerType::WRITE; + next_trigger_.store(TriggerType::WRITE, std::memory_order_release); return ret; } else { const auto ret = write(time, period); - next_trigger_ = TriggerType::READ; + next_trigger_.store(TriggerType::READ, std::memory_order_release); return ret; } }, @@ -366,7 +366,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::ERROR; if (info_.is_async) { - if (next_trigger_ == TriggerType::WRITE) + if (next_trigger_.load(std::memory_order_acquire) == TriggerType::WRITE) { RCLCPP_WARN( get_logger(), @@ -435,7 +435,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::ERROR; if (info_.is_async) { - if (next_trigger_ == TriggerType::READ) + if (next_trigger_.load(std::memory_order_acquire) == TriggerType::READ) { RCLCPP_WARN( get_logger(), diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 71ba717f0a..2afc7cd003 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -128,13 +128,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI if (next_trigger_ == TriggerType::READ) { const auto ret = read(time, period); - next_trigger_ = TriggerType::WRITE; + next_trigger_.store(TriggerType::WRITE, std::memory_order_release); return ret; } else { const auto ret = write(time, period); - next_trigger_ = TriggerType::READ; + next_trigger_.store(TriggerType::READ, std::memory_order_release); return ret; } }, @@ -395,7 +395,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::ERROR; if (info_.is_async) { - if (next_trigger_ == TriggerType::WRITE) + if (next_trigger_.load(std::memory_order_acquire) == TriggerType::WRITE) { RCLCPP_WARN( get_logger(), @@ -464,7 +464,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::ERROR; if (info_.is_async) { - if (next_trigger_ == TriggerType::READ) + if (next_trigger_.load(std::memory_order_acquire) == TriggerType::READ) { RCLCPP_WARN( get_logger(), From bac1ac2baccff3e418fbdaa994af5e54396bc651 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 31 Jan 2025 14:07:21 +0100 Subject: [PATCH 35/38] first implementation of the serialized trigger --- .../hardware_interface/actuator_interface.hpp | 85 +++++++------------ .../hardware_interface/system_interface.hpp | 85 +++++++------------ .../test/test_components/test_actuator.cpp | 10 +++ .../test/test_components/test_system.cpp | 10 +++ .../test/test_resource_manager.cpp | 20 ++--- 5 files changed, 94 insertions(+), 116 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 15ea8d6eec..70966f0a55 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -122,18 +122,25 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod async_handler_->init( [this](const rclcpp::Time & time, const rclcpp::Duration & period) { - if (next_trigger_ == TriggerType::READ) + const auto read_start_time = std::chrono::steady_clock::now(); + const auto ret_read = read(time, period); + const auto read_end_time = std::chrono::steady_clock::now(); + read_return_info_.store(ret_read, std::memory_order_release); + read_execution_time_.store( + std::chrono::duration_cast(read_end_time - read_start_time), + std::memory_order_release); + if (ret_read != return_type::OK) { - const auto ret = read(time, period); - next_trigger_.store(TriggerType::WRITE, std::memory_order_release); - return ret; - } - else - { - const auto ret = write(time, period); - next_trigger_.store(TriggerType::READ, std::memory_order_release); - return ret; + return ret_read; } + const auto write_start_time = std::chrono::steady_clock::now(); + const auto ret_write = write(time, period); + const auto write_end_time = std::chrono::steady_clock::now(); + write_return_info_.store(ret_write, std::memory_order_release); + write_execution_time_.store( + std::chrono::duration_cast(write_end_time - write_start_time), + std::memory_order_release); + return ret_write; }, info_.thread_priority); async_handler_->start_thread(); @@ -366,30 +373,20 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::ERROR; if (info_.is_async) { - if (next_trigger_.load(std::memory_order_acquire) == TriggerType::WRITE) + status.result = read_return_info_.load(std::memory_order_acquire); + const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire); + if (read_exec_time.count() > 0) { - RCLCPP_WARN( - get_logger(), - "Trigger read called while write async handler call is still pending for hardware " - "interface : '%s'. Skipping read cycle and will wait for a write cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; + status.execution_time = read_exec_time; } const auto result = async_handler_->trigger_async_callback(time, period); status.successful = result.first; - status.result = result.second; - const auto execution_time = async_handler_->get_last_execution_time(); - if (execution_time.count() > 0) - { - status.execution_time = execution_time; - } if (!status.successful) { RCLCPP_WARN( get_logger(), - "Trigger read called while write async trigger is still in progress for hardware " - "interface : '%s'. Failed to trigger read cycle!", + "Trigger read/write called while the previous async trigger is still in progress for " + "hardware interface : '%s'. Failed to trigger read/write cycle!", info_.name.c_str()); status.result = return_type::OK; return status; @@ -435,34 +432,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod status.result = return_type::ERROR; if (info_.is_async) { - if (next_trigger_.load(std::memory_order_acquire) == TriggerType::READ) - { - RCLCPP_WARN( - get_logger(), - "Trigger write called while read async handler call is still pending for hardware " - "interface : '%s'. Skipping write cycle and will wait for a read cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; - } - const auto result = async_handler_->trigger_async_callback(time, period); - status.successful = result.first; - status.result = result.second; - const auto execution_time = async_handler_->get_last_execution_time(); - if (execution_time.count() > 0) - { - status.execution_time = execution_time; - } - if (!status.successful) + status.successful = true; + const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire); + if (write_exec_time.count() > 0) { - RCLCPP_WARN( - get_logger(), - "Trigger write called while read async trigger is still in progress for hardware " - "interface : '%s'. Failed to trigger write cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; + status.execution_time = write_exec_time; } + status.result = write_return_info_.load(std::memory_order_acquire); } else { @@ -592,7 +568,10 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // interface names to Handle accessed through getters/setters std::unordered_map actuator_states_; std::unordered_map actuator_commands_; - std::atomic next_trigger_ = TriggerType::READ; + std::atomic read_return_info_; + std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); + std::atomic write_return_info_; + std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); protected: pal_statistics::RegistrationsRAII stats_registrations_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 2afc7cd003..51e6f9c8a8 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -125,18 +125,25 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI async_handler_->init( [this](const rclcpp::Time & time, const rclcpp::Duration & period) { - if (next_trigger_ == TriggerType::READ) + const auto read_start_time = std::chrono::steady_clock::now(); + const auto ret_read = read(time, period); + const auto read_end_time = std::chrono::steady_clock::now(); + read_return_info_.store(ret_read, std::memory_order_release); + read_execution_time_.store( + std::chrono::duration_cast(read_end_time - read_start_time), + std::memory_order_release); + if (ret_read != return_type::OK) { - const auto ret = read(time, period); - next_trigger_.store(TriggerType::WRITE, std::memory_order_release); - return ret; - } - else - { - const auto ret = write(time, period); - next_trigger_.store(TriggerType::READ, std::memory_order_release); - return ret; + return ret_read; } + const auto write_start_time = std::chrono::steady_clock::now(); + const auto ret_write = write(time, period); + const auto write_end_time = std::chrono::steady_clock::now(); + write_return_info_.store(ret_write, std::memory_order_release); + write_execution_time_.store( + std::chrono::duration_cast(write_end_time - write_start_time), + std::memory_order_release); + return ret_write; }, info_.thread_priority); async_handler_->start_thread(); @@ -395,30 +402,20 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::ERROR; if (info_.is_async) { - if (next_trigger_.load(std::memory_order_acquire) == TriggerType::WRITE) + status.result = read_return_info_.load(std::memory_order_acquire); + const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire); + if (read_exec_time.count() > 0) { - RCLCPP_WARN( - get_logger(), - "Trigger read called while write async handler call is still pending for hardware " - "interface : '%s'. Skipping read cycle and will wait for a write cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; + status.execution_time = read_exec_time; } const auto result = async_handler_->trigger_async_callback(time, period); status.successful = result.first; - status.result = result.second; - const auto execution_time = async_handler_->get_last_execution_time(); - if (execution_time.count() > 0) - { - status.execution_time = execution_time; - } if (!status.successful) { RCLCPP_WARN( get_logger(), - "Trigger read called while write async trigger is still in progress for hardware " - "interface : '%s'. Failed to trigger read cycle!", + "Trigger read/write called while the previous async trigger is still in progress for " + "hardware interface : '%s'. Failed to trigger read/write cycle!", info_.name.c_str()); status.result = return_type::OK; return status; @@ -464,34 +461,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI status.result = return_type::ERROR; if (info_.is_async) { - if (next_trigger_.load(std::memory_order_acquire) == TriggerType::READ) - { - RCLCPP_WARN( - get_logger(), - "Trigger write called while read async handler call is still pending for hardware " - "interface : '%s'. Skipping write cycle and will wait for a read cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; - } - const auto result = async_handler_->trigger_async_callback(time, period); - status.successful = result.first; - status.result = result.second; - const auto execution_time = async_handler_->get_last_execution_time(); - if (execution_time.count() > 0) - { - status.execution_time = execution_time; - } - if (!status.successful) + status.successful = true; + const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire); + if (write_exec_time.count() > 0) { - RCLCPP_WARN( - get_logger(), - "Trigger write called while read async trigger is still in progress for hardware " - "interface : '%s'. Failed to trigger write cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; + status.execution_time = write_exec_time; } + status.result = write_return_info_.load(std::memory_order_acquire); } else { @@ -631,7 +607,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // interface names to Handle accessed through getters/setters std::unordered_map system_states_; std::unordered_map system_commands_; - std::atomic next_trigger_ = TriggerType::READ; + std::atomic read_return_info_; + std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); + std::atomic write_return_info_; + std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); protected: pal_statistics::RegistrationsRAII stats_registrations_; diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 630cb9f27c..730378b445 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 / (3 * 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..7f77666477 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 / (3 * 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 b1099732c4..5c49c76fc3 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -2118,7 +2118,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) { @@ -2132,7 +2131,6 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest ASSERT_NEAR(state_itfs[0].get_value(), prev_act_state_value, actuator_increment / 2.0); ASSERT_NEAR(state_itfs[1].get_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()); node_.get_clock()->sleep_until(time + duration); @@ -2147,7 +2145,7 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest 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)))); + 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)))); @@ -2157,7 +2155,7 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest 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)))); + 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)))); @@ -2165,28 +2163,30 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest if (check_for_updated_values) { - check_periodicity(TEST_ACTUATOR_HARDWARE_NAME, 100u); - check_periodicity(TEST_SYSTEM_HARDWARE_NAME, 100u); + const unsigned int rw_rate = 100u; + const double expec_execution_time = (1.e6 / (3 * 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, - 100); + expec_execution_time); EXPECT_LT( status_map[TEST_ACTUATOR_HARDWARE_NAME] .write_statistics->execution_time.get_statistics() .average, - 100); + expec_execution_time); EXPECT_LT( status_map[TEST_SYSTEM_HARDWARE_NAME] .read_statistics->execution_time.get_statistics() .average, - 100); + expec_execution_time); EXPECT_LT( status_map[TEST_SYSTEM_HARDWARE_NAME] .write_statistics->execution_time.get_statistics() .average, - 100); + expec_execution_time); } } From cae6bca69d87ebb997dbf194fb4c86f78a146462 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Feb 2025 18:54:35 +0100 Subject: [PATCH 36/38] Add test case for the async with different read/write rate --- .../hardware_interface/actuator_interface.hpp | 4 +- .../hardware_interface/system_interface.hpp | 4 +- hardware_interface/src/actuator.cpp | 4 +- hardware_interface/src/system.cpp | 4 +- .../test/test_resource_manager.cpp | 152 ++++++++++++++++-- .../ros2_control_test_assets/descriptions.hpp | 49 ++++++ 6 files changed, 196 insertions(+), 21 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 70966f0a55..af5a500e91 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -568,9 +568,9 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // interface names to Handle accessed through getters/setters std::unordered_map actuator_states_; std::unordered_map actuator_commands_; - std::atomic read_return_info_; + std::atomic read_return_info_ = return_type::OK; std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); - std::atomic write_return_info_; + std::atomic write_return_info_ = return_type::OK; std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); protected: diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 51e6f9c8a8..1c01a31617 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -607,9 +607,9 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // interface names to Handle accessed through getters/setters std::unordered_map system_states_; std::unordered_map system_commands_; - std::atomic read_return_info_; + std::atomic read_return_info_ = return_type::OK; std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); - std::atomic write_return_info_; + std::atomic write_return_info_ = return_type::OK; std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); protected: diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index dbaf01bf5f..237db71dc0 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -299,12 +299,12 @@ const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_c const HardwareComponentStatisticsCollector & Actuator::get_read_statistics() const { - return !impl_->get_hardware_info().is_async ? read_statistics_ : write_statistics_; + return read_statistics_; } const HardwareComponentStatisticsCollector & Actuator::get_write_statistics() const { - return !impl_->get_hardware_info().is_async ? write_statistics_ : read_statistics_; + return write_statistics_; } return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 6bc0db6464..21edc681db 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -297,12 +297,12 @@ const rclcpp::Time & System::get_last_write_time() const { return last_write_cyc const HardwareComponentStatisticsCollector & System::get_read_statistics() const { - return !impl_->get_hardware_info().is_async ? read_statistics_ : write_statistics_; + return read_statistics_; } const HardwareComponentStatisticsCollector & System::get_write_statistics() const { - return !impl_->get_hardware_info().is_async ? write_statistics_ : read_statistics_; + return write_statistics_; } return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 5c49c76fc3..7c83ce4a84 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,8 +1850,30 @@ 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_value(), prev_act_state_value); - ASSERT_EQ(state_itfs[1].get_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_value(), testing::AnyOf( + testing::DoubleEq(prev_act_state_value), + testing::DoubleEq(prev_act_state_value + 5.0))); + } + else + { + ASSERT_EQ(state_itfs[0].get_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_value(), testing::AnyOf( + testing::DoubleEq(prev_system_state_value), + testing::DoubleEq(prev_system_state_value + 10.0))); + } + else + { + ASSERT_EQ(state_itfs[1].get_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()); @@ -1854,9 +1885,10 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage { if (i > (cm_update_rate_ / rate)) { + const double expec_execution_time = (1.e6 / (3 * rate)) + 200.0; EXPECT_LT( status_map[component_name].read_statistics->execution_time.get_statistics().average, - 100); + expec_execution_time); EXPECT_LT( status_map[component_name].read_statistics->periodicity.get_statistics().average, 1.2 * rate); @@ -1869,7 +1901,7 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage EXPECT_LT( status_map[component_name].write_statistics->execution_time.get_statistics().average, - 100); + expec_execution_time); EXPECT_LT( status_map[component_name].write_statistics->periodicity.get_statistics().average, 1.2 * rate); @@ -1893,6 +1925,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; @@ -1906,7 +1939,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); } @@ -1915,7 +1957,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( @@ -1943,7 +2013,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( @@ -1971,7 +2069,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( @@ -2072,14 +2198,14 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest { 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()); } node_.get_clock()->sleep_until(time + duration); time = node_.get_clock()->now(); 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 b1ca75690b2cf22c24071878fd48dd8e53d6b544 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Feb 2025 19:32:59 +0100 Subject: [PATCH 37/38] add different sleeps for better statistics testing --- .../test/test_components/test_actuator.cpp | 2 +- .../test/test_components/test_system.cpp | 2 +- .../test/test_resource_manager.cpp | 18 ++++++++++-------- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 730378b445..01171fe0fa 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -143,7 +143,7 @@ class TestActuator : public ActuatorInterface if (get_hardware_info().is_async) { std::this_thread::sleep_for( - std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); + 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 7f77666477..a9db764306 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -132,7 +132,7 @@ class TestSystem : public SystemInterface if (get_hardware_info().is_async) { std::this_thread::sleep_for( - std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); + 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 7c83ce4a84..7350a43744 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1885,10 +1885,11 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage { if (i > (cm_update_rate_ / rate)) { - const double expec_execution_time = (1.e6 / (3 * rate)) + 200.0; + 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_execution_time); + expec_read_execution_time); EXPECT_LT( status_map[component_name].read_statistics->periodicity.get_statistics().average, 1.2 * rate); @@ -1901,7 +1902,7 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage EXPECT_LT( status_map[component_name].write_statistics->execution_time.get_statistics().average, - expec_execution_time); + expec_write_execution_time); EXPECT_LT( status_map[component_name].write_statistics->periodicity.get_statistics().average, 1.2 * rate); @@ -2290,29 +2291,30 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest if (check_for_updated_values) { const unsigned int rw_rate = 100u; - const double expec_execution_time = (1.e6 / (3 * rw_rate)) + 200.0; + 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_execution_time); + expec_read_execution_time); EXPECT_LT( status_map[TEST_ACTUATOR_HARDWARE_NAME] .write_statistics->execution_time.get_statistics() .average, - expec_execution_time); + expec_write_execution_time); EXPECT_LT( status_map[TEST_SYSTEM_HARDWARE_NAME] .read_statistics->execution_time.get_statistics() .average, - expec_execution_time); + expec_read_execution_time); EXPECT_LT( status_map[TEST_SYSTEM_HARDWARE_NAME] .write_statistics->execution_time.get_statistics() .average, - expec_execution_time); + expec_write_execution_time); } } From d5b83c1d99f27697826aee2f4b301891b3b7486f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Feb 2025 21:34:25 +0100 Subject: [PATCH 38/38] add prepare_for_activation for resetting the variables --- .../hardware_interface/actuator_interface.hpp | 12 ++++++++++++ .../include/hardware_interface/system_interface.hpp | 12 ++++++++++++ hardware_interface/src/actuator.cpp | 1 + hardware_interface/src/system.cpp | 1 + 4 files changed, 26 insertions(+) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index af5a500e91..871b575b4c 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -527,6 +527,18 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ const HardwareInfo & get_hardware_info() const { return info_; } + /// Prepare for the activation of the hardware. + /** + * This method is called before the hardware is activated by the resource manager. + */ + void prepare_for_activation() + { + read_return_info_.store(return_type::OK, std::memory_order_release); + read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); + write_return_info_.store(return_type::OK, std::memory_order_release); + write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); + } + /// Enable or disable introspection of the hardware. /** * \param[in] enable Enable introspection if true, disable otherwise. diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 1c01a31617..a448069126 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -556,6 +556,18 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ const HardwareInfo & get_hardware_info() const { return info_; } + /// Prepare for the activation of the hardware. + /** + * This method is called before the hardware is activated by the resource manager. + */ + void prepare_for_activation() + { + read_return_info_.store(return_type::OK, std::memory_order_release); + read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); + write_return_info_.store(return_type::OK, std::memory_order_release); + write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); + } + /// Enable or disable introspection of the hardware. /** * \param[in] enable Enable introspection if true, disable otherwise. diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 237db71dc0..d30602f73b 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -147,6 +147,7 @@ const rclcpp_lifecycle::State & Actuator::activate() write_statistics_.reset_statistics(); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + impl_->prepare_for_activation(); switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 21edc681db..1cb34eaef9 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -145,6 +145,7 @@ const rclcpp_lifecycle::State & System::activate() write_statistics_.reset_statistics(); if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + impl_->prepare_for_activation(); switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: