Added GPIO parsing and test

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -31,13 +31,15 @@ struct InterfaceInfo
 {
   /**
    * Name of the command interfaces that can be set, e.g. "position", "velocity", etc.
-   * Used by joints.
+   * Used by joints and GPIOs.
    */
   std::string name;
   /// (Optional) Minimal allowed values of the interface.
   std::string min;
   /// (Optional) Maximal allowed values of the interface.
   std::string max;
+  /// (Optional) The datatype of the interface, e.g. "bool", "int". Defaults to double.
+  std::string data_type;
 };
 
 /**
@@ -48,16 +50,16 @@ struct ComponentInfo
 {
   /// Name of the component.
   std::string name;
-  /// Type of the component: sensor or joint.
+  /// Type of the component: sensor, joint, or GPIO.
   std::string type;
   /**
    * Name of the command interfaces that can be set, e.g. "position", "velocity", etc.
-   * Used by joints.
+   * Used by joints and GPIOs.
    */
   std::vector<InterfaceInfo> command_interfaces;
   /**
    * Name of the state interfaces that can be read, e.g. "position", "velocity", etc.
-   * Used by Joints and Sensors.
+   * Used by Joints, Sensors and GPIOs.
    */
   std::vector<InterfaceInfo> state_interfaces;
   /// (Optional) Key-value pairs of component parameters, e.g. min/max values or serial number.
@@ -84,6 +86,11 @@ struct HardwareInfo
    * Required for Sensor and optional for System Hardware.
    */
   std::vector<ComponentInfo> sensors;
+  /**
+   * Map of GPIO provided by the hardware where the key is a descriptive name of the GPIO.
+   * Optional for any hardware components.
+   */
+  std::vector<ComponentInfo> gpios;
   /**
    * Map of transmissions to calcualte ration between joints and physical actuators.
    * Optional for Actuator and System Hardware.

hardware_interface/src/component_parser.cpp

@@ -17,6 +17,7 @@
 #include <string>
 #include <unordered_map>
 #include <vector>
+#include <regex>
 
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/component_parser.hpp"
@@ -30,11 +31,14 @@ constexpr const auto kClassTypeTag = "plugin";
 constexpr const auto kParamTag = "param";
 constexpr const auto kJointTag = "joint";
 constexpr const auto kSensorTag = "sensor";
+constexpr const auto kGPIOTag = "gpio";
 constexpr const auto kTransmissionTag = "transmission";
 constexpr const auto kCommandInterfaceTag = "command_interface";
 constexpr const auto kStateInterfaceTag = "state_interface";
 constexpr const auto kMinTag = "min";
 constexpr const auto kMaxTag = "max";
+constexpr const auto kDataTypeAttribute = "data_type";
+constexpr const auto kSizeAttribute = "size";
 constexpr const auto kNameAttribute = "name";
 constexpr const auto kTypeAttribute = "type";
 }  // namespace
@@ -103,6 +107,56 @@ std::string get_attribute_value(
   return get_attribute_value(element_it, attribute_name, std::string(tag_name));
 }
 
+/// Parse optional size attribute
+/**
+ * Parses an XMLElement and returns the vector of suffixes to apply.
+ *
+ * Example behavior:
+ *  - size="3" returns the suffixes {"1","2","3"}
+ *  - size="1" returns the suffixes {"1"}
+ *  - size="" returns the suffixes {""}
+ *  - size="x y z" returns the suffixes {"x", "y", "z"}
+ *  - size="shoulder elbow wrist" returns the suffixes {"shoulder", "elbow", "wrist"}
+ *
+ *
+ * \param[in] elem XMLElement to check the size attribute to parse.
+ * \return vector of strings denoting the suffixes.
+ */
+std::vector<std::string> parse_size_attribute(
+  const tinyxml2::XMLElement * elem)
+{
+  const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kSizeAttribute);
+  std::vector<std::string> suffixes;
+  // Is the attribute null as nothing was found?
+  if (!attr) {
+    suffixes.push_back("");
+    return suffixes;
+  }
+
+  // Try to parse it as a whitespace delimited list of strings
+  std::string s = attr->Value();
+  std::regex ws("\\s+");
+  std::sregex_token_iterator it(s.begin(), s.end(), ws, -1);
+  for (; it != std::sregex_token_iterator(); it++) {
+    suffixes.push_back(it->str());
+  }
+  if (suffixes.size() == 0) {
+    // Empty value
+    suffixes.push_back("");
+  } else if (suffixes.size() == 1) {
+    // Regex used since QueryIntValue parses "0 1 2" to value=0.
+    std::regex int_re("[1-9][0-9]*");
+    if (std::regex_match(suffixes[0], int_re)) {
+      int maxval = std::stoi(suffixes[0]);
+      suffixes.clear();
+      for (int i = 1; i <= maxval; i++) {
+        suffixes.push_back(std::to_string(i));
+      }
+    }
+  }
+  return suffixes;
+}
+
 /// Search XML snippet from URDF for parameters.
 /**
  * \param[in] params_it pointer to the iterator where parameters info should be found
@@ -159,6 +213,37 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml(
     interface.max = interface_param->second;
   }
 
+  // Optional data_type attribute
+  const tinyxml2::XMLAttribute * attr;
+  attr = interfaces_it->FindAttribute(kDataTypeAttribute);
+  if (!attr) {
+    interface.data_type = "double";
+  } else {
+    interface.data_type = interfaces_it->Attribute(kDataTypeAttribute);
+  }
+
+  return interface;
+}
+
+/// Search XML snippet for interface information.
+/**
+ * \param[in] interfaces_it pointer to the iterator over the interfaces.
+ * \param[in] suffix suffix to append to the interface name.
+ * \param[in] data_type data_type of handle. Defaults to double.
+ * \return InterfaceInfo filled with information about component.
+ * \throws std::runtime_error if an interface attribute or tag is not found.
+ */
+hardware_interface::InterfaceInfo parse_interfaces_from_xml(
+  const tinyxml2::XMLElement * interfaces_it,
+  std::string suffix,
+  std::string data_type)
+{
+  hardware_interface::InterfaceInfo interface;
+  interface = parse_interfaces_from_xml(interfaces_it);
+  interface.name += suffix;
+  if (data_type != "") {
+    interface.data_type = data_type;
+  }
   return interface;
 }
 
