[HW Interface] Use new handle API inside the hardware components (ros-controls#2092)

Author: saikishor

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -517,24 +517,77 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
     lifecycle_state_ = new_state;
   }
 
-  void set_state(const std::string & interface_name, const double & value)
+  template <typename T>
+  void set_state(const std::string & interface_name, const T & value)
   {
-    actuator_states_.at(interface_name)->set_value(value);
+    auto it = actuator_states_.find(interface_name);
+    if (it == actuator_states_.end())
+    {
+      throw std::runtime_error(
+        "State interface not found: " + interface_name +
+        " in actuator hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::unique_lock<std::shared_mutex> lock(handle->get_mutex());
+    std::ignore = handle->set_value(lock, value);
   }
 
-  double get_state(const std::string & interface_name) const
+  template <typename T = double>
+  T get_state(const std::string & interface_name) const
   {
-    return actuator_states_.at(interface_name)->get_value();
+    auto it = actuator_states_.find(interface_name);
+    if (it == actuator_states_.end())
+    {
+      throw std::runtime_error(
+        "State interface not found: " + interface_name +
+        " in actuator hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::shared_lock<std::shared_mutex> lock(handle->get_mutex());
+    const auto opt_value = handle->get_optional<T>(lock);
+    if (!opt_value)
+    {
+      throw std::runtime_error(
+        "Failed to get state value from interface: " + interface_name +
+        ". This should not happen.");
+    }
+    return opt_value.value();
   }
 
   void set_command(const std::string & interface_name, const double & value)
   {
-    actuator_commands_.at(interface_name)->set_value(value);
+    auto it = actuator_commands_.find(interface_name);
+    if (it == actuator_commands_.end())
+    {
+      throw std::runtime_error(
+        "Command interface not found: " + interface_name +
+        " in actuator hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::unique_lock<std::shared_mutex> lock(handle->get_mutex());
+    std::ignore = handle->set_value(lock, value);
   }
 
-  double get_command(const std::string & interface_name) const
+  template <typename T = double>
+  T get_command(const std::string & interface_name) const
   {
-    return actuator_commands_.at(interface_name)->get_value();
+    auto it = actuator_commands_.find(interface_name);
+    if (it == actuator_commands_.end())
+    {
+      throw std::runtime_error(
+        "Command interface not found: " + interface_name +
+        " in actuator hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::shared_lock<std::shared_mutex> lock(handle->get_mutex());
+    const auto opt_value = handle->get_optional<double>(lock);
+    if (!opt_value)
+    {
+      throw std::runtime_error(
+        "Failed to get command value from interface: " + interface_name +
+        ". This should not happen.");
+    }
+    return opt_value.value();
   }
 
   /// Get the logger of the ActuatorInterface.

hardware_interface/include/hardware_interface/handle.hpp

@@ -172,6 +172,22 @@ class Handle
   [[nodiscard]] std::optional<T> get_optional() const
   {
     std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
+    return get_optional<T>(lock);
+  }
+  /**
+   * @brief Get the value of the handle.
+   * @tparam T The type of the value to be retrieved.
+   * @param lock The lock to access the value.
+   * @return The value of the handle if it accessed successfully, std::nullopt otherwise.
+   *
+   * @note The method is thread-safe and non-blocking.
+   * @note When different threads access the same handle at same instance, and if they are unable to
+   * lock the handle to access the value, the handle returns std::nullopt. If the operation is
+   * successful, the value is returned.
+   */
+  template <typename T = double>
+  [[nodiscard]] std::optional<T> get_optional(std::shared_lock<std::shared_mutex> & lock) const
+  {
     if (!lock.owns_lock())
     {
       return std::nullopt;
@@ -259,6 +275,24 @@ class Handle
   [[nodiscard]] bool set_value(const T & value)
   {
     std::unique_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
+    return set_value(lock, value);
+  }
+
+  /**
+   * @brief Set the value of the handle.
+   * @tparam T The type of the value to be set.
+   * @param lock The lock to set the value.
+   * @param value The value to be set.
+   * @return true if the value is set successfully, false otherwise.
+   *
+   * @note The method is thread-safe and non-blocking.
+   * @note When different threads access the same handle at same instance, and if they are unable to
+   * lock the handle to set the value, the handle returns false. If the operation is successful, the
+   * handle is updated and returns true.
+   */
+  template <typename T>
+  [[nodiscard]] bool set_value(std::unique_lock<std::shared_mutex> & lock, const T & value)
+  {
     if (!lock.owns_lock())
     {
       return false;
@@ -285,6 +319,8 @@ class Handle
     // END
   }
 
+  std::shared_mutex & get_mutex() { return handle_mutex_; }
+
   HandleDataType get_data_type() const { return data_type_; }
 
 private:

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -325,14 +325,41 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     lifecycle_state_ = new_state;
   }
 
-  void set_state(const std::string & interface_name, const double & value)
+  template <typename T>
+  void set_state(const std::string & interface_name, const T & value)
   {
-    sensor_states_map_.at(interface_name)->set_value(value);
+    auto it = sensor_states_map_.find(interface_name);
+    if (it == sensor_states_map_.end())
+    {
+      throw std::runtime_error(
+        "State interface not found: " + interface_name +
+        " in sensor hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::unique_lock<std::shared_mutex> lock(handle->get_mutex());
+    std::ignore = handle->set_value(lock, value);
   }
 
-  double get_state(const std::string & interface_name) const
+  template <typename T = double>
+  T get_state(const std::string & interface_name) const
   {
-    return sensor_states_map_.at(interface_name)->get_value();
+    auto it = sensor_states_map_.find(interface_name);
+    if (it == sensor_states_map_.end())
+    {
+      throw std::runtime_error(
+        "State interface not found: " + interface_name +
+        " in sensor hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::shared_lock<std::shared_mutex> lock(handle->get_mutex());
+    const auto opt_value = handle->get_optional<T>(lock);
+    if (!opt_value)
+    {
+      throw std::runtime_error(
+        "Failed to get state value from interface: " + interface_name +
+        ". This should not happen.");
+    }
+    return opt_value.value();
   }
 
   /// Get the logger of the SensorInterface.

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -547,24 +547,77 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     lifecycle_state_ = new_state;
   }
 
-  void set_state(const std::string & interface_name, const double & value)
+  template <typename T>
+  void set_state(const std::string & interface_name, const T & value)
   {
-    system_states_.at(interface_name)->set_value(value);
+    auto it = system_states_.find(interface_name);
+    if (it == system_states_.end())
+    {
+      throw std::runtime_error(
+        "State interface not found: " + interface_name +
+        " in system hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::unique_lock<std::shared_mutex> lock(handle->get_mutex());
+    std::ignore = handle->set_value(lock, value);
   }
 
-  double get_state(const std::string & interface_name) const
+  template <typename T = double>
+  T get_state(const std::string & interface_name) const
   {
-    return system_states_.at(interface_name)->get_value();
+    auto it = system_states_.find(interface_name);
+    if (it == system_states_.end())
+    {
+      throw std::runtime_error(
+        "State interface not found: " + interface_name +
+        " in system hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::shared_lock<std::shared_mutex> lock(handle->get_mutex());
+    const auto opt_value = handle->get_optional<T>(lock);
+    if (!opt_value)
+    {
+      throw std::runtime_error(
+        "Failed to get state value from interface: " + interface_name +
+        ". This should not happen.");
+    }
+    return opt_value.value();
   }
 
   void set_command(const std::string & interface_name, const double & value)
   {
-    system_commands_.at(interface_name)->set_value(value);
+    auto it = system_commands_.find(interface_name);
+    if (it == system_commands_.end())
+    {
+      throw std::runtime_error(
+        "Command interface not found: " + interface_name +
+        " in system hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::unique_lock<std::shared_mutex> lock(handle->get_mutex());
+    std::ignore = handle->set_value(lock, value);
   }
 
-  double get_command(const std::string & interface_name) const
+  template <typename T = double>
+  T get_command(const std::string & interface_name) const
   {
-    return system_commands_.at(interface_name)->get_value();
+    auto it = system_commands_.find(interface_name);
+    if (it == system_commands_.end())
+    {
+      throw std::runtime_error(
+        "Command interface not found: " + interface_name +
+        " in system hardware component: " + info_.name + ". This should not happen.");
+    }
+    auto & handle = it->second;
+    std::shared_lock<std::shared_mutex> lock(handle->get_mutex());
+    const auto opt_value = handle->get_optional<double>(lock);
+    if (!opt_value)
+    {
+      throw std::runtime_error(
+        "Failed to get command value from interface: " + interface_name +
+        ". This should not happen.");
+    }
+    return opt_value.value();
   }
 
   /// Get the logger of the SystemInterface.

hardware_interface/test/test_component_interfaces.cpp

@@ -186,6 +186,11 @@ class DummyActuatorDefault : public hardware_interface::ActuatorInterface
     {
       set_command("joint1/velocity", 0.0);
     }
+    // Should throw as the interface is unknown
+    EXPECT_THROW(get_state("joint1/nonexisting/interface"), std::runtime_error);
+    EXPECT_THROW(get_command("joint1/nonexisting/interface"), std::runtime_error);
+    EXPECT_THROW(set_state("joint1/nonexisting/interface", 0.0), std::runtime_error);
+    EXPECT_THROW(set_command("joint1/nonexisting/interface", 0.0), std::runtime_error);
 
     read_calls_ = 0;
     write_calls_ = 0;
@@ -329,6 +334,10 @@ class DummySensorDefault : public hardware_interface::SensorInterface
   CallbackReturn on_configure(const rclcpp_lifecycle::State & /*previous_state*/) override
   {
     set_state("sens1/voltage", 0.0);
+    // Should throw as the interface is unknown
+    EXPECT_THROW(get_state("joint1/nonexisting/interface"), std::runtime_error);
+    EXPECT_THROW(set_state("joint1/nonexisting/interface", 0.0), std::runtime_error);
+
     read_calls_ = 0;
     return CallbackReturn::SUCCESS;
   }
@@ -600,6 +609,11 @@ class DummySystemDefault : public hardware_interface::SystemInterface
         set_command(velocity_commands_[i], 0.0);
       }
     }
+    // Should throw as the interface is unknown
+    EXPECT_THROW(get_state("joint1/nonexisting/interface"), std::runtime_error);
+    EXPECT_THROW(get_command("joint1/nonexisting/interface"), std::runtime_error);
+    EXPECT_THROW(set_state("joint1/nonexisting/interface", 0.0), std::runtime_error);
+    EXPECT_THROW(set_command("joint1/nonexisting/interface", 0.0), std::runtime_error);
 
     read_calls_ = 0;
     write_calls_ = 0;