diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 4ea7fdc2f7..a5debef3b7 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils TinyXML2 tinyxml2_vendor + joint_limits urdf ) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 97abc00438..cb548b9bf4 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -19,6 +19,8 @@ #include #include +#include "joint_limits/joint_limits.hpp" + namespace hardware_interface { /** @@ -42,6 +44,8 @@ struct InterfaceInfo std::string data_type; /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. int size; + /// (Optional) enable or disable the limits for the command interfaces + bool enable_limits; }; /// @brief This structure stores information about a joint that is mimicking another joint @@ -163,6 +167,10 @@ struct HardwareInfo * The XML contents prior to parsing */ std::string original_xml; + /** + * The URDF parsed limits of the hardware components joint command interfaces + */ + std::unordered_map limits; }; } // namespace hardware_interface diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 5274726257..b79d280a11 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -16,6 +16,7 @@ rclcpp_lifecycle rcpputils tinyxml2_vendor + joint_limits urdf rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0841265e81..aad1d827ec 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -26,6 +26,8 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limits_urdf.hpp" namespace { @@ -43,6 +45,8 @@ constexpr const auto kCommandInterfaceTag = "command_interface"; constexpr const auto kStateInterfaceTag = "state_interface"; constexpr const auto kMinTag = "min"; constexpr const auto kMaxTag = "max"; +constexpr const auto kLimitsTag = "limits"; +constexpr const auto kEnableAttribute = "enable"; constexpr const auto kInitialValueTag = "initial_value"; constexpr const auto kMimicAttribute = "mimic"; constexpr const auto kDataTypeAttribute = "data_type"; @@ -289,6 +293,15 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( interface.max = interface_param->second; } + // Option enable or disable the interface limits, by default they are enabled + interface.enable_limits = true; + const auto * limits_it = interfaces_it->FirstChildElement(kLimitsTag); + if (limits_it) + { + interface.enable_limits = + parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name())); + } + // Optional initial_value attribute interface_param = interface_params.find(kInitialValueTag); if (interface_param != interface_params.end()) @@ -333,11 +346,21 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it } } + // Option enable or disable the interface limits, by default they are enabled + bool enable_limits = true; + const auto * limits_it = component_it->FirstChildElement(kLimitsTag); + if (limits_it) + { + enable_limits = parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name())); + } + // Parse all command interfaces const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag); while (command_interfaces_it) { - component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it)); + InterfaceInfo cmd_info = parse_interfaces_from_xml(command_interfaces_it); + cmd_info.enable_limits &= enable_limits; + component.command_interfaces.push_back(cmd_info); command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag); } @@ -345,7 +368,9 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTag); while (state_interfaces_it) { - component.state_interfaces.push_back(parse_interfaces_from_xml(state_interfaces_it)); + InterfaceInfo state_info = parse_interfaces_from_xml(state_interfaces_it); + state_info.enable_limits &= enable_limits; + component.state_interfaces.push_back(state_info); state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag); } @@ -589,6 +614,171 @@ HardwareInfo parse_resource_from_xml( return hardware; } +/** + * @brief Retrieve the min and max values from the interface tag. + * @param itf The interface tag to retrieve the values from. + * @param min The minimum value to be retrieved. + * @param max The maximum value to be retrieved. + * @return true if the values are retrieved, false otherwise. + */ +bool retrieve_min_max_interface_values(const InterfaceInfo & itf, double & min, double & max) +{ + try + { + if (itf.min.empty() && itf.max.empty()) + { + // If the limits don't exist then return false as they are not retrieved + return false; + } + if (!itf.min.empty()) + { + min = hardware_interface::stod(itf.min); + } + if (!itf.max.empty()) + { + max = hardware_interface::stod(itf.max); + } + return true; + } + 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 << " tag inside the URDF. Skipping it" + << std::endl; + return false; + } +} + +/** + * @brief Set custom values for acceleration and jerk limits (no URDF standard) + * @param itr The interface tag to retrieve the values from. + * @param limits The joint limits to be set. + */ +void set_custom_interface_values(const InterfaceInfo & itr, joint_limits::JointLimits & limits) +{ + if (itr.name == hardware_interface::HW_IF_ACCELERATION) + { + double max_decel(std::numeric_limits::quiet_NaN()), + max_accel(std::numeric_limits::quiet_NaN()); + if (detail::retrieve_min_max_interface_values(itr, max_decel, max_accel)) + { + if (std::isfinite(max_decel)) + { + limits.max_deceleration = std::fabs(max_decel); + if (!std::isfinite(max_accel)) + { + limits.max_acceleration = std::fabs(limits.max_deceleration); + } + limits.has_deceleration_limits = itr.enable_limits; + } + if (std::isfinite(max_accel)) + { + limits.max_acceleration = max_accel; + + if (!std::isfinite(limits.max_deceleration)) + { + limits.max_deceleration = std::fabs(limits.max_acceleration); + } + limits.has_acceleration_limits = itr.enable_limits; + } + } + } + else if (itr.name == "jerk") + { + if (!itr.min.empty()) + { + 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::quiet_NaN()), + max_jerk(std::numeric_limits::quiet_NaN()); + if ( + !itr.max.empty() && detail::retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && + std::isfinite(max_jerk)) + { + limits.max_jerk = std::abs(max_jerk); + limits.has_jerk_limits = itr.enable_limits; + } + } + else + { + if (!itr.min.empty() || !itr.max.empty()) + { + 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: " + "position, velocity, effort, acceleration and jerk." + << std::endl; + } + } +} + +/** + * @brief Retrieve the limits from ros2_control command interface tags and override URDF limits if + * restrictive + * @param interfaces The interfaces to retrieve the limits from. + * @param limits The joint limits to be set. + */ +void update_interface_limits( + const std::vector & interfaces, joint_limits::JointLimits & limits) +{ + for (auto & itr : interfaces) + { + if (itr.name == hardware_interface::HW_IF_POSITION) + { + limits.min_position = limits.has_position_limits && itr.enable_limits + ? limits.min_position + : -std::numeric_limits::max(); + limits.max_position = limits.has_position_limits && itr.enable_limits + ? limits.max_position + : std::numeric_limits::max(); + double min_pos(limits.min_position), max_pos(limits.max_position); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_pos, max_pos)) + { + limits.min_position = std::max(min_pos, limits.min_position); + limits.max_position = std::min(max_pos, limits.max_position); + limits.has_position_limits = true; + } + limits.has_position_limits &= itr.enable_limits; + } + else if (itr.name == hardware_interface::HW_IF_VELOCITY) + { + limits.max_velocity = + limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits::max(); + // Apply the most restrictive one in the case + double min_vel(-limits.max_velocity), max_vel(limits.max_velocity); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_vel, max_vel)) + { + max_vel = std::min(std::abs(min_vel), max_vel); + limits.max_velocity = std::min(max_vel, limits.max_velocity); + limits.has_velocity_limits = true; + } + limits.has_velocity_limits &= itr.enable_limits; + } + else if (itr.name == hardware_interface::HW_IF_EFFORT) + { + limits.max_effort = + limits.has_effort_limits ? limits.max_effort : std::numeric_limits::max(); + // Apply the most restrictive one in the case + double min_eff(-limits.max_effort), max_eff(limits.max_effort); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_eff, max_eff)) + { + max_eff = std::min(std::abs(min_eff), max_eff); + limits.max_effort = std::min(max_eff, limits.max_effort); + limits.has_effort_limits = true; + } + limits.has_effort_limits &= itr.enable_limits; + } + else + { + detail::set_custom_interface_values(itr, limits); + } + } +} + } // namespace detail std::vector parse_control_resources_from_urdf(const std::string & urdf) @@ -716,6 +906,27 @@ std::vector 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) + { + std::cerr << "Joint: '" + joint.name + "' not found in URDF. Skipping limits parsing!" + << std::endl; + continue; + } + if (urdf_joint->type == urdf::Joint::FIXED) + { + throw std::runtime_error( + "Joint '" + joint.name + + "' is of type 'fixed'. " + "Fixed joints do not make sense in ros2_control."); + } + joint_limits::JointLimits limits; + getJointLimits(urdf_joint, limits); + // Take the most restricted one. Also valid for continuous-joint type only + detail::update_interface_limits(joint.command_interfaces, limits); + hw_info.limits[joint.name] = limits; } } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 968d41ed97..946fda7c15 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -32,6 +32,7 @@ class TestComponentParser : public Test void SetUp() override {} }; +using hardware_interface::HW_IF_ACCELERATION; using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; @@ -137,6 +138,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1"); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface) @@ -174,6 +190,22 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor) @@ -218,6 +250,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("lower_limits"), "-100"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("upper_limits"), "100"); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor) @@ -248,6 +295,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte ASSERT_THAT(hardware_info.sensors, SizeIs(0)); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } + hardware_info = control_hardware.at(1); EXPECT_EQ(hardware_info.name, "RRBotForceTorqueSensor2D"); @@ -259,6 +321,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor"); EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp"); + ASSERT_THAT(hardware_info.limits, SizeIs(0)); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot) @@ -281,6 +344,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } hardware_info = control_hardware.at(1); @@ -294,6 +371,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint2"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot_with_sensors) @@ -328,6 +419,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info.transmissions[0].joints[0].mechanical_reduction, DoubleNear(1024.0 / M_PI, 0.01)); ASSERT_THAT(hardware_info.transmissions[0].actuators, SizeIs(1)); EXPECT_THAT(hardware_info.transmissions[0].actuators[0].name, "actuator1"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } hardware_info = control_hardware.at(1); @@ -345,6 +449,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } hardware_info = control_hardware.at(2); @@ -362,6 +479,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } hardware_info = control_hardware.at(3); @@ -379,6 +509,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_transmission) @@ -451,6 +594,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.sensors[1].type, "sensor"); ASSERT_THAT(hardware_info.sensors[1].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.sensors[1].state_interfaces[0].name, "image"); + // There will be no limits as the ros2_control tag has only sensor info + ASSERT_THAT(hardware_info.limits, SizeIs(0)); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) @@ -500,6 +645,17 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) EXPECT_THAT(actuator.offset, DoubleEq(0.0)); ASSERT_THAT(transmission.parameters, SizeIs(1)); EXPECT_EQ(transmission.parameters.at("additional_special_parameter"), "1337"); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + // effort and velocity limits won't change as they are above the main URDF hard limits + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); } TEST_F(TestComponentParser, successfully_parse_locale_independent_double) @@ -568,6 +724,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ(hardware_info.gpios[1].command_interfaces[0].name, "vacuum"); EXPECT_THAT(hardware_info.transmissions, IsEmpty()); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + // Position limits are limited in the ros2_control tag + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_data_type) @@ -616,6 +786,362 @@ 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_valid_urdf_system_and_disabled_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_gpio_and_disabled_interface_limits + + 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(1)); + 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::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint1").min_position, + DoubleNear(-std::numeric_limits::max(), 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::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint2").min_position, + DoubleNear(-std::numeric_limits::max(), 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_TRUE(hardware_info.limits.at("joint3").has_acceleration_limits); + EXPECT_FALSE(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_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(0)); + + EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint1").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint1").min_position, + DoubleNear(-std::numeric_limits::max(), 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::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint2").min_position, + DoubleNear(-std::numeric_limits::max(), 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, throw_on_parse_invalid_urdf_system_missing_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_revolute_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); + + urdf_to_test = std::string(ros2_control_test_assets::urdf_head_prismatic_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, throw_on_parse_urdf_system_with_command_fixed_joint) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::invalid_urdf_ros2_control_system_with_command_fixed_joint + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_missing_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces + + ros2_control_test_assets::urdf_tail; + EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test)); + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + ASSERT_GT(hardware_info.limits.count("joint1"), 0); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 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)) + << "velocity constraint from ros2_control_tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.2, 1e-5)) + << "effort constraint from ros2_control_tag"; + 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)); + + ASSERT_GT(hardware_info.limits.count("joint2"), 0); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + 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); +} + +TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces + + ros2_control_test_assets::urdf_tail; + EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test)); + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + ASSERT_GT(hardware_info.limits.count("joint1"), 0); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 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)) + << "velocity URDF constraint overridden by ros2_control tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)) + << "effort constraint from URDF"; + 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)); + + ASSERT_GT(hardware_info.limits.count("joint2"), 0); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_TRUE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5)) + << "velocity constraint from URDF"; + EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5)) + << "effort constraint from URDF"; + 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); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = @@ -640,6 +1166,19 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) 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"); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5)); + } } TEST_F(TestComponentParser, negative_size_throws_error) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 7b2dd46e7a..7b46eda9c0 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -400,6 +400,166 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = )"; +// 12. Industrial Robots with integrated GPIO with few disabled limits in joints +const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + -1 + 1 + + + + + -0.3 + 0.3 + + + -2.0 + 2.0 + + + + + + 1.0 + + + + + + + + +)"; +const auto valid_urdf_ros2_control_system_robot_with_unavailable_interfaces = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + -1 + 1 + + + + + -0.3 + 0.3 + + + + + + + 1.0 + + + -0.3 + 0.3 + + + +)"; +const auto valid_urdf_ros2_control_system_robot_with_all_interfaces = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + + + + + + +)"; + const auto valid_urdf_ros2_control_parameter_empty = R"( @@ -567,6 +727,21 @@ const auto invalid_urdf2_transmission_given_too_many_joints = )"; +const auto invalid_urdf_ros2_control_system_with_command_fixed_joint = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + + + + +)"; + } // namespace ros2_control_test_assets #endif // ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_ diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 308751e0d8..f5f668fd12 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -23,10 +23,6 @@ namespace ros2_control_test_assets const auto urdf_head = R"( - - - - @@ -149,13 +145,370 @@ const auto urdf_head = )"; + +const auto urdf_head_revolute_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_continuous_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_continuous_with_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +const auto urdf_head_prismatic_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + const auto urdf_head_mimic = R"( - - - - @@ -503,6 +856,7 @@ const auto diffbot_urdf = + @@ -538,6 +892,7 @@ const auto diffbot_urdf = +