From a0ed63e68a29f9121289f148654eb7b00e44aceb Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 23 Oct 2020 14:31:49 -0700 Subject: [PATCH 1/3] adapt component parser to new xml schema Signed-off-by: Karsten Knese --- .../hardware_interface/component_info.hpp | 6 +- hardware_interface/src/component_parser.cpp | 104 ++--- .../test/test_component_parser.cpp | 392 ++++++++---------- 3 files changed, 220 insertions(+), 282 deletions(-) diff --git a/hardware_interface/include/hardware_interface/component_info.hpp b/hardware_interface/include/hardware_interface/component_info.hpp index cd9be6b9ca..258ada906a 100644 --- a/hardware_interface/include/hardware_interface/component_info.hpp +++ b/hardware_interface/include/hardware_interface/component_info.hpp @@ -54,13 +54,9 @@ struct ComponentInfo */ std::string name; /** - * \brief type of the component: sensor or actuator. + * \brief type of the component: sensor or joint. */ std::string type; - /** - * \brief component's class, which will be dynamically loaded. - */ - std::string class_type; /** * \brief name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints. diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 1c1915ae4d..7bd1f4d2ad 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -27,15 +27,17 @@ namespace constexpr const auto kRobotTag = "robot"; constexpr const auto kROS2ControlTag = "ros2_control"; constexpr const auto kHardwareTag = "hardware"; -constexpr const auto kClassTypeTag = "classType"; +constexpr const auto kClassTypeTag = "plugin"; constexpr const auto kParamTag = "param"; constexpr const auto kJointTag = "joint"; constexpr const auto kSensorTag = "sensor"; constexpr const auto kTransmissionTag = "transmission"; -constexpr const auto kCommandInterfaceTypeTag = "commandInterfaceType"; -constexpr const auto kStateInterfaceTypeTag = "stateInterfaceType"; +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 kNameAttribute = "name"; +constexpr const auto kTypeAttribute = "type"; } // namespace namespace hardware_interface @@ -118,11 +120,11 @@ std::unordered_map parse_parameters_from_xml( while (params_it) { // Fill the map with parameters - attr = params_it->FindAttribute("name"); + attr = params_it->FindAttribute(kNameAttribute); if (!attr) { throw std::runtime_error("no parameter name attribute set in param tag"); } - const std::string parameter_name = params_it->Attribute("name"); + const std::string parameter_name = params_it->Attribute(kNameAttribute); const std::string parameter_value = get_text_for_element(params_it, parameter_name); parameters[parameter_name] = parameter_value; @@ -139,47 +141,29 @@ std::unordered_map parse_parameters_from_xml( * \return std::vector< std::__cxx11::string > list of interface types * \throws std::runtime_error if the interfaceType text not set in a tag */ -std::vector parse_interfaces_from_xml( - const tinyxml2::XMLElement * interfaces_it, const char * interfaceTag) +components::InterfaceInfo parse_interfaces_from_xml( + const tinyxml2::XMLElement * interfaces_it) { - std::vector interfaces; - - while (interfaces_it) { - hardware_interface::InterfaceInfo interface; - - // Joint interfaces have a name attribute - if (std::string(interfaceTag) == "commandInterfaceType") { - const std::string interface_name = get_attribute_value( - interfaces_it, "name", - std::string(interfaceTag)); - interface.name = interface_name; - - // Optional min/max attributes - std::unordered_map interface_params = - parse_parameters_from_xml(interfaces_it->FirstChildElement(kParamTag)); - std::unordered_map::const_iterator interface_param = - interface_params.find(kMinTag); - if (interface_param != interface_params.end()) { - interface.min = interface_param->second; - } - interface_param = interface_params.find(kMaxTag); - if (interface_param != interface_params.end()) { - interface.max = interface_param->second; - } - } - // State interfaces have an element to define the type, not a name attribute - if (std::string(interfaceTag) == "stateInterfaceType") { - const std::string interface_type = get_text_for_element( - interfaces_it, - std::string(interfaceTag) + " type "); - interface.name = interface_type; - } - - interfaces.push_back(interface); - interfaces_it = interfaces_it->NextSiblingElement(interfaceTag); + hardware_interface::components::InterfaceInfo interface; + + const std::string interface_name = get_attribute_value( + interfaces_it, kNameAttribute, interfaces_it->Name()); + interface.name = interface_name; + + // Optional min/max attributes + std::unordered_map interface_params = + parse_parameters_from_xml(interfaces_it->FirstChildElement(kParamTag)); + std::unordered_map::const_iterator interface_param = + interface_params.find(kMinTag); + if (interface_param != interface_params.end()) { + interface.min = interface_param->second; + } + interface_param = interface_params.find(kMaxTag); + if (interface_param != interface_params.end()) { + interface.max = interface_param->second; } - return interfaces; + return interface; } /** @@ -195,29 +179,23 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it // Find name, type and class of a component component.type = component_it->Name(); - component.name = get_attribute_value(component_it, "name", component.type); - - const auto * classType_it = component_it->FirstChildElement(kClassTypeTag); - if (!classType_it) { - throw std::runtime_error("no class type tag found in " + component.name); - } - component.class_type = get_text_for_element(classType_it, component.name + " " + kClassTypeTag); + component.name = get_attribute_value(component_it, kNameAttribute, component.type); - // Parse commandInterfaceType tags - const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTypeTag); - if (command_interfaces_it) { - component.command_interfaces = parse_interfaces_from_xml( - command_interfaces_it, kCommandInterfaceTypeTag); + // 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)); + command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag); } - // Parse stateInterfaceType tags - const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTypeTag); - if (state_interfaces_it) { - component.state_interfaces = parse_interfaces_from_xml( - state_interfaces_it, kStateInterfaceTypeTag); + // 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)); + state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag); } - // Parse paramter tags + // Parse parameters const auto * params_it = component_it->FirstChildElement(kParamTag); if (params_it) { component.parameters = parse_parameters_from_xml(params_it); @@ -236,8 +214,8 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it HardwareInfo parse_resource_from_xml(const tinyxml2::XMLElement * ros2_control_it) { HardwareInfo hardware; - hardware.name = get_attribute_value(ros2_control_it, "name", kROS2ControlTag); - hardware.type = get_attribute_value(ros2_control_it, "type", kROS2ControlTag); + hardware.name = get_attribute_value(ros2_control_it, kNameAttribute, kROS2ControlTag); + hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); // Parse everything under ros2_control tag hardware.hardware_class_type = ""; diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 4f563886af..09feded135 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -155,19 +155,23 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only 2 2 - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + )"; @@ -178,45 +182,36 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_MultiInterface + ros2_control_demo_hardware/2DOF_System_Hardware_MultiInterface 2.2 2.3 + A42B1 - ros2_control_components/MultiInterfaceJoint - + -1 1 - - - + + -1 1 - - - + + -0.5 0.5 - - - position - velocity - effort - - A42B1 + + + + - ros2_control_components/MultiInterfaceJoint - + -1 1 - - - position - velocity - effort - - A42B2 + + + + )"; @@ -226,22 +221,31 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Sensor + ros2_control_demo_hardware/2DOF_System_Hardware_Sensor 2 2 - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + - ros2_control_components/ForceTorqueSensor + + + + + + kuka_tcp -100 100 @@ -254,31 +258,42 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only 2 2 - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + - ros2_control_demo_hardware/2D_Sensor_Force_Torque + ros2_control_demo_hardware/2D_Sensor_Force_Torque 0.43 + kuka_tcp - ros2_control_components/ForceTorqueSensor - kuka_tcp - -100 - 100 + + -100 + 100 + + + -100 + 100 + + + )"; @@ -286,28 +301,32 @@ class TestComponentParser : public Test // 5. Modular Robots with separate communication to each actuator valid_urdf_ros2_control_actuator_modular_robot_ = R"( - + - ros2_control_demo_hardware/Position_Actuator_Hadware + ros2_control_demo_hardware/Position_Actuator_Hadware 1.23 3 - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + - + - ros2_control_demo_hardware/Position_Actuator_Hadware + ros2_control_demo_hardware/Position_Actuator_Hadware 1.23 2.12 - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + )"; @@ -316,62 +335,61 @@ class TestComponentParser : public Test // Example for simple transmission valid_urdf_ros2_control_actuator_modular_robot_sensors_ = R"( - + - ros2_control_demo_hardware/Velocity_Actuator_Hardware + ros2_control_demo_hardware/Velocity_Actuator_Hardware 1.23 3 - ros2_control_components/VelocityJoint - + -1 1 - - - D + D + - transmission_interface/SimpleTansmission + transmission_interface/SimpleTansmission ${1024/PI} - ros2_control_demo_hardware/Velocity_Actuator_Hardware + ros2_control_demo_hardware/Velocity_Actuator_Hardware 1.23 3 - ros2_control_components/VelocityJoint - + -1 1 - + - ros2_control_demo_hardware/Position_Sensor_Hardware + ros2_control_demo_hardware/Position_Sensor_Hardware 2 - ros2_control_components/PositionJoint - position - ${-PI} - ${PI} + + ${-PI} + ${PI} + + - ros2_control_demo_hardware/Position_Sensor_Hardware + ros2_control_demo_hardware/Position_Sensor_Hardware 2 - ros2_control_components/PositionJoint - position - ${-PI} - ${PI} + + ${-PI} + ${PI} + + )"; @@ -380,24 +398,28 @@ class TestComponentParser : public Test // Example for complex transmission (multi-joint/multi-actuator) - (system component is used) valid_urdf_ros2_control_system_muti_joints_transmission_ = R"( - + - ros2_control_demo_hardware/Actuator_Hadware_MultiDOF + ros2_control_demo_hardware/Actuator_Hadware_MultiDOF 1.23 3 - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + + - transmission_interface/SomeComplex_2x2_Transmission + transmission_interface/SomeComplex_2x2_Transmission {joint1, joint2} {output2, output2} 1.5 @@ -411,23 +433,23 @@ class TestComponentParser : public Test // 8. Sensor only valid_urdf_ros2_control_sensor_only_ = R"( - + - ros2_control_demo_hardware/CameraWithIMU_Sensor + ros2_control_demo_hardware/CameraWithIMU_Sensor 2 - ros2_control_components/IMUSensor - velocity - acceleration - -54 - 23 - -10 - 10 + + -54 + 23 + + + -10 + 10 + - ros2_control_components/2DImageSensor - image + 0 255 @@ -437,19 +459,20 @@ class TestComponentParser : public Test // 9. Actuator Only valid_urdf_ros2_control_actuator_only_ = R"( - + - ros2_control_demo_hardware/Velocity_Actuator_Hardware + ros2_control_demo_hardware/Velocity_Actuator_Hardware 1.13 3 - ros2_control_components/VelocityJoint - -1 - 1 + + -1 + 1 + - transmission_interface/RotationToLinerTansmission + transmission_interface/RotationToLinerTansmission ${1024/PI} @@ -460,7 +483,7 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only 2 2 @@ -471,7 +494,7 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only 2 2 @@ -482,7 +505,7 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only 2 2 @@ -497,12 +520,12 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only 2 2 - ros2_control_components/PositionJoint + ros2_control_components/PositionJoint -1 1 @@ -514,12 +537,12 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only 2 2 - + -1 1 @@ -530,13 +553,13 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only 2 2 - ros2_control_components/PositionJoint - + ros2_control_components/PositionJoint + -1 1 @@ -547,14 +570,15 @@ class TestComponentParser : public Test R"( - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only 2 - ros2_control_components/PositionJoint - -1 - 1 + + -1 + 1 + )"; @@ -613,14 +637,6 @@ TEST_F(TestComponentParser, invalid_child_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); } -TEST_F(TestComponentParser, component_missing_class_type_throws_error) -{ - const std::string broken_urdf_string = urdf_xml_head_ + - invalid_urdf_ros2_control_component_missing_class_type_ + urdf_xml_tail_; - - ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); -} - TEST_F(TestComponentParser, missing_attribute_throws_error) { const std::string broken_urdf_string = urdf_xml_head_ + @@ -637,14 +653,6 @@ TEST_F(TestComponentParser, parameter_missing_name_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); } -TEST_F(TestComponentParser, component_class_type_empty_throws_error) -{ - const std::string broken_urdf_string = urdf_xml_head_ + - invalid_urdf_ros2_control_component_class_type_empty_ + urdf_xml_tail_; - - ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error); -} - TEST_F(TestComponentParser, component_interface_type_empty_throws_error) { const std::string broken_urdf_string = urdf_xml_head_ + @@ -681,15 +689,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_position_value"), "1"); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); + ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, "position"); EXPECT_EQ(hardware_info.joints[1].name, "joint2"); EXPECT_EQ(hardware_info.joints[1].type, "joint"); - EXPECT_EQ(hardware_info.joints[1].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[1].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[1].parameters.at("min_position_value"), "-1"); + ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, "position"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-1"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1"); + ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, "position"); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface) @@ -705,29 +719,24 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/2DOF_System_Hardware_MultiInterface"); - ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(3)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2.2"); + EXPECT_EQ(hardware_info.hardware_parameters.at("serial_number"), "A42B1"); 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[0].class_type, "ros2_control_components/MultiInterfaceJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(3)); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position"); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(3)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, "velocity"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(1)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("serial_number"), "A42B1"); EXPECT_EQ(hardware_info.joints[1].name, "joint2"); EXPECT_EQ(hardware_info.joints[1].type, "joint"); - EXPECT_EQ(hardware_info.joints[1].class_type, "ros2_control_components/MultiInterfaceJoint"); ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, "effort"); - ASSERT_THAT(hardware_info.joints[1].parameters, SizeIs(1)); - EXPECT_EQ(hardware_info.joints[1].parameters.at("serial_number"), "A42B2"); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor) @@ -750,21 +759,15 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_position_value"), "1"); EXPECT_EQ(hardware_info.joints[1].name, "joint2"); EXPECT_EQ(hardware_info.joints[1].type, "joint"); - EXPECT_EQ(hardware_info.joints[1].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[1].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[1].parameters.at("min_position_value"), "-1"); ASSERT_THAT(hardware_info.sensors, SizeIs(1)); EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor"); EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); - EXPECT_EQ(hardware_info.sensors[0].class_type, "ros2_control_components/ForceTorqueSensor"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(6)); ASSERT_THAT(hardware_info.sensors[0].parameters, SizeIs(3)); EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp"); } @@ -789,15 +792,9 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_position_value"), "1"); EXPECT_EQ(hardware_info.joints[1].name, "joint2"); EXPECT_EQ(hardware_info.joints[1].type, "joint"); - EXPECT_EQ(hardware_info.joints[1].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[1].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[1].parameters.at("min_position_value"), "-1"); ASSERT_THAT(hardware_info.sensors, SizeIs(0)); @@ -805,17 +802,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_ForceTorqueSensor"); EXPECT_EQ(hardware_info.type, "sensor"); - EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/2D_Sensor_Force_Torque"); - ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "0.43"); + EXPECT_EQ(hardware_info.hardware_parameters.at("frame_id"), "kuka_tcp"); ASSERT_THAT(hardware_info.sensors, SizeIs(1)); EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor"); EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); - EXPECT_EQ(hardware_info.sensors[0].class_type, "ros2_control_components/ForceTorqueSensor"); - ASSERT_THAT(hardware_info.sensors[0].parameters, SizeIs(3)); - EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp"); - EXPECT_EQ(hardware_info.sensors[0].parameters.at("lower_limits"), "-100"); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot) @@ -837,9 +830,6 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_position_value"), "1"); hardware_info = control_hardware.at(1); @@ -854,9 +844,6 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint2"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("min_position_value"), "-1"); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot_with_sensors) @@ -878,16 +865,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/VelocityJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(1)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("example_param_current_rating"), "D"); ASSERT_THAT(hardware_info.transmissions, SizeIs(1)); EXPECT_EQ(hardware_info.transmissions[0].name, "transmission1"); EXPECT_EQ(hardware_info.transmissions[0].type, "transmission"); - EXPECT_EQ(hardware_info.transmissions[0].class_type, "transmission_interface/SimpleTansmission"); +// EXPECT_EQ(hardware_info.transmissions[0].class_type, "transmission_interface/SimpleTansmission"); ASSERT_THAT(hardware_info.transmissions[0].parameters, SizeIs(1)); EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint_to_actuator"), "${1024/PI}"); @@ -904,7 +888,6 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint2"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/VelocityJoint"); ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity"); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); @@ -924,13 +907,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(0)); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, "position"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_position_value"), "${PI}"); - EXPECT_EQ(hardware_info.joints[0].parameters.at("min_position_value"), "${-PI}"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "${-PI}"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "${PI}"); hardware_info = control_hardware.at(3); @@ -946,13 +927,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint2"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(0)); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, "position"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_position_value"), "${PI}"); - EXPECT_EQ(hardware_info.joints[0].parameters.at("min_position_value"), "${-PI}"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "${-PI}"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "${PI}"); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_transmission) @@ -974,22 +953,15 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_tr 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[0].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_position_value"), "1"); - EXPECT_EQ(hardware_info.joints[1].name, "joint2"); EXPECT_EQ(hardware_info.joints[1].type, "joint"); - EXPECT_EQ(hardware_info.joints[1].class_type, "ros2_control_components/PositionJoint"); - ASSERT_THAT(hardware_info.joints[1].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[1].parameters.at("min_position_value"), "-1"); ASSERT_THAT(hardware_info.transmissions, SizeIs(1)); EXPECT_EQ(hardware_info.transmissions[0].name, "transmission1"); EXPECT_EQ(hardware_info.transmissions[0].type, "transmission"); - EXPECT_EQ( - hardware_info.transmissions[0].class_type, - "transmission_interface/SomeComplex_2x2_Transmission"); + // EXPECT_EQ( + // hardware_info.transmissions[0].class_type, + // "transmission_interface/SomeComplex_2x2_Transmission"); ASSERT_THAT(hardware_info.transmissions[0].parameters, SizeIs(6)); EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint1_output1"), "1.5"); EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint1_output2"), "3.2"); @@ -1013,17 +985,12 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) ASSERT_THAT(hardware_info.sensors, SizeIs(2)); EXPECT_EQ(hardware_info.sensors[0].name, "sensor1"); EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); - EXPECT_EQ(hardware_info.sensors[0].class_type, "ros2_control_components/IMUSensor"); ASSERT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(2)); EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0].name, "velocity"); EXPECT_EQ(hardware_info.sensors[0].state_interfaces[1].name, "acceleration"); - ASSERT_THAT(hardware_info.sensors[0].parameters, SizeIs(4)); - EXPECT_EQ(hardware_info.sensors[0].parameters.at("min_acceleration_value"), "-10"); - EXPECT_EQ(hardware_info.sensors[0].parameters.at("max_velocity_value"), "23"); EXPECT_EQ(hardware_info.sensors[1].name, "sensor2"); EXPECT_EQ(hardware_info.sensors[1].type, "sensor"); - EXPECT_EQ(hardware_info.sensors[1].class_type, "ros2_control_components/2DImageSensor"); ASSERT_THAT(hardware_info.sensors[1].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.sensors[1].state_interfaces[0].name, "image"); ASSERT_THAT(hardware_info.sensors[1].parameters, SizeIs(2)); @@ -1050,16 +1017,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_EQ(hardware_info.joints[0].class_type, "ros2_control_components/VelocityJoint"); - ASSERT_THAT(hardware_info.joints[0].parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.joints[0].parameters.at("max_velocity_value"), "1"); ASSERT_THAT(hardware_info.transmissions, SizeIs(1)); EXPECT_EQ(hardware_info.transmissions[0].name, "transmission1"); EXPECT_EQ(hardware_info.transmissions[0].type, "transmission"); - EXPECT_EQ( - hardware_info.transmissions[0].class_type, - "transmission_interface/RotationToLinerTansmission"); + // EXPECT_EQ( + // hardware_info.transmissions[0].class_type, + // "transmission_interface/RotationToLinerTansmission"); ASSERT_THAT(hardware_info.transmissions[0].parameters, SizeIs(1)); EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint_to_actuator"), "${1024/PI}"); } From 4bcb481cc717d8cecbbd9626b83aeb8cfdae6799 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sun, 25 Oct 2020 11:06:45 -0700 Subject: [PATCH 2/3] Update hardware_interface/src/component_parser.cpp Co-authored-by: Bence Magyar --- hardware_interface/src/component_parser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 7bd1f4d2ad..5797207043 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -153,7 +153,7 @@ components::InterfaceInfo parse_interfaces_from_xml( // Optional min/max attributes std::unordered_map interface_params = parse_parameters_from_xml(interfaces_it->FirstChildElement(kParamTag)); - std::unordered_map::const_iterator interface_param = + auto interface_param = interface_params.find(kMinTag); if (interface_param != interface_params.end()) { interface.min = interface_param->second; From 46d18bf6eb70864007e14878b9af62591f4bd811 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 27 Oct 2020 06:55:00 +0000 Subject: [PATCH 3/3] Fix build & touch up docs --- hardware_interface/src/component_parser.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 5797207043..e0d7d3679e 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -141,10 +141,10 @@ std::unordered_map parse_parameters_from_xml( * \return std::vector< std::__cxx11::string > list of interface types * \throws std::runtime_error if the interfaceType text not set in a tag */ -components::InterfaceInfo parse_interfaces_from_xml( +hardware_interface::InterfaceInfo parse_interfaces_from_xml( const tinyxml2::XMLElement * interfaces_it) { - hardware_interface::components::InterfaceInfo interface; + hardware_interface::InterfaceInfo interface; const std::string interface_name = get_attribute_value( interfaces_it, kNameAttribute, interfaces_it->Name()); @@ -170,7 +170,7 @@ components::InterfaceInfo parse_interfaces_from_xml( * \brief Search XML snippet from URDF for information about a control component. * * \param component_it pointer to the iterator where component info should be found - * \return robot_control_components::ComponentInfo filled with information about component + * \return ComponentInfo filled with information about component * \throws std::runtime_error if a component attribute or tag is not found */ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it) @@ -208,7 +208,7 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it * \brief Parse a control resource from an "ros2_control" tag. * * \param ros2_control_it pointer to ros2_control element with informtions about resource. - * \return robot_control_components::ComponentInfo filled with information about the robot + * \return ComponentInfo filled with information about the robot * \throws std::runtime_error if a attributes or tag are not found */ HardwareInfo parse_resource_from_xml(const tinyxml2::XMLElement * ros2_control_it)