Change interface export step 2

hardware_interface/include/hardware_interface/actuator.hpp

@@ -21,6 +21,8 @@
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/visibility_control.h"
 #include "rclcpp/duration.hpp"
@@ -70,6 +72,19 @@ class Actuator final
   HARDWARE_INTERFACE_PUBLIC
   std::vector<CommandInterface> export_command_interfaces();
 
+  HARDWARE_INTERFACE_PUBLIC
+  std::vector<InterfaceDescription> export_state_interface_descriptions();
+
+  HARDWARE_INTERFACE_PUBLIC
+  std::vector<InterfaceDescription> export_command_interface_descriptions();
+
+  HARDWARE_INTERFACE_PUBLIC
+  LoanedCommandInterface create_loaned_command_interface(
+    const std::string & interface_name, std::function<void(void)> && release_callback);
+
+  HARDWARE_INTERFACE_PUBLIC
+  LoanedStateInterface create_loaned_state_interface(const std::string & interface_name);
+
   HARDWARE_INTERFACE_PUBLIC
   return_type prepare_command_mode_switch(
     const std::vector<std::string> & start_interfaces,

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -15,12 +15,16 @@
 #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
@@ -122,6 +126,37 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   virtual std::vector<CommandInterface> export_command_interfaces() = 0;
 
+  /**
+   * Exports all information about the available StateInterfaces for this hardware interface.
+   *
+   * \return vector of InterfaceDescription
+   */
+  virtual std::vector<InterfaceDescription> export_state_interface_descriptions() const
+  {
+    return joint_states_descriptions_;
+  }
+
+  /**
+   * Exports all information about the available CommandInterfaces for this hardware interface.
+   *
+   * \return vector of InterfaceDescription
+   */
+  virtual std::vector<InterfaceDescription> export_command_interface_descriptions() const
+  {
+    return joint_commands_descriptions_;
+  }
+
+  virtual LoanedCommandInterface create_loaned_command_interface(
+    const std::string & interface_name, std::function<void(void)> && release_callback)
+  {
+    return LoanedCommandInterface(joint_commands_.at(interface_name), std::move(release_callback));
+  }
+
+  virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name)
+  {
+    return LoanedStateInterface(joint_states_.at(interface_name));
+  }
+
   /// Prepare for a new command interface switch.
   /**
    * Prepare for any mode-switching required by the new command interface combination.
@@ -202,8 +237,35 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
 
+  double joint_state_get_value(const InterfaceDescription & interface_descr) const
+  {
+    return joint_states_.at(interface_descr.get_name()).get_value();
+  }
+
+  void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value)
+  {
+    joint_states_.at(interface_descr.get_name()).set_value(value);
+  }
+
+  double joint_command_get_value(const InterfaceDescription & interface_descr) const
+  {
+    return joint_commands_.at(interface_descr.get_name()).get_value();
+  }
+
+  void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value)
+  {
+    joint_commands_.at(interface_descr.get_name()).set_value(value);
+  }
+
 protected:
   HardwareInfo info_;
+  std::vector<InterfaceDescription> joint_states_descriptions_;
+  std::vector<InterfaceDescription> joint_commands_descriptions_;
+
+private:
+  std::map<std::string, StateInterface> joint_states_;
+  std::map<std::string, CommandInterface> joint_commands_;
+
   rclcpp_lifecycle::State lifecycle_state_;
 };
 

hardware_interface/include/hardware_interface/handle.hpp

@@ -15,6 +15,7 @@
 #ifndef HARDWARE_INTERFACE__HANDLE_HPP_
 #define HARDWARE_INTERFACE__HANDLE_HPP_
 
+#include <limits>
 #include <string>
 #include <utility>
 
@@ -25,40 +26,62 @@
 namespace hardware_interface
 {
 /// A handle used to get and set a value on a given interface.
-class ReadOnlyHandle
+class Handle
 {
 public:
-  ReadOnlyHandle(
+  [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]] Handle(
     const std::string & prefix_name, const std::string & interface_name,
     double * value_ptr = nullptr)
   : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr)
   {
   }
 
-  explicit ReadOnlyHandle(const std::string & interface_name)
+  explicit Handle(const InterfaceDescription & interface_description)
+  : interface_description_(interface_description),
+    prefix_name_(interface_description.get_prefix_name()),
+    interface_name_(interface_description.get_interface_type()),
+    value_(std::numeric_limits<double>::quiet_NaN()),
+    value_ptr_(&value_)
+  {
+  }
+
+  [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]]
+
+  explicit Handle(const std::string & interface_name)
   : interface_name_(interface_name), value_ptr_(nullptr)
   {
   }
 
-  explicit ReadOnlyHandle(const char * interface_name)
+  [[deprecated("Use InterfaceDescription for initializing the Command-/StateIntefaces.")]]
+
+  explicit Handle(const char * interface_name)
   : interface_name_(interface_name), value_ptr_(nullptr)
   {
   }
 
-  ReadOnlyHandle(const ReadOnlyHandle & other) = default;
+  Handle(const Handle & other) = default;
 
-  ReadOnlyHandle(ReadOnlyHandle && other) = default;
+  Handle(Handle && other) = default;
 
-  ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default;
+  Handle & operator=(const Handle & other) = default;
 
-  ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default;
+  Handle & operator=(Handle && other) = default;
 
-  virtual ~ReadOnlyHandle() = default;
+  virtual ~Handle() = default;
 
   /// Returns true if handle references a value.
   inline operator bool() const { return value_ptr_ != nullptr; }
 
-  const std::string get_name() const { return prefix_name_ + "/" + interface_name_; }
+  const std::string & get_component_type() const
+  {
+    return interface_description_.get_component_type();
+  }
+
+  const std::string get_name() const
+  {
+    // deprecated is going to be replaced by interface_description_.get_name()
+    return prefix_name_ + "/" + interface_name_;
+  }
 
   const std::string & get_interface_name() const { return interface_name_; }
 
@@ -69,7 +92,14 @@ class ReadOnlyHandle
     return get_name();
   }
 
-  const std::string & get_prefix_name() const { return prefix_name_; }
+  // Only used for hw side. LoanedStateInterface does not expose this to controllers
+  void set_value(const double & value) { *value_ptr_ = value; }
+
+  const std::string & get_prefix_name() const
+  {
+    // deprecated is going to be replaced by interface_description_.get_prefix_name()
+    return prefix_name_;
+  }
 
   double get_value() const
   {
@@ -78,66 +108,36 @@ class ReadOnlyHandle
   }
 
 protected:
+  InterfaceDescription interface_description_;
+  // deprecated is going to be replaced by InterfaceDescription
   std::string prefix_name_;
+  // deprecated is going to be replaced by InterfaceDescription
   std::string interface_name_;
+  double value_;
+  // deprecated is going to be replaced by value_
   double * value_ptr_;
 };
 
-class ReadWriteHandle : public ReadOnlyHandle
-{
-public:
-  ReadWriteHandle(
-    const std::string & prefix_name, const std::string & interface_name,
-    double * value_ptr = nullptr)
-  : ReadOnlyHandle(prefix_name, interface_name, value_ptr)
-  {
-  }
-
-  explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {}
-
-  explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {}
-
-  ReadWriteHandle(const ReadWriteHandle & other) = default;
-
-  ReadWriteHandle(ReadWriteHandle && other) = default;
-
-  ReadWriteHandle & operator=(const ReadWriteHandle & other) = default;
-
-  ReadWriteHandle & operator=(ReadWriteHandle && other) = default;
-
-  virtual ~ReadWriteHandle() = default;
-
-  void set_value(double value)
-  {
-    THROW_ON_NULLPTR(this->value_ptr_);
-    *this->value_ptr_ = value;
-  }
-};
-
-class StateInterface : public ReadOnlyHandle
+class StateInterface : public Handle
 {
 public:
-  explicit StateInterface(
-    const InterfaceDescription & interface_description, double * value_ptr = nullptr)
-  : ReadOnlyHandle(
-      interface_description.prefix_name, interface_description.interface_info.name, value_ptr)
+  explicit StateInterface(const InterfaceDescription & interface_description)
+  : Handle(interface_description)
   {
   }
 
   StateInterface(const StateInterface & other) = default;
 
   StateInterface(StateInterface && other) = default;
 
-  using ReadOnlyHandle::ReadOnlyHandle;
+  using Handle::Handle;
 };
 
-class CommandInterface : public ReadWriteHandle
+class CommandInterface : public Handle
 {
 public:
-  explicit CommandInterface(
-    const InterfaceDescription & interface_description, double * value_ptr = nullptr)
-  : ReadWriteHandle(
-      interface_description.prefix_name, interface_description.interface_info.name, value_ptr)
+  explicit CommandInterface(const InterfaceDescription & interface_description)
+  : Handle(interface_description)
   {
   }
   /// CommandInterface copy constructor is actively deleted.
@@ -148,9 +148,11 @@ class CommandInterface : public ReadWriteHandle
    */
   CommandInterface(const CommandInterface & other) = delete;
 
