diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 07ba1cd98e..6be7eaf3a7 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -16,7 +16,9 @@ #define HARDWARE_INTERFACE__HANDLE_HPP_ #include +#include #include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -68,13 +70,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; @@ -94,8 +140,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::unique_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_); @@ -103,12 +155,33 @@ class Handle // END } - void set_value(double value) + [[nodiscard]] bool get_value(double & value) const + { + 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(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 } @@ -121,6 +194,7 @@ class Handle // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; // END + mutable std::recursive_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 bb5807c398..d6f754a102 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "hardware_interface/handle.hpp" @@ -63,9 +64,23 @@ 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); } + void set_value(double val) + { + while (!command_interface_.set_value(val)) + { + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } - double get_value() const { return command_interface_.get_value(); } + double get_value() const + { + double value; + while (!command_interface_.get_value(value)) + { + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + return value; + } protected: CommandInterface & command_interface_; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..57015ba0a2 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "hardware_interface/handle.hpp" @@ -63,7 +64,15 @@ 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; + while (!state_interface_.get_value(value)) + { + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + return value; + } protected: StateInterface & state_interface_;