Try supporting nested canonical links

Relax requirements that every model must have a link
if it has nested models instead.
When building FrameAttachedToGraph, if model has no links
look for the first link of the first nested model instead.
This doesn't yet supported explicitly nested canonical links
with :: syntax in `//model/@canonical_link`.
Repeat the logic for Model::CanonicalLink method.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

src/FrameSemantics.cc

@@ -190,7 +190,7 @@ Errors buildFrameAttachedToGraph(
         "Invalid model element in sdf::Model."});
     return errors;
   }
-  else if (_model->LinkCount() < 1)
+  else if (_model->LinkCount() < 1 && _model->ModelCount() < 1)
   {
     errors.push_back({ErrorCode::MODEL_WITHOUT_LINK,
                      "A model must have at least one link."});
@@ -201,7 +201,32 @@ Errors buildFrameAttachedToGraph(
   const sdf::Link *canonicalLink = nullptr;
   if (_model->CanonicalLinkName().empty())
   {
-    canonicalLink = _model->LinkByIndex(0);
+    if (_model->LinkCount() >= 1)
+    {
+      canonicalLink = _model->LinkByIndex(0);
+    }
+    else
+    {
+      // Choose the first link of the first nested model
+      // as the canonical link.
+      // This does not recurse past the first nested model.
+      auto firstNestedModel = _model->ModelByIndex(0);
+      if (firstNestedModel->LinkCount() < 1)
+      {
+        errors.push_back({ErrorCode::MODEL_WITHOUT_LINK,
+                         "A model must have at least one link."});
+        return errors;
+      }
+      canonicalLink = firstNestedModel->LinkByIndex(0);
+
+      // Add vertex for this nested link with edge from __model__.
+      auto canonicalLinkName =
+          firstNestedModel->Name() + "::" + canonicalLink->Name();
+      auto linkId =
+          _out.graph.AddVertex(canonicalLinkName, sdf::FrameType::LINK).Id();
+      _out.map[canonicalLinkName] = linkId;
+      _out.graph.AddEdge({modelFrameId, linkId}, true);
+    }
   }
   else
   {

src/Model.cc

@@ -269,10 +269,11 @@ Errors Model::Load(ElementPtr _sdf)
     frameNames.insert(linkName);
   }
 
-  // If the model is not static:
+  // If the model is not static and has no nested models:
   // Require at least one link so the implicit model frame can be attached to
   // something.
-  if (!this->Static() && this->dataPtr->links.empty())
+  if (!this->Static() && this->dataPtr->links.empty() &&
+      this->dataPtr->models.empty())
   {
     errors.push_back({ErrorCode::MODEL_WITHOUT_LINK,
                      "A model must have at least one link."});
@@ -620,7 +621,26 @@ const Link *Model::CanonicalLink() const
 {
   if (this->CanonicalLinkName().empty())
   {
-    return this->LinkByIndex(0);
+    if (this->LinkCount() >= 1)
+    {
+      return this->LinkByIndex(0);
+    }
+    else if (this->ModelCount() >= 1)
+    {
+      // Choose the first link of the first nested model
+      // as the canonical link.
+      // This does not recurse past the first nested model.
+      auto firstNestedModel = this->ModelByIndex(0);
+      if (firstNestedModel->LinkCount() < 1)
+      {
+        return nullptr;
+      }
+      return firstNestedModel->LinkByIndex(0);
+    }
+    else
+    {
+      return nullptr;
+    }
   }
   else
   {

src/ign_TEST.cc

@@ -296,6 +296,28 @@ TEST(check, SDF)
     EXPECT_EQ("Valid.\n", output) << output;
   }
 
+  // Check an SDF file with a model that has a nested canonical link.
+  {
+    std::string path = pathBase +"/nested_canonical_link.sdf";
+
+    // Check nested_canonical_link.sdf
+    std::string output =
+      custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+    EXPECT_EQ("Valid.\n", output) << output;
+  }
+
+  // Check an SDF file with a model that has a nested canonical link
+  // that is explicitly specified by //model/@canonical_link using ::
+  // syntax.
+  {
+    std::string path = pathBase +"/nested_explicit_canonical_link.sdf";
+
+    // Check nested_explicit_canonical_link.sdf
+    std::string output =
+      custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+    EXPECT_EQ("Valid.\n", output) << output;
+  }
+
   // Check an invalid SDF file that uses reserved names.
   {
     std::string path = pathBase +"/model_invalid_reserved_names.sdf";

test/integration/model_dom.cc

@@ -22,6 +22,7 @@
 #include "sdf/Element.hh"
 #include "sdf/Error.hh"
 #include "sdf/Filesystem.hh"
+#include "sdf/Frame.hh"
 #include "sdf/Link.hh"
 #include "sdf/Model.hh"
 #include "sdf/Root.hh"
@@ -313,5 +314,49 @@ TEST(DOMRoot, LoadCanonicalLink)
 
   EXPECT_EQ(0u, model->JointCount());
   EXPECT_EQ(nullptr, model->JointByIndex(0));
+
+  EXPECT_EQ(1u, model->FrameCount());
+  EXPECT_NE(nullptr, model->FrameByIndex(0));
+  EXPECT_EQ(nullptr, model->FrameByIndex(1));
+
+  std::string body;
+  EXPECT_TRUE(model->FrameByName("F")->ResolveAttachedToBody(body).empty());
+  EXPECT_EQ("link2", body);
+}
+
+/////////////////////////////////////////////////
+TEST(DOMRoot, LoadNestedCanonicalLink)
+{
+  const std::string testFile =
+    sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf",
+        "nested_canonical_link.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  EXPECT_TRUE(root.Load(testFile).empty());
+
+  // Get the first model
+  const sdf::Model *model = root.ModelByIndex(0);
+  ASSERT_NE(nullptr, model);
+  EXPECT_EQ("top", model->Name());
+  EXPECT_EQ(0u, model->LinkCount());
+  EXPECT_EQ(nullptr, model->LinkByIndex(0));
+
+  EXPECT_TRUE(model->CanonicalLinkName().empty());
+
+  ASSERT_NE(nullptr, model->CanonicalLink());
+  // this reports the local name, not the nested name "nested::link"
+  EXPECT_EQ("link", model->CanonicalLink()->Name());
+
+  EXPECT_EQ(0u, model->JointCount());
+  EXPECT_EQ(nullptr, model->JointByIndex(0));
+
+  EXPECT_EQ(1u, model->FrameCount());
+  EXPECT_NE(nullptr, model->FrameByIndex(0));
+  EXPECT_EQ(nullptr, model->FrameByIndex(1));
+
+  std::string body;
+  EXPECT_TRUE(model->FrameByName("F")->ResolveAttachedToBody(body).empty());
+  EXPECT_EQ("nested::link", body);
 }
 

test/sdf/model_canonical_link.sdf

@@ -7,5 +7,6 @@
     <link name="link2">
       <pose>0 2 0 0 0 0</pose>
     </link>
+    <frame name="F" attached_to="__model__"/>
   </model>
 </sdf>

test/sdf/nested_canonical_link.sdf

@@ -0,0 +1,8 @@
+<sdf version='1.7'>
+  <model name="top">
+    <frame name="F"/>
+    <model name="nested">
+      <link name="link"/>
+    </model>
+  </model>
+</sdf>

test/sdf/nested_explicit_canonical_link.sdf

@@ -0,0 +1,7 @@
+<sdf version='1.7'>
+  <model name="top" canonical_link="nested::link">
+    <model name="nested">
+      <link name="link"/>
+    </model>
+  </model>
+</sdf>