diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 09feded135..a4354f93a2 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -153,9 +153,9 @@ class TestComponentParser : public Test // 1. Industrial Robots with only one interface valid_urdf_ros2_control_system_one_interface_ = R"( - + - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware 2 2 @@ -180,12 +180,11 @@ class TestComponentParser : public Test // Note, joint parameters can be any string valid_urdf_ros2_control_system_multi_interface_ = R"( - + - ros2_control_demo_hardware/2DOF_System_Hardware_MultiInterface - 2.2 - 2.3 - A42B1 + ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware + 2 + 2 @@ -198,7 +197,7 @@ class TestComponentParser : public Test -0.5 - 0.5 + 0.5" @@ -219,23 +218,23 @@ class TestComponentParser : public Test // 3. Industrial Robots with integrated sensor valid_urdf_ros2_control_system_robot_with_sensor_ = R"( - + - ros2_control_demo_hardware/2DOF_System_Hardware_Sensor + ros2_control_demo_hardware/RRBotSystemWithSensorHardware 2 2 - -1 - 1 + -1 + 1 - -1 - 1 + -1 + 1 @@ -256,9 +255,9 @@ class TestComponentParser : public Test // 4. Industrial Robots with externally connected sensor valid_urdf_ros2_control_system_robot_with_external_sensor_ = R"( - + - ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware 2 2 @@ -277,23 +276,21 @@ class TestComponentParser : public Test - + - ros2_control_demo_hardware/2D_Sensor_Force_Torque + ros2_control_demo_hardware/ForceTorqueSensor2DHardware 0.43 - kuka_tcp - - -100 - 100 - - - -100 - 100 - + + + + + kuka_tcp + -100 + 100 )"; @@ -301,9 +298,9 @@ 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/PositionActuatorHardware 1.23 3 @@ -315,11 +312,11 @@ class TestComponentParser : public Test - + - ros2_control_demo_hardware/Position_Actuator_Hadware + ros2_control_demo_hardware/PositionActuatorHardware 1.23 - 2.12 + 3 @@ -335,9 +332,9 @@ 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/VelocityActuatorHardware 1.23 3 @@ -345,17 +342,17 @@ class TestComponentParser : public Test -1 1 - D + transmission_interface/SimpleTansmission ${1024/PI} - + - ros2_control_demo_hardware/Velocity_Actuator_Hardware + ros2_control_demo_hardware/VelocityActuatorHardware 1.23 3 @@ -364,31 +361,24 @@ class TestComponentParser : public Test -1 1 + - + - ros2_control_demo_hardware/Position_Sensor_Hardware + ros2_control_demo_hardware/PositionSensorHardware 2 - - ${-PI} - ${PI} - - + - ros2_control_demo_hardware/Position_Sensor_Hardware + ros2_control_demo_hardware/PositionSensorHardware 2 - - ${-PI} - ${PI} - @@ -398,9 +388,9 @@ 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/ActuatorHardwareMultiDOF 1.23 3 @@ -419,7 +409,7 @@ class TestComponentParser : public Test - transmission_interface/SomeComplex_2x2_Transmission + transmission_interface/SomeComplex2by2Transmission {joint1, joint2} {output2, output2} 1.5 @@ -433,25 +423,18 @@ class TestComponentParser : public Test // 8. Sensor only valid_urdf_ros2_control_sensor_only_ = R"( - + - ros2_control_demo_hardware/CameraWithIMU_Sensor + ros2_control_demo_hardware/CameraWithIMUSensor 2 - - -54 - 23 - - - -10 - 10 - + + + - 0 - 255 )"; @@ -459,17 +442,18 @@ 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/VelocityActuatorHardware 1.13 3 - -1 - 1 + -1 + 1 + transmission_interface/RotationToLinerTansmission @@ -677,11 +661,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) ASSERT_THAT(control_hardware, SizeIs(1)); const auto hardware_info = control_hardware.front(); - EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_Position_Only"); + EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnly"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only"); + "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -714,14 +698,14 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface ASSERT_THAT(control_hardware, SizeIs(1)); const auto hardware_info = control_hardware.front(); - EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_MultiInterface"); + EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/2DOF_System_Hardware_MultiInterface"); - 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"); + "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); ASSERT_THAT(hardware_info.joints, SizeIs(2)); @@ -747,11 +731,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens ASSERT_THAT(control_hardware, SizeIs(1)); const auto hardware_info = control_hardware.front(); - EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_with_Sensor"); + EXPECT_EQ(hardware_info.name, "RRBotSystemWithSensor"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/2DOF_System_Hardware_Sensor"); + "ros2_control_demo_hardware/RRBotSystemWithSensorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -768,8 +752,18 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor"); EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(6)); + EXPECT_THAT(hardware_info.sensors[0].command_interfaces, IsEmpty()); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[0].name, "fx"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[1].name, "fy"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[2].name, "fz"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[3].name, "tx"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[4].name, "ty"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[5].name, "tz"); + 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"); + EXPECT_EQ(hardware_info.sensors[0].parameters.at("upper_limits"), "100"); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor) @@ -780,11 +774,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte ASSERT_THAT(control_hardware, SizeIs(2)); auto hardware_info = control_hardware.at(0); - EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_Position_Only_External_Sensor"); + EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnlyWithExternalSensor"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only"); + "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -800,15 +794,15 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte hardware_info = control_hardware.at(1); - EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_ForceTorqueSensor"); + EXPECT_EQ(hardware_info.name, "RRBotForceTorqueSensor2D"); EXPECT_EQ(hardware_info.type, "sensor"); - ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); + ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); 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].parameters.at("frame_id"), "kuka_tcp"); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot) @@ -819,11 +813,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(control_hardware, SizeIs(2)); auto hardware_info = control_hardware.at(0); - EXPECT_EQ(hardware_info.name, "2DOF_Modular_Robot_joint1"); + EXPECT_EQ(hardware_info.name, "RRBotModularJoint1"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Position_Actuator_Hadware"); + "ros2_control_demo_hardware/PositionActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); @@ -833,13 +827,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info = control_hardware.at(1); - EXPECT_EQ(hardware_info.name, "2DOF_Modular_Robot_joint2"); + EXPECT_EQ(hardware_info.name, "RRBotModularJoint2"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Position_Actuator_Hadware"); + "ros2_control_demo_hardware/PositionActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); - EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2.12"); + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "3"); ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint2"); @@ -854,11 +848,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(control_hardware, SizeIs(4)); auto hardware_info = control_hardware.at(0); - EXPECT_EQ(hardware_info.name, "2DOF_Modular_Robot_joint1"); + EXPECT_EQ(hardware_info.name, "RRBotModularJoint1"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Velocity_Actuator_Hardware"); + "ros2_control_demo_hardware/VelocityActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); @@ -877,11 +871,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info = control_hardware.at(1); - EXPECT_EQ(hardware_info.name, "2DOF_Modular_Robot_joint2"); + EXPECT_EQ(hardware_info.name, "RRBotModularJoint2"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Velocity_Actuator_Hardware"); + "ros2_control_demo_hardware/VelocityActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "3"); @@ -895,11 +889,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info = control_hardware.at(2); - EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_Position_Sensor_joint1"); + EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint1"); EXPECT_EQ(hardware_info.type, "sensor"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Position_Sensor_Hardware"); + "ros2_control_demo_hardware/PositionSensorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); @@ -907,19 +901,16 @@ 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"); - ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); 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[0].command_interfaces[0].min, "${-PI}"); - EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "${PI}"); + hardware_info = control_hardware.at(3); - EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_Position_Sensor_joint2"); + EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint2"); EXPECT_EQ(hardware_info.type, "sensor"); - EXPECT_EQ( - hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Position_Sensor_Hardware"); + EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionSensorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); @@ -927,11 +918,9 @@ 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"); - ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); + ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); 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[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) @@ -942,11 +931,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_tr ASSERT_THAT(control_hardware, SizeIs(1)); auto hardware_info = control_hardware.at(0); - EXPECT_EQ(hardware_info.name, "2DOF_Modular_Robot_Wrist"); + EXPECT_EQ(hardware_info.name, "RRBotModularWrist"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Actuator_Hadware_MultiDOF"); + "ros2_control_demo_hardware/ActuatorHardwareMultiDOF"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); @@ -976,26 +965,24 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) ASSERT_THAT(control_hardware, SizeIs(1)); auto hardware_info = control_hardware.at(0); - EXPECT_EQ(hardware_info.name, "Camera_with_IMU"); + EXPECT_EQ(hardware_info.name, "CameraWithIMU"); EXPECT_EQ(hardware_info.type, "sensor"); - EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/CameraWithIMU_Sensor"); + EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/CameraWithIMUSensor"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); ASSERT_THAT(hardware_info.sensors, SizeIs(2)); EXPECT_EQ(hardware_info.sensors[0].name, "sensor1"); EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); - 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].state_interfaces, SizeIs(3)); + EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0].name, "roll"); + EXPECT_EQ(hardware_info.sensors[0].state_interfaces[1].name, "pitch"); + EXPECT_EQ(hardware_info.sensors[0].state_interfaces[2].name, "yaw"); EXPECT_EQ(hardware_info.sensors[1].name, "sensor2"); EXPECT_EQ(hardware_info.sensors[1].type, "sensor"); 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)); - EXPECT_EQ(hardware_info.sensors[1].parameters.at("min_image_value"), "0"); - EXPECT_EQ(hardware_info.sensors[1].parameters.at("max_image_value"), "255"); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) @@ -1006,17 +993,24 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) ASSERT_THAT(control_hardware, SizeIs(1)); auto hardware_info = control_hardware.at(0); - EXPECT_EQ(hardware_info.name, "2DOF_Modular_Robot_joint1"); + EXPECT_EQ(hardware_info.name, "ActuatorModularJoint1"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( hardware_info.hardware_class_type, - "ros2_control_demo_hardware/Velocity_Actuator_Hardware"); + "ros2_control_demo_hardware/VelocityActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.13"); ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); + 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"); + 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, "velocity"); + ASSERT_THAT(hardware_info.transmissions, SizeIs(1)); EXPECT_EQ(hardware_info.transmissions[0].name, "transmission1");