From 92ebd3625c245ccb931bf4f9d7c0c78c909eabf7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 00:00:34 +0100 Subject: [PATCH 01/40] 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 d9ec925d44b96fbadbbdbc8723544308885b5372 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 00:00:59 +0100 Subject: [PATCH 02/40] 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 5f3727a7b522637251e2e41687fd30cf00e30942 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 00:02:14 +0100 Subject: [PATCH 03/40] 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 8e9c19aae2f95f5fca90b621a446e9ea2a819914 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 00:02:53 +0100 Subject: [PATCH 04/40] 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 13128afd49df4920ee197796aca920589fbd20aa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 12:29:07 +0100 Subject: [PATCH 05/40] 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 b496f0b679432ffca8808233dc0a4f42216e3634 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 12:30:28 +0100 Subject: [PATCH 06/40] 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 39055f097b4722d68d29ad4e6dd747771936b257 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 12:31:31 +0100 Subject: [PATCH 07/40] 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 c3553ffc3d9c8a7ef4eaa81a5a42b125fb30ab25 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 14:03:05 +0100 Subject: [PATCH 08/40] 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 64bab89cc12a7808b9a0c180efe16ee40513882a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 20:02:44 +0100 Subject: [PATCH 09/40] 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 04cefadf86d7f5a223aaeb08546c0ff6909c44b7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 22:33:07 +0100 Subject: [PATCH 10/40] 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 ae844faa4ff790b3cd77596136c46139abea1871 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 22:35:42 +0100 Subject: [PATCH 11/40] 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 09c76dbb95da5920c7f47368ae9ec805c6669032 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:06:34 +0100 Subject: [PATCH 12/40] 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 b900c7f715c467851ead7cfe9dbd4bf942d94ae0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:07:04 +0100 Subject: [PATCH 13/40] 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 5d1b15132184a97f50f65746b069e34c2218219e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:09:34 +0100 Subject: [PATCH 14/40] 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 df9e4e205476a15f9bb40db2d8c7f152c498d10e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:17:29 +0100 Subject: [PATCH 15/40] 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 1e27aa5e7d..1ac073068b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3235,7 +3235,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) @@ -3390,7 +3390,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 b7a373899e87c101cff3be44b3a2f9bb1e8620ad Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 15 Jan 2025 23:22:55 +0100 Subject: [PATCH 16/40] 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 34eb35e8f070db8fb81e1fcdd83af49123b6e4f3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 16:43:57 +0100 Subject: [PATCH 17/40] 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 1ac073068b..70059ad176 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3388,8 +3388,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) { @@ -3403,30 +3415,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 155d68911c6ec035f6b379c9d676e2ddbb37f0ed Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 22:07:17 +0100 Subject: [PATCH 18/40] 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 70059ad176..5a468b7842 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3399,13 +3399,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; @@ -3457,7 +3454,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; @@ -3468,64 +3464,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 b4f31bcc117b661e8772ba378bc62e2498d95514 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 22:12:42 +0100 Subject: [PATCH 19/40] 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 7abb369454c94131eaa1bd50ae1ddb4050582f4f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 23:36:06 +0100 Subject: [PATCH 20/40] 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 75ce7fe5f7..3cf18be204 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 e742ba734384bee746cd9da182134eda6cfedcf2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 16 Jan 2025 23:51:36 +0100 Subject: [PATCH 21/40] 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 c7493dec43355ed58e6c894c4fe651205cdd4d64 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 17 Jan 2025 00:28:42 +0100 Subject: [PATCH 22/40] 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 71122711cff35535a1d7fd4a3cc41442237638ab Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 17 Jan 2025 00:40:42 +0100 Subject: [PATCH 23/40] 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 5a468b7842..0d0c87d59a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3533,12 +3533,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 7456d029f44aa2d13c819bfa941a0e5c03c0c4de Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 18 Jan 2025 11:37:55 +0100 Subject: [PATCH 24/40] 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 b02e3ec1b4e3edb11589885776df7e8c64796646 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 18 Jan 2025 11:38:32 +0100 Subject: [PATCH 25/40] 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 4c61d9ab908064fff3243fd1f0a3d4d5d2262373 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 18 Jan 2025 13:16:31 +0100 Subject: [PATCH 26/40] 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 0d0c87d59a..c7169565b2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -3470,6 +3470,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"; @@ -3538,12 +3542,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 c19fe6af38e2fe71e5d98a895fd35341ad06cadc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 22:15:09 +0100 Subject: [PATCH 27/40] 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 8e04a8f62cc10235d3dc947d6c67f3de7161d0e3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 22:16:16 +0100 Subject: [PATCH 28/40] 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 3cf18be204..9d702dd84a 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); } @@ -2099,7 +2137,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 cf1deb26163e9b116a39d3d83451967e0b0e350b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 22:41:30 +0100 Subject: [PATCH 29/40] 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 34866b47bda593ea2a5d0952778a36d53cdd9499 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 22:50:02 +0100 Subject: [PATCH 30/40] 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 1ba762205681b3246996261c384d08eb767b51b9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 19 Jan 2025 23:20:45 +0100 Subject: [PATCH 31/40] 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 b27369858ce42048442f11a9d9b48881c915028d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Jan 2025 10:17:41 +0100 Subject: [PATCH 32/40] 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 2bfda2c2afc6ca856a77c7ef994b3b3a53ab3c4f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Jan 2025 11:36:28 +0100 Subject: [PATCH 33/40] 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 f49689ed62d366002101bf104bec6c7133e94872 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 28 Jan 2025 15:58:29 +0100 Subject: [PATCH 34/40] 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 c6176b569e241f2f85c8114dccab80a7f95cce56 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 31 Jan 2025 14:07:21 +0100 Subject: [PATCH 35/40] 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 9d702dd84a..6bd285f251 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) { @@ -2134,7 +2133,6 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest ASSERT_NEAR( state_itfs[1].get_optional().value(), prev_system_state_value, system_increment / 2.0); auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); EXPECT_TRUE(ok_write); EXPECT_TRUE(failed_hardware_names_write.empty()); node_.get_clock()->sleep_until(time + duration); @@ -2149,7 +2147,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)))); @@ -2159,7 +2157,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)))); @@ -2167,28 +2165,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 741ba8fd6a6fd969245ed573a1c59e6bf93ed3b8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Feb 2025 18:54:35 +0100 Subject: [PATCH 36/40] 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 6bd285f251..fed884884f 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_optional().value(), prev_act_state_value); - ASSERT_EQ(state_itfs[1].get_optional().value(), prev_system_state_value); + if (actuator_is_async_) + { + // check it is either the previous value or the new one + EXPECT_THAT( + state_itfs[0].get_optional(), testing::AnyOf( + testing::DoubleEq(prev_act_state_value), + testing::DoubleEq(prev_act_state_value + 5.0))); + } + else + { + ASSERT_EQ(state_itfs[0].get_optional(), prev_act_state_value); + } + if (system_is_async_) + { + // check it is either the previous value or the new one + EXPECT_THAT( + state_itfs[1].get_optional(), testing::AnyOf( + testing::DoubleEq(prev_system_state_value), + testing::DoubleEq(prev_system_state_value + 10.0))); + } + else + { + ASSERT_EQ(state_itfs[1].get_optional(), 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 b3c05b5bb276e7cc2f7446afa9e0d8751ef278ab Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Feb 2025 19:32:59 +0100 Subject: [PATCH 37/40] 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 fed884884f..9f8f732328 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); @@ -2292,29 +2293,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 b9f2a532a52a5f84969e05a3caf320e85eef173f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 2 Feb 2025 21:34:25 +0100 Subject: [PATCH 38/40] 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: From 9a91376a4e70b23ad151b7e109a3f07277cec8e5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 27 Feb 2025 09:31:10 +0100 Subject: [PATCH 39/40] apply pre-commit changes --- hardware_interface_testing/test/test_resource_manager.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 9f8f732328..dad9f159db 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1855,8 +1855,8 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage // check it is either the previous value or the new one EXPECT_THAT( state_itfs[0].get_optional(), testing::AnyOf( - testing::DoubleEq(prev_act_state_value), - testing::DoubleEq(prev_act_state_value + 5.0))); + testing::DoubleEq(prev_act_state_value), + testing::DoubleEq(prev_act_state_value + 5.0))); } else { @@ -1867,8 +1867,8 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage // check it is either the previous value or the new one EXPECT_THAT( state_itfs[1].get_optional(), testing::AnyOf( - testing::DoubleEq(prev_system_state_value), - testing::DoubleEq(prev_system_state_value + 10.0))); + testing::DoubleEq(prev_system_state_value), + testing::DoubleEq(prev_system_state_value + 10.0))); } else { From f698c5e5a2750366b04dc57ae5cc208b547e53a5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 27 Feb 2025 10:21:34 +0100 Subject: [PATCH 40/40] add missing .value() --- .../test/test_resource_manager.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index dad9f159db..803a51da9e 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1854,25 +1854,26 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage { // check it is either the previous value or the new one EXPECT_THAT( - state_itfs[0].get_optional(), testing::AnyOf( - testing::DoubleEq(prev_act_state_value), - testing::DoubleEq(prev_act_state_value + 5.0))); + state_itfs[0].get_optional().value(), testing::AnyOf( + testing::DoubleEq(prev_act_state_value), + testing::DoubleEq(prev_act_state_value + 5.0))); } else { - ASSERT_EQ(state_itfs[0].get_optional(), prev_act_state_value); + ASSERT_EQ(state_itfs[0].get_optional().value(), prev_act_state_value); } if (system_is_async_) { // check it is either the previous value or the new one EXPECT_THAT( - state_itfs[1].get_optional(), testing::AnyOf( - testing::DoubleEq(prev_system_state_value), - testing::DoubleEq(prev_system_state_value + 10.0))); + state_itfs[1].get_optional().value(), + testing::AnyOf( + testing::DoubleEq(prev_system_state_value), + testing::DoubleEq(prev_system_state_value + 10.0))); } else { - ASSERT_EQ(state_itfs[1].get_optional(), prev_system_state_value); + ASSERT_EQ(state_itfs[1].get_optional().value(), prev_system_state_value); } auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); EXPECT_TRUE(ok_write);