Handle __model__ as //joint/child in merge included interface joint

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
gazebosim · Jan 28, 2022 · 1991fbf · 1991fbf
1 parent 2d16a82
commit 1991fbf
Show file tree

Hide file tree

Showing 3 changed files with 59 additions and 6 deletions.
diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc
@@ -470,7 +470,7 @@ struct ModelWrapper : public WrapperBase
       this->frames.emplace_back(proxyModelFrameName,
                                 nestedInclude->IncludeRawPose().value_or(
                                     model->ModelFramePoseInParentFrame()),
-                                poseRelativeTo, this->canonicalLinkName);
+                                poseRelativeTo, model->CanonicalLinkName());
 
       for (const auto &item : model->Links())
       {
@@ -489,10 +489,13 @@ struct ModelWrapper : public WrapperBase
       }
       for (const auto &item : model->Joints())
       {
-        // TODO(azeey) In theory, the child could be "__model__" in which case,
-        // we'd have to use the proxy frame here, but not sure if "__model__" is
-        // allowed for //joint/child.
-        this->joints.emplace_back(item);
+        std::string childName = item.ChildName();
+        if (item.ChildName() == "__model__")
+        {
+          childName = proxyModelFrameName;
+        }
+        this->joints.emplace_back(sdf::InterfaceJoint(item.Name(), childName,
+                                                      item.PoseInChildFrame()));
       }
       if (nestedInclude->PlacementFrame().has_value())
       {

diff --git a/test/integration/interface_api.cc b/test/integration/interface_api.cc
@@ -968,7 +968,7 @@ TEST_F(InterfaceAPIMergeInclude, Reposturing)
       return nullptr;
 
     // Use parent name because we know merge=true
-    const std::string absoluteModelName = _include.AbsoluteParentName();
+    const std::string &absoluteModelName = _include.AbsoluteParentName();
     // The following is equivalent to
     // <model name="M0"> <!-- Merged into parent model
     //   <pose relative_to="F1">0 0 0  π/2 0 0</pose> <!-- From //include -->
@@ -1218,3 +1218,32 @@ R"(<include>
   printConfig.SetPreserveIncludes(true);
   EXPECT_EQ(includeElem->ToString("", printConfig), expectedIncludeStr);
 }
+
+/////////////////////////////////////////////////
+TEST_F(InterfaceAPIMergeInclude, JointModelChild)
+{
+    const std::string testSdf = R"(
+  <sdf version="1.9">
+    <model name="parent_model">
+      <link name="L1"/>
+      <include merge="true">
+        <uri>joint_model_parent_or_child.toml</uri>
+      </include>
+      <frame name="frame_1" attached_to="joint_model_child"/>
+    </model>
+  </sdf>)";
+
+  this->config.RegisterCustomModelParser(customTomlParser);
+  sdf::Root root;
+  sdf::Errors errors = root.LoadSdfString(testSdf, this->config);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  const sdf::Model *model = root.Model();
+  ASSERT_NE(nullptr, model);
+  {
+    auto frame = model->FrameByName("frame_1");
+    std::string body;
+    frame->ResolveAttachedToBody(body);
+    EXPECT_EQ("base", body);
+  }
+}
diff --git a/test/integration/model/joint_model_parent_or_child.toml b/test/integration/model/joint_model_parent_or_child.toml
@@ -0,0 +1,21 @@
+# This file is part of the interface_api integration test. The test contains a
+# very limited toml parser that is not capable of parsing the full toml syntax.
+# Specifically, arrays are not supported.
+name = "joint_model_child"
+pose = "0 0 0 0 0 0"
+canonical_link = "base"
+
+[links.base]
+pose = "0 0 0 0 0 0"
+
+# Test use of __model__ in joint parent or child
+[links.parent_link]
+pose = "0 0 0 0 0 0"
+
+[links.child_link]
+pose = "0 0 0 0 0 0"
+
+[joints.joint_model_child]
+parent = "parent_link"
+child = "__model__"
+type = "fixed"