Change mimic attribute to default true

ros-controls · Jan 3, 2024 · a2a50d9 · a2a50d9
1 parent 03fc973
commit a2a50d9
Show file tree

Hide file tree

Showing 4 changed files with 81 additions and 41 deletions.
diff --git 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 <optional>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -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<bool> 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
@@ -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<HardwareInfo> 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<HardwareInfo> 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 <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);
         }
       }
     }

diff --git 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<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);
+}
+
 // 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
@@ -857,6 +857,22 @@ const auto gripper_urdf_head_two_base_links =
   </joint>
 )";
 
+const auto gripper_hardware_resources_no_command_if =
+  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">
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
+  )";
+
 const auto gripper_hardware_resources_mimic_true_no_command_if =
   R"(
   <ros2_control name="TestGripper" type="system">