@@ -177,17 +262,32 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
   component.type = component_it->Name();
   component.name = get_attribute_value(component_it, kNameAttribute, component.type);
 
+  // Find optional data_type specified by component
+  std::string data_type = "";
+  const char * val = component_it->Attribute(kDataTypeAttribute);
+  if (val) {
+    data_type = val;
+  }
+
   // Parse all command interfaces
   const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag);
   while (command_interfaces_it) {
-    component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it));
+    std::vector<std::string> suffixes = parse_size_attribute(command_interfaces_it);
+    for (std::string suffix : suffixes) {
+      component.command_interfaces.push_back(
+        parse_interfaces_from_xml(command_interfaces_it, suffix, data_type));
+    }
     command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag);
   }
 
   // Parse state interfaces
   const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTag);
   while (state_interfaces_it) {
-    component.state_interfaces.push_back(parse_interfaces_from_xml(state_interfaces_it));
+    std::vector<std::string> suffixes = parse_size_attribute(state_interfaces_it);
+    for (std::string suffix : suffixes) {
+      component.state_interfaces.push_back(
+        parse_interfaces_from_xml(state_interfaces_it, suffix, data_type));
+    }
     state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag);
   }
 
@@ -200,6 +300,21 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
   return component;
 }
 
+/// Search XML snippet from URDF for information about a control component.
+/**
+ * \param[in] component_it pointer to the iterator where component
+ * info should befound
+ * \param[in] suffix suffix to append to the component name.
+ */
+ComponentInfo parse_component_from_xml(
+  const tinyxml2::XMLElement * component_it,
+  std::string suffix)
+{
+  ComponentInfo component = parse_component_from_xml(component_it);
+  component.name += suffix;
+  return component;
+}
+
 /// Parse a control resource from an "ros2_control" tag.
 /**
  * \param[in] ros2_control_it pointer to ros2_control element
@@ -226,9 +341,23 @@ HardwareInfo parse_resource_from_xml(const tinyxml2::XMLElement * ros2_control_i
         hardware.hardware_parameters = parse_parameters_from_xml(params_it);
       }
     } else if (!std::string(kJointTag).compare(ros2_control_child_it->Name())) {
-      hardware.joints.push_back(parse_component_from_xml(ros2_control_child_it) );
+      std::vector<std::string> suffixes = parse_size_attribute(ros2_control_child_it);
+      for (std::string suffix : suffixes) {
+        hardware.joints.push_back(
+          parse_component_from_xml(ros2_control_child_it, suffix));
+      }
     } else if (!std::string(kSensorTag).compare(ros2_control_child_it->Name())) {
-      hardware.sensors.push_back(parse_component_from_xml(ros2_control_child_it) );
+      std::vector<std::string> suffixes = parse_size_attribute(ros2_control_child_it);
+      for (std::string suffix : suffixes) {
+        hardware.sensors.push_back(
+          parse_component_from_xml(ros2_control_child_it, suffix));
+      }
+    } else if (!std::string(kGPIOTag).compare(ros2_control_child_it->Name())) {
+      std::vector<std::string> suffixes = parse_size_attribute(ros2_control_child_it);
+      for (std::string suffix : suffixes) {
+        hardware.gpios.push_back(
+          parse_component_from_xml(ros2_control_child_it, suffix));
+      }
     } else if (!std::string(kTransmissionTag).compare(ros2_control_child_it->Name())) {
       hardware.transmissions.push_back(parse_component_from_xml(ros2_control_child_it) );
     } else {

hardware_interface/test/test_component_parser.cpp

@@ -497,3 +497,116 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only)
   ASSERT_THAT(hardware_info.transmissions[0].parameters, SizeIs(1));
   EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint_to_actuator"), "${1024/PI}");
 }
+
+TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio)
+{
+  std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head) +
+    ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio +
+    ros2_control_test_assets::urdf_tail;
+  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
+  ASSERT_THAT(control_hardware, SizeIs(1));
+  auto hardware_info = control_hardware.front();
+
+  EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO");
+  EXPECT_EQ(hardware_info.type, "system");
+  EXPECT_EQ(
+    hardware_info.hardware_class_type,
+    "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware");
+
+  ASSERT_THAT(hardware_info.joints, SizeIs(2));
+
+  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
+  EXPECT_EQ(hardware_info.joints[0].type, "joint");
+
+  EXPECT_EQ(hardware_info.joints[1].name, "joint2");
+  EXPECT_EQ(hardware_info.joints[1].type, "joint");
+
+  ASSERT_THAT(hardware_info.gpios, SizeIs(2));
+
+  EXPECT_EQ(hardware_info.gpios[0].name, "flange_analog_IOs");
+  EXPECT_EQ(hardware_info.gpios[0].type, "gpio");
+  EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(3));
+  EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(1));
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_output1");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "analog_input1");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[2].name, "analog_input2");
+
+  EXPECT_EQ(hardware_info.gpios[1].name, "flange_vacuum");
+  EXPECT_EQ(hardware_info.gpios[1].type, "gpio");
+  EXPECT_THAT(hardware_info.gpios[1].state_interfaces, SizeIs(1));
+  EXPECT_THAT(hardware_info.gpios[1].command_interfaces, SizeIs(1));
+  EXPECT_EQ(hardware_info.gpios[1].state_interfaces[0].name, "vacuum");
+  EXPECT_EQ(hardware_info.gpios[1].command_interfaces[0].name, "vacuum");
+}
+
+TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_data_type)
+{
+  std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head) +
+    ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_size_and_data_type +
+    ros2_control_test_assets::urdf_tail;
+  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
+  ASSERT_THAT(control_hardware, SizeIs(1));
+  auto hardware_info = control_hardware.front();
+
+  EXPECT_EQ(hardware_info.name, "RRBotSystemWithSizeAndDataType");
+  EXPECT_EQ(hardware_info.type, "system");
+  EXPECT_EQ(
+    hardware_info.hardware_class_type,
+    "ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType");
+
+  ASSERT_THAT(hardware_info.joints, SizeIs(2));
+
+  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
+  EXPECT_EQ(hardware_info.joints[0].type, "joint");
+  EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1));
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double");
+  EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double");
+  EXPECT_EQ(hardware_info.joints[1].name, "joint2");
+  EXPECT_EQ(hardware_info.joints[1].type, "joint");
+  EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1));
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double");
+  EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1));
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double");
+
+  ASSERT_THAT(hardware_info.gpios, SizeIs(1));
+
+  EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS");
+  EXPECT_EQ(hardware_info.gpios[0].type, "gpio");
+  EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(2));
+  EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output1");
+  EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool");
+  EXPECT_EQ(hardware_info.gpios[0].command_interfaces[1].name, "digital_output2");
+  EXPECT_EQ(hardware_info.gpios[0].command_interfaces[1].data_type, "bool");
+  EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(3));
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input0");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "analog_input1");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "double");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[2].name, "analog_input2");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[2].data_type, "double");
+
+  ASSERT_THAT(hardware_info.sensors, SizeIs(2));
+
+  EXPECT_EQ(hardware_info.sensors[0].name, "ft_ee");
+  EXPECT_EQ(hardware_info.sensors[0].type, "sensor");
+  EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(6));
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0].name, "force.x");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0].data_type, "double");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[1].name, "force.y");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[1].data_type, "double");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[2].name, "force.z");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[2].data_type, "double");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[3].name, "torque.x");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[3].data_type, "double");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[4].name, "torque.y");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[4].data_type, "double");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[5].name, "torque.z");
+  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[5].data_type, "double");
+}