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

Improve test coverage #6

Merged
merged 4 commits into from
Apr 22, 2024
Merged

Improve test coverage #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4736e1d
Format error strings
christophfroehlich Apr 22, 2024
6a6efaf
Add comment to remove duplicate code
christophfroehlich Apr 22, 2024
1393ca5
Add another test with different min/max values
christophfroehlich Apr 22, 2024
a49aa20
Rename tests
christophfroehlich Apr 22, 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
29 changes: 15 additions & 14 deletions hardware_interface/src/component_parser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -679,9 +679,9 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
}
catch (const std::invalid_argument & err)
{
std::cerr << "Error parsing the limits for the interface : " << itf.name
<< " from the tags [" << kMinTag << " : '" << itf.min << "' and " << kMaxTag
<< " : '" << itf.max << "'] within " << kROS2ControlTag
std::cerr << "Error parsing the limits for the interface: " << itf.name
<< " from the tags [" << kMinTag << ": '" << itf.min << "' and " << kMaxTag
<< ": '" << itf.max << "'] within " << kROS2ControlTag
<< " tag inside the URDF. Skipping it" << std::endl;
return false;
}
Expand Down Expand Up @@ -763,29 +763,28 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
{
if (!itr.min.empty())
{
std::cerr << "Error parsing the limits for the interface : " << itr.name
<< " from the tag : " << kMinTag << " within " << kROS2ControlTag
std::cerr << "Error parsing the limits for the interface: " << itr.name
<< " from the tag: " << kMinTag << " within " << kROS2ControlTag
<< " tag inside the URDF. Jerk only accepts max limits." << std::endl;
}
double min_jerk(std::numeric_limits<double>::quiet_NaN()),
max_jerk(std::numeric_limits<double>::quiet_NaN());
if (!itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk))
if (
!itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk) &&
std::isfinite(max_jerk))
{
if (std::isfinite(max_jerk))
{
limits.max_jerk = std::abs(max_jerk);
limits.has_jerk_limits = true && itr.enable_limits;
}
limits.max_jerk = std::abs(max_jerk);
limits.has_jerk_limits = true && itr.enable_limits;
}
}
else
{
if (!itr.min.empty() || !itr.max.empty())
{
std::cerr << "Unable to parse the limits for the interface : " << itr.name
std::cerr << "Unable to parse the limits for the interface: " << itr.name
<< " from the tags [" << kMinTag << " and " << kMaxTag << "] within "
<< kROS2ControlTag
<< " tag inside the URDF. Supported interfaces for joint limits are : "
<< " tag inside the URDF. Supported interfaces for joint limits are: "
"position, velocity, effort, acceleration and jerk."
<< std::endl;
}
Expand Down Expand Up @@ -880,6 +879,8 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
hw_info.mimic_joints.push_back(mimic_joint);
}
}
// TODO(christophfroehlich) remove this code if deprecated mimic attribute - branch
// from above is removed (double check here, but throws already above if not found in URDF)
auto urdf_joint = model.getJoint(joint.name);
if (!urdf_joint)
{
Expand All @@ -894,7 +895,7 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
(urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC ||
urdf_joint->type == urdf::Joint::CONTINUOUS))
{
throw std::runtime_error("Missing URDF joint limits for the Joint : " + joint.name);
throw std::runtime_error("Missing URDF joint limits for the Joint: " + joint.name);
}
if (has_hard_limits)
{
Expand Down
133 changes: 133 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 @@ -920,6 +920,139 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in
EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5));
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable_interfaces)
{
std::string urdf_to_test =
std::string(ros2_control_test_assets::urdf_head) +
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_unavailable_interfaces +
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, "RRBotSystemWithGPIO");
EXPECT_EQ(hardware_info.type, "system");
EXPECT_EQ(
hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware");

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

EXPECT_EQ(hardware_info.joints[0].name, "joint1");
EXPECT_EQ(hardware_info.joints[0].type, "joint");
EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(5));
EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION);
EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double");
EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1);
EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY);
EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double");
EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1);
EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT);
EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double");
EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1);
EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].name, HW_IF_ACCELERATION);
EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].data_type, "double");
EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].size, 1);
EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].name, "jerk");
EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].data_type, "double");
EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].size, 1);
EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double");
EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1);

