diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 41a5cb4005..931f7159df 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/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 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 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 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 gpios; /** * Map of transmissions to calcualte ration between joints and physical actuators. * Optional for Actuator and System Hardware. diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 862058eb33..5cdcbbe645 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #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 { diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 6d2ccde2a7..55891d0a59 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/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); +} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 526bb8663a..ba53d1a415 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -332,6 +332,63 @@ const auto valid_urdf_ros2_control_actuator_only = )"; +// 10. Industrial Robots with integrated GPIO +const auto valid_urdf_ros2_control_system_robot_with_gpio = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + +)"; + +// 11. Industrial Robots using size and data_type attributes +const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType + 2 + 2 + + + + + + + + + + + +)"; + // Errors const auto invalid_urdf_ros2_control_invalid_child = R"( @@ -437,6 +494,29 @@ const auto invalid_urdf_ros2_control_parameter_empty = )"; +const auto invalid_urdf2_ros2_control_illegal_size = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithIllegalSize + + + + + +)"; + +const auto invalid_urdf2_ros2_control_illegal_size2 = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithIllegalSize2 + + + + + +)"; } // namespace ros2_control_test_assets #endif // ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_