Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add Support for SDF #1763

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
+272 −4
Open

Add Support for SDF #1763

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
424228b
Add support for sdf
Amronos Sep 25, 2024
c04067a
Merge branch 'master' into sdf-compatibility
Amronos Sep 26, 2024
a8660a4
Add Test
Amronos Sep 27, 2024
4716b34
Update release_notes.rst
Amronos Sep 27, 2024
8fb4fda
Merge branch 'master' into sdf-compatibility
Amronos Oct 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 17 additions & 4 deletions hardware_interface/src/component_parser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down Expand Up @@ -812,15 +814,26 @@ std::vector<HardwareInfo> 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");
Expand Down
35 changes: 35 additions & 0 deletions hardware_interface/test/test_component_parser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
219 changes: 219 additions & 0 deletions ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1715,6 +1715,225 @@ const auto gripper_hardware_resources_mimic_false_command_if =
</ros2_control>
)";

const auto diff_drive_robot_sdf =
R"(
<?xml version="1.0" ?>
<sdf version="1.8">
<model canonical_link="base_link" name="my_robot">
<!-- BASE -->
<link name="base_link">
<must_be_base_link>true</must_be_base_link>
</link>
<!-- CHASSIS -->
<joint name="chassis_joint" type="fixed">
<parent>base_link</parent>
<child>chassis_link</child>
<pose relative_to="base_link">0 0 0.075 0 0 0</pose>
</joint>
<link name="chassis_link">
<pose relative_to="chassis_joint"/>
<visual name="chassis_link_visual">
<geometry>
<box>
<size>
0.3 0.3 0.15
</size>
</box>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
</material>
</visual>
<collision name="chassis_link_collision">
<geometry>
<box>
<size>
0.3 0.3 0.15
</size>
</box>
</geometry>
</collision>
<inertial>
<mass>0.5</mass>
<inertia>
<ixx>0.0046875</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.0046875</iyy>
<iyz>0.0</iyz>
<izz>0.0075</izz>
</inertia>
</inertial>
</link>
<!-- lEFT WHEEL -->
<joint name="left_wheel_joint" type="revolute">
<parent>chassis_link</parent>
<child>left_wheel_link</child>
<pose relative_to="chassis_link">0.09 0.16999999999999998 -0.075 -1.5707963267948966 0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-inf</lower>
<upper>inf</upper>
</limit>
</axis>
</joint>
<link name="left_wheel_link">
<pose relative_to="left_wheel_joint"/>
<visual name="left_wheel_link_visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.04</length>
</cylinder>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
<collision name="left_wheel_link_collision">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.04</length>
</cylinder>
</geometry>
</collision>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>7.583333333333335e-05</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>7.583333333333335e-05</iyy>
<iyz>0.0</iyz>
<izz>0.00012500000000000003</izz>
</inertia>
</inertial>
</link>
<!-- RIGHT WHEEL -->
<joint name="right_wheel_joint" type="revolute">
<parent>chassis_link</parent>
<child>right_wheel_link</child>
<pose relative_to="chassis_link">0.09 -0.16999999999999998 -0.075 -1.5707963267948966 0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<!-- limit would not be specified because if the type was continuous -->
<limit>
<lower>-inf</lower>
<upper>inf</upper>
</limit>
</axis>
</joint>
<link name="right_wheel_link">
<pose relative_to="right_wheel_joint"/>
<visual name="right_wheel_link_visual">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.04</length>
</cylinder>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
<collision name="right_wheel_link_collision">
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.04</length>
</cylinder>
</geometry>
</collision>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>7.583333333333335e-05</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>7.583333333333335e-05</iyy>
<iyz>0.0</iyz>
<izz>0.00012500000000000003</izz>
</inertia>
</inertial>
</link>
<!-- CASTER -->
<joint name="caster_joint" type="revolute">
<parent>chassis_link</parent>
<child>caster_link</child>
<pose relative_to="chassis_link">-0.09 0 -0.075 0 0 0</pose>
<axis>
<xyz>1 1 1</xyz>
<limit>
<lower>-inf</lower>
<upper>inf</upper>
</limit>
</axis>
</joint>
<link name="caster_link">
<pose relative_to="caster_joint"/>
<visual name="caster_link_visual">
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
<material>
<ambient>0 0 1</ambient>
<diffuse>0 0 1</diffuse>
</material>
</visual>
<collision name="caster_link_collision">
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</collision>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.00010000000000000002</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00010000000000000002</iyy>
<iyz>0.0</iyz>
<izz>0.00010000000000000002</izz>
</inertia>
</inertial>
</link>
<ros2_control name="GazeboSimSystem" type="system">
<hardware>
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
</hardware>
<joint name="left_wheel_joint">
<command_interface name="velocity">
<param name="min">-10</param>
<param name="max">10</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/>
</joint>
<joint name="right_wheel_joint">
<command_interface name="velocity">
<param name="min">-10</param>
<param name="max">10</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/>
</joint>
</ros2_control>
<plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<parameters>/path/to/config.yml</parameters>
</plugin>
</model>
</sdf>
)";

const auto minimal_robot_urdf =
std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail);
const auto minimal_async_robot_urdf =
Expand Down