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.

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

Add Support for SDF #1763

Merged
merged 16 commits into from
Nov 7, 2024
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
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
Prev Previous commit
Next Next commit
Add Test
Signed-off-by: Aarav Gupta <134804732+Amronos@users.noreply.github.com>
  • Loading branch information
Amronos committed Sep 27, 2024
commit a8660a4c6ddb99207dffb1db37304b916a3d98d1
15 changes: 10 additions & 5 deletions hardware_interface/src/component_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -821,14 +821,19 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
if (std::string(kRobotTag) == robot_it->Name())
{
ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag);
} else if (std::string(kSDFTag) == robot_it->Name()) {
//find model tag in sdf tag
}
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");
}

else
{
throw std::runtime_error(
"the robot tag is not root element in URDF or sdf tag is not root element in SDF");
}

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
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);
}
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