+  CommandInterface & operator=(const CommandInterface & other) = delete;
+
   CommandInterface(CommandInterface && other) = default;
 
-  using ReadWriteHandle::ReadWriteHandle;
+  using Handle::Handle;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -108,8 +108,14 @@ struct TransmissionInfo
  */
 struct InterfaceDescription
 {
-  InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in)
-  : prefix_name(prefix_name_in), interface_info(interface_info_in)
+  InterfaceDescription() {}
+  InterfaceDescription(
+    const std::string & prefix_name_in, const std::string & component_type_in,
+    const InterfaceInfo & interface_info_in)
+  : prefix_name(prefix_name_in),
+    component_type(component_type_in),
+    interface_info(interface_info_in),
+    name(get_prefix_name() + "/" + get_interface_type())
   {
   }
 
@@ -128,7 +134,16 @@ struct InterfaceDescription
    */
   InterfaceInfo interface_info;
 
-  std::string get_name() const { return prefix_name + "/" + interface_info.name; }
+  /**
+   *  Full qualified name of the Interface
+   */
+  std::string name;
+
+  std::string get_prefix_name() const { return prefix_name; }
+
+  std::string get_component_type() const { return component_type; }
+
+  std::string get_name() const { return name; }
 
   std::string get_interface_type() const { return interface_info.name; }
 };

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -99,6 +99,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   bool is_urdf_already_loaded() const;
 
+  bool component_creates_loaned_state_interface(const std::string & key);
+
+  bool component_creates_loaned_command_interface(const std::string & key);
+
   /// Claim a state interface given its key.
   /**
    * The resource is claimed as long as being in scope.

hardware_interface/include/hardware_interface/sensor.hpp

@@ -22,6 +22,7 @@
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/visibility_control.h"
 #include "rclcpp/duration.hpp"
@@ -68,6 +69,12 @@ class Sensor final
   HARDWARE_INTERFACE_PUBLIC
   std::vector<StateInterface> export_state_interfaces();
 
+  HARDWARE_INTERFACE_PUBLIC
+  std::vector<InterfaceDescription> export_state_interface_descriptions();
+
+  HARDWARE_INTERFACE_PUBLIC
+  LoanedStateInterface create_loaned_state_interface(const std::string & interface_name);
+
   HARDWARE_INTERFACE_PUBLIC
   std::string get_name() const;