[HW_IF] Prepare the handles for async operations (ros-controls#1750)

pal-robotics-forks · Nov 12, 2024 · d763d19 · d763d19
1 parent 81d22b8
commit d763d19
Show file tree

Hide file tree

Showing 3 changed files with 217 additions and 9 deletions.
diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp
@@ -17,7 +17,10 @@
 
 #include <limits>
 #include <memory>
+#include <mutex>
+#include <shared_mutex>
 #include <string>
+#include <utility>
 #include <variant>
 
 #include "hardware_interface/hardware_info.hpp"
@@ -69,13 +72,57 @@ class Handle
   {
   }
 
-  Handle(const Handle & other) = default;
+  Handle(const Handle & other) noexcept
+  {
+    std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
+    std::unique_lock<std::shared_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::shared_mutex> lock(other.handle_mutex_);
+    std::unique_lock<std::shared_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::shared_mutex> lock(other.handle_mutex_);
+      std::unique_lock<std::shared_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::shared_mutex> lock(other.handle_mutex_);
+      std::unique_lock<std::shared_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 +142,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::shared_lock<std::shared_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::shared_lock<std::shared_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::shared_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 +196,7 @@ class Handle
   // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
   double * value_ptr_;
   // END
+  mutable std::shared_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
@@ -16,10 +16,13 @@
 #define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
 
 #include <functional>
+#include <limits>
 #include <string>
+#include <thread>
 #include <utility>
 
 #include "hardware_interface/handle.hpp"
+#include "rclcpp/logging.hpp"
 
 namespace hardware_interface
 {
@@ -51,6 +54,27 @@ class LoanedCommandInterface
 
   virtual ~LoanedCommandInterface()
   {
+    auto logger = rclcpp::get_logger(command_interface_.get_name());
+    RCLCPP_WARN_EXPRESSION(
+      rclcpp::get_logger(get_name()),
+      (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
+      "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of "
+      "%u get_value calls",
+      get_name().c_str(), get_value_statistics_.timeout_counter,
+      (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter,
+      get_value_statistics_.failed_counter,
+      (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter,
+      get_value_statistics_.total_counter);
+    RCLCPP_WARN_EXPRESSION(
+      rclcpp::get_logger(get_name()),
+      (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0),
+      "LoanedCommandInterface %s has %u (%.4f %%) timeouts and  %u (~ %.4f %%) missed calls out of "
+      "%u set_value calls",
+      get_name().c_str(), set_value_statistics_.timeout_counter,
+      (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter,
+      set_value_statistics_.failed_counter,
+      (set_value_statistics_.failed_counter * 10.0) / set_value_statistics_.total_counter,
+      set_value_statistics_.total_counter);
     if (deleter_)
     {
       deleter_();
@@ -70,13 +94,70 @@ 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); }
+  template <typename T>
+  [[nodiscard]] bool set_value(T value, unsigned int max_tries = 10)
+  {
+    unsigned int nr_tries = 0;
+    ++set_value_statistics_.total_counter;
+    while (!command_interface_.set_value(value))
+    {
+      ++set_value_statistics_.failed_counter;
+      ++nr_tries;
+      if (nr_tries == max_tries)
+      {
+        ++set_value_statistics_.timeout_counter;
+        return false;
+      }
+      std::this_thread::yield();
+    }
+    return true;
+  }
 
-  double get_value() const { return command_interface_.get_value(); }
+  double get_value() const
+  {
+    double value;
+    if (get_value(value))
+    {
+      return value;
+    }
+    else
+    {
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+  }
+
+  template <typename T>
+  [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const
+  {
+    unsigned int nr_tries = 0;
+    ++get_value_statistics_.total_counter;
+    while (!command_interface_.get_value(value))
+    {
+      ++get_value_statistics_.failed_counter;
+      ++nr_tries;
+      if (nr_tries == max_tries)
+      {
+        ++get_value_statistics_.timeout_counter;
+        return false;
+      }
+      std::this_thread::yield();
+    }
+    return true;
+  }
 
 protected:
   CommandInterface & command_interface_;
   Deleter deleter_;
+
+private:
+  struct HandleRTStatistics
+  {
+    unsigned int total_counter = 0;
+    unsigned int failed_counter = 0;
+    unsigned int timeout_counter = 0;
+  };
+  mutable HandleRTStatistics get_value_statistics_;
+  HandleRTStatistics set_value_statistics_;
 };
 
 }  // 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
@@ -16,11 +16,13 @@
 #define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_
 
 #include <functional>
+#include <limits>
 #include <string>
+#include <thread>
 #include <utility>
 
 #include "hardware_interface/handle.hpp"
-
+#include "rclcpp/logging.hpp"
 namespace hardware_interface
 {
 class LoanedStateInterface
@@ -56,6 +58,17 @@ class LoanedStateInterface
 
   virtual ~LoanedStateInterface()
   {
+    auto logger = rclcpp::get_logger(state_interface_.get_name());
+    RCLCPP_WARN_EXPRESSION(
+      logger,
+      (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
+      "LoanedStateInterface %s has %u (%.4f %%) timeouts and %u (%.4f %%) missed calls out of %u "
+      "get_value calls",
+      state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter,
+      (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter,
+      get_value_statistics_.failed_counter,
+      (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter,
+      get_value_statistics_.total_counter);
     if (deleter_)
     {
       deleter_();
@@ -75,11 +88,50 @@ 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;
+    if (get_value(value))
+    {
+      return value;
+    }
+    else
+    {
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+  }
+
+  template <typename T>
+  [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const
+  {
+    unsigned int nr_tries = 0;
+    ++get_value_statistics_.total_counter;
+    while (!state_interface_.get_value(value))
+    {
+      ++get_value_statistics_.failed_counter;
+      ++nr_tries;
+      if (nr_tries == max_tries)
+      {
+        ++get_value_statistics_.timeout_counter;
+        return false;
+      }
+      std::this_thread::yield();
+    }
+    return true;
+  }
 
 protected:
   const StateInterface & state_interface_;
   Deleter deleter_;
+
+private:
+  struct HandleRTStatistics
+  {
+    unsigned int total_counter = 0;
+    unsigned int failed_counter = 0;
+    unsigned int timeout_counter = 0;
+  };
+  mutable HandleRTStatistics get_value_statistics_;
 };
 
 }  // namespace hardware_interface