Support implicit nested canonical links

Changelog.md

@@ -6,6 +6,7 @@
 
 1. Support nested models in DOM and frame semantics.
  * [Pull request 316](https:/osrf/sdformat/pull/316)
+ + [Pull request 341](https:/osrf/sdformat/pull/341)
 
 1. Find python3 in cmake, fix cmake warning.
  * [Pull request 328](https:/osrf/sdformat/pull/328)

Migration.md

@@ -12,6 +12,21 @@ forward programmatically.
 This document aims to contain similar information to those files
 but with improved human-readability..
 
+## 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:/osrf/sdformat/pull/341)
+
 ## SDFormat 8.x to 9.0
 
 ### Additions
@@ -45,10 +60,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**

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"
@@ -254,12 +255,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);
 
@@ -310,9 +313,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;
  };

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)
  {

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."});
@@ -617,14 +618,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());
  }
 }
 

src/ign_TEST.cc

@@ -296,6 +296,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";

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);
 }
 

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,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>

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>