Rework handles to support atomic doubles

hardware_interface/include/hardware_interface/handle.hpp

@@ -17,35 +17,40 @@
 
 #include <string>
 #include <utility>
+#include <atomic>
+#include <limits>
 
 #include "hardware_interface/macros.hpp"
 #include "hardware_interface/visibility_control.h"
 
 namespace hardware_interface
 {
 /// A handle used to get and set a value on a given interface.
+
+
+template <typename T>
 class ReadOnlyHandle
 {
+  static_assert(std::is_floating_point<T>::value || std::is_same<T, std::atomic<double>>::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 U = T>
+  typename std::enable_if_t<std::is_floating_point<U>::value, U> get_value() const
+  {    
     THROW_ON_NULLPTR(value_ptr_);
     return *value_ptr_;
   }
 
+  template<typename U = T>
+  typename std::enable_if_t<std::is_same<U, std::atomic<double>>::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 <typename T>
+class ReadWriteHandle : public ReadOnlyHandle<T>
 {
+  static_assert(std::is_floating_point<T>::value || std::is_same<T, std::atomic<double>>::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<T>(prefix_name, interface_name, value_ptr)
   {
   }
 
-  explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {}
+  explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle<T>(interface_name) {}
 
-  explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {}
+  explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle<T>(interface_name) {}
 
-  ReadWriteHandle(const ReadWriteHandle & other) = default;
+  ReadWriteHandle(const ReadWriteHandle & other) : ReadOnlyHandle<T>(other) {}
 
-  ReadWriteHandle(ReadWriteHandle && other) = default;
+  ReadWriteHandle(ReadWriteHandle && other) : ReadOnlyHandle<T>(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 <typename U = T>
+  std::enable_if_t<std::is_floating_point<U>::value, void> set_value(T value)
+  {
+      //THROW_ON_NULLPTR(std::get<1>(ReadOnlyHandle<T>::value_ptr_));
+      //std::get<1>(ReadOnlyHandle<T>::value_ptr_)->store(value, std::memory_order_relaxed);
+      THROW_ON_NULLPTR(ReadOnlyHandle<T>::value_ptr_);
+      *(ReadOnlyHandle<T>::value_ptr_) = value;
+  }
+
+  template <typename U = T>
+  std::enable_if_t<std::is_same<U, std::atomic<double>>::value, void> set_value(T value)
   {
-    THROW_ON_NULLPTR(this->value_ptr_);
-    *this->value_ptr_ = value;
+      THROW_ON_NULLPTR(ReadOnlyHandle<T>::value_ptr_);
+      ReadOnlyHandle<T>::value_ptr_->store(value, std::memory_order_relaxed);
   }
 };
 
-class StateInterface : public ReadOnlyHandle
+class StateInterface : public ReadOnlyHandle<double>
 {
 public:
   StateInterface(const StateInterface & other) = default;
 
   StateInterface(StateInterface && other) = default;
 
-  using ReadOnlyHandle::ReadOnlyHandle;
+  using ReadOnlyHandle<double>::ReadOnlyHandle;
 };
 
-class CommandInterface : public ReadWriteHandle
+class CommandInterface : public ReadWriteHandle<double>
 {
 public:
   /// CommandInterface copy constructor is actively deleted.
@@ -136,9 +163,36 @@ class CommandInterface : public ReadWriteHandle
 
   CommandInterface(CommandInterface && other) = default;
 
-  using ReadWriteHandle::ReadWriteHandle;
+  using ReadWriteHandle<double>::ReadWriteHandle;
+};
+
+class AsyncStateInterface : public ReadOnlyHandle<std::atomic<double>>
+{
+public:
+  AsyncStateInterface(const AsyncStateInterface & other) = default;
+
+  AsyncStateInterface(AsyncStateInterface && other) = default;
+
+  using ReadOnlyHandle<std::atomic<double>>::ReadOnlyHandle;
+};
+
+class AsyncCommandInterface : public ReadWriteHandle<std::atomic<double>>
+{
+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<std::atomic<double>>::ReadWriteHandle;
 };
 
+
 }  // namespace hardware_interface
 
 #endif  // HARDWARE_INTERFACE__HANDLE_HPP_

hardware_interface/include/hardware_interface/loaned_command_interface.hpp

@@ -18,6 +18,7 @@
 #include <functional>
 #include <string>
 #include <utility>
+#include <variant>
 
 #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>(deleter))
+  : command_interface_(&command_interface), deleter_(std::forward<Deleter>(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>(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<CommandInterface*, AsyncCommandInterface*> command_interface_;
   Deleter deleter_;
+  const bool using_async_command_interface_;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/loaned_state_interface.hpp

@@ -18,6 +18,7 @@
 #include <functional>
 #include <string>
 #include <utility>
+#include <variant>
 
 #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>(deleter))
+  : state_interface_(&state_interface), deleter_(std::forward<Deleter>(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>(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<StateInterface*, AsyncStateInterface*> state_interface_;
   Deleter deleter_;
+  const bool using_async_state_interface_ = false;
+
 };
 
 }  // namespace hardware_interface

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 <thread>
 #include <gmock/gmock.h>
 #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<double> state_if_value = 1.558;
+  std::atomic<double> 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);
+}

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<double>
 {
 public:
-  using hardware_interface::ReadWriteHandle::ReadWriteHandle;
+  using hardware_interface::ReadWriteHandle<double>::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<double>
 {
 public:
-  using hardware_interface::ReadWriteHandle::ReadWriteHandle;
+  using hardware_interface::ReadWriteHandle<double>::ReadWriteHandle;
 };
 
 }  // namespace transmission_interface