add parse_state_interface_descriptions and parse_command_interface_description and reuse components

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -123,39 +123,11 @@ 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_);
+    parse_state_interface_descriptions(info_.joints, joint_state_interfaces_);
+    parse_command_interface_descriptions(info_.joints, joint_command_interfaces_);
     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)
-  {
-    auto joint_state_interface_descriptions =
-      parse_state_interface_descriptions(hardware_info.joints);
-    for (const auto & description : joint_state_interface_descriptions)
-    {
-      joint_state_interfaces_.insert(std::make_pair(description.get_name(), description));
-    }
-  }
-
-  /**
-   * 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)
-  {
-    auto joint_command_interface_descriptions =
-      parse_command_interface_descriptions(hardware_info.joints);
-    for (const auto & description : joint_command_interface_descriptions)
-    {
-      joint_command_interfaces_.insert(std::make_pair(description.get_name(), description));
-    }
-  }
-
   /// Exports all state interfaces for this hardware interface.
   /**
    * Old way of exporting the StateInterfaces. If a empty vector is returned then

hardware_interface/include/hardware_interface/component_parser.hpp

@@ -16,6 +16,7 @@
 #define HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
 
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "hardware_interface/hardware_info.hpp"
@@ -41,6 +42,16 @@ HARDWARE_INTERFACE_PUBLIC
 std::vector<InterfaceDescription> parse_state_interface_descriptions(
   const std::vector<ComponentInfo> & component_info);
 
+/**
+ * \param[in] component_info information about a component (gpio, joint, sensor)
+ * \param[out] state_interfaces_map unordered_map filled with information about hardware's
+ * StateInterfaces for the component which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+void parse_state_interface_descriptions(
+  const std::vector<ComponentInfo> & component_info,
+  std::unordered_map<std::string, InterfaceDescription> & state_interfaces_map);
+
 /**
  * \param[in] component_info information about a component (gpio, joint, sensor)
  * \return vector filled with information about hardware's CommandInterfaces for the component
@@ -50,5 +61,14 @@ HARDWARE_INTERFACE_PUBLIC
 std::vector<InterfaceDescription> parse_command_interface_descriptions(
   const std::vector<ComponentInfo> & component_info);
 
+/**
+ * \param[in] component_info information about a component (gpio, joint, sensor)
+ * \param[out] command_interfaces_map unordered_map filled with information about hardware's
+ * CommandInterfaces for the component which are exported
+ */
+HARDWARE_INTERFACE_PUBLIC
+void parse_command_interface_descriptions(
+  const std::vector<ComponentInfo> & component_info,
+  std::unordered_map<std::string, InterfaceDescription> & command_interfaces_map);
 }  // namespace hardware_interface
 #endif  // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -123,24 +123,10 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
     info_ = hardware_info;
-    import_state_interface_descriptions(info_);
+    parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_);
     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)
-  {
-    auto sensor_state_interface_descriptions =
-      parse_state_interface_descriptions(hardware_info.sensors);
-    for (const auto & description : sensor_state_interface_descriptions)
-    {
-      sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description));
-    }
-  }
-
   /// Exports all state interfaces for this hardware interface.
   /**
    * Old way of exporting the StateInterfaces. If a empty vector is returned then

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -126,57 +126,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
     info_ = hardware_info;
-    import_state_interface_descriptions(info_);
-    import_command_interface_descriptions(info_);
+    parse_state_interface_descriptions(info_.joints, joint_state_interfaces_);
+    parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_);
+    parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_);
+    parse_command_interface_descriptions(info_.joints, joint_command_interfaces_);
+    parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_);
     return CallbackReturn::SUCCESS;
   };
 
-  /**
-   * 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)
-  {
-    auto joint_state_interface_descriptions =
-      parse_state_interface_descriptions(hardware_info.joints);
-    for (const auto & description : joint_state_interface_descriptions)
-    {
-      joint_state_interfaces_.insert(std::make_pair(description.get_name(), description));
-    }
-    auto sensor_state_interface_descriptions =
-      parse_state_interface_descriptions(hardware_info.sensors);
-    for (const auto & description : sensor_state_interface_descriptions)
-    {
-      sensor_state_interfaces_.insert(std::make_pair(description.get_name(), description));
-    }
-    auto gpio_state_interface_descriptions =
-      parse_state_interface_descriptions(hardware_info.gpios);
-    for (const auto & description : gpio_state_interface_descriptions)
-    {
-      gpio_state_interfaces_.insert(std::make_pair(description.get_name(), description));
-    }
-  }
-
-  /**
-   * 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)
-  {
-    auto joint_command_interface_descriptions =
-      parse_command_interface_descriptions(hardware_info.joints);
-    for (const auto & description : joint_command_interface_descriptions)
-    {
-      joint_command_interfaces_.insert(std::make_pair(description.get_name(), description));
-    }
-    auto gpio_command_interface_descriptions =
-      parse_command_interface_descriptions(hardware_info.gpios);
-    for (const auto & description : gpio_command_interface_descriptions)
-    {
-      gpio_command_interfaces_.insert(std::make_pair(description.get_name(), description));
-    }
-  }
-
   /// Exports all state interfaces for this hardware interface.
   /**
    * Old way of exporting the StateInterfaces. If a empty vector is returned then

hardware_interface/src/component_parser.cpp

@@ -929,6 +929,22 @@ std::vector<InterfaceDescription> parse_state_interface_descriptions(
   return component_state_interface_descriptions;
 }
 
+void parse_state_interface_descriptions(
+  const std::vector<ComponentInfo> & component_info,
+  std::unordered_map<std::string, InterfaceDescription> & state_interfaces_map)
+{
+  state_interfaces_map.reserve(state_interfaces_map.size() + component_info.size());
+
+  for (const auto & component : component_info)
+  {
+    for (const auto & state_interface : component.state_interfaces)
+    {
+      InterfaceDescription description(component.name, state_interface);
+      state_interfaces_map.insert(std::make_pair(description.get_name(), description));
+    }
+  }
+}
+
 std::vector<InterfaceDescription> parse_command_interface_descriptions(
   const std::vector<ComponentInfo> & component_info)
 {
@@ -946,4 +962,20 @@ std::vector<InterfaceDescription> parse_command_interface_descriptions(
   return component_command_interface_descriptions;
 }
 
+void parse_command_interface_descriptions(
+  const std::vector<ComponentInfo> & component_info,
+  std::unordered_map<std::string, InterfaceDescription> & command_interfaces_map)
+{
+  command_interfaces_map.reserve(command_interfaces_map.size() + component_info.size());
+
+  for (const auto & component : component_info)
+  {
+    for (const auto & command_interface : component.command_interfaces)
+    {
+      InterfaceDescription description(component.name, command_interface);
+      command_interfaces_map.insert(std::make_pair(description.get_name(), description));
+    }
+  }
+}
+
 }  // namespace hardware_interface