EXPECT_EQ(hardware_info.joints[1].name, "joint2");
EXPECT_EQ(hardware_info.joints[1].type, "joint");
EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(5));
EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION);
EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double");
EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1);
EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].name, HW_IF_VELOCITY);
EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].data_type, "double");
EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].size, 1);
EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT);
EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double");
EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1);
EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].name, HW_IF_ACCELERATION);
EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].data_type, "double");
EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].size, 1);
EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].name, "jerk");
EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].data_type, "double");
EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].size, 1);
EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1));
EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION);
EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double");
EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1);

EXPECT_EQ(hardware_info.joints[2].name, "joint3");
EXPECT_EQ(hardware_info.joints[2].type, "joint");
EXPECT_THAT(hardware_info.joints[2].command_interfaces, SizeIs(2));
EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].name, HW_IF_ACCELERATION);
EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double");
EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1);

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

EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS");
EXPECT_EQ(hardware_info.gpios[0].type, "gpio");
EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(1));
EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output");
EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool");
EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].size, 2);
EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(2));
EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input");
EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double");
EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].size, 3);
EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "image");
EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat");
EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1);

EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits);
EXPECT_THAT(
hardware_info.limits.at("joint1").max_position,
DoubleNear(std::numeric_limits<double>::max(), 1e-5));
EXPECT_THAT(
hardware_info.limits.at("joint1").min_position,
DoubleNear(std::numeric_limits<double>::min(), 1e-5));
EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits);
EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5));

EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits);
EXPECT_THAT(
hardware_info.limits.at("joint2").max_position,
DoubleNear(std::numeric_limits<double>::max(), 1e-5));
EXPECT_THAT(
hardware_info.limits.at("joint2").min_position,
DoubleNear(std::numeric_limits<double>::min(), 1e-5));
EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits);
EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits);
EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits);
EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits);
EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5));

EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits);
EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint3").max_position, DoubleNear(M_PI, 1e-5));
EXPECT_TRUE(hardware_info.limits.at("joint3").has_velocity_limits);
EXPECT_TRUE(hardware_info.limits.at("joint3").has_effort_limits);
EXPECT_FALSE(hardware_info.limits.at("joint3").has_acceleration_limits);
EXPECT_TRUE(hardware_info.limits.at("joint3").has_deceleration_limits);
EXPECT_FALSE(hardware_info.limits.at("joint3").has_jerk_limits);
EXPECT_THAT(hardware_info.limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint3").max_acceleration, DoubleNear(1.0, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5));
}

TEST_F(TestComponentParser, successfully_parse_parameter_empty)
{
const std::string urdf_to_test =
Expand Down
57 changes: 57 additions & 0 deletions ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -462,6 +462,63 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface
</gpio>
</ros2_control>
)";
const auto valid_urdf_ros2_control_system_robot_with_unavailable_interfaces =
R"(
<ros2_control name="RRBotSystemWithGPIO" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemWithGPIOHardware</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<limits enable="false"/>
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-0.05</param>
<param name="max">0.1</param>
</command_interface>
<command_interface name="effort">
<param name="min">-0.2</param>
<param name="max">0.2</param>
</command_interface>
<command_interface name="acceleration">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<command_interface name="jerk">
<param name="max">5.0</param>
</command_interface>
<state_interface name="position"/>
</joint>
<joint name="joint2">
<limits enable="false"/>
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="acceleration">
<param name="min">-0.3</param>
<param name="max">0.3</param>
</command_interface>
<command_interface name="jerk"/>
<state_interface name="position"/>
</joint>
<joint name="joint3">
<command_interface name="acceleration">
<param name="min">1.0</param>
</command_interface>
<command_interface name="unavailable">
<param name="min">-0.3</param>
<param name="max">0.3</param>
</command_interface>
</joint>
</ros2_control>
)";

const auto valid_urdf_ros2_control_parameter_empty =
R"(
Expand Down