add export_of_description and creation of loans for system

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,12 @@ class Actuator final
   HARDWARE_INTERFACE_PUBLIC
   std::vector<CommandInterface> export_command_interfaces();
 
+  HARDWARE_INTERFACE_PUBLIC
+  LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name);
+
+  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,15 @@
 #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
 #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 +125,16 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   virtual std::vector<CommandInterface> export_command_interfaces() = 0;
 
+  virtual LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name)
+  {
+    return LoanedCommandInterface(joint_commands_.at(interface_name));
+  }
+
+  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.
@@ -204,6 +217,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
 
 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/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,9 @@ class Sensor final
   HARDWARE_INTERFACE_PUBLIC
   std::vector<StateInterface> export_state_interfaces();
 
+  HARDWARE_INTERFACE_PUBLIC
+  LoanedStateInterface create_loaned_state_interface(const std::string & interface_name);
+
   HARDWARE_INTERFACE_PUBLIC
   std::string get_name() const;
 

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -15,12 +15,14 @@
 #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
 
 #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/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
@@ -111,6 +113,11 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   virtual std::vector<StateInterface> export_state_interfaces() = 0;
 
+  virtual LoanedStateInterface create_loaned_state_interface(const std::string & interface_name)
+  {
+    return LoanedStateInterface(sensor_states_.at(interface_name));
+  }
+
   /// Read the current state values from the actuator.
   /**
    * The data readings from the physical hardware has to be updated
@@ -143,6 +150,11 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
 
 protected:
   HardwareInfo info_;
+  std::vector<InterfaceDescription> sensor_states_descriptions_;
+
+private:
+  std::map<std::string, StateInterface> sensor_states_;
+
   rclcpp_lifecycle::State lifecycle_state_;
 };
 

hardware_interface/include/hardware_interface/system.hpp

@@ -22,6 +22,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"
@@ -77,6 +79,12 @@ class System final
   HARDWARE_INTERFACE_PUBLIC
   std::vector<InterfaceDescription> export_command_interface_descriptions();
 
+  HARDWARE_INTERFACE_PUBLIC
+  LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name);
+
+  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/system_interface.hpp

@@ -25,6 +25,8 @@
 #include "hardware_interface/component_parser.hpp"
 #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_component_type_values.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
@@ -161,7 +163,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    *
    * \return vector of InterfaceDescription
    */
-  virtual std::vector<InterfaceDescription> export_state_interface_descriptions()
+  virtual std::vector<InterfaceDescription> export_state_interface_descriptions() const
   {
     std::vector<InterfaceDescription> state_interface_descriptions;
     state_interface_descriptions.reserve(
@@ -189,7 +191,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    *
    * \return vector of InterfaceDescription
    */
-  virtual std::vector<InterfaceDescription> export_command_interface_descriptions()
+  virtual std::vector<InterfaceDescription> export_command_interface_descriptions() const
   {
     std::vector<InterfaceDescription> command_interface_descriptions;
     command_interface_descriptions.reserve(
@@ -265,6 +267,16 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     return command_interfaces;
   }
 
+  virtual LoanedCommandInterface create_loaned_command_interface(const std::string & interface_name)
+  {
+    return LoanedCommandInterface(joint_commands_.at(interface_name));
+  }
+
+  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.

hardware_interface/src/actuator.cpp

@@ -198,6 +198,16 @@ std::vector<CommandInterface> Actuator::export_command_interfaces()
   return impl_->export_command_interfaces();
 }
 
+LoanedCommandInterface Actuator::create_loaned_command_interface(const std::string & interface_name)
+{
+  return impl_->create_loaned_command_interface(interface_name);
+}
+
+LoanedStateInterface Actuator::create_loaned_state_interface(const std::string & interface_name)
+{
+  return impl_->create_loaned_state_interface(interface_name);
+}
+
 return_type Actuator::prepare_command_mode_switch(
   const std::vector<std::string> & start_interfaces,
   const std::vector<std::string> & stop_interfaces)