From 93a2a68f6ab4110b7e5816bce134437780ba3bf3 Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Fri, 8 Nov 2024 00:53:02 +0530 Subject: [PATCH] Add Support for SDF (#1763) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Signed-off-by: Aarav Gupta <134804732+Amronos@users.noreply.github.com> Co-authored-by: Christoph Fröhlich Co-authored-by: Bence Magyar --- doc/release_notes.rst | 1 + hardware_interface/package.xml | 1 + hardware_interface/src/component_parser.cpp | 21 +- .../test/test_component_parser.cpp | 35 +++ .../ros2_control_test_assets/descriptions.hpp | 219 ++++++++++++++++++ 5 files changed, 273 insertions(+), 4 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b11a1351fe..a6ebb16e5f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -113,6 +113,7 @@ hardware_interface * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. +* With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported. joint_limits ************ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 96d593e8f6..05cff62957 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -18,6 +18,7 @@ tinyxml2_vendor joint_limits urdf + sdformat_urdf rcutils rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index d2ec0f9d53..a49b83b964 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -31,6 +31,8 @@ namespace { constexpr const auto kRobotTag = "robot"; +constexpr const auto kSDFTag = "sdf"; +constexpr const auto kModelTag = "model"; constexpr const auto kROS2ControlTag = "ros2_control"; constexpr const auto kHardwareTag = "hardware"; constexpr const auto kPluginNameTag = "plugin"; @@ -812,15 +814,26 @@ std::vector parse_control_resources_from_urdf(const std::string & "invalid URDF passed in to robot parser: " + std::string(doc.ErrorStr())); } - // Find robot tag + // Find robot or sdf tag const tinyxml2::XMLElement * robot_it = doc.RootElement(); + const tinyxml2::XMLElement * ros2_control_it; - if (std::string(kRobotTag) != robot_it->Name()) + if (std::string(kRobotTag) == robot_it->Name()) { - throw std::runtime_error("the robot tag is not root element in URDF"); + ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); + } + else if (std::string(kSDFTag) == robot_it->Name()) + { + // find model tag in sdf tag + const tinyxml2::XMLElement * model_it = robot_it->FirstChildElement(kModelTag); + ros2_control_it = model_it->FirstChildElement(kROS2ControlTag); + } + else + { + throw std::runtime_error( + "the robot tag is not root element in URDF or sdf tag is not root element in SDF"); } - const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); if (!ros2_control_it) { throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag"); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 20e098b62e..3c24b0cc2a 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1616,3 +1616,38 @@ TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardw EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum"); EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); } + +TEST_F(TestComponentParser, successfully_parse_valid_sdf) +{ + std::string sdf_to_test = ros2_control_test_assets::diff_drive_robot_sdf; + const auto control_hardware = parse_control_resources_from_urdf(sdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "GazeboSimSystem"); + EXPECT_EQ(hardware_info.type, "system"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + EXPECT_EQ(hardware_info.hardware_plugin_name, "gz_ros2_control/GazeboSimSystem"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + EXPECT_EQ(hardware_info.joints[0].name, "left_wheel_joint"); + 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, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-10"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "10"); + ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_POSITION); + + EXPECT_EQ(hardware_info.joints[1].name, "right_wheel_joint"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-10"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "10"); + ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_POSITION); +} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index cc2b1798d4..e94d4e6736 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -1715,6 +1715,225 @@ const auto gripper_hardware_resources_mimic_false_command_if = )"; +const auto diff_drive_robot_sdf = + R"( + + + + + + true + + + + base_link + chassis_link + 0 0 0.075 0 0 0 + + + + + + + + 0.3 0.3 0.15 + + + + + 1 1 1 1 + 1 1 1 1 + + + + + + + 0.3 0.3 0.15 + + + + + + 0.5 + + 0.0046875 + 0.0 + 0.0 + 0.0046875 + 0.0 + 0.0075 + + + + + + chassis_link + left_wheel_link + 0.09 0.16999999999999998 -0.075 -1.5707963267948966 0 0 + + 0 0 1 + + -inf + inf + + + + + + + + + 0.05 + 0.04 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + 0.04 + + + + + 0.1 + + 7.583333333333335e-05 + 0.0 + 0.0 + 7.583333333333335e-05 + 0.0 + 0.00012500000000000003 + + + + + + chassis_link + right_wheel_link + 0.09 -0.16999999999999998 -0.075 -1.5707963267948966 0 0 + + 0 0 1 + + + -inf + inf + + + + + + + + + 0.05 + 0.04 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + 0.04 + + + + + 0.1 + + 7.583333333333335e-05 + 0.0 + 0.0 + 7.583333333333335e-05 + 0.0 + 0.00012500000000000003 + + + + + + chassis_link + caster_link + -0.09 0 -0.075 0 0 0 + + 1 1 1 + + -inf + inf + + + + + + + + + 0.05 + + + + 0 0 1 + 0 0 1 + + + + + + 0.05 + + + + + 0.1 + + 0.00010000000000000002 + 0.0 + 0.0 + 0.00010000000000000002 + 0.0 + 0.00010000000000000002 + + + + + + gz_ros2_control/GazeboSimSystem + + + + -10 + 10 + + + + + + + -10 + 10 + + + + + + + /path/to/config.yml + + + +)"; + const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_async_robot_urdf =