Support implicit nested canonical links (gazebosim#341)

Allow models without links if they have nested models instead. When building FrameAttachedToGraph, if model has no links choose the first link of the first nested model as canonical link instead. A new private function `Model::CanonicalLinkAndRelativeName` is added that provides a `Link*` pointer to the canonical link and its nested name relative to the current model, which is needed in the FrameAttachedToGraph. This private prevents duplicate code in `FrameSemantics.cc` and `Model::CanonicalLink`. The method is private to hide :: syntax from libsdformat9, at least until there is a compelling reason to make the API public. A helper function is added to FrameSemantics.cc as a friend of Model so that buildFrameAttachedToGraph can call the private API. That function can't be added directly as a friend since it uses a `FrameAttachedToGraph&` as an argument, which isn't defined in Model.hh. Signed-off-by: Steve Peters <[email protected]>
scpeters · Sep 2, 2020 · 9dcb45a · 9dcb45a
1 parent 021eb8b
commit 9dcb45a
Show file tree

Hide file tree

Showing 10 changed files with 231 additions and 24 deletions.
diff --git a/Changelog.md b/Changelog.md
@@ -46,6 +46,7 @@
 
 1. Support nested models in DOM and frame semantics.
     * [Pull request 316](https://github.com/osrf/sdformat/pull/316)
+    + [Pull request 341](https://github.com/osrf/sdformat/pull/341)
 
 1. Find python3 in cmake, fix cmake warning.
     * [Pull request 328](https://github.com/osrf/sdformat/pull/328)

diff --git a/Migration.md b/Migration.md
@@ -59,6 +59,21 @@ but with improved human-readability..
     + std::optional<std::string> GetMaxValueAsString() const;
     + bool ValidateValue() const;
 
+## SDFormat 9.0 to 9.3
+
+### Additions
+
+1. **sdf/Model.hh**
+    + uint64\_t ModelCount() const
+    + const Model \*ModelByIndex(const uint64\_t) const
+    + const Model \*ModelByName(const std::string &) const
+    + bool ModelNameExists(const std::string &) const
+
+### Modifications
+
+1. Permit models without links if they contain a nested canonical link.
+    + [pull request 341](https://github.com/osrf/sdformat/pull/341)
+
 ## SDFormat 8.x to 9.0
 
 ### Additions
@@ -92,10 +107,6 @@ but with improved human-readability..
     + const Frame \*FrameByIndex(const uint64\_t) const
     + const Frame \*FrameByName(const std::string &) const
     + bool FrameNameExists(const std::string &) const
-    + uint64\_t ModelCount() const
-    + const Model \*ModelByIndex(const uint64\_t) const
-    + const Model \*ModelByName(const std::string &) const
-    + bool ModelNameExists(const std::string &) const
     + sdf::SemanticPose SemanticPose() const
 
 1. **sdf/SDFImpl.hh**

diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh
@@ -19,6 +19,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <ignition/math/Pose3.hh>
 #include "sdf/Element.hh"
 #include "sdf/SemanticPose.hh"
@@ -238,12 +239,14 @@ namespace sdf
     public: const Link *CanonicalLink() const;
 
     /// \brief Get the name of the model's canonical link. An empty value
-    /// indicates that the first link in the model is the canonical link.
+    /// indicates that the first link in the model or the first link found
+    /// in a depth first search of nested models is the canonical link.
     /// \return The name of the canonical link.
     public: const std::string &CanonicalLinkName() const;
 
     /// \brief Set the name of the model's canonical link. An empty value
-    /// indicates that the first link in the model is the canonical link.
+    /// indicates that the first link in the model or the first link found
+    /// in a depth first search of nested models is the canonical link.
     /// \param[in] _canonicalLink The name of the canonical link.
     public: void SetCanonicalLinkName(const std::string &_canonicalLink);
 
@@ -287,9 +290,21 @@ namespace sdf
     private: sdf::Errors SetPoseRelativeToGraph(
         std::weak_ptr<const PoseRelativeToGraph> _graph);
 
+    /// \brief Get the model's canonical link and the nested name of the link
+    /// relative to the current model, delimited by "::".
+    /// \return An immutable pointer to the canonical link and the nested
+    /// name of the link relative to the current model.
+    private: std::pair<const Link *, std::string> CanonicalLinkAndRelativeName()
+        const;
+
     /// \brief Allow World::Load to call SetPoseRelativeToGraph.
     friend class World;
 
+    /// \brief Allow helper function in FrameSemantics.cc to call
+    /// CanonicalLinkAndRelativeName.
+    friend std::pair<const Link *, std::string>
+        modelCanonicalLinkAndRelativeName(const Model *);
+
     /// \brief Private data pointer.
     private: ModelPrivate *dataPtr = nullptr;
   };

diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc
@@ -15,6 +15,8 @@
  *
 */
 #include <string>
+#include <utility>
+#include <vector>
 
 #include "sdf/Element.hh"
 #include "sdf/Error.hh"
@@ -164,6 +166,17 @@ FindSinkVertex(
   return PairType(vertex, edges);
 }
 
+/////////////////////////////////////////////////
+std::pair<const Link *, std::string>
+    modelCanonicalLinkAndRelativeName(const Model *_model)
+{
+  if (nullptr == _model)
+  {
+    return std::make_pair(nullptr, "");
+  }
+  return _model->CanonicalLinkAndRelativeName();
+}
+
 /////////////////////////////////////////////////
 Errors buildFrameAttachedToGraph(
             FrameAttachedToGraph &_out, const Model *_model)
@@ -190,32 +203,47 @@ 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."});
     return errors;
   }
 
-  // identify canonical link
-  const sdf::Link *canonicalLink = nullptr;
-  if (_model->CanonicalLinkName().empty())
-  {
-    canonicalLink = _model->LinkByIndex(0);
-  }
-  else
-  {
-    canonicalLink = _model->LinkByName(_model->CanonicalLinkName());
-  }
+  // identify canonical link, which may be nested
+  auto canonicalLinkAndName = modelCanonicalLinkAndRelativeName(_model);
+  const sdf::Link *canonicalLink = canonicalLinkAndName.first;
+  const std::string canonicalLinkName = canonicalLinkAndName.second;
   if (nullptr == canonicalLink)
   {
+    if (canonicalLinkName.empty())
+    {
+      errors.push_back({ErrorCode::MODEL_WITHOUT_LINK,
+                       "A model must have at least one link."});
+    }
+    else
+    {
+      errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID,
+        "canonical_link with name[" + canonicalLinkName +
+        "] not found in model with name[" + _model->Name() + "]."});
+    }
     // return early
-    errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID,
-      "canonical_link with name[" + _model->CanonicalLinkName() +
-      "] not found in model with name[" + _model->Name() + "]."});
     return errors;
   }
 
+  // Identify if the canonical link is in a nested model.
+  if (_model->LinkByName(canonicalLink->Name()) != canonicalLink)
+  {
+    // The canonical link is nested, so its vertex should be added
+    // here with an edge from __model__.
+    // The nested canonical link name should be a nested name
+    // relative to _model, delimited by "::".
+    auto linkId =
+        _out.graph.AddVertex(canonicalLinkName, sdf::FrameType::LINK).Id();
+    _out.map[canonicalLinkName] = linkId;
+    _out.graph.AddEdge({modelFrameId, linkId}, true);
+  }
+
   // add link vertices
   for (uint64_t l = 0; l < _model->LinkCount(); ++l)
   {

diff --git a/src/Model.cc b/src/Model.cc
@@ -276,10 +276,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."});
@@ -710,14 +711,43 @@ const Model *Model::ModelByName(const std::string &_name) const
 
 /////////////////////////////////////////////////
 const Link *Model::CanonicalLink() const
+{
+  return this->CanonicalLinkAndRelativeName().first;
+}
+
+/////////////////////////////////////////////////
+std::pair<const Link*, std::string> Model::CanonicalLinkAndRelativeName() const
 {
   if (this->CanonicalLinkName().empty())
   {
-    return this->LinkByIndex(0);
+    if (this->LinkCount() > 0)
+    {
+      auto firstLink = this->LinkByIndex(0);
+      return std::make_pair(firstLink, firstLink->Name());
+    }
+    else if (this->ModelCount() > 0)
+    {
+      // Recursively choose the canonical link of the first nested model
+      // (depth first search).
+      auto firstModel = this->ModelByIndex(0);
+      auto canonicalLinkAndName = firstModel->CanonicalLinkAndRelativeName();
+      // Prepend firstModelName if a valid link is found.
+      if (nullptr != canonicalLinkAndName.first)
+      {
+        canonicalLinkAndName.second =
+            firstModel->Name() + "::" + canonicalLinkAndName.second;
+      }
+      return canonicalLinkAndName;
+    }
+    else
+    {
+      return std::make_pair(nullptr, "");
+    }
   }
   else
   {
-    return this->LinkByName(this->CanonicalLinkName());
+    return std::make_pair(this->LinkByName(this->CanonicalLinkName()),
+                          this->CanonicalLinkName());
   }
 }
 

diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc
@@ -358,6 +358,32 @@ 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_invalid_explicit_canonical_link.sdf";
+
+    // Check nested_invalid_explicit_canonical_link.sdf
+    std::string output =
+      custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+    EXPECT_NE(output.find("Error: canonical_link with name[nested::link] not "
+                          "found in model with name[top]."),
+              std::string::npos) << output;
+    EXPECT_NE(output.find("Error: A model must have at least one link."),
+              std::string::npos) << output;
+  }
+
   // Check an invalid SDF file that uses reserved names.
   {
     std::string path = pathBase +"/model_invalid_reserved_names.sdf";

diff --git a/test/integration/model_dom.cc b/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,66 @@ 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_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));
+
+  EXPECT_EQ(2u, model->ModelCount());
+  EXPECT_TRUE(model->ModelNameExists("nested"));
+  EXPECT_TRUE(model->ModelNameExists("shallow"));
+  EXPECT_EQ(model->ModelByName("nested"), model->ModelByIndex(0));
+  EXPECT_EQ(model->ModelByName("shallow"), model->ModelByIndex(1));
+  EXPECT_EQ(nullptr, model->ModelByIndex(2));
+
+  // expect implicit canonical link
+  EXPECT_TRUE(model->CanonicalLinkName().empty());
+
+  // frame F is attached to __model__ and resolves to canonical link,
+  // which is "nested::link2"
+  std::string body;
+  EXPECT_TRUE(model->FrameByName("F")->ResolveAttachedToBody(body).empty());
+  EXPECT_EQ("nested::link2", body);
+
+  EXPECT_EQ(model->ModelByName("nested")->LinkByName("link2"),
+            model->CanonicalLink());
+  // this reports the local name, not the nested name "nested::link2"
+  EXPECT_EQ("link2", model->CanonicalLink()->Name());
+
+  const sdf::Model *shallowModel = model->ModelByName("shallow");
+  EXPECT_EQ(1u, shallowModel->FrameCount());
+  EXPECT_TRUE(
+      shallowModel->FrameByName("F")->ResolveAttachedToBody(body).empty());
+  EXPECT_EQ("deep::deeper::deepest::deepest_link", body);
 }
 
diff --git a/test/sdf/model_canonical_link.sdf b/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>
diff --git a/test/sdf/nested_canonical_link.sdf b/test/sdf/nested_canonical_link.sdf
@@ -0,0 +1,19 @@
+<sdf version='1.7'>
+  <model name="top">
+    <frame name="F"/>
+    <model name="nested" canonical_link="link2">
+      <link name="link1"/>
+      <link name="link2"/>
+    </model>
+    <model name="shallow">
+      <frame name="F"/>
+      <model name="deep">
+        <model name="deeper">
+          <model name="deepest">
+            <link name="deepest_link"/>
+          </model>
+        </model>
+      </model>
+    </model>
+  </model>
+</sdf>
diff --git a/test/sdf/nested_invalid_explicit_canonical_link.sdf b/test/sdf/nested_invalid_explicit_canonical_link.sdf
@@ -0,0 +1,14 @@
+<sdf version='1.7'>
+  <model name="top" canonical_link="nested::link">
+    <model name="nested">
+      <link name="link"/>
+    </model>
+    <model name="nested_without_links1">
+      <model name="nested_without_links2">
+        <model name="nested_without_links3">
+          <frame name="F"/>
+        </model>
+      </model>
+    </model>
+  </model>
+</sdf>