[HW_IF] Prepare the handles for async operations

hardware_interface/include/hardware_interface/handle.hpp

@@ -17,7 +17,9 @@
 
 #include <limits>
 #include <memory>
+#include <mutex>
 #include <string>
+#include <utility>
 #include <variant>
 
 #include "hardware_interface/hardware_info.hpp"
@@ -69,13 +71,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;
 
@@ -95,21 +141,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
  }
 
@@ -122,6 +195,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

hardware_interface/include/hardware_interface/loaned_command_interface.hpp

@@ -17,6 +17,7 @@
 
 #include <functional>
 #include <string>
+#include <thread>
 #include <utility>
 
 #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_;

hardware_interface/include/hardware_interface/loaned_state_interface.hpp

@@ -17,6 +17,7 @@
 
 #include <functional>
 #include <string>
+#include <thread>
 #include <utility>
 
 #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_;