diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 4769c3bb58..97926030c3 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ +#include #include #include #include @@ -64,8 +65,8 @@ struct ComponentInfo /// Type of the component: sensor, joint, or GPIO. std::string type; - /// If the component is a mimic joint - bool is_mimic; + /// If the component has a mimic joint tag, for opt-out + std::optional is_mimic{}; /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index cfec648616..a855550a21 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -346,18 +346,20 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it if (!component.type.compare(kJointTag)) { - std::string mimic_str = get_attribute_value_or(component_it, kMimicAttribute, "false"); - component.is_mimic = mimic_str.compare("true") == 0; + try + { + component.is_mimic = + parse_bool(get_attribute_value(component_it, kMimicAttribute, kJointTag)); + } + catch (const std::runtime_error & e) + { + // mimic attribute not set + component.is_mimic = {}; + } } // Parse all command interfaces const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag); - if (component.is_mimic && command_interfaces_it) - { - throw std::runtime_error( - "Component '" + std::string(component.name) + - "' has mimic attribute set to true: Mimic joints cannot have command interfaces."); - } while (command_interfaces_it) { component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it)); @@ -696,7 +698,6 @@ std::vector parse_control_resources_from_urdf(const std::string & mimic_joint.offset = parse_double(joint.parameters.at("offset")); } hw_info.mimic_joints.push_back(mimic_joint); - hw_info.joints.at(i).is_mimic = true; } else { @@ -705,33 +706,39 @@ std::vector parse_control_resources_from_urdf(const std::string & { throw std::runtime_error("Joint " + joint.name + " not found in URDF"); } - if (joint.is_mimic) + if (!urdf_joint->mimic && joint.is_mimic.value_or(false)) { - if (urdf_joint->mimic) + throw std::runtime_error( + "Joint '" + std::string(joint.name) + "' has no mimic information in the URDF."); + } + if (urdf_joint->mimic && joint.is_mimic.value_or(true)) + { + if (joint.command_interfaces.size() > 0) { - auto find_joint = [&hw_info](const std::string & name) - { - auto it = std::find_if( - hw_info.joints.begin(), hw_info.joints.end(), - [&name](const auto & j) { return j.name == name; }); - if (it == hw_info.joints.end()) - { - throw std::runtime_error("Joint '" + name + "' not found in hw_info.joints"); - } - return std::distance(hw_info.joints.begin(), it); - }; - - MimicJoint mimic_joint; - mimic_joint.joint_index = i; - mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); - mimic_joint.multiplier = urdf_joint->mimic->multiplier; - mimic_joint.offset = urdf_joint->mimic->offset; - hw_info.mimic_joints.push_back(mimic_joint); + throw std::runtime_error( + "Joint '" + std::string(joint.name) + + "' has mimic attribute not set to false: Mimic joints cannot have command " + "interfaces."); } - else + auto find_joint = [&hw_info](const std::string & name) { - throw std::runtime_error("Joint '" + joint.name + "' has no mimic information in URDF"); - } + auto it = std::find_if( + hw_info.joints.begin(), hw_info.joints.end(), + [&name](const auto & j) { return j.name == name; }); + if (it == hw_info.joints.end()) + { + throw std::runtime_error( + "Mimic joint '" + name + "' not found in tag"); + } + return std::distance(hw_info.joints.begin(), it); + }; + + MimicJoint mimic_joint; + mimic_joint.joint_index = i; + mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name); + mimic_joint.multiplier = urdf_joint->mimic->multiplier; + mimic_joint.offset = urdf_joint->mimic->offset; + hw_info.mimic_joints.push_back(mimic_joint); } } } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index cbf81ee3ab..5c880b9571 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -675,7 +675,7 @@ TEST_F(TestComponentParser, transmission_given_too_many_joints_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } -TEST_F(TestComponentParser, mimic_true_valid_config) +TEST_F(TestComponentParser, gripper_mimic_true_valid_config) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + @@ -691,8 +691,24 @@ TEST_F(TestComponentParser, mimic_true_valid_config) EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); } +TEST_F(TestComponentParser, gripper_no_mimic_valid_config) +{ + const auto urdf_to_test = + std::string(ros2_control_test_assets::gripper_urdf_head) + + std::string(ros2_control_test_assets::gripper_hardware_resources_no_command_if) + + std::string(ros2_control_test_assets::urdf_tail); + std::vector hw_info; + ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test)); + ASSERT_THAT(hw_info, SizeIs(1)); + ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1)); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0); + EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0); + EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0); + EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); +} + // TODO(christophfroehlich) delete deprecated config test -TEST_F(TestComponentParser, mimic_deprecated_valid_config) +TEST_F(TestComponentParser, gripper_mimic_deprecated_valid_config) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + @@ -708,7 +724,7 @@ TEST_F(TestComponentParser, mimic_deprecated_valid_config) EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); } -TEST_F(TestComponentParser, mimic_deprecated_unknown_joint_throws_error) +TEST_F(TestComponentParser, gripper_mimic_deprecated_unknown_joint_throws_error) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + @@ -720,7 +736,7 @@ TEST_F(TestComponentParser, mimic_deprecated_unknown_joint_throws_error) } // end delete deprecated config test -TEST_F(TestComponentParser, mimic_with_unknown_joint_throws_error) +TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head_unknown_joint) + @@ -729,7 +745,7 @@ TEST_F(TestComponentParser, mimic_with_unknown_joint_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } -TEST_F(TestComponentParser, mimic_true_without_mimic_info_throws_error) +TEST_F(TestComponentParser, gripper_mimic_true_without_mimic_info_throws_error) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head_no_mimic) + @@ -738,7 +754,7 @@ TEST_F(TestComponentParser, mimic_true_without_mimic_info_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } -TEST_F(TestComponentParser, mimic_true_invalid_config_throws_error) +TEST_F(TestComponentParser, gripper_mimic_true_invalid_config_throws_error) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + @@ -747,7 +763,7 @@ TEST_F(TestComponentParser, mimic_true_invalid_config_throws_error) ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } -TEST_F(TestComponentParser, mimic_false_valid_config) +TEST_F(TestComponentParser, gripper_mimic_false_valid_config) { const auto urdf_to_test = std::string(ros2_control_test_assets::gripper_urdf_head) + 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 05998d0a89..c34e97c67f 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 @@ -857,6 +857,22 @@ const auto gripper_urdf_head_two_base_links = )"; +const auto gripper_hardware_resources_no_command_if = + R"( + + + + + + + + + + + + + )"; + const auto gripper_hardware_resources_mimic_true_no_command_if = R"(