Handle __model__ in joint parent or child when using merge-include (#…

…835) #833 allowed using __model__ in //joint/parent or //joint/child. When merge-including models that include such cases, the __model__ has to be changed to the proxy frame. Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
gazebosim · Jan 28, 2022 · 1d833c1 · 1d833c1
1 parent 0351a39
commit 1d833c1
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 4 deletions.
diff --git a/src/parser.cc b/src/parser.cc
@@ -324,6 +324,18 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF,
     {
       setAttributeToProxyFrame("relative_to", elem->GetElementImpl("pose"),
                                false);
+
+      auto parent = elem->FindElement("parent");
+      if (nullptr != parent && parent->Get<std::string>() == "__model__")
+      {
+        parent->Set(proxyModelFrameName);
+      }
+      auto child = elem->FindElement("child");
+      if (nullptr != child && child->Get<std::string>() == "__model__")
+      {
+        child->Set(proxyModelFrameName);
+      }
+
       // cppcheck-suppress syntaxError
       // cppcheck-suppress unmatchedSuppression
       if (auto axis = elem->GetElementImpl("axis"); axis)

diff --git a/test/integration/includes.cc b/test/integration/includes.cc
@@ -411,8 +411,8 @@ TEST(IncludesTest, MergeInclude)
   ASSERT_NE(nullptr, world);
   auto model = world->ModelByIndex(0);
   EXPECT_EQ("robot1", model->Name());
-  EXPECT_EQ(5u, model->LinkCount());
-  EXPECT_EQ(4u, model->JointCount());
+  EXPECT_EQ(7u, model->LinkCount());
+  EXPECT_EQ(6u, model->JointCount());
   EXPECT_EQ(1u, model->ModelCount());
   ASSERT_NE(nullptr, model->CanonicalLink());
   EXPECT_EQ(model->LinkByIndex(0), model->CanonicalLink());
@@ -521,6 +521,20 @@ TEST(IncludesTest, MergeInclude)
     EXPECT_EQ(expectedXyz, xyz);
   }
 
+  // Check joint parent set as __model__
+  {
+    // left_wheel_joint's axis is expressed in __model__.
+    auto joint = model->JointByName("test_model_parent");
+    ASSERT_NE(nullptr, joint);
+    EXPECT_EQ(prefixedFrameName, joint->ParentLinkName());
+  }
+  // Check joint child set as __model__
+  {
+    // left_wheel_joint's axis is expressed in __model__.
+    auto joint = model->JointByName("test_model_child");
+    ASSERT_NE(nullptr, joint);
+    EXPECT_EQ(prefixedFrameName, joint->ChildLinkName());
+  }
 
   // Verify that plugins get merged
   auto modelElem = model->Element();
@@ -557,8 +571,8 @@ TEST(IncludesTest, MergeIncludePlacementFrame)
   ASSERT_NE(nullptr, world);
   auto model = world->ModelByIndex(1);
   EXPECT_EQ("robot2", model->Name());
-  EXPECT_EQ(5u, model->LinkCount());
-  EXPECT_EQ(4u, model->JointCount());
+  EXPECT_EQ(7u, model->LinkCount());
+  EXPECT_EQ(6u, model->JointCount());
   auto topLink = model->LinkByName("top");
   ASSERT_NE(nullptr, topLink);
   Pose3d topLinkPose;

diff --git a/test/integration/model/merge_robot/model.sdf b/test/integration/model/merge_robot/model.sdf
@@ -289,6 +289,20 @@
       <child>caster</child>
     </joint>
 
+
+    <link name="test_child_link"/>
+    <link name="test_parent_link"/>
+
+    <joint name="test_model_parent" type="fixed">
+      <parent>__model__</parent>
+      <child>test_child_link</child>
+    </joint>
+
+    <joint name="test_model_child" type="fixed">
+      <parent>test_parent_link</parent>
+      <child>__model__</child>
+    </joint>
+
     <frame name="sensor_frame" attached_to="top">
       <pose relative_to="__model__" degrees="true">0 1 0  0 45 0</pose>
     </frame>