From f0847f33ab9fcfc412175732da8dde83d1d0e7bf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 14 Oct 2024 19:25:30 +0200 Subject: [PATCH] update Loaned command and state handles with statistics, templated set and get value method with some statistics on misses and timeouts --- .../loaned_command_interface.hpp | 89 +++++++++++++++++-- .../loaned_state_interface.hpp | 56 +++++++++++- 2 files changed, 137 insertions(+), 8 deletions(-) diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 0015ad7d35..58db14dc63 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -16,11 +16,13 @@ #define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_ #include +#include #include #include #include #include "hardware_interface/handle.hpp" +#include "rclcpp/logging.hpp" namespace hardware_interface { @@ -33,17 +35,23 @@ class LoanedCommandInterface CommandInterface & command_interface) : LoanedCommandInterface(command_interface, nullptr) { + get_value_statistics_.reset(); + set_value_statistics_.reset(); } [[deprecated("Replaced by the new version using shared_ptr")]] LoanedCommandInterface( CommandInterface & command_interface, Deleter && deleter) : command_interface_(command_interface), deleter_(std::forward(deleter)) { + get_value_statistics_.reset(); + set_value_statistics_.reset(); } LoanedCommandInterface(CommandInterface::SharedPtr command_interface, Deleter && deleter) : command_interface_(*command_interface), deleter_(std::forward(deleter)) { + get_value_statistics_.reset(); + set_value_statistics_.reset(); } LoanedCommandInterface(const LoanedCommandInterface & other) = delete; @@ -52,6 +60,27 @@ class LoanedCommandInterface virtual ~LoanedCommandInterface() { + auto logger = rclcpp::get_logger(command_interface_.get_name()); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%f %%) timeouts and %u (%f %%) missed calls out of %u " + "get_value calls", + get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); + RCLCPP_WARN_EXPRESSION( + rclcpp::get_logger(get_name()), + (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0), + "LoanedCommandInterface %s has %u (%f %%) timeouts and %u (%f %%) missed calls out of %u " + "set_value calls", + get_name().c_str(), set_value_statistics_.timeout_counter, + (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter, + set_value_statistics_.failed_counter, + (set_value_statistics_.failed_counter * 100.0) / set_value_statistics_.total_counter, + set_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -71,27 +100,77 @@ class LoanedCommandInterface const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } - void set_value(double val) + template + [[nodiscard]] bool set_value(T value, unsigned int max_tries = 10) { - while (!command_interface_.set_value(val)) + unsigned int nr_tries = 0; + set_value_statistics_.total_counter++; + while (!command_interface_.set_value(value)) { - std::this_thread::sleep_for(std::chrono::microseconds(10)); + set_value_statistics_.failed_counter++; + ++nr_tries; + if (nr_tries == max_tries) + { + set_value_statistics_.timeout_counter++; + return false; + } + std::this_thread::yield(); } + return true; } double get_value() const { double value; + if (get_value(value)) + { + return value; + } + else + { + return std::numeric_limits::quiet_NaN(); + } + } + + template + [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + get_value_statistics_.total_counter++; while (!command_interface_.get_value(value)) { - std::this_thread::sleep_for(std::chrono::microseconds(10)); + get_value_statistics_.failed_counter++; + ++nr_tries; + if (nr_tries == max_tries) + { + get_value_statistics_.timeout_counter++; + return false; + } + std::this_thread::yield(); } - return value; + return true; } protected: CommandInterface & command_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter; + unsigned int failed_counter; + unsigned int timeout_counter; + + void reset() + { + total_counter = 0; + failed_counter = 0; + timeout_counter = 0; + } + }; + mutable HandleRTStatistics get_value_statistics_; + HandleRTStatistics set_value_statistics_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 5f1475c4e9..a4f32305db 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -16,12 +16,13 @@ #define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_ #include +#include #include #include #include #include "hardware_interface/handle.hpp" - +#include "rclcpp/logging.hpp" namespace hardware_interface { class LoanedStateInterface @@ -57,6 +58,17 @@ class LoanedStateInterface virtual ~LoanedStateInterface() { + auto logger = rclcpp::get_logger(state_interface_.get_name()); + RCLCPP_WARN_EXPRESSION( + logger, + (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0), + "LoanedStateInterface %s has %u (%.2f %%) timeouts and %u (%.2f %%) missed calls out of %u " + "get_value calls", + state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter, + (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.failed_counter, + (get_value_statistics_.failed_counter * 100.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -79,16 +91,54 @@ class LoanedStateInterface double get_value() const { double value; + if (get_value(value)) + { + return value; + } + else + { + return std::numeric_limits::quiet_NaN(); + } + } + + template + [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const + { + unsigned int nr_tries = 0; + get_value_statistics_.total_counter++; while (!state_interface_.get_value(value)) { - std::this_thread::sleep_for(std::chrono::microseconds(10)); + get_value_statistics_.failed_counter++; + ++nr_tries; + if (nr_tries == max_tries) + { + get_value_statistics_.timeout_counter++; + return false; + } + std::this_thread::yield(); } - return value; + return true; } protected: const StateInterface & state_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter; + unsigned int failed_counter; + unsigned int timeout_counter; + + void reset() + { + total_counter = 0; + failed_counter = 0; + timeout_counter = 0; + } + }; + mutable HandleRTStatistics get_value_statistics_; }; } // namespace hardware_interface