From ed305de86ece48b85abc3d054ebbe08bf5d9dbab Mon Sep 17 00:00:00 2001 From: VX792 Date: Sat, 11 Nov 2023 23:08:22 +0100 Subject: [PATCH] Rework handles to support atomic doubles --- .../include/hardware_interface/handle.hpp | 100 ++++++++++++++---- .../loaned_command_interface.hpp | 28 +++-- .../loaned_state_interface.hpp | 27 +++-- hardware_interface/test/test_handle.cpp | 37 +++++++ .../include/transmission_interface/handle.hpp | 8 +- 5 files changed, 158 insertions(+), 42 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..b493ba5814 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" @@ -24,28 +26,31 @@ namespace hardware_interface { /// A handle used to get and set a value on a given interface. + + +template class ReadOnlyHandle { + static_assert(std::is_floating_point::value || std::is_same>::value, "Invalid template argument for class ReadOnlyHandle. Only floating point, and atomic double types are supported for now."); public: ReadOnlyHandle( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) + T* value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } explicit ReadOnlyHandle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + : interface_name_(interface_name), value_ptr_((T*)nullptr) { } explicit ReadOnlyHandle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + : interface_name_(interface_name), value_ptr_((T*)nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; - + ReadOnlyHandle(const ReadOnlyHandle & other) = default; ReadOnlyHandle(ReadOnlyHandle && other) = default; ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; @@ -70,60 +75,82 @@ class ReadOnlyHandle const std::string & get_prefix_name() const { return prefix_name_; } - double get_value() const - { + template + typename std::enable_if_t::value, U> get_value() const + { THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; } + template + typename std::enable_if_t>::value, double> get_value() const + { + THROW_ON_NULLPTR(value_ptr_); + return value_ptr_->load(std::memory_order_relaxed); + } + protected: std::string prefix_name_; std::string interface_name_; - double * value_ptr_; + + T* value_ptr_; + }; -class ReadWriteHandle : public ReadOnlyHandle +template +class ReadWriteHandle : public ReadOnlyHandle { + static_assert(std::is_floating_point::value || std::is_same>::value, "Invalid template argument for class ReadWriteHandle. Only floating point, and atomic double types are supported for now."); public: ReadWriteHandle( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) + T * value_ptr = nullptr) + : ReadOnlyHandle(prefix_name, interface_name, value_ptr) { } - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} - ReadWriteHandle(const ReadWriteHandle & other) = default; + ReadWriteHandle(const ReadWriteHandle & other) : ReadOnlyHandle(other) {} - ReadWriteHandle(ReadWriteHandle && other) = default; + ReadWriteHandle(ReadWriteHandle && other) : ReadOnlyHandle(other) {} - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; + ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; ReadWriteHandle & operator=(ReadWriteHandle && other) = default; virtual ~ReadWriteHandle() = default; - void set_value(double value) + template + std::enable_if_t::value, void> set_value(T value) + { + //THROW_ON_NULLPTR(std::get<1>(ReadOnlyHandle::value_ptr_)); + //std::get<1>(ReadOnlyHandle::value_ptr_)->store(value, std::memory_order_relaxed); + THROW_ON_NULLPTR(ReadOnlyHandle::value_ptr_); + *(ReadOnlyHandle::value_ptr_) = value; + } + + template + std::enable_if_t>::value, void> set_value(T value) { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; + THROW_ON_NULLPTR(ReadOnlyHandle::value_ptr_); + ReadOnlyHandle::value_ptr_->store(value, std::memory_order_relaxed); } }; -class StateInterface : public ReadOnlyHandle +class StateInterface : public ReadOnlyHandle { public: StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using ReadOnlyHandle::ReadOnlyHandle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public ReadWriteHandle { public: /// CommandInterface copy constructor is actively deleted. @@ -136,9 +163,36 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using ReadWriteHandle::ReadWriteHandle; +}; + +class AsyncStateInterface : public ReadOnlyHandle> +{ +public: + AsyncStateInterface(const AsyncStateInterface & other) = default; + + AsyncStateInterface(AsyncStateInterface && other) = default; + + using ReadOnlyHandle>::ReadOnlyHandle; +}; + +class AsyncCommandInterface : public ReadWriteHandle> +{ +public: + /// CommandInterface copy constructor is actively deleted. + /** + * Command interfaces are having a unique ownership and thus + * can't be copied in order to avoid simultaneous writes to + * the same resource. + */ + AsyncCommandInterface(const AsyncCommandInterface & other) = delete; + + AsyncCommandInterface(AsyncCommandInterface && other) = default; + + using ReadWriteHandle>::ReadWriteHandle; }; + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..29c3d9b9cd 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "hardware_interface/handle.hpp" @@ -34,7 +35,17 @@ class LoanedCommandInterface } LoanedCommandInterface(CommandInterface & command_interface, Deleter && deleter) - : command_interface_(command_interface), deleter_(std::forward(deleter)) + : command_interface_(&command_interface), deleter_(std::forward(deleter)), using_async_command_interface_(false) + { + } + + explicit LoanedCommandInterface(AsyncCommandInterface & command_interface) + : LoanedCommandInterface(command_interface, nullptr) + { + } + + LoanedCommandInterface(AsyncCommandInterface & command_interface, Deleter && deleter) + : command_interface_(&command_interface), deleter_(std::forward(deleter)), using_async_command_interface_(true) { } @@ -50,26 +61,27 @@ class LoanedCommandInterface } } - const std::string get_name() const { return command_interface_.get_name(); } + const std::string get_name() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_name() : std::get<0>(command_interface_)->get_name(); } - const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } + const std::string & get_interface_name() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_interface_name() : std::get<0>(command_interface_)->get_interface_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return command_interface_.get_name(); + return using_async_command_interface_ ? std::get<1>(command_interface_)->get_name() : std::get<0>(command_interface_)->get_name(); } - const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } + const std::string & get_prefix_name() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_prefix_name() : std::get<0>(command_interface_)->get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } + void set_value(double val) { using_async_command_interface_ ? std::get<1>(command_interface_)->set_value(val) : std::get<0>(command_interface_)->set_value(val); } - double get_value() const { return command_interface_.get_value(); } + double get_value() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_value() : std::get<0>(command_interface_)->get_value(); } protected: - CommandInterface & command_interface_; + std::variant command_interface_; Deleter deleter_; + const bool using_async_command_interface_; }; } // 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 4fe67d1290..c3e0cd4bde 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "hardware_interface/handle.hpp" @@ -34,7 +35,17 @@ class LoanedStateInterface } LoanedStateInterface(StateInterface & state_interface, Deleter && deleter) - : state_interface_(state_interface), deleter_(std::forward(deleter)) + : state_interface_(&state_interface), deleter_(std::forward(deleter)), using_async_state_interface_(false) + { + } + + explicit LoanedStateInterface(AsyncStateInterface & state_interface) + : LoanedStateInterface(state_interface, nullptr) + { + } + + LoanedStateInterface(AsyncStateInterface & state_interface, Deleter && deleter) + : state_interface_(&state_interface), deleter_(std::forward(deleter)), using_async_state_interface_(true) { } @@ -50,24 +61,26 @@ class LoanedStateInterface } } - const std::string get_name() const { return state_interface_.get_name(); } + const std::string get_name() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_name() : std::get<0>(state_interface_)->get_name(); } - const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } + const std::string & get_interface_name() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_interface_name() : std::get<0>(state_interface_)->get_interface_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return state_interface_.get_name(); + return using_async_state_interface_ ? std::get<1>(state_interface_)->get_name() : std::get<0>(state_interface_)->get_name(); } - const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } + const std::string & get_prefix_name() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_prefix_name() : std::get<0>(state_interface_)->get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + double get_value() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_value() : std::get<0>(state_interface_)->get_value(); } protected: - StateInterface & state_interface_; + std::variant state_interface_; Deleter deleter_; + const bool using_async_state_interface_ = false; + }; } // namespace hardware_interface diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 16ca710e9d..3975616e0f 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -12,16 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include "hardware_interface/handle.hpp" using hardware_interface::CommandInterface; using hardware_interface::StateInterface; +using hardware_interface::AsyncCommandInterface; +using hardware_interface::AsyncStateInterface; + + namespace { constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; +constexpr auto BAR_INTERFACE = "BarInterface"; + } // namespace TEST(TestHandle, command_interface) @@ -68,3 +75,33 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } + +TEST(TestHandle, no_data_race_when_accessing_value) // fails when used with regular handles due to thread sanitizer +{ + std::atomic state_if_value = 1.558; + std::atomic cmd_if_value = 1.337; + + AsyncStateInterface state_handle{JOINT_NAME, FOO_INTERFACE, &state_if_value}; + AsyncCommandInterface command_handle{JOINT_NAME, FOO_INTERFACE, &cmd_if_value}; + + + std::thread hwif_read = std::thread([&]() { + state_if_value.store(1.338, std::memory_order_relaxed); + }); + + std::thread controller_update = std::thread([&]() { + command_handle.set_value(state_handle.get_value() + 0.33); + }); + + std::thread hwif_write = std::thread([&]() { + double k = command_handle.get_value(); + cmd_if_value.store(2.0, std::memory_order_relaxed); + }); + + + hwif_read.join(); + controller_update.join(); + hwif_write.join(); + + EXPECT_TRUE(true); +} \ No newline at end of file diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index bc1c0a78d8..adfc3b508d 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -22,17 +22,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::ReadWriteHandle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::ReadWriteHandle::ReadWriteHandle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::ReadWriteHandle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::ReadWriteHandle::ReadWriteHandle; }; } // namespace transmission_interface