Skip to content

Commit

Permalink
Add Test
Browse files Browse the repository at this point in the history 
Signed-off-by: Aarav Gupta <134804732+Amronos@users.noreply.github.com>
  • Loading branch information
@Amronos
Amronos committed Sep 27, 2024
1 parent c04067a commit a8660a4
Show file tree
Hide file tree
Showing 3 changed files with 264 additions and 5 deletions.
15 changes: 10 additions & 5 deletions hardware_interface/src/component_parser.cpp
View file
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
View file
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
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

0 comments on commit a8660a4

Please sign in to comment.