Add support for merge-includes in worlds (#1233)

Implements merge-includes for worlds as discussed in gazebosim/sdf_tutorials#86. 

Signed-off-by: Addisu Z. Taddese <[email protected]>
Co-authored-by: Steve Peters <[email protected]>

Migration.md

@@ -558,6 +558,7 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1.
 ### Additions
 
 1. **world.sdf**: A joint can be specified directly in a world.
+1. **world.sdf**: Merge-includes are now allowed in worlds. The included models must not contain top-level links or grippers.
 
 ### Modifications
 

sdf/1.10/world.sdf

@@ -25,6 +25,9 @@
     <description>
         Include resources from a URI. Included resources can only contain one 'model', 'light' or 'actor' element. The URI can point to a directory or a file. If the URI is a directory, it must conform to the model database structure (see /tutorials?tut=composition&amp;cat=specification&amp;#defining-models-in-separate-files).
     </description>
+    <attribute name="merge" type="bool" default="false" required="0">
+      <description>Merge the included model into the top model. Only elements valid in 'world' are allowed in the included model</description>
+    </attribute>
     <element name="uri" type="string" default="__default__" required="1">
       <description>URI to a resource, such as a model</description>
     </element>

src/parser.cc

@@ -220,7 +220,7 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF,
     _errors.push_back(unsupportedError);
     return;
   }
-  else if (_parent->GetName() != "model")
+  else if (_parent->GetName() != "model" && _parent->GetName() != "world")
   {
     Error unsupportedError(
         ErrorCode::MERGE_INCLUDE_UNSUPPORTED,
@@ -281,11 +281,18 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF,
   // //include/pose/@relative_to.
   std::string modelPoseRelativeTo = model->PoseRelativeTo();
 
-  // If empty, use "__model__", since leaving it empty would make it
+  // If empty, use "__model__" or "world", since leaving it empty would make it
   // relative_to the canonical link frame specified in //frame/@attached_to.
   if (modelPoseRelativeTo.empty())
   {
-    modelPoseRelativeTo = "__model__";
+    if (_parent->GetName() == "model")
+    {
+      modelPoseRelativeTo = "__model__";
+    }
+    else
+    {
+      modelPoseRelativeTo = "world";
+    }
   }
 
   proxyModelFramePose->GetAttribute("relative_to")->Set(modelPoseRelativeTo);
@@ -363,6 +370,17 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF,
         (elem->GetName() == "gripper") || (elem->GetName() == "plugin") ||
         (elem->GetName().find(':') != std::string::npos))
     {
+      if (_parent->GetName() == "world" &&
+          (elem->GetName() == "link" || elem->GetName() == "gripper"))
+      {
+        Error unsupportedError(
+            ErrorCode::MERGE_INCLUDE_UNSUPPORTED,
+            "Merge-include for <world> does not support element of type " +
+                elem->GetName() + " in included model");
+        _sourceLoc.SetSourceLocationOnError(unsupportedError);
+        _errors.push_back(unsupportedError);
+        continue;
+      }
       _parent->InsertElement(elem, true);
     }
   }

test/integration/includes.cc

@@ -555,6 +555,72 @@ TEST(IncludesTest, MergeInclude)
   EXPECT_FALSE(modelElem->HasElement("static"));
 }
 
