Added GPIO parsing and test (#436)

* Added support for GPIO tag in URDF.

Co-authored-by: Denis Štogl <[email protected]>

Author: mahaarbo

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -31,13 +31,17 @@ 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". Used by GPIOs.
+  std::string data_type;
+  /// (Optional) If the handle is an array, the size of the array. Used by GPIOs.
+  int size;
 };
 
 /**
@@ -48,16 +52,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 +88,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,61 @@ 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 value of the size attribute.
+ * If not specified, defaults to 1. If not given a positive integer, throws an error.
+ *
+ * \param[in] elem XMLElement that has the size attribute.
+ * \return The size.
+ * \throws std::runtime_error if not given a positive non-zero integer as value.
+ */
+std::size_t parse_size_attribute(
+  const tinyxml2::XMLElement * elem)
+{
+  const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kSizeAttribute);
+
+  if (!attr) {
+    return 1;
+  }
+
+  std::size_t size;
+  // Regex used to check for non-zero positive int
+  std::string s = attr->Value();
+  std::regex int_re("[1-9][0-9]*");
+  if (std::regex_match(s, int_re)) {
+    size = std::stoi(s);
+  } else {
+    throw std::runtime_error(
+            "Could not parse size tag in \"" + std::string(elem->Name()) + "\"." +
+            "Got \"" + s + "\", but expected a non-zero positive integer.");
+  }
+
+  return size;
+}
+
+/// Parse data_type attribute
+/**
+ * Parses an XMLElement and returns the value of the data_type attribute.
+ * Defaults to "double" if not specified.
+ *
+ * \param[in] elem XMLElement that has the data_type attribute.
+ * \return string specifying the data type.
+ */
+std::string parse_data_type_attribute(
+  const tinyxml2::XMLElement * elem)
+{
+  const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kDataTypeAttribute);
+  std::string data_type;
+  if (!attr) {
+    data_type = "double";
+  } else {
+    data_type = attr->Value();
+  }
+
+  return data_type;
+}
+
 /// Search XML snippet from URDF for parameters.
 /**
  * \param[in] params_it pointer to the iterator where parameters info should be found
@@ -159,6 +218,10 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml(
     interface.max = interface_param->second;
   }
 
+  // Default to a single double
+  interface.data_type = "double";
+  interface.size = 1;
+
   return interface;
 }
 
@@ -200,6 +263,52 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it
   return component;
 }
 
+/// Search XML snippet from URDF for information about a complex component.
+/**
+ * A complex component can have a non-double data type specified on its interfaces,
+ *  and the interface may be an array of a fixed size of the data type.
+ *
+ * \param[in] component_it pointer to the iterator where component
+ * info should befound
+ * \throws std::runtime_error if a required component attribute or tag is not found.
+ */
+ComponentInfo parse_complex_component_from_xml(
+  const tinyxml2::XMLElement * component_it)
+{
+  ComponentInfo component;
+
+  // Find name, type and class of a component
+  component.type = component_it->Name();
+  component.name = get_attribute_value(component_it, kNameAttribute, component.type);
+
+  // 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));
+    component.command_interfaces.back().data_type =
+      parse_data_type_attribute(command_interfaces_it);
+    component.command_interfaces.back().size = parse_size_attribute(command_interfaces_it);
+    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));
+    component.state_interfaces.back().data_type = parse_data_type_attribute(state_interfaces_it);
+    component.state_interfaces.back().size = parse_size_attribute(state_interfaces_it);
+    state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag);
+  }
+
+  // Parse parameters
+  const auto * params_it = component_it->FirstChildElement(kParamTag);
+  if (params_it) {
+    component.parameters = parse_parameters_from_xml(params_it);
+  }
+
+  return component;
+}
+
 /// Parse a control resource from an "ros2_control" tag.
 /**
  * \param[in] ros2_control_it pointer to ros2_control element
@@ -229,6 +338,9 @@ HardwareInfo parse_resource_from_xml(const tinyxml2::XMLElement * ros2_control_i
       hardware.joints.push_back(parse_component_from_xml(ros2_control_child_it) );
     } else if (!std::string(kSensorTag).compare(ros2_control_child_it->Name())) {
       hardware.sensors.push_back(parse_component_from_xml(ros2_control_child_it) );
+    } else if (!std::string(kGPIOTag).compare(ros2_control_child_it->Name())) {
+      hardware.gpios.push_back(
+        parse_complex_component_from_xml(ros2_control_child_it));
     } 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,109 @@ 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(1));
+
+  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_EQ(hardware_info.joints[0].command_interfaces[0].size, 1);
+  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[0].state_interfaces[0].size, 1);
+
+  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(1));
+  EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output");
+  EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool");
+  EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].size, 2);
+  EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(2));
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].size, 3);
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "image");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat");
+  EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1);
+}
+
+TEST_F(TestComponentParser, negative_size_throws_error)
+{
+  std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head) +
+    ros2_control_test_assets::invalid_urdf2_ros2_control_illegal_size +
+    ros2_control_test_assets::urdf_tail;
+  ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}
+
+TEST_F(TestComponentParser, noninteger_size_throws_error)
+{
+  std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head) +
+    ros2_control_test_assets::invalid_urdf2_ros2_control_illegal_size2 +
+    ros2_control_test_assets::urdf_tail;
+  ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
+}