Skip to content

Commit

Permalink
Add Support for SDF (#1763)
Browse files Browse the repository at this point in the history 
---------

Signed-off-by: Aarav Gupta <134804732+Amronos@users.noreply.github.com>
Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
  • Loading branch information
@Amronos @christophfroehlich @bmagyar
3 people authored Nov 7, 2024
1 parent d714e8b commit 93a2a68
Show file tree
Hide file tree
Showing 5 changed files with 273 additions and 4 deletions.
1 change: 1 addition & 0 deletions doc/release_notes.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <https://github.com/ros-controls/ros2_control/pull/1643>`_)
* With (`#1683 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/1240>`_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
* With (`#1421 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/1763>`_) parsing for SDF published to ``robot_description`` topic is now also supported.

joint_limits
************
Expand Down
1 change: 1 addition & 0 deletions hardware_interface/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<depend>tinyxml2_vendor</depend>
<depend>joint_limits</depend>
<depend>urdf</depend>
<depend>sdformat_urdf</depend>

<build_depend>rcutils</build_depend>
<exec_depend>rcutils</exec_depend>
Expand Down
21 changes: 17 additions & 4 deletions hardware_interface/src/component_parser.cpp
View file
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
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 93a2a68

Please sign in to comment.