+//////////////////////////////////////////////////
+TEST(IncludesTest, WorldMergeInclude)
+{
+  using gz::math::Pose3d;
+  sdf::ParserConfig config;
+  config.SetFindCallback(findFileCb);
+
+  sdf::Root root;
+  sdf::Errors errors = root.Load(
+      sdf::testing::TestFile("sdf", "world_merge_include.sdf"),
+      config);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  auto *world = root.WorldByIndex(0);
+  ASSERT_NE(nullptr, world);
+
+  std::string proxyFrameName;
+    proxyFrameName = "_merged__model_for_world_merge_include__model__";
+
+  auto *proxyFrame = world->FrameByName(proxyFrameName);
+  ASSERT_NE(nullptr, proxyFrame);
+
+  EXPECT_EQ(Pose3d(100, 0, 0, 0, 0, 0), proxyFrame->RawPose());
+
+  // Expect that the merged models have poses relative to the proxy frame
+  ASSERT_EQ(2, world->ModelCount());
+  auto *model1 = world->ModelByIndex(0);
+  ASSERT_NE(nullptr, model1);
+  EXPECT_EQ(proxyFrameName, model1->PoseRelativeTo());
+
+  auto *model2 = world->ModelByIndex(1);
+  ASSERT_NE(nullptr, model2);
+  EXPECT_EQ(proxyFrameName, model2->PoseRelativeTo());
+
+  // F1, since it had an empty attached_to in the original model, once it's
+  // merged, it should be attached to the proxy frame
+  auto *frame1 = world->FrameByName("F1");
+  ASSERT_NE(nullptr, frame1);
+  EXPECT_EQ(proxyFrameName, frame1->AttachedTo());
+
+  // Check that the joint was also merged
+  auto *joint1 = world->JointByName("J1");
+  ASSERT_NE(nullptr, joint1);
+}
+
+//////////////////////////////////////////////////
+TEST(IncludesTest, WorldMergeIncludeInvalidElements)
+{
+  std::string sdfString = R"(
+  <sdf version="1.10">
+    <world name="invalid_world">
+      <include merge="true">
+        <uri>test_model</uri>
+      </include>
+    </world>
+  </sdf>)";
+
+  sdf::ParserConfig config;
+  config.SetFindCallback(findFileCb);
+
+  sdf::Root root;
+  sdf::Errors errors = root.LoadSdfString(sdfString, config);
+  ASSERT_FALSE(errors.empty());
+  EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED);
+}
+
 //////////////////////////////////////////////////
 TEST(IncludesTest, MergeIncludePlacementFrame)
 {
@@ -657,23 +723,25 @@ TEST(IncludesTest, InvalidMergeInclude)
     EXPECT_EQ(4, *errors[0].LineNumber());
   }
 
-  // merge-include cannot be used directly under //world
+  // merge-include can only be used directly under //model or //world
   {
     const std::string sdfString = R"(
     <sdf version="1.9">
       <world name="default">
-        <include merge="true">
-          <uri>file://merge_robot</uri> <!-- NOLINT -->
-        </include>
+        <frame name="test_frame">
+          <include merge="true">
+            <uri>file://merge_robot</uri> <!-- NOLINT -->
+          </include>
+        </frame>
       </world>
     </sdf>)";
     sdf::Root root;
     sdf::Errors errors = root.LoadSdfString(sdfString, config);
     ASSERT_FALSE(errors.empty());
     EXPECT_EQ(sdf::ErrorCode::MERGE_INCLUDE_UNSUPPORTED, errors[0].Code());
-    EXPECT_EQ("Merge-include does not support parent element of type world",
+    EXPECT_EQ("Merge-include does not support parent element of type frame",
               errors[0].Message());
-    EXPECT_EQ(4, errors[0].LineNumber());
+    EXPECT_EQ(5, errors[0].LineNumber());
   }
 
   // Syntax error in included file

test/integration/model/model_for_world_merge_include.sdf

@@ -0,0 +1,21 @@
+<?xml version="1.0" ?>
+<sdf version="1.10">
+  <model name="model_for_world_merge_include">
+    <include>
+      <uri>test_model</uri>
+      <name>M1</name>
+    </include>
+    <include>
+      <uri>test_model</uri>
+      <name>M2</name>
+      <pose>0 10 0  0 0 0</pose>
+    </include>
+    <frame name="F1"/>
+    <frame name="F2" attached_to="M2"/>
+
+    <joint name="J1" type="fixed">
+      <parent>F1</parent>
+      <child>F2</child>
+    </joint>
+  </model>
+</sdf>

test/sdf/world_merge_include.sdf

@@ -0,0 +1,10 @@
+<?xml version="1.0" ?>
+<sdf version="1.10">
+  <world name="world_merge_include">
+    <include merge="true">
+      <!-- The path for this model has to be added by the test -->
+      <uri>model_for_world_merge_include.sdf</uri>
+      <pose>100 0 0   0 0 0</pose>
+    </include>
+  </world>
+</sdf>