Skip to content

Commit

Permalink
Added GPIO parsing and test (#436)
Browse files Browse the repository at this point in the history 
* Added support for GPIO tag in URDF.

Co-authored-by: Denis Štogl <destogl@users.noreply.github.com>
  • Loading branch information
@mahaarbo @destogl
mahaarbo and destogl authored Jul 4, 2021
1 parent 6328200 commit 949aa8c
Show file tree
Hide file tree
Showing 4 changed files with 311 additions and 4 deletions.
17 changes: 13 additions & 4 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};

/**
Expand All @@ -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.
Expand All @@ -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.
Expand Down
112 changes: 112 additions & 0 deletions hardware_interface/src/component_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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 {
Expand Down
106 changes: 106 additions & 0 deletions hardware_interface/test/test_component_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Loading

0 comments on commit 949aa8c

Please sign in to comment.