move parsing of interface description to component parser

hardware_interface/include/hardware_interface/component_parser.hpp

@@ -33,6 +33,51 @@ namespace hardware_interface
 HARDWARE_INTERFACE_PUBLIC
 std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf);
 
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's SommandInterfaces for the joints
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_joint_state_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info);
+
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's SommandInterfaces for the sensors
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_sensor_state_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info);
+
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's SommandInterfaces for the gpios
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_gpio_state_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info);
+
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's CommandInterfaces for the joints
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_joint_command_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info);
+
+/**
+ * \param[in] hw_info the hardware description
+ * \return vector filled with information about robot's CommandInterfaces for the gpios
+ * which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+std::vector<InterfaceDescription> parse_gpio_command_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info);
+
 HARDWARE_INTERFACE_PUBLIC
 bool parse_bool(const std::string & bool_string);
 

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -118,10 +118,19 @@ struct InterfaceDescription
    */
   std::string prefix_name;
 
+  /**
+   * What type of component is exported: joint, sensor or gpio
+   */
+  std::string component_type;
+
+  /**
+   * Information about the Interface type (position, velocity,...) as well as limits and so on.
+   */
   InterfaceInfo interface_info;
 
   std::string get_name() const { return prefix_name + "/" + interface_info.name; }
-  std::string get_type() const { return interface_info.name; }
+
+  std::string get_interface_type() const { return interface_info.name; }
 };
 
 /// This structure stores information about hardware defined in a robot's URDF.

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -15,11 +15,13 @@
 #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_
 #define HARDWARE_INTERFACE__SYSTEM_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"
@@ -107,47 +109,18 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
 
   virtual void add_state_interface_descriptions()
   {
-    for (const auto & joint : info_.joints)
-    {
-      for (const auto & state_interface : joint.state_interfaces)
-      {
-        joint_states_descr_.emplace_back(InterfaceDescription(joint.name, state_interface));
-      }
-    }
-
-    for (const auto & sensor : info_.sensors)
-    {
-      for (const auto & state_interface : sensor.state_interfaces)
-      {
-        sensor_states_descr_.emplace_back(InterfaceDescription(sensor.name, state_interface));
-      }
-    }
-
-    for (const auto & gpio : info_.gpios)
-    {
-      for (const auto & state_interface : gpio.state_interfaces)
-      {
-        gpio_states_descr_.emplace_back(InterfaceDescription(gpio.name, state_interface));
-      }
-    }
+    joint_states_descriptions_ = parse_joint_state_interface_descriptions_from_hardware_info(info_);
+    gpio_states_descriptions_ = parse_gpio_state_interface_descriptions_from_hardware_info(info_);
+    sensor_states_descriptions_ =
+      parse_sensor_state_interface_descriptions_from_hardware_info(info_);
   }
 
   virtual void add_command_interface_descriptions()
   {
-    for (const auto & joint : info_.joints)
-    {
-      for (const auto & command_interface : joint.command_interfaces)
-      {
-        joint_commands_descr_.emplace_back(InterfaceDescription(joint.name, command_interface));
-      }
-    }
-    for (const auto & gpio : info_.gpios)
-    {
-      for (const auto & command_interface : gpio.command_interfaces)
-      {
-        gpio_commands_descr_.emplace_back(InterfaceDescription(gpio.name, command_interface));
-      }
-    }
+    joint_commands_descriptions_ =
+      parse_joint_command_interface_descriptions_from_hardware_info(info_);
+    gpio_commands_descriptions_ =
+      parse_gpio_command_interface_descriptions_from_hardware_info(info_);
   }
 
   /// Exports all state interfaces for this hardware interface.
@@ -162,9 +135,9 @@ 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_descr_.size());
+    state_interfaces.reserve(joint_states_descriptions_.size());
 
-    for (const auto & descr : joint_states_descr_)
+    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()]));
@@ -185,9 +158,9 @@ 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_descr_.size());
+    command_interfaces.reserve(joint_commands_descriptions_.size());
 
-    for (const auto & descr : joint_commands_descr_)
+    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()]));
@@ -328,13 +301,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
 
 protected:
   HardwareInfo info_;
-  std::vector<InterfaceDescription> joint_states_descr_;
-  std::vector<InterfaceDescription> joint_commands_descr_;
+  std::vector<InterfaceDescription> joint_states_descriptions_;
+  std::vector<InterfaceDescription> joint_commands_descriptions_;
 
-  std::vector<InterfaceDescription> sensor_states_descr_;
+  std::vector<InterfaceDescription> sensor_states_descriptions_;
 
-  std::vector<InterfaceDescription> gpio_states_descr_;
-  std::vector<InterfaceDescription> gpio_commands_descr_;
+  std::vector<InterfaceDescription> gpio_states_descriptions_;
+  std::vector<InterfaceDescription> gpio_commands_descriptions_;
 
 private:
   std::map<std::string, double> joint_states_;

hardware_interface/src/component_parser.cpp

@@ -621,4 +621,91 @@ bool parse_bool(const std::string & bool_string)
   return bool_string == "true" || bool_string == "True";
 }
 
+std::vector<InterfaceDescription> parse_joint_state_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info)
+{
+  std::vector<InterfaceDescription> joint_state_interface_descriptions;
+  joint_state_interface_descriptions.reserve(hw_info.joints.size());
+
+  for (const auto & joint : hw_info.joints)
+  {
+    for (const auto & state_interface : joint.state_interfaces)
+    {
+      joint_state_interface_descriptions.emplace_back(
+        InterfaceDescription(joint.name, state_interface));
+    }
+  }
+  return joint_state_interface_descriptions;
+}
+
+std::vector<InterfaceDescription> parse_sensor_state_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info)
+{
+  std::vector<InterfaceDescription> sensor_state_interface_descriptions;
+  sensor_state_interface_descriptions.reserve(hw_info.sensors.size());
+
+  for (const auto & sensor : hw_info.sensors)
+  {
+    for (const auto & state_interface : sensor.state_interfaces)
+    {
+      sensor_state_interface_descriptions.emplace_back(
+        InterfaceDescription(sensor.name, state_interface));
+    }
+  }
+  return sensor_state_interface_descriptions;
+}
+
+std::vector<InterfaceDescription> parse_gpio_state_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info)
+{
+  std::vector<InterfaceDescription> gpio_state_interface_descriptions;
+  gpio_state_interface_descriptions.reserve(hw_info.gpios.size());
+
+  for (const auto & gpio : hw_info.gpios)
+  {
+    for (const auto & state_interface : gpio.state_interfaces)
+    {
+      gpio_state_interface_descriptions.emplace_back(
+        InterfaceDescription(gpio.name, state_interface));
+    }
+  }
+  return gpio_state_interface_descriptions;
+}
+
+std::vector<InterfaceDescription> parse_joint_command_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info)
+{
+  std::vector<InterfaceDescription>
+    gpio_state_intejoint_command_interface_descriptionsrface_descriptions;
+  gpio_state_intejoint_command_interface_descriptionsrface_descriptions.reserve(
+    hw_info.joints.size());
+
+  for (const auto & joint : hw_info.joints)
+  {
+    for (const auto & command_interface : joint.command_interfaces)
+    {
+      gpio_state_intejoint_command_interface_descriptionsrface_descriptions.emplace_back(
+        InterfaceDescription(joint.name, command_interface));
+    }
+  }
+  return gpio_state_intejoint_command_interface_descriptionsrface_descriptions;
+}
+
+std::vector<InterfaceDescription> parse_gpio_command_interface_descriptions_from_hardware_info(
+  const HardwareInfo & hw_info)
+{
+  std::vector<InterfaceDescription> gpio_command_interface_descriptions;
+  gpio_command_interface_descriptions.reserve(hw_info.gpios.size());
+
+  for (const auto & gpio : hw_info.gpios)
+  {
+    for (const auto & command_interface : gpio.command_interfaces)
+    {
+      gpio_command_interface_descriptions.emplace_back(
+        InterfaceDescription(gpio.name, command_interface));
+    }
+  }
+  return gpio_command_interface_descriptions;
+}
+
 }  // namespace hardware_interface