diff --git a/controller_manager/doc/parameters_context.yaml b/controller_manager/doc/parameters_context.yaml index a015765c79..c9e95f6396 100644 --- a/controller_manager/doc/parameters_context.yaml +++ b/controller_manager/doc/parameters_context.yaml @@ -17,5 +17,11 @@ hardware_components_initial_state: | diagnostics.threshold.controllers.periodicity: | The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity. -diagnostics.threshold.controllers.execution_time: | - The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle. +diagnostics.threshold.controllers.execution_time.mean_error: | + The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period to execute its update cycle. + +diagnostics.threshold.hardware_components.periodicity: | + The ``periodicity`` diagnostics will be published only for the asynchronous hardware components, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity. + +diagnostics.threshold.hardware_components.execution_time.mean_error: | + The ``execution_time`` diagnostics will be published for all hardware components. The ``mean_error`` for a synchronous hardware component will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous hardware component will be computed against its desired read/write period, as the hardware component can take a maximum of the desired period to execute the read/write cycle. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1e27aa5e7d..c7169565b2 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) @@ -3388,12 +3388,21 @@ void ControllerManager::controller_activity_diagnostic_callback( void ControllerManager::hardware_components_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { + if (!is_resource_manager_initialized()) + { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Resource manager is not yet initialized!"); + return; + } + bool all_active = true; bool atleast_one_hw_active = false; - const auto hw_components_info = resource_manager_->get_components_status(); + const std::string read_cycle_suffix = ".read_cycle"; + const std::string write_cycle_suffix = ".write_cycle"; + const std::string state_suffix = ".state"; + const auto & hw_components_info = resource_manager_->get_components_status(); for (const auto & [component_name, component_info] : hw_components_info) { - stat.add(component_name, component_info.state.label()); if (component_info.state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { all_active = false; @@ -3403,31 +3412,165 @@ void ControllerManager::hardware_components_diagnostic_callback( atleast_one_hw_active = true; } } - if (!is_resource_manager_initialized()) + if (hw_components_info.empty()) { stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Resource manager is not yet initialized!"); + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are loaded!"); + return; } - else if (hw_components_info.empty()) + else if (!atleast_one_hw_active) { stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No hardware components are loaded!"); + diagnostic_msgs::msg::DiagnosticStatus::WARN, "No hardware components are currently active"); + return; } - else + + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::OK, + all_active ? "All hardware components are active" : "Not all hardware components are active"); + + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + + auto make_stats_string = + [](const auto & statistics_data, const std::string & measurement_unit) -> std::string { - if (!atleast_one_hw_active) + std::ostringstream oss; + oss << std::fixed << std::setprecision(2); + oss << "Avg: " << statistics_data.average << " [" << statistics_data.min << " - " + << statistics_data.max << "] " << measurement_unit + << ", StdDev: " << statistics_data.standard_deviation; + return oss.str(); + }; + + // Variable to define the overall status of the controller diagnostics + auto level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + std::vector high_exec_time_hw; + std::vector bad_periodicity_async_hw; + + for (const auto & [component_name, component_info] : hw_components_info) + { + stat.add(component_name + state_suffix, component_info.state.label()); + if (component_info.state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, - "No hardware components are currently active"); + all_active = false; } else { - stat.summary( - diagnostic_msgs::msg::DiagnosticStatus::OK, all_active - ? "All hardware components are active" - : "Not all hardware components are active"); + atleast_one_hw_active = true; } + if (component_info.state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + auto update_stats = + [&bad_periodicity_async_hw, &high_exec_time_hw, &stat, &make_stats_string]( + const std::string & comp_name, const auto & statistics, + const std::string & statistics_type_suffix, auto & diag_level, const auto & comp_info, + const auto & params) + { + if (!statistics) + { + return; + } + const bool is_async = comp_info.is_async; + const std::string periodicity_suffix = ".periodicity"; + const std::string exec_time_suffix = ".execution_time"; + const auto periodicity_stats = statistics->periodicity.get_statistics(); + const auto exec_time_stats = statistics->execution_time.get_statistics(); + stat.add( + comp_name + statistics_type_suffix + exec_time_suffix, + make_stats_string(exec_time_stats, "us")); + if (is_async) + { + stat.add( + comp_name + statistics_type_suffix + periodicity_suffix, + make_stats_string(periodicity_stats, "Hz") + + " -> Desired : " + std::to_string(comp_info.rw_rate) + " Hz"); + const double periodicity_error = + std::abs(periodicity_stats.average - static_cast(comp_info.rw_rate)); + if ( + periodicity_error > + params->diagnostics.threshold.hardware_components.periodicity.mean_error.error || + periodicity_stats.standard_deviation > params->diagnostics.threshold.hardware_components + .periodicity.standard_deviation.error) + { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + add_element_to_list(bad_periodicity_async_hw, comp_name); + } + else if ( + periodicity_error > + params->diagnostics.threshold.hardware_components.periodicity.mean_error.warn || + periodicity_stats.standard_deviation > + params->diagnostics.threshold.hardware_components.periodicity.standard_deviation.warn) + { + if (diag_level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + add_element_to_list(bad_periodicity_async_hw, comp_name); + } + } + const double max_exp_exec_time = + is_async ? 1.e6 / static_cast(comp_info.rw_rate) : 0.0; + if ( + (exec_time_stats.average - max_exp_exec_time) > + params->diagnostics.threshold.hardware_components.execution_time.mean_error.error || + exec_time_stats.standard_deviation > params->diagnostics.threshold.hardware_components + .execution_time.standard_deviation.error) + { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + high_exec_time_hw.push_back(comp_name); + } + else if ( + (exec_time_stats.average - max_exp_exec_time) > + params->diagnostics.threshold.hardware_components.execution_time.mean_error.warn || + exec_time_stats.standard_deviation > params->diagnostics.threshold.hardware_components + .execution_time.standard_deviation.warn) + { + if (diag_level != diagnostic_msgs::msg::DiagnosticStatus::ERROR) + { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } + high_exec_time_hw.push_back(comp_name); + } + }; + + // For components : {actuator, sensor and system} + update_stats( + component_name, component_info.read_statistics, read_cycle_suffix, level, component_info, + params_); + // For components : {actuator and system} + update_stats( + component_name, component_info.write_statistics, write_cycle_suffix, level, component_info, + params_); + } + } + + if (!high_exec_time_hw.empty()) + { + std::string high_exec_time_hw_string; + for (const auto & hw_comp : high_exec_time_hw) + { + high_exec_time_hw_string.append(hw_comp); + high_exec_time_hw_string.append(" "); + } + stat.mergeSummary( + level, + "\nHardware Components with high execution time : [ " + high_exec_time_hw_string + "]"); + } + if (!bad_periodicity_async_hw.empty()) + { + std::string bad_periodicity_async_hw_string; + for (const auto & hw_comp : bad_periodicity_async_hw) + { + bad_periodicity_async_hw_string.append(hw_comp); + bad_periodicity_async_hw_string.append(" "); + } + stat.mergeSummary( + level, + "\nHardware Components with bad periodicity : [ " + bad_periodicity_async_hw_string + "]"); } } diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 1bb9b152b1..049c76fc9f 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, } @@ -134,3 +134,74 @@ controller_manager: gt<>: 0.0, } } + hardware_components: + periodicity: + mean_error: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the mean error of the hardware component's read/write loop. If the mean error exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the mean error of the hardware component's read/write loop. If the mean error exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 5.0, + description: "The warning threshold for the standard deviation of the hardware component's read/write loop. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 10.0, + description: "The error threshold for the standard deviation of the hardware component's read/write loop. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + execution_time: + mean_error: + warn: { + type: double, + default_value: 1000.0, + description: "The warning threshold for the mean error of the hardware component's read/write cycle execution time in microseconds. If the mean error exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 2000.0, + description: "The error threshold for the mean error of the hardware component's read/write cycle execution time in microseconds. If the mean error exceeds this threshold, a error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + standard_deviation: + warn: { + type: double, + default_value: 100.0, + description: "The warning threshold for the standard deviation of the hardware component's read/write cycle execution time. If the standard deviation exceeds this threshold, a warning diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } + error: { + type: double, + default_value: 200.0, + description: "The error threshold for the standard deviation of the hardware component's update cycle execution time. If the standard deviation exceeds this threshold, an error diagnostic will be published.", + validation: { + gt<>: 0.0, + } + } diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index aab829089a..5ac67c1c36 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/statistics_types.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" @@ -82,6 +83,10 @@ class Actuator final const rclcpp::Time & get_last_write_time() const; + const HardwareComponentStatisticsCollector & get_read_statistics() const; + + const HardwareComponentStatisticsCollector & get_write_statistics() const; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -95,6 +100,9 @@ class Actuator final rclcpp::Time last_read_cycle_time_; // Last write cycle time rclcpp::Time last_write_cycle_time_; + // Component statistics + HardwareComponentStatisticsCollector read_statistics_; + HardwareComponentStatisticsCollector write_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1f693d1d34..871b575b4c 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_ = TriggerType::WRITE; - return ret; - } - else - { - const auto ret = write(time, period); - next_trigger_ = TriggerType::READ; - 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(); @@ -359,37 +366,41 @@ 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; - if (next_trigger_ == 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()); - return return_type::OK; + status.execution_time = read_exec_time; } - std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); - if (!trigger_status) + const auto result = async_handler_->trigger_async_callback(time, period); + status.successful = result.first; + 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()); - return return_type::OK; + status.result = return_type::OK; + return status; } } else { - result = read(time, period); + 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); } - return result; + return status; } /// Read the current state values from the actuator. @@ -414,37 +425,30 @@ 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; - if (next_trigger_ == 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()); - return return_type::OK; - } - std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); - if (!trigger_status) + 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()); - return return_type::OK; + status.execution_time = write_exec_time; } + status.result = write_return_info_.load(std::memory_order_acquire); } else { - result = write(time, period); + 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); } - return result; + return status; } /// Write the current command values to the actuator. @@ -523,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. @@ -564,7 +580,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_ = return_type::OK; + std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); + std::atomic write_return_info_ = return_type::OK; + std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); protected: pal_statistics::RegistrationsRAII stats_registrations_; diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 70a0482811..a7861ac93a 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -19,14 +19,21 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_ +#include #include #include -#include +#include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "hardware_interface/types/statistics_types.hpp" namespace hardware_interface { +struct HardwareComponentStatisticsData +{ + ros2_control::MovingAverageStatisticsData execution_time; + ros2_control::MovingAverageStatisticsData periodicity; +}; /// Hardware Component Information /** * This struct contains information about a given hardware component. @@ -59,6 +66,12 @@ struct HardwareComponentInfo /// List of provided command interfaces by the component. std::vector command_interfaces; + + /// Read cycle statistics of the component. + std::shared_ptr read_statistics = nullptr; + + /// Write cycle statistics of the component. + std::shared_ptr write_statistics = nullptr; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/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/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index e47e34c0d7..4cba2e761f 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/statistics_types.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" @@ -70,6 +71,8 @@ class Sensor final const rclcpp::Time & get_last_read_time() const; + const HardwareComponentStatisticsCollector & get_read_statistics() const; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } @@ -81,6 +84,8 @@ class Sensor final mutable std::recursive_mutex sensors_mutex_; // Last read cycle time rclcpp::Time last_read_cycle_time_; + // Component statistics + HardwareComponentStatisticsCollector read_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 58a8b4790a..f117e6a7d5 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -236,28 +236,41 @@ 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); - if (!trigger_status) + 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 (!status.successful) { RCLCPP_WARN( get_logger(), "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); + 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); } - return result; + return status; } /// Read the current state values from the actuator. diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 750cbb301a..134ce40d9d 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -23,6 +23,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/statistics_types.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" @@ -82,6 +83,10 @@ class System final const rclcpp::Time & get_last_write_time() const; + const HardwareComponentStatisticsCollector & get_read_statistics() const; + + const HardwareComponentStatisticsCollector & get_write_statistics() const; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -95,6 +100,9 @@ class System final rclcpp::Time last_read_cycle_time_; // Last write cycle time rclcpp::Time last_write_cycle_time_; + // Component statistics + HardwareComponentStatisticsCollector read_statistics_; + HardwareComponentStatisticsCollector write_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 7577d0ebdc..a448069126 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_ = TriggerType::WRITE; - return ret; - } - else - { - const auto ret = write(time, period); - next_trigger_ = TriggerType::READ; - 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(); @@ -388,37 +395,41 @@ 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; - if (next_trigger_ == 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()); - return return_type::OK; + status.execution_time = read_exec_time; } - std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); - if (!trigger_status) + const auto result = async_handler_->trigger_async_callback(time, period); + status.successful = result.first; + 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()); - return return_type::OK; + status.result = return_type::OK; + return status; } } else { - result = read(time, period); + 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); } - return result; + return status; } /// Read the current state values from the actuator. @@ -443,37 +454,30 @@ 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; - if (next_trigger_ == 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()); - return return_type::OK; - } - std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); - if (!trigger_status) + 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()); - return return_type::OK; + status.execution_time = write_exec_time; } + status.result = write_return_info_.load(std::memory_order_acquire); } else { - result = write(time, period); + 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); } - return result; + return status; } /// Write the current command values to the actuator. @@ -552,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. @@ -603,7 +619,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_ = return_type::OK; + std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); + std::atomic write_return_info_ = return_type::OK; + std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); protected: pal_statistics::RegistrationsRAII stats_registrations_; 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..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 @@ -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,22 @@ 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. + */ +struct HardwareComponentCycleStatus +{ + bool successful = true; + return_type result = return_type::OK; + std::optional execution_time = std::nullopt; +}; + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_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..ba22a47c99 --- /dev/null +++ b/hardware_interface/include/hardware_interface/types/statistics_types.hpp @@ -0,0 +1,141 @@ +// Copyright 2025 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Sai Kishor Kothakota + +#ifndef HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ +#define HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ + +#include +#include + +#include "libstatistics_collector/moving_average_statistics/moving_average.hpp" +#include "libstatistics_collector/moving_average_statistics/types.hpp" +#include "realtime_tools/mutex.hpp" + +namespace ros2_control +{ +/** + * @brief Data structure to store the statistics of a moving average. The data is protected by a + * mutex and the data can be updated and retrieved. + */ +class MovingAverageStatisticsData +{ + using MovingAverageStatisticsCollector = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; + using StatisticData = libstatistics_collector::moving_average_statistics::StatisticData; + +public: + MovingAverageStatisticsData() + { + reset(); + reset_statistics_sample_count_ = std::numeric_limits::max(); + } + + /** + * @brief Update the statistics data with the new statistics data. + * @param statistics statistics collector to update the current statistics data. + */ + void update_statistics(const std::shared_ptr & statistics) + { + std::unique_lock lock(mutex_); + if (statistics->GetCount() > 0) + { + statistics_data.average = statistics->Average(); + statistics_data.min = statistics->Min(); + statistics_data.max = statistics->Max(); + statistics_data.standard_deviation = statistics->StandardDeviation(); + statistics_data.sample_count = statistics->GetCount(); + statistics_data = statistics->GetStatistics(); + } + if (statistics->GetCount() >= reset_statistics_sample_count_) + { + statistics->Reset(); + } + } + + /** + * @brief Set the number of samples to reset the statistics. + * @param reset_sample_count number of samples to reset the statistics. + */ + void set_reset_statistics_sample_count(unsigned int reset_sample_count) + { + std::unique_lock lock(mutex_); + reset_statistics_sample_count_ = reset_sample_count; + } + + void reset() + { + statistics_data.average = std::numeric_limits::quiet_NaN(); + statistics_data.min = std::numeric_limits::quiet_NaN(); + statistics_data.max = std::numeric_limits::quiet_NaN(); + statistics_data.standard_deviation = std::numeric_limits::quiet_NaN(); + statistics_data.sample_count = 0; + } + + /** + * @brief Get the statistics data. + * @return statistics data. + */ + const StatisticData & get_statistics() const + { + std::unique_lock lock(mutex_); + return statistics_data; + } + +private: + /// Statistics data + StatisticData statistics_data; + /// Number of samples to reset the statistics + unsigned int reset_statistics_sample_count_ = std::numeric_limits::max(); + /// Mutex to protect the statistics data + mutable realtime_tools::prio_inherit_mutex mutex_; +}; +} // namespace ros2_control + +namespace hardware_interface +{ +/** + * @brief Data structure with two moving average statistics collectors. One for the execution time + * and the other for the periodicity. + */ +struct HardwareComponentStatisticsCollector +{ + HardwareComponentStatisticsCollector() + { + execution_time = std::make_shared(); + periodicity = std::make_shared(); + } + + using MovingAverageStatisticsCollector = + libstatistics_collector::moving_average_statistics::MovingAverageStatistics; + + /** + * @brief Resets the internal statistics of the execution time and periodicity statistics + * collectors. + */ + void reset_statistics() + { + execution_time->Reset(); + periodicity->Reset(); + } + + /// Execution time statistics collector + std::shared_ptr execution_time = nullptr; + /// Periodicity statistics collector + std::shared_ptr periodicity = nullptr; +}; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TYPES__STATISTICS_TYPES_HPP_ diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 65b337aa62..d30602f73b 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,8 +141,13 @@ 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) { + impl_->prepare_for_activation(); switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: @@ -295,6 +298,16 @@ const rclcpp::Time & Actuator::get_last_read_time() const { return last_read_cyc const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_cycle_time_; } +const HardwareComponentStatisticsCollector & Actuator::get_read_statistics() const +{ + return read_statistics_; +} + +const HardwareComponentStatisticsCollector & Actuator::get_write_statistics() const +{ + return write_statistics_; +} + return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) @@ -302,19 +315,32 @@ 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); - 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; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + read_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_read_cycle_time_).seconds()); + } + last_read_cycle_time_ = time; + } + return trigger_result.result; } - return result; + return return_type::OK; } return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) @@ -324,19 +350,32 @@ 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); - 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; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + write_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + write_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_write_cycle_time_).seconds()); + } + last_write_cycle_time_ = time; + } + return trigger_result.result; } - return result; + return return_type::OK; } std::recursive_mutex & Actuator::get_mutex() { return actuators_mutex_; } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index e43c650d5d..b4c215a9c0 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)); @@ -1461,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) { @@ -1768,24 +1776,36 @@ HardwareReadWriteStatus ResourceManager::read( auto ret_val = return_type::OK; try { + 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 = current_time - component.get_last_read_time(); 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); + ret_val = component.read(current_time, period); } else { - const double read_rate = - resource_storage_->hardware_info_map_[component.get_name()].rw_rate; - const auto current_time = resource_storage_->get_clock()->now(); - const rclcpp::Duration actual_period = current_time - component.get_last_read_time(); + 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); } } + 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); @@ -1854,24 +1874,35 @@ HardwareReadWriteStatus ResourceManager::write( auto ret_val = return_type::OK; try { + auto & hardware_component_info = + resource_storage_->hardware_info_map_[component.get_name()]; + const auto current_time = resource_storage_->get_clock()->now(); 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); + ret_val = component.write(current_time, period); } else { - const double write_rate = - resource_storage_->hardware_info_map_[component.get_name()].rw_rate; - const auto current_time = resource_storage_->get_clock()->now(); - const rclcpp::Duration actual_period = current_time - component.get_last_write_time(); + 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); } } + if (hardware_component_info.write_statistics) + { + const auto & write_statistics_collector = component.get_write_statistics(); + hardware_component_info.write_statistics->execution_time.update_statistics( + write_statistics_collector.execution_time); + hardware_component_info.write_statistics->periodicity.update_statistics( + write_statistics_collector.periodicity); + } const auto component_group = component.get_group_name(); ret_val = resource_storage_->update_hardware_component_group_state(component_group, ret_val); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 87ea42790e..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,8 @@ 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) { switch (impl_->on_activate(impl_->get_lifecycle_state())) @@ -249,6 +250,11 @@ const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle_time_; } +const HardwareComponentStatisticsCollector & Sensor::get_read_statistics() const +{ + return read_statistics_; +} + return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) @@ -256,19 +262,32 @@ 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); - 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; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + read_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_read_cycle_time_).seconds()); + } + last_read_cycle_time_ = time; + } + return trigger_result.result; } - return 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 8f626f7c8d..1cb34eaef9 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,8 +139,13 @@ 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) { + impl_->prepare_for_activation(); switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: @@ -293,6 +296,16 @@ const rclcpp::Time & System::get_last_read_time() const { return last_read_cycle const rclcpp::Time & System::get_last_write_time() const { return last_write_cycle_time_; } +const HardwareComponentStatisticsCollector & System::get_read_statistics() const +{ + return read_statistics_; +} + +const HardwareComponentStatisticsCollector & System::get_write_statistics() const +{ + return write_statistics_; +} + return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) @@ -300,19 +313,32 @@ 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); - 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; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + read_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_read_cycle_time_).seconds()); + } + last_read_cycle_time_ = time; + } + return trigger_result.result; } - return result; + return return_type::OK; } return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) @@ -322,19 +348,32 @@ 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); - 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; + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + write_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + write_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_write_cycle_time_).seconds()); + } + last_write_cycle_time_ = time; + } + return trigger_result.result; } - return result; + return return_type::OK; } std::recursive_mutex & System::get_mutex() { return system_mutex_; } diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 630cb9f27c..01171fe0fa 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -111,6 +111,11 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (get_hardware_info().is_async) + { + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); + } // simulate error on read if (velocity_command_ == test_constants::READ_FAIL_VALUE) { @@ -135,6 +140,11 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (get_hardware_info().is_async) + { + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate))); + } // simulate error on write if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) { diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 395935e314..a9db764306 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -100,6 +100,11 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (get_hardware_info().is_async) + { + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate))); + } // simulate error on read if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) { @@ -124,6 +129,11 @@ class TestSystem : public SystemInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + if (get_hardware_info().is_async) + { + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (6 * get_hardware_info().rw_rate))); + } // simulate error on write if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) { diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 75ce7fe5f7..803a51da9e 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1739,10 +1739,16 @@ TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManagerTest { public: - void setup_resource_manager_and_do_initial_checks() + void setup_resource_manager_and_do_initial_checks(bool async_components) { - rm = std::make_shared( - node_, ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate, false); + const auto minimal_robot_urdf_with_different_hw_rw_rate_with_async = + std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_with_different_rw_rates_with_async) + + std::string(ros2_control_test_assets::urdf_tail); + const std::string urdf = + async_components ? minimal_robot_urdf_with_different_hw_rw_rate_with_async + : ros2_control_test_assets::minimal_robot_urdf_with_different_hw_rw_rate; + rm = std::make_shared(node_, urdf, false); activate_components(*rm); cm_update_rate_ = 100u; // The default value inside @@ -1767,6 +1773,9 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage actuator_rw_rate_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].rw_rate; system_rw_rate_ = status_map[TEST_SYSTEM_HARDWARE_NAME].rw_rate; + actuator_is_async_ = status_map[TEST_ACTUATOR_HARDWARE_NAME].is_async; + system_is_async_ = status_map[TEST_SYSTEM_HARDWARE_NAME].is_async; + claimed_itfs.push_back( rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); @@ -1841,11 +1850,75 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage } // Even though we skip some read and write iterations, the state interfaces should be the same // as previous updated one until the next cycle - ASSERT_EQ(state_itfs[0].get_optional().value(), prev_act_state_value); - ASSERT_EQ(state_itfs[1].get_optional().value(), prev_system_state_value); + if (actuator_is_async_) + { + // check it is either the previous value or the new one + EXPECT_THAT( + state_itfs[0].get_optional().value(), testing::AnyOf( + testing::DoubleEq(prev_act_state_value), + testing::DoubleEq(prev_act_state_value + 5.0))); + } + else + { + ASSERT_EQ(state_itfs[0].get_optional().value(), prev_act_state_value); + } + if (system_is_async_) + { + // check it is either the previous value or the new one + EXPECT_THAT( + state_itfs[1].get_optional().value(), + testing::AnyOf( + testing::DoubleEq(prev_system_state_value), + testing::DoubleEq(prev_system_state_value + 10.0))); + } + else + { + ASSERT_EQ(state_itfs[1].get_optional().value(), prev_system_state_value); + } auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); EXPECT_TRUE(ok_write); EXPECT_TRUE(failed_hardware_names_write.empty()); + + if (test_for_changing_values) + { + auto status_map = rm->get_components_status(); + auto check_periodicity = [&](const std::string & component_name, unsigned int rate) + { + if (i > (cm_update_rate_ / rate)) + { + const double expec_read_execution_time = (1.e6 / (3 * rate)) + 200.0; + const double expec_write_execution_time = (1.e6 / (6 * rate)) + 200.0; + EXPECT_LT( + status_map[component_name].read_statistics->execution_time.get_statistics().average, + expec_read_execution_time); + EXPECT_LT( + status_map[component_name].read_statistics->periodicity.get_statistics().average, + 1.2 * rate); + EXPECT_THAT( + status_map[component_name].read_statistics->periodicity.get_statistics().min, + testing::AllOf(testing::Ge(0.5 * rate), testing::Lt((1.2 * rate)))); + EXPECT_THAT( + status_map[component_name].read_statistics->periodicity.get_statistics().max, + testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate)))); + + EXPECT_LT( + status_map[component_name].write_statistics->execution_time.get_statistics().average, + expec_write_execution_time); + EXPECT_LT( + status_map[component_name].write_statistics->periodicity.get_statistics().average, + 1.2 * rate); + EXPECT_THAT( + status_map[component_name].write_statistics->periodicity.get_statistics().min, + testing::AllOf(testing::Ge(0.5 * rate), testing::Lt((1.2 * rate)))); + EXPECT_THAT( + status_map[component_name].write_statistics->periodicity.get_statistics().max, + testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate)))); + } + }; + + check_periodicity(TEST_ACTUATOR_HARDWARE_NAME, actuator_rw_rate_); + check_periodicity(TEST_SYSTEM_HARDWARE_NAME, system_rw_rate_); + } node_.get_clock()->sleep_until(time + duration); time = node_.get_clock()->now(); } @@ -1854,6 +1927,7 @@ class ResourceManagerTestReadWriteDifferentReadWriteRate : public ResourceManage public: std::shared_ptr rm; unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + bool actuator_is_async_, system_is_async_; std::vector claimed_itfs; std::vector state_itfs; @@ -1867,7 +1941,16 @@ TEST_F( ResourceManagerTestReadWriteDifferentReadWriteRate, test_components_with_different_read_write_freq_on_activate) { - setup_resource_manager_and_do_initial_checks(); + setup_resource_manager_and_do_initial_checks(false); + + check_read_and_write_cycles(true); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_activate_with_async) +{ + setup_resource_manager_and_do_initial_checks(true); check_read_and_write_cycles(true); } @@ -1876,7 +1959,35 @@ TEST_F( ResourceManagerTestReadWriteDifferentReadWriteRate, test_components_with_different_read_write_freq_on_deactivate) { - setup_resource_manager_and_do_initial_checks(); + setup_resource_manager_and_do_initial_checks(false); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_inactive( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + check_read_and_write_cycles(true); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_deactivate_with_async) +{ + setup_resource_manager_and_do_initial_checks(true); // Now deactivate all the components and test the same as above rclcpp_lifecycle::State state_inactive( @@ -1904,7 +2015,35 @@ TEST_F( ResourceManagerTestReadWriteDifferentReadWriteRate, test_components_with_different_read_write_freq_on_unconfigured) { - setup_resource_manager_and_do_initial_checks(); + setup_resource_manager_and_do_initial_checks(false); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_unconfigured( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + hardware_interface::lifecycle_state_names::UNCONFIGURED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + check_read_and_write_cycles(false); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_unconfigured_with_async) +{ + setup_resource_manager_and_do_initial_checks(true); // Now deactivate all the components and test the same as above rclcpp_lifecycle::State state_unconfigured( @@ -1932,7 +2071,35 @@ TEST_F( ResourceManagerTestReadWriteDifferentReadWriteRate, test_components_with_different_read_write_freq_on_finalized) { - setup_resource_manager_and_do_initial_checks(); + setup_resource_manager_and_do_initial_checks(false); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_finalized( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + hardware_interface::lifecycle_state_names::FINALIZED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + check_read_and_write_cycles(false); +} + +TEST_F( + ResourceManagerTestReadWriteDifferentReadWriteRate, + test_components_with_different_read_write_freq_on_finalized_with_async) +{ + setup_resource_manager_and_do_initial_checks(true); // Now deactivate all the components and test the same as above rclcpp_lifecycle::State state_finalized( @@ -1968,6 +2135,7 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest rm = std::make_shared(node_, minimal_robot_urdf_async, false); activate_components(*rm); + time = node_.get_clock()->now(); auto status_map = rm->get_components_status(); EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), @@ -1991,21 +2159,58 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1])); state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1])); + auto check_statistics_for_nan = [&](const std::string & component_name) + { + EXPECT_TRUE(std::isnan( + status_map[component_name].read_statistics->periodicity.get_statistics().average)); + EXPECT_TRUE(std::isnan( + status_map[component_name].write_statistics->periodicity.get_statistics().average)); + + EXPECT_TRUE( + std::isnan(status_map[component_name].read_statistics->periodicity.get_statistics().min)); + EXPECT_TRUE( + std::isnan(status_map[component_name].write_statistics->periodicity.get_statistics().min)); + + EXPECT_TRUE( + std::isnan(status_map[component_name].read_statistics->periodicity.get_statistics().max)); + EXPECT_TRUE( + std::isnan(status_map[component_name].write_statistics->periodicity.get_statistics().max)); + + EXPECT_TRUE(std::isnan( + status_map[component_name].read_statistics->execution_time.get_statistics().average)); + EXPECT_TRUE(std::isnan( + status_map[component_name].write_statistics->execution_time.get_statistics().average)); + + EXPECT_TRUE(std::isnan( + status_map[component_name].read_statistics->execution_time.get_statistics().min)); + EXPECT_TRUE(std::isnan( + status_map[component_name].write_statistics->execution_time.get_statistics().min)); + + EXPECT_TRUE(std::isnan( + status_map[component_name].read_statistics->execution_time.get_statistics().max)); + EXPECT_TRUE(std::isnan( + status_map[component_name].write_statistics->execution_time.get_statistics().max)); + }; + + check_statistics_for_nan(TEST_ACTUATOR_HARDWARE_NAME); + check_statistics_for_nan(TEST_SYSTEM_HARDWARE_NAME); + check_if_interface_available(true, true); // with default values read and write should run without any problems { auto [ok, failed_hardware_names] = rm->read(time, duration); EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); + EXPECT_THAT(failed_hardware_names, testing::IsEmpty()); } { // claimed_itfs[0].set_value(10.0); // claimed_itfs[1].set_value(20.0); auto [ok, failed_hardware_names] = rm->write(time, duration); EXPECT_TRUE(ok); - EXPECT_TRUE(failed_hardware_names.empty()); + EXPECT_THAT(failed_hardware_names, testing::IsEmpty()); } - time = time + duration; + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); check_if_interface_available(true, true); } @@ -2041,7 +2246,6 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest auto [ok, failed_hardware_names] = rm->read(time, duration); EXPECT_TRUE(ok); EXPECT_TRUE(failed_hardware_names.empty()); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); // The values are computations exactly within the test_components if (check_for_updated_values) { @@ -2057,10 +2261,63 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest ASSERT_NEAR( state_itfs[1].get_optional().value(), prev_system_state_value, system_increment / 2.0); auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); - std::this_thread::sleep_for(std::chrono::milliseconds(1)); EXPECT_TRUE(ok_write); EXPECT_TRUE(failed_hardware_names_write.empty()); - time = time + duration; + node_.get_clock()->sleep_until(time + duration); + time = node_.get_clock()->now(); + } + + auto status_map = rm->get_components_status(); + auto check_periodicity = [&](const std::string & component_name, unsigned int rate) + { + EXPECT_LT( + status_map[component_name].read_statistics->periodicity.get_statistics().average, + 1.2 * rate); + EXPECT_THAT( + status_map[component_name].read_statistics->periodicity.get_statistics().min, + testing::AllOf(testing::Ge(0.4 * rate), testing::Lt((1.2 * rate)))); + EXPECT_THAT( + status_map[component_name].read_statistics->periodicity.get_statistics().max, + testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate)))); + + EXPECT_LT( + status_map[component_name].write_statistics->periodicity.get_statistics().average, + 1.2 * rate); + EXPECT_THAT( + status_map[component_name].write_statistics->periodicity.get_statistics().min, + testing::AllOf(testing::Ge(0.4 * rate), testing::Lt((1.2 * rate)))); + EXPECT_THAT( + status_map[component_name].write_statistics->periodicity.get_statistics().max, + testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate)))); + }; + + if (check_for_updated_values) + { + const unsigned int rw_rate = 100u; + const double expec_read_execution_time = (1.e6 / (3 * rw_rate)) + 200.0; + const double expec_write_execution_time = (1.e6 / (6 * rw_rate)) + 200.0; + check_periodicity(TEST_ACTUATOR_HARDWARE_NAME, rw_rate); + check_periodicity(TEST_SYSTEM_HARDWARE_NAME, rw_rate); + EXPECT_LT( + status_map[TEST_ACTUATOR_HARDWARE_NAME] + .read_statistics->execution_time.get_statistics() + .average, + expec_read_execution_time); + EXPECT_LT( + status_map[TEST_ACTUATOR_HARDWARE_NAME] + .write_statistics->execution_time.get_statistics() + .average, + expec_write_execution_time); + EXPECT_LT( + status_map[TEST_SYSTEM_HARDWARE_NAME] + .read_statistics->execution_time.get_statistics() + .average, + expec_read_execution_time); + EXPECT_LT( + status_map[TEST_SYSTEM_HARDWARE_NAME] + .write_statistics->execution_time.get_statistics() + .average, + expec_write_execution_time); } } diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 0be4ed369b..5c0f4dcbb7 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -758,6 +758,55 @@ const auto hardware_resources_with_different_rw_rates = )"; +const auto hardware_resources_with_different_rw_rates_with_async = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + const auto hardware_resources_with_negative_rw_rates = R"(