adjusted actuator- and sensor_interface as well

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -15,10 +15,13 @@
 #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
 
+#include <limits>
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
@@ -97,30 +100,85 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
     info_ = hardware_info;
+    import_state_interface_descriptions(info_);
+    import_command_interface_descriptions(info_);
     return CallbackReturn::SUCCESS;
   };
 
+  /**
+   * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
+   * Separate them into the possible types: Joint and store them.
+   */
+  virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info)
+  {
+    joint_states_descriptions_ =
+      parse_joint_state_interface_descriptions_from_hardware_info(hardware_info);
+  }
+
+  /**
+   * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo.
+   * Separate them into the possible types: Joint and store them.
+   */
+  virtual void import_command_interface_descriptions(const HardwareInfo & hardware_info)
+  {
+    joint_commands_descriptions_ =
+      parse_joint_command_interface_descriptions_from_hardware_info(hardware_info);
+  }
+
   /// Exports all state interfaces for this hardware interface.
   /**
+   * Default implementation for exporting the StateInterfaces. The StateInterfaces are created
+   * according to the InterfaceDescription. The memory accessed by the controllers and hardware is
+   * assigned here and resides in the system_interface.
+   *
+   * If overwritten:
    * The state interfaces have to be created and transferred according
    * to the hardware info passed in for the configuration.
    *
    * Note the ownership over the state interfaces is transferred to the caller.
    *
    * \return vector of state interfaces
    */
-  virtual std::vector<StateInterface> export_state_interfaces() = 0;
+  virtual std::vector<StateInterface> export_state_interfaces()
+  {
+    std::vector<hardware_interface::StateInterface> state_interfaces;
+    state_interfaces.reserve(joint_states_descriptions_.size());
+
+    for (const auto & descr : joint_states_descriptions_)
+    {
+      joint_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
+      state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()]));
+    }
+
+    return state_interfaces;
+  }
 
   /// Exports all command interfaces for this hardware interface.
   /**
+   * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created
+   * according to the InterfaceDescription. The memory accessed by the controllers and hardware is
+   * assigned here and resides in the system_interface.
+   *
    * The command interfaces have to be created and transferred according
    * to the hardware info passed in for the configuration.
    *
    * Note the ownership over the state interfaces is transferred to the caller.
    *
    * \return vector of command interfaces
    */
-  virtual std::vector<CommandInterface> export_command_interfaces() = 0;
+  virtual std::vector<CommandInterface> export_command_interfaces()
+  {
+    std::vector<hardware_interface::CommandInterface> command_interfaces;
+    command_interfaces.reserve(joint_commands_descriptions_.size());
+
+    for (const auto & descr : joint_commands_descriptions_)
+    {
+      joint_commands_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
+      command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()]));
+    }
+
+    return command_interfaces;
+  }
 
   /// Prepare for a new command interface switch.
   /**
@@ -202,8 +260,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());
+  }
+
+  void joint_state_set_value(const InterfaceDescription & interface_descr, const double & value)
+  {
+    joint_states_[interface_descr.get_name()] = value;
+  }
+
+  double joint_command_get_value(const InterfaceDescription & interface_descr) const
+  {
+    return joint_commands_.at(interface_descr.get_name());
+  }
+
+  void joint_command_set_value(const InterfaceDescription & interface_descr, const double & value)
+  {
+    joint_commands_[interface_descr.get_name()] = value;
+  }
+
 protected:
   HardwareInfo info_;
+  std::vector<InterfaceDescription> joint_states_descriptions_;
+  std::vector<InterfaceDescription> joint_commands_descriptions_;
+
+private:
+  std::map<std::string, double> joint_states_;
+  std::map<std::string, double> joint_commands_;
+
   rclcpp_lifecycle::State lifecycle_state_;
 };
 

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -15,10 +15,13 @@
 #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
 
+#include <limits>
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
@@ -97,19 +100,47 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
     info_ = hardware_info;
+    import_state_interface_descriptions(info_);
     return CallbackReturn::SUCCESS;
   };
 
+  /**
+   * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
+   * Separate them into the possible types: Sensor and store them.
+   */
+  virtual void import_state_interface_descriptions(const HardwareInfo & hardware_info)
+  {
+    sensor_states_descriptions_ =
+      parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info);
+  }
+
   /// Exports all state interfaces for this hardware interface.
   /**
+   * Default implementation for exporting the StateInterfaces. The StateInterfaces are created
+   * according to the InterfaceDescription. The memory accessed by the controllers and hardware is
+   * assigned here and resides in the system_interface.
+   *
+   * If overwritten:
    * The state interfaces have to be created and transferred according
    * to the hardware info passed in for the configuration.
    *
    * Note the ownership over the state interfaces is transferred to the caller.
    *
    * \return vector of state interfaces
    */
-  virtual std::vector<StateInterface> export_state_interfaces() = 0;
+  virtual std::vector<StateInterface> export_state_interfaces()
+  {
+    std::vector<hardware_interface::StateInterface> state_interfaces;
+    state_interfaces.reserve(sensor_states_descriptions_.size());
+
+    for (const auto & descr : sensor_states_descriptions_)
+    {
+      sensor_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
+      state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()]));
+    }
+
+    return state_interfaces;
+  }
 
   /// Read the current state values from the actuator.
   /**
@@ -141,8 +172,24 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
 
+  double sensor_state_get_value(const InterfaceDescription & interface_descr) const
+  {
+    return sensor_states_.at(interface_descr.get_name());
+  }
+
+  void sensor_state_set_value(const InterfaceDescription & interface_descr, const double & value)
+  {
+    sensor_states_[interface_descr.get_name()] = value;
+  }
+
 protected:
   HardwareInfo info_;
+
+  std::vector<InterfaceDescription> sensor_states_descriptions_;
+
+private:
+  std::map<std::string, double> sensor_states_;
+
   rclcpp_lifecycle::State lifecycle_state_;
 };
 

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -102,29 +102,44 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
     info_ = hardware_info;
-    add_state_interface_descriptions();
-    add_command_interface_descriptions();
+    import_state_interface_descriptions(info_);
+    import_command_interface_descriptions(info_);
     return CallbackReturn::SUCCESS;
   };
 
-  virtual void add_state_interface_descriptions()
+  /**
+   * Import the InterfaceDescription for the StateInterfaces from the HardwareInfo.
+   * Separate them into the possible types: Joint, GPIO, Sensor and store them.
+   */
+  void import_state_interface_descriptions(const HardwareInfo & hardware_info)
   {
-    joint_states_descriptions_ = parse_joint_state_interface_descriptions_from_hardware_info(info_);
-    gpio_states_descriptions_ = parse_gpio_state_interface_descriptions_from_hardware_info(info_);
+    joint_states_descriptions_ =
+      parse_joint_state_interface_descriptions_from_hardware_info(hardware_info);
+    gpio_states_descriptions_ =
+      parse_gpio_state_interface_descriptions_from_hardware_info(hardware_info);
     sensor_states_descriptions_ =
-      parse_sensor_state_interface_descriptions_from_hardware_info(info_);
+      parse_sensor_state_interface_descriptions_from_hardware_info(hardware_info);
   }
 
-  virtual void add_command_interface_descriptions()
+  /**
+   * Import the InterfaceDescription for the CommandInterfaces from the HardwareInfo.
+   * Separate them into the possible types: Joint and GPIO and store them.
+   */
+  void import_command_interface_descriptions(const HardwareInfo & hardware_info)
   {
     joint_commands_descriptions_ =
-      parse_joint_command_interface_descriptions_from_hardware_info(info_);
+      parse_joint_command_interface_descriptions_from_hardware_info(hardware_info);
     gpio_commands_descriptions_ =
-      parse_gpio_command_interface_descriptions_from_hardware_info(info_);
+      parse_gpio_command_interface_descriptions_from_hardware_info(hardware_info);
   }
 
   /// Exports all state interfaces for this hardware interface.
   /**
+   * Default implementation for exporting the StateInterfaces. The StateInterfaces are created
+   * according to the InterfaceDescription. The memory accessed by the controllers and hardware is
+   * assigned here and resides in the system_interface.
+   *
+   * If overwritten:
    * The state interfaces have to be created and transferred according
    * to the hardware info passed in for the configuration.
    *
@@ -135,19 +150,37 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   virtual std::vector<StateInterface> export_state_interfaces()
   {
     std::vector<hardware_interface::StateInterface> state_interfaces;
-    state_interfaces.reserve(joint_states_descriptions_.size());
+    state_interfaces.reserve(
+      joint_states_descriptions_.size() + sensor_states_descriptions_.size() +
+      gpio_states_descriptions_.size());
 
     for (const auto & descr : joint_states_descriptions_)
     {
       joint_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
       state_interfaces.emplace_back(StateInterface(descr, &joint_states_[descr.get_name()]));
     }
 
+    for (const auto & descr : sensor_states_descriptions_)
+    {
+      sensor_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
+      state_interfaces.emplace_back(StateInterface(descr, &sensor_states_[descr.get_name()]));
+    }
+
+    for (const auto & descr : gpio_states_descriptions_)
+    {
+      gpio_states_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
+      state_interfaces.emplace_back(StateInterface(descr, &gpio_states_[descr.get_name()]));
+    }
+
     return state_interfaces;
   }
 
   /// Exports all command interfaces for this hardware interface.
   /**
+   * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created
+   * according to the InterfaceDescription. The memory accessed by the controllers and hardware is
+   * assigned here and resides in the system_interface.
+   *
    * The command interfaces have to be created and transferred according
    * to the hardware info passed in for the configuration.
    *
@@ -158,14 +191,21 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   virtual std::vector<CommandInterface> export_command_interfaces()
   {
     std::vector<hardware_interface::CommandInterface> command_interfaces;
-    command_interfaces.reserve(joint_commands_descriptions_.size());
+    command_interfaces.reserve(
+      joint_commands_descriptions_.size() + gpio_commands_descriptions_.size());
 
     for (const auto & descr : joint_commands_descriptions_)
     {
       joint_commands_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
       command_interfaces.emplace_back(CommandInterface(descr, &joint_commands_[descr.get_name()]));
     }
 
+    for (const auto & descr : gpio_commands_descriptions_)
+    {
+      gpio_commands_[descr.get_name()] = std::numeric_limits<double>::quiet_NaN();
+      command_interfaces.emplace_back(CommandInterface(descr, &gpio_commands_[descr.get_name()]));
+    }
+
     return command_interfaces;
   }