Remove mimic parameter from ros2_control tag (#1553)

Author: christophfroehlich

hardware_interface/src/component_parser.cpp

@@ -838,98 +838,53 @@ std::vector<HardwareInfo> 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 <ros2_control> 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 <ros2_control> 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(
           "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

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<hardware_interface::HardwareInfo> 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<hardware_interface::HardwareInfo> 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 =

ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp

@@ -1664,47 +1664,6 @@ const auto gripper_hardware_resources_mimic_true_no_command_if =
   </ros2_control>
   )";
 
-// TODO(christophfroehlich) delete deprecated config test
-const auto gripper_hardware_resources_mimic_deprecated =
-  R"(
-  <ros2_control name="TestGripper" type="system">
-    <joint name="right_finger_joint">
-      <command_interface name="effort"/>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="left_finger_joint">
-      <param name="mimic">right_finger_joint</param>
-      <param name="multiplier">2</param>
-      <param name="offset">1</param>
-      <command_interface name="position"/>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-  </ros2_control>
-  )";
-
-const auto gripper_hardware_resources_mimic_deprecated_unknown_joint =
-  R"(
-  <ros2_control name="TestGripper" type="system">
-    <joint name="right_finger_joint">
-      <command_interface name="effort"/>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
-    </joint>
-    <joint name="left_finger_joint">
-      <param name="mimic">middle_finger_joint</param>
-      <param name="multiplier">1</param>
-      <command_interface name="position"/>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-    </joint>
-  </ros2_control>
-  )";
-// end delete deprecated config test
-
 const auto gripper_hardware_resources_mimic_true_command_if =
   R"(
   <ros2_control name="TestGripper" type="system">