Merge c02e63d into 2905643

gazebosim · Nov 11, 2021 · 854315c · 854315c
2 parents 2905643 + c02e63d
commit 854315c
Show file tree

Hide file tree

Showing 3 changed files with 134 additions and 10 deletions.
diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
@@ -3588,17 +3588,18 @@ void ReduceSDFExtensionPluginFrameReplace(
 {
   std::string linkName = _link->name;
   std::string parentLinkName = _link->getParent()->name;
-  if ((*_blobIt)->FirstChildElement()->Name() == _pluginName)
+  tinyxml2::XMLElement *rootElement = (*_blobIt)->FirstChildElement();
+  if (rootElement->Name() == _pluginName)
   {
     // replace element containing _link names to parent link names
     // find first instance of xyz and rpy, replace with reduction transform
     tinyxml2::XMLNode *elementNode =
-      (*_blobIt)->FirstChildElement(_elementName.c_str());
+      rootElement->FirstChildElement(_elementName.c_str());
     if (elementNode)
     {
       if (GetKeyValueAsString(elementNode->ToElement()) == linkName)
       {
-        (*_blobIt)->DeleteChild(elementNode);
+        rootElement->DeleteChild(elementNode);
         auto* doc = elementNode->GetDocument();
         tinyxml2::XMLElement *bodyNameKey =
           doc->NewElement(_elementName.c_str());
@@ -3607,27 +3608,27 @@ void ReduceSDFExtensionPluginFrameReplace(
         tinyxml2::XMLText *bodyNameTxt =
           doc->NewText(bodyNameStream.str().c_str());
         bodyNameKey->LinkEndChild(bodyNameTxt);
-        (*_blobIt)->LinkEndChild(bodyNameKey);
+        rootElement->LinkEndChild(bodyNameKey);
         /// @todo update transforms for this sdf plugin too
 
         // look for offset transforms, add reduction transform
-        tinyxml2::XMLNode *xyzKey = (*_blobIt)->FirstChildElement("xyzOffset");
+        tinyxml2::XMLNode *xyzKey = rootElement->FirstChildElement("xyzOffset");
         if (xyzKey)
         {
           urdf::Vector3 v1 = ParseVector3(xyzKey);
           _reductionTransform.Pos() =
             ignition::math::Vector3d(v1.x, v1.y, v1.z);
           // remove xyzOffset and rpyOffset
-          (*_blobIt)->DeleteChild(xyzKey);
+          rootElement->DeleteChild(xyzKey);
         }
-        tinyxml2::XMLNode *rpyKey = (*_blobIt)->FirstChildElement("rpyOffset");
+        tinyxml2::XMLNode *rpyKey = rootElement->FirstChildElement("rpyOffset");
         if (rpyKey)
         {
           urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0);
           _reductionTransform.Rot() =
             ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z);
           // remove xyzOffset and rpyOffset
-          (*_blobIt)->DeleteChild(rpyKey);
+          rootElement->DeleteChild(rpyKey);
         }
 
         // pass through the parent transform from fixed joint reduction
@@ -3661,8 +3662,8 @@ void ReduceSDFExtensionPluginFrameReplace(
         xyzKey->LinkEndChild(xyzTxt);
         rpyKey->LinkEndChild(rpyTxt);
 
-        (*_blobIt)->LinkEndChild(xyzKey);
-        (*_blobIt)->LinkEndChild(rpyKey);
+        rootElement->LinkEndChild(xyzKey);
+        rootElement->LinkEndChild(rpyKey);
       }
     }
   }

diff --git a/test/integration/fixed_joint_reduction.cc b/test/integration/fixed_joint_reduction.cc
@@ -36,6 +36,8 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT[] =
     "fixed_joint_reduction_collision_visual_empty_root.urdf";
 const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] =
     "fixed_joint_reduction_collision_visual_empty_root.sdf";
+const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] =
+    "fixed_joint_reduction_plugin_frame_extension.urdf";
 
 static std::string GetFullTestFilePath(const char *_input)
 {
@@ -734,3 +736,21 @@ TEST(SDFParser, FixedJointReductionSimple)
     EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance);
   }
 }
+
+/////////////////////////////////////////////////
+// This test uses a urdf that has chained fixed joints with plugin that
+// contains bodyName, xyzOffset and rpyOffset.
+// Test to make sure that the bodyName is updated to the new base link.
+TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest)
+{
+  sdf::SDFPtr robot(new sdf::SDF());
+  sdf::init(robot);
+  ASSERT_TRUE(sdf::readFile(
+      GetFullTestFilePath(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION), robot));
+
+  sdf::ElementPtr model = robot->Root()->GetElement("model");
+  sdf::ElementPtr plugin = model->GetElement("plugin");
+
+  auto bodyName = plugin->Get<std::string>("bodyName");
+  EXPECT_EQ("base_link", bodyName);
+}
diff --git a/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf b/test/integration/fixed_joint_reduction_plugin_frame_extension.urdf
@@ -0,0 +1,103 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="chained_fixed_joint_links">
+  <gazebo>
+    <plugin name='test_plugin' filename='libtest_plugin.so'>
+      <serviceName>/test/plugin/service</serviceName>
+      <topicName>/test/plugin/topic</topicName>
+      <bodyName>link2</bodyName>
+      <updateRate>100</updateRate>
+      <xyzOffset>0 0 0</xyzOffset>
+      <rpyOffset>0 0 0</rpyOffset>
+    </plugin>
+  </gazebo>
+
+  <!-- Base Link -->
+  <link name="base_link">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="1.0 1.0 1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 1.0"/>
+      <geometry>
+        <box size="0.1 0.1 2"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <origin xyz="0 0 1" rpy="0 0 0"/>
+      <mass value="1"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+  </link>
+
+  <!-- Link 1 -->
+  <link name="link1">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="1.0 1.0 1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <origin xyz="0 0 1" rpy="0 0 0"/>
+      <mass value="1"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+  </link>
+
+  <!-- Link 2 -->
+  <link name="link2">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="1.0 1.0 1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <origin xyz="0 0 1" rpy="0 0 0"/>
+      <mass value="1"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+  </link>
+
+  <!-- Joint 1 -->
+  <joint name="joint1" type="fixed">
+    <parent link="base_link"/>
+    <child link="link1"/>
+    <origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
+    <dynamics damping="0.7"/>
+  </joint>
+
+  <!-- Joint 2 -->
+  <joint name="joint2" type="fixed">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="0.7"/>
+  </joint>
+</robot>
+