From 3bdc0816effef72e351c96c08aa7c83d9e8949ca Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 May 2024 18:07:17 +0000 Subject: [PATCH] Remove mimic parameter from ros2_control tag --- hardware_interface/src/component_parser.cpp | 105 +++++------------- .../test/test_component_parser.cpp | 29 ----- .../ros2_control_test_assets/descriptions.hpp | 41 ------- 3 files changed, 30 insertions(+), 145 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index ef585c971b..17c4c9fddf 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -836,91 +836,45 @@ std::vector parse_control_resources_from_urdf(const std::string & for (auto i = 0u; i < hw_info.joints.size(); ++i) { const auto & joint = hw_info.joints.at(i); - - // Search for mimic joints defined in ros2_control tag (deprecated) - // TODO(christophfroehlich) delete deprecated config with ROS-J - if (joint.parameters.find("mimic") != joint.parameters.cend()) + auto urdf_joint = model.getJoint(joint.name); + if (!urdf_joint) { - std::cerr << "Warning: Mimic joints defined in ros2_control tag are deprecated. " - << "Please define mimic joints in URDF." << std::endl; - const auto mimicked_joint_it = std::find_if( - hw_info.joints.begin(), hw_info.joints.end(), - [&mimicked_joint = - joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info) - { return joint_info.name == mimicked_joint; }); - if (mimicked_joint_it == hw_info.joints.cend()) - { - throw std::runtime_error( - "Mimicked joint '" + joint.parameters.at("mimic") + "' not found"); - } - hardware_interface::MimicJoint mimic_joint; - mimic_joint.joint_index = i; - mimic_joint.multiplier = 1.0; - mimic_joint.offset = 0.0; - mimic_joint.mimicked_joint_index = std::distance(hw_info.joints.begin(), mimicked_joint_it); - auto param_it = joint.parameters.find("multiplier"); - if (param_it != joint.parameters.end()) - { - mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); - } - param_it = joint.parameters.find("offset"); - if (param_it != joint.parameters.end()) - { - mimic_joint.offset = hardware_interface::stod(joint.parameters.at("offset")); - } - hw_info.mimic_joints.push_back(mimic_joint); + throw std::runtime_error("Joint " + joint.name + " not found in URDF"); } - else + if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE) { - auto urdf_joint = model.getJoint(joint.name); - if (!urdf_joint) - { - throw std::runtime_error("Joint " + joint.name + " not found in URDF"); - } - if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE) + throw std::runtime_error( + "Joint '" + joint.name + "' has no mimic information in the URDF."); + } + if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE) + { + if (joint.command_interfaces.size() > 0) { throw std::runtime_error( - "Joint '" + joint.name + "' has no mimic information in the URDF."); + "Joint '" + joint.name + + "' has mimic attribute not set to false: Activated mimic joints cannot have command " + "interfaces."); } - if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE) + auto find_joint = [&hw_info](const std::string & name) { - if (joint.command_interfaces.size() > 0) + 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 '" + joint.name + - "' has mimic attribute not set to false: Activated mimic joints cannot have command " - "interfaces."); + throw std::runtime_error("Mimic joint '" + name + "' not found in tag"); } - 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( - "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); - } - } - // 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; + 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); } + if (urdf_joint->type == urdf::Joint::FIXED) { throw std::runtime_error( @@ -928,6 +882,7 @@ std::vector parse_control_resources_from_urdf(const std::string & "' 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 diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 2e2cae9807..be891787f3 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1341,35 +1341,6 @@ TEST_F(TestComponentParser, gripper_no_mimic_valid_config) EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1); } -// TODO(christophfroehlich) delete deprecated config test -TEST_F(TestComponentParser, gripper_mimic_deprecated_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_mimic_deprecated) + - 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); -} - -TEST_F(TestComponentParser, gripper_mimic_deprecated_unknown_joint_throws_error) -{ - const auto urdf_to_test = - std::string(ros2_control_test_assets::gripper_urdf_head) + - std::string( - ros2_control_test_assets::gripper_hardware_resources_mimic_deprecated_unknown_joint) + - std::string(ros2_control_test_assets::urdf_tail); - std::vector hw_info; - ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); -} -// end delete deprecated config test - TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error) { const auto urdf_to_test = 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 31e630059b..7923d09c3b 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 @@ -1290,47 +1290,6 @@ const auto gripper_hardware_resources_mimic_true_no_command_if = )"; -// TODO(christophfroehlich) delete deprecated config test -const auto gripper_hardware_resources_mimic_deprecated = - R"( - - - - - - - - - right_finger_joint - 2 - 1 - - - - - - )"; - -const auto gripper_hardware_resources_mimic_deprecated_unknown_joint = - R"( - - - - - - - - - middle_finger_joint - 1 - - - - - - )"; -// end delete deprecated config test - const auto gripper_hardware_resources_mimic_true_command_if = R"(