diff --git a/Changelog.md b/Changelog.md index b8c4ed8ca..76cc42ac2 100644 --- a/Changelog.md +++ b/Changelog.md @@ -6,6 +6,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 index 90385a0c9..653dc7caf 100644 --- a/Migration.md +++ b/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://github.com/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** diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 677407212..cfa25454e 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -19,6 +19,7 @@ #include #include +#include #include #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 _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 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 + modelCanonicalLinkAndRelativeName(const Model *); + /// \brief Private data pointer. private: ModelPrivate *dataPtr = nullptr; }; diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index a3918ab7a..57a0fe30a 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -15,6 +15,8 @@ * */ #include +#include +#include #include "sdf/Element.hh" #include "sdf/Error.hh" @@ -164,6 +166,17 @@ FindSinkVertex( return PairType(vertex, edges); } +///////////////////////////////////////////////// +std::pair + 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 index f359075ca..03629a798 100644 --- a/src/Model.cc +++ b/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 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 index 49ef7e680..46a0b0143 100644 --- a/src/ign_TEST.cc +++ b/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"; diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 299100509..713d4cab0 100644 --- 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 index c3edfe0cb..a8dc55341 100644 --- a/test/sdf/model_canonical_link.sdf +++ b/test/sdf/model_canonical_link.sdf @@ -7,5 +7,6 @@ 0 2 0 0 0 0 + diff --git a/test/sdf/nested_canonical_link.sdf b/test/sdf/nested_canonical_link.sdf new file mode 100644 index 000000000..03ecc13bd --- /dev/null +++ b/test/sdf/nested_canonical_link.sdf @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/test/sdf/nested_invalid_explicit_canonical_link.sdf b/test/sdf/nested_invalid_explicit_canonical_link.sdf new file mode 100644 index 000000000..dfce81c1e --- /dev/null +++ b/test/sdf/nested_invalid_explicit_canonical_link.sdf @@ -0,0 +1,14 @@ + + + + + + + + + + + + + +