[HW_IF] Prepare the handles for async operations

ros-controls · Sep 12, 2024 · 0630604 · 0630604
1 parent eb4c19d
commit 0630604
Show file tree

Hide file tree

Showing 3 changed files with 104 additions and 8 deletions.
diff --git 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 <limits>
+#include <mutex>
 #include <string>
+#include <utility>
 #include <variant>
 
 #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<std::recursive_mutex> lock(other.handle_mutex_);
+    std::unique_lock<std::recursive_mutex> 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<std::recursive_mutex> lock(other.handle_mutex_);
+    std::unique_lock<std::recursive_mutex> 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<std::recursive_mutex> lock(other.handle_mutex_);
+      std::unique_lock<std::recursive_mutex> 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<std::recursive_mutex> lock(other.handle_mutex_);
+      std::unique_lock<std::recursive_mutex> 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,21 +140,48 @@ 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<std::recursive_mutex> lock(handle_mutex_, std::try_to_lock);
+    if (!lock.owns_lock())
+    {
+      return std::numeric_limits<double>::quiet_NaN();
+    }
     // BEGIN (Handle export change): for backward compatibility
     // TODO(Manuel) return value_ if old functionality is removed
     THROW_ON_NULLPTR(value_ptr_);
     return *value_ptr_;
     // END
   }
 
-  void set_value(double value)
+  [[nodiscard]] bool get_value(double & value) const
+  {
+    std::unique_lock<std::recursive_mutex> 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<std::recursive_mutex> 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
@@ -63,9 +63,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
@@ -63,7 +63,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_;