Skip to content

Commit

Permalink
[URDF Parser] Allow empty urdf tag, e.g., parameter (#1017)
Browse files Browse the repository at this point in the history
* Do not explode with empty tag
* Update tests to allow empty URDF parameter to hardware interface
* Test checks that empty parameter tags work and actually checks the parsed urdf.

---------

Co-authored-by: Bence Magyar <[email protected]>
Co-authored-by: Dr. Denis <[email protected]>
  • Loading branch information
3 people authored May 19, 2023
1 parent 775dd6a commit 7c07430
Show file tree
Hide file tree
Showing 3 changed files with 44 additions and 28 deletions.
4 changes: 3 additions & 1 deletion hardware_interface/src/component_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

#include <tinyxml2.h>
#include <charconv>
#include <iostream>
#include <regex>
#include <stdexcept>
#include <string>
Expand Down Expand Up @@ -68,7 +69,8 @@ std::string get_text_for_element(
const auto get_text_output = element_it->GetText();
if (!get_text_output)
{
throw std::runtime_error("text not specified in the " + tag_name + " tag");
std::cerr << "text not specified in the " << tag_name << " tag" << std::endl;
return "";
}
return get_text_output;
}
Expand Down
36 changes: 26 additions & 10 deletions hardware_interface/test/test_component_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,16 +100,6 @@ TEST_F(TestComponentParser, component_interface_type_empty_throws_error)
ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error);
}

TEST_F(TestComponentParser, parameter_empty_throws_error)
{
const std::string broken_urdf_string =
std::string(ros2_control_test_assets::urdf_head) +
ros2_control_test_assets::invalid_urdf_ros2_control_parameter_empty +
ros2_control_test_assets::urdf_tail;

ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error);
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface)
{
std::string urdf_to_test =
Expand Down Expand Up @@ -625,6 +615,32 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d
EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1);
}

TEST_F(TestComponentParser, successfully_parse_parameter_empty)
{
const std::string urdf_to_test =
std::string(ros2_control_test_assets::urdf_head) +
ros2_control_test_assets::valid_urdf_ros2_control_parameter_empty +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
ASSERT_THAT(control_hardware, SizeIs(1));
auto hardware_info = control_hardware.front();

EXPECT_EQ(hardware_info.name, "2DOF_System_Robot_Position_Only");
EXPECT_EQ(hardware_info.type, "system");
EXPECT_EQ(
hardware_info.hardware_plugin_name,
"ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only");

ASSERT_THAT(hardware_info.joints, SizeIs(1));

EXPECT_EQ(hardware_info.joints[0].name, "joint1");
EXPECT_EQ(hardware_info.joints[0].type, "joint");
EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position");

EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "");
EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");
}

TEST_F(TestComponentParser, negative_size_throws_error)
{
std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) +
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -398,6 +398,21 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type =
</ros2_control>
)";

const auto valid_urdf_ros2_control_parameter_empty =
R"(
<ros2_control name="2DOF_System_Robot_Position_Only" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
<param name="example_param_write_for_sec"></param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
</command_interface>
</joint>
</ros2_control>
)";

// Errors
const auto invalid_urdf_ros2_control_invalid_child =
R"(
Expand Down Expand Up @@ -485,23 +500,6 @@ const auto invalid_urdf_ros2_control_component_interface_type_empty =
</ros2_control>
)";

const auto invalid_urdf_ros2_control_parameter_empty =
R"(
<ros2_control name="2DOF_System_Robot_Position_Only" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
<param name="example_param_write_for_sec"></param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min_position_value">-1</param>
<param name="max_position_value">1</param>
</command_interface>
</joint>
</ros2_control>
)";

const auto invalid_urdf2_ros2_control_illegal_size =
R"(
<ros2_control name="RRBotSystemWithIllegalSize" type="system">
Expand Down

0 comments on commit 7c07430

Please sign in to comment.