diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 6fe4f25663..1dfd499c2c 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,7 +17,10 @@ #include #include +#include +#include #include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -69,13 +72,57 @@ class Handle { } - Handle(const Handle & other) = default; + Handle(const Handle & other) noexcept + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = other.prefix_name_; + interface_name_ = other.interface_name_; + handle_name_ = other.handle_name_; + value_ = other.value_; + value_ptr_ = other.value_ptr_; + } - Handle(Handle && other) = default; + Handle(Handle && other) noexcept + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = std::move(other.prefix_name_); + interface_name_ = std::move(other.interface_name_); + handle_name_ = std::move(other.handle_name_); + value_ = std::move(other.value_); + value_ptr_ = std::move(other.value_ptr_); + } - Handle & operator=(const Handle & other) = default; + Handle & operator=(const Handle & other) + { + if (this != &other) + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = other.prefix_name_; + interface_name_ = other.interface_name_; + handle_name_ = other.handle_name_; + value_ = other.value_; + value_ptr_ = other.value_ptr_; + } + return *this; + } - Handle & operator=(Handle && other) = default; + Handle & operator=(Handle && other) + { + if (this != &other) + { + std::unique_lock lock(other.handle_mutex_); + std::unique_lock lock_this(handle_mutex_); + prefix_name_ = std::move(other.prefix_name_); + interface_name_ = std::move(other.interface_name_); + handle_name_ = std::move(other.handle_name_); + value_ = std::move(other.value_); + value_ptr_ = std::move(other.value_ptr_); + } + return *this; + } virtual ~Handle() = default; @@ -95,8 +142,14 @@ class Handle const std::string & get_prefix_name() const { return prefix_name_; } + [[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]] double get_value() const { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return std::numeric_limits::quiet_NaN(); + } // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); @@ -104,12 +157,33 @@ class Handle // END } - void set_value(double value) + [[nodiscard]] bool get_value(double & value) const + { + std::shared_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return false; + } + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value directly if old functionality is removed + THROW_ON_NULLPTR(value_ptr_); + value = *value_ptr_; + return true; + // END + } + + [[nodiscard]] bool set_value(double value) { + std::unique_lock lock(handle_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + return false; + } // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) set value_ directly if old functionality is removed THROW_ON_NULLPTR(this->value_ptr_); *this->value_ptr_ = value; + return true; // END } @@ -122,6 +196,7 @@ class Handle // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; // END + mutable std::shared_mutex handle_mutex_; }; class StateInterface : public Handle diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index aa306870a1..6013dea778 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -16,10 +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 { @@ -51,6 +54,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 (%.4f %%) timeouts and %u (~ %.4f %%) 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 * 10.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 (%.4f %%) timeouts and %u (~ %.4f %%) 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 * 10.0) / set_value_statistics_.total_counter, + set_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -70,13 +94,70 @@ class LoanedCommandInterface const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } + template + [[nodiscard]] bool set_value(T value, unsigned int max_tries = 10) + { + unsigned int nr_tries = 0; + ++set_value_statistics_.total_counter; + while (!command_interface_.set_value(value)) + { + ++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 { return command_interface_.get_value(); } + 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)) + { + ++get_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++get_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } protected: CommandInterface & command_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter = 0; + unsigned int failed_counter = 0; + unsigned int 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 96cc3e89df..3ebc8c7ca0 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -16,11 +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 @@ -56,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 (%.4f %%) timeouts and %u (%.4f %%) 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 * 10.0) / get_value_statistics_.total_counter, + get_value_statistics_.total_counter); if (deleter_) { deleter_(); @@ -75,11 +88,50 @@ class LoanedStateInterface const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + 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)) + { + ++get_value_statistics_.failed_counter; + ++nr_tries; + if (nr_tries == max_tries) + { + ++get_value_statistics_.timeout_counter; + return false; + } + std::this_thread::yield(); + } + return true; + } protected: const StateInterface & state_interface_; Deleter deleter_; + +private: + struct HandleRTStatistics + { + unsigned int total_counter = 0; + unsigned int failed_counter = 0; + unsigned int timeout_counter = 0; + }; + mutable HandleRTStatistics get_value_statistics_; }; } // namespace hardware_interface