From 05a9037f7093155f995ef42b235280137dec4bf3 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 2 Sep 2020 11:54:00 -0700 Subject: [PATCH 01/70] Expect nested_explicit_canonical_link.sdf is valid Split nested_invalid_explicit_canonical_link.sdf into nested_explicit_canonical_link.sdf and nested_without_links_invalid.sdf and update UNIT_ign_TEST and an integration test. Signed-off-by: Steve Peters --- src/ign_TEST.cc | 17 +++-- test/integration/model_dom.cc | 76 +++++++++++++++++++ test/sdf/nested_explicit_canonical_link.sdf | 9 +++ ...k.sdf => nested_without_links_invalid.sdf} | 6 +- 4 files changed, 99 insertions(+), 9 deletions(-) create mode 100644 test/sdf/nested_explicit_canonical_link.sdf rename test/sdf/{nested_invalid_explicit_canonical_link.sdf => nested_without_links_invalid.sdf} (67%) diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 4923fea66..741eba8b2 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -372,14 +372,21 @@ TEST(check, SDF) // that is explicitly specified by //model/@canonical_link using :: // syntax. { - std::string path = pathBase +"/nested_invalid_explicit_canonical_link.sdf"; + std::string path = pathBase +"/nested_explicit_canonical_link.sdf"; - // Check nested_invalid_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 SDF file with a model that a nested model without a link. + { + std::string path = pathBase +"/nested_without_links_invalid.sdf"; + + // Check nested_without_links_invalid.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; } diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 713d4cab0..5366653a8 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -377,3 +377,79 @@ TEST(DOMRoot, LoadNestedCanonicalLink) EXPECT_EQ("deep::deeper::deepest::deepest_link", body); } +///////////////////////////////////////////////// +TEST(DOMRoot, LoadNestedExplicitCanonicalLink) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "nested_explicit_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(1u, model->ModelCount()); + EXPECT_TRUE(model->ModelNameExists("nested")); + EXPECT_EQ(model->ModelByName("nested"), model->ModelByIndex(0)); + EXPECT_EQ(nullptr, model->ModelByIndex(1)); + + // expect implicit canonical link + EXPECT_EQ("nested::link2", model->CanonicalLinkName()); + + // 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->LinkByName("nested::link2"), model->CanonicalLink()); + 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()); +} + +///////////////////////////////////////////////// +TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "nested_without_links_invalid.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + for (auto e : errors) + std::cout << e << std::endl; + EXPECT_FALSE(errors.empty()); + EXPECT_EQ(10u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::MODEL_WITHOUT_LINK); + EXPECT_NE(std::string::npos, + errors[0].Message().find("A model must have at least one link")); + EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::MODEL_WITHOUT_LINK); + EXPECT_NE(std::string::npos, + errors[1].Message().find("A model must have at least one link")); + // errors[2] + // errors[3] + // errors[4] + // errors[5] + // errors[6] + // errors[7] + // errors[8] + // errors[9] +} + diff --git a/test/sdf/nested_explicit_canonical_link.sdf b/test/sdf/nested_explicit_canonical_link.sdf new file mode 100644 index 000000000..3f4be8dbd --- /dev/null +++ b/test/sdf/nested_explicit_canonical_link.sdf @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/test/sdf/nested_invalid_explicit_canonical_link.sdf b/test/sdf/nested_without_links_invalid.sdf similarity index 67% rename from test/sdf/nested_invalid_explicit_canonical_link.sdf rename to test/sdf/nested_without_links_invalid.sdf index dfce81c1e..79cfcb417 100644 --- a/test/sdf/nested_invalid_explicit_canonical_link.sdf +++ b/test/sdf/nested_without_links_invalid.sdf @@ -1,8 +1,6 @@ - - - - + + From 0931e658fbf4154afa83f4ae12783415f4d56de0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 12 Aug 2020 12:11:20 -0700 Subject: [PATCH 02/70] Support :: syntax in *NameExists and *ByName APIs This extends the `Model::*NameExists` and `Model::*ByName` APIs (like LinkNameExists and LinkByName) that allow passing nested names that can begin with a sequence of nested model names separated by :: and may end with the name of an object of the specified type, such as "outer_model::inner_model::inner_joint". For now, if a nested model is not found that matches the nested name preceding the final ::, then it checks for objects in the current model that match the entire name. This extra check should be disabled when "::" is reserved and not allowed in frame names. Signed-off-by: Steve Peters --- include/sdf/Model.hh | 16 +++++ src/Model.cc | 100 +++++++++++++++++--------- src/Model_TEST.cc | 24 +++++++ test/integration/model_dom.cc | 100 ++++++++++++++++++++++++++ test/sdf/model_multi_nested_model.sdf | 28 ++++++++ 5 files changed, 233 insertions(+), 35 deletions(-) create mode 100644 test/sdf/model_multi_nested_model.sdf diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index b652b9fe4..30b2353b6 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -145,11 +145,15 @@ namespace sdf /// \brief Get a link based on a name. /// \param[in] _name Name of the link. + /// To get a link in a nested model, prefix the link name with the + /// sequence of nested models containing this link, delimited by "::". /// \return Pointer to the link. Nullptr if the name does not exist. public: const Link *LinkByName(const std::string &_name) const; /// \brief Get whether a link name exists. /// \param[in] _name Name of the link to check. + /// To check for a link in a nested model, prefix the link name with + /// the sequence of nested models containing this link, delimited by "::". /// \return True if there exists a link with the given name. public: bool LinkNameExists(const std::string &_name) const; @@ -166,11 +170,15 @@ namespace sdf /// \brief Get whether a joint name exists. /// \param[in] _name Name of the joint to check. + /// To check for a joint in a nested model, prefix the link name with + /// the sequence of nested models containing this joint, delimited by "::". /// \return True if there exists a joint with the given name. public: bool JointNameExists(const std::string &_name) const; /// \brief Get a joint based on a name. /// \param[in] _name Name of the joint. + /// To get a joint in a nested model, prefix the joint name with the + /// sequence of nested models containing this joint, delimited by "::". /// \return Pointer to the joint. Nullptr if a joint with the given name /// does not exist. /// \sa bool JointNameExists(const std::string &_name) const @@ -190,12 +198,16 @@ namespace sdf /// \brief Get an explicit frame based on a name. /// \param[in] _name Name of the explicit frame. + /// To get a frame in a nested model, prefix the frame name with the + /// sequence of nested models containing this frame, delimited by "::". /// \return Pointer to the explicit frame. Nullptr if the name does not /// exist. public: const Frame *FrameByName(const std::string &_name) const; /// \brief Get whether an explicit frame name exists. /// \param[in] _name Name of the explicit frame to check. + /// To check for a frame in a nested model, prefix the frame name with + /// the sequence of nested models containing this frame, delimited by "::". /// \return True if there exists an explicit frame with the given name. public: bool FrameNameExists(const std::string &_name) const; @@ -212,11 +224,15 @@ namespace sdf /// \brief Get whether a nested model name exists. /// \param[in] _name Name of the nested model to check. + /// To check for a model nested in other models, prefix the model name + /// with the sequence of nested model names, delimited by "::". /// \return True if there exists a nested model with the given name. public: bool ModelNameExists(const std::string &_name) const; /// \brief Get a nested model based on a name. /// \param[in] _name Name of the nested model. + /// To get a model nested in other models, prefix the model name + /// with the sequence of nested model names, delimited by "::". /// \return Pointer to the model. Nullptr if a model with the given name /// does not exist. /// \sa bool ModelNameExists(const std::string &_name) const diff --git a/src/Model.cc b/src/Model.cc index 8819c70c8..55b17fa37 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -579,14 +579,7 @@ const Link *Model::LinkByIndex(const uint64_t _index) const ///////////////////////////////////////////////// bool Model::LinkNameExists(const std::string &_name) const { - for (auto const &l : this->dataPtr->links) - { - if (l.Name() == _name) - { - return true; - } - } - return false; + return nullptr != this->LinkByName(_name); } ///////////////////////////////////////////////// @@ -606,19 +599,28 @@ const Joint *Model::JointByIndex(const uint64_t _index) const ///////////////////////////////////////////////// bool Model::JointNameExists(const std::string &_name) const { - for (auto const &j : this->dataPtr->joints) - { - if (j.Name() == _name) - { - return true; - } - } - return false; + return nullptr != this->JointByName(_name); } ///////////////////////////////////////////////// const Joint *Model::JointByName(const std::string &_name) const { + auto index = _name.rfind("::"); + if (index != std::string::npos) + { + const Model *model = this->ModelByName(_name.substr(0, index)); + if (nullptr != model) + { + return model->JointByName(_name.substr(index + 2)); + } + + // The nested model name preceding the last "::" could not be found. + // For now, try to find a link that matches _name exactly. + // When "::" are reserved and not allowed in names, then uncomment + // the following line to return a nullptr. + // return nullptr; + } + for (auto const &j : this->dataPtr->joints) { if (j.Name() == _name) @@ -646,19 +648,28 @@ const Frame *Model::FrameByIndex(const uint64_t _index) const ///////////////////////////////////////////////// bool Model::FrameNameExists(const std::string &_name) const { - for (auto const &f : this->dataPtr->frames) - { - if (f.Name() == _name) - { - return true; - } - } - return false; + return nullptr != this->FrameByName(_name); } ///////////////////////////////////////////////// const Frame *Model::FrameByName(const std::string &_name) const { + auto index = _name.rfind("::"); + if (index != std::string::npos) + { + const Model *model = this->ModelByName(_name.substr(0, index)); + if (nullptr != model) + { + return model->FrameByName(_name.substr(index + 2)); + } + + // The nested model name preceding the last "::" could not be found. + // For now, try to find a link that matches _name exactly. + // When "::" are reserved and not allowed in names, then uncomment + // the following line to return a nullptr. + // return nullptr; + } + for (auto const &f : this->dataPtr->frames) { if (f.Name() == _name) @@ -686,27 +697,30 @@ const Model *Model::ModelByIndex(const uint64_t _index) const ///////////////////////////////////////////////// bool Model::ModelNameExists(const std::string &_name) const { - for (auto const &m : this->dataPtr->models) - { - if (m.Name() == _name) - { - return true; - } - } - return false; + return nullptr != this->ModelByName(_name); } ///////////////////////////////////////////////// const Model *Model::ModelByName(const std::string &_name) const { + auto index = _name.find("::"); + const std::string nextModelName = _name.substr(0, index); + const Model *nextModel = nullptr; + for (auto const &m : this->dataPtr->models) { - if (m.Name() == _name) + if (m.Name() == nextModelName) { - return &m; + nextModel = &m; + break; } } - return nullptr; + + if (nullptr != nextModel && index != std::string::npos) + { + return nextModel->ModelByName(_name.substr(index + 2)); + } + return nextModel; } ///////////////////////////////////////////////// @@ -832,6 +846,22 @@ sdf::SemanticPose Model::SemanticPose() const ///////////////////////////////////////////////// const Link *Model::LinkByName(const std::string &_name) const { + auto index = _name.rfind("::"); + if (index != std::string::npos) + { + const Model *model = this->ModelByName(_name.substr(0, index)); + if (nullptr != model) + { + return model->LinkByName(_name.substr(index + 2)); + } + + // The nested model name preceding the last "::" could not be found. + // For now, try to find a link that matches _name exactly. + // When "::" are reserved and not allowed in names, then uncomment + // the following line to return a nullptr. + // return nullptr; + } + for (auto const &l : this->dataPtr->links) { if (l.Name() == _name) diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 290e9f9a9..cafa44788 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -52,32 +52,56 @@ TEST(DOMModel, Construction) EXPECT_EQ(nullptr, model.ModelByIndex(1)); EXPECT_EQ(nullptr, model.ModelByName("")); EXPECT_EQ(nullptr, model.ModelByName("default")); + EXPECT_EQ(nullptr, model.ModelByName("a::b")); + EXPECT_EQ(nullptr, model.ModelByName("a::b::c")); + EXPECT_EQ(nullptr, model.ModelByName("::::")); EXPECT_FALSE(model.ModelNameExists("")); EXPECT_FALSE(model.ModelNameExists("default")); + EXPECT_FALSE(model.ModelNameExists("a::b")); + EXPECT_FALSE(model.ModelNameExists("a::b::c")); + EXPECT_FALSE(model.ModelNameExists("::::")); EXPECT_EQ(0u, model.LinkCount()); EXPECT_EQ(nullptr, model.LinkByIndex(0)); EXPECT_EQ(nullptr, model.LinkByIndex(1)); EXPECT_EQ(nullptr, model.LinkByName("")); EXPECT_EQ(nullptr, model.LinkByName("default")); + EXPECT_EQ(nullptr, model.LinkByName("a::b")); + EXPECT_EQ(nullptr, model.LinkByName("a::b::c")); + EXPECT_EQ(nullptr, model.LinkByName("::::")); EXPECT_FALSE(model.LinkNameExists("")); EXPECT_FALSE(model.LinkNameExists("default")); + EXPECT_FALSE(model.LinkNameExists("a::b")); + EXPECT_FALSE(model.LinkNameExists("a::b::c")); + EXPECT_FALSE(model.LinkNameExists("::::")); EXPECT_EQ(0u, model.JointCount()); EXPECT_EQ(nullptr, model.JointByIndex(0)); EXPECT_EQ(nullptr, model.JointByIndex(1)); EXPECT_EQ(nullptr, model.JointByName("")); EXPECT_EQ(nullptr, model.JointByName("default")); + EXPECT_EQ(nullptr, model.JointByName("a::b")); + EXPECT_EQ(nullptr, model.JointByName("a::b::c")); + EXPECT_EQ(nullptr, model.JointByName("::::")); EXPECT_FALSE(model.JointNameExists("")); EXPECT_FALSE(model.JointNameExists("default")); + EXPECT_FALSE(model.JointNameExists("a::b")); + EXPECT_FALSE(model.JointNameExists("a::b::c")); + EXPECT_FALSE(model.JointNameExists("::::")); EXPECT_EQ(0u, model.FrameCount()); EXPECT_EQ(nullptr, model.FrameByIndex(0)); EXPECT_EQ(nullptr, model.FrameByIndex(1)); EXPECT_EQ(nullptr, model.FrameByName("")); EXPECT_EQ(nullptr, model.FrameByName("default")); + EXPECT_EQ(nullptr, model.FrameByName("a::b")); + EXPECT_EQ(nullptr, model.FrameByName("a::b::c")); + EXPECT_EQ(nullptr, model.FrameByName("::::")); EXPECT_FALSE(model.FrameNameExists("")); EXPECT_FALSE(model.FrameNameExists("default")); + EXPECT_FALSE(model.FrameNameExists("a::b")); + EXPECT_FALSE(model.FrameNameExists("a::b::c")); + EXPECT_FALSE(model.FrameNameExists("::::")); EXPECT_TRUE(model.CanonicalLinkName().empty()); EXPECT_EQ(nullptr, model.CanonicalLink()); diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 5366653a8..1a495fe75 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -197,6 +197,106 @@ TEST(DOMRoot, NestedModel) EXPECT_EQ(0u, nestedModel->JointCount()); EXPECT_EQ(0u, nestedModel->FrameCount()); + + // get nested link from parent model + const std::string linkNestedName = "nested_model::nested_link01"; + EXPECT_TRUE(model->LinkNameExists(linkNestedName)); + const sdf::Link *nestedLink01 = model->LinkByName(linkNestedName); + EXPECT_NE(nullptr, nestedLink01); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, MultiNestedModel) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_multi_nested_model.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(1u, root.ModelCount()); + + // Get the outer model + const sdf::Model *outerModel = root.ModelByIndex(0); + ASSERT_NE(nullptr, outerModel); + EXPECT_EQ("outer_model", outerModel->Name()); + EXPECT_EQ(1u, outerModel->LinkCount()); + EXPECT_NE(nullptr, outerModel->LinkByIndex(0)); + EXPECT_EQ(nullptr, outerModel->LinkByIndex(1)); + + EXPECT_TRUE(outerModel->LinkNameExists("outer_link")); + + EXPECT_EQ(1u, outerModel->JointCount()); + EXPECT_TRUE(outerModel->JointNameExists("outer_joint")); + + EXPECT_EQ(1u, outerModel->FrameCount()); + EXPECT_TRUE(outerModel->FrameNameExists("outer_frame")); + + EXPECT_EQ(1u, outerModel->ModelCount()); + EXPECT_NE(nullptr, outerModel->ModelByIndex(0)); + EXPECT_EQ(nullptr, outerModel->ModelByIndex(1)); + + EXPECT_TRUE(outerModel->ModelNameExists("mid_model")); + + // Get the middle model + const sdf::Model *midModel = outerModel->ModelByIndex(0); + ASSERT_NE(nullptr, midModel); + EXPECT_EQ("mid_model", midModel->Name()); + EXPECT_EQ(1u, midModel->LinkCount()); + EXPECT_NE(nullptr, midModel->LinkByIndex(0)); + EXPECT_EQ(nullptr, midModel->LinkByIndex(1)); + + EXPECT_TRUE(midModel->LinkNameExists("mid_link")); + + EXPECT_EQ(1u, midModel->JointCount()); + EXPECT_TRUE(midModel->JointNameExists("mid_joint")); + + EXPECT_EQ(1u, midModel->FrameCount()); + EXPECT_TRUE(midModel->FrameNameExists("mid_frame")); + + EXPECT_EQ(1u, midModel->ModelCount()); + EXPECT_NE(nullptr, midModel->ModelByIndex(0)); + EXPECT_EQ(nullptr, midModel->ModelByIndex(1)); + + EXPECT_TRUE(midModel->ModelNameExists("inner_model")); + + // Get the inner model + const sdf::Model *innerModel = midModel->ModelByIndex(0); + ASSERT_NE(nullptr, innerModel); + EXPECT_EQ("inner_model", innerModel->Name()); + EXPECT_EQ(1u, innerModel->LinkCount()); + EXPECT_NE(nullptr, innerModel->LinkByIndex(0)); + EXPECT_EQ(nullptr, innerModel->LinkByIndex(1)); + + EXPECT_TRUE(innerModel->LinkNameExists("inner_link")); + + EXPECT_EQ(1u, innerModel->JointCount()); + EXPECT_TRUE(innerModel->JointNameExists("inner_joint")); + + EXPECT_EQ(1u, innerModel->FrameCount()); + EXPECT_TRUE(innerModel->FrameNameExists("inner_frame")); + + EXPECT_EQ(0u, innerModel->ModelCount()); + + // test nested names from outer model + const std::string innerModelNestedName = "mid_model::inner_model"; + EXPECT_TRUE(outerModel->ModelNameExists(innerModelNestedName)); + EXPECT_EQ(innerModel, outerModel->ModelByName(innerModelNestedName)); + + const std::string innerLinkNestedName = innerModelNestedName + "::inner_link"; + EXPECT_TRUE(outerModel->LinkNameExists(innerLinkNestedName)); + EXPECT_NE(nullptr, outerModel->LinkByName(innerLinkNestedName)); + + std::string innerJointNestedName = innerModelNestedName + "::inner_joint"; + EXPECT_TRUE(outerModel->JointNameExists(innerJointNestedName)); + EXPECT_NE(nullptr, outerModel->JointByName(innerJointNestedName)); + + std::string innerFrameNestedName = innerModelNestedName + "::inner_frame"; + EXPECT_TRUE(outerModel->FrameNameExists(innerFrameNestedName)); + EXPECT_NE(nullptr, outerModel->FrameByName(innerFrameNestedName)); } ///////////////////////////////////////////////// diff --git a/test/sdf/model_multi_nested_model.sdf b/test/sdf/model_multi_nested_model.sdf new file mode 100644 index 000000000..80942767d --- /dev/null +++ b/test/sdf/model_multi_nested_model.sdf @@ -0,0 +1,28 @@ + + + + + + world + outer_link + + + + 1 0 0 0 1.5707963267948966 0 + + + world + mid_link + + + + + + world + inner_link + + + + + + From 79fc3e6812609fa6bc0f850a13fc8023aac89ac7 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 3 Sep 2020 01:06:33 -0700 Subject: [PATCH 03/70] Changelog and Migration guide Signed-off-by: Steve Peters --- Changelog.md | 9 +++++++++ Migration.md | 12 ++++++++++++ 2 files changed, 21 insertions(+) diff --git a/Changelog.md b/Changelog.md index 1d452e5e5..7dface310 100644 --- a/Changelog.md +++ b/Changelog.md @@ -7,6 +7,15 @@ 1. Initial version of SDFormat 1.8 specification. * [BitBucket pull request 682](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/682) +1. SDFormat 1.8: Support specifying frames as joint child/parent. + * [Pull request 304](https://github.com/osrf/sdformat/pull/304) + +1. SDFormat 1.8: Add support for `//include/placement_frame` and `//model/@placement_frame`. + * [Pull request 324](https://github.com/osrf/sdformat/pull/324) + +1. SDFormat 1.8: Support explicit nested canonical links with :: syntax. + * [Pull request 353](https://github.com/osrf/sdformat/pull/353) + 1. SDFormat 1.8: Deprecate //joint/axis/initial_position. * [BitBucket pull request 683](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/683) diff --git a/Migration.md b/Migration.md index 9fd527e65..18ec08034 100644 --- a/Migration.md +++ b/Migration.md @@ -20,6 +20,18 @@ but with improved human-readability.. + Errors ResolveChildLink(std::string&) const + Errors ResolveParentLink(std::string&) const +### Modifications + +1. **sdf/Model.hh**: the following methods now accept nested names + that can begin with a sequence of nested model names separated + by `::` and may end with the name of an object of the specified type. + + const Frame \*FrameByName(const std::string &) const + + const Joint \*JointByName(const std::string &) const + + const Link \*LinkByName(const std::string &) const + + bool FrameNameExists(const std::string &) const + + bool JointNameExists(const std::string &) const + + bool LinkNameExists(const std::string &) const + ## SDFormat 9.x to 10.0 ### Modifications From 8b92f41f96ed6f2e2c164a785b902ab0283f262e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 3 Sep 2020 12:53:21 -0700 Subject: [PATCH 04/70] Ensure CanonicalLink pointer is valid Signed-off-by: Steve Peters --- test/integration/model_dom.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 1a495fe75..394d0408a 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -504,6 +504,7 @@ TEST(DOMRoot, LoadNestedExplicitCanonicalLink) EXPECT_EQ(1u, model->ModelCount()); EXPECT_TRUE(model->ModelNameExists("nested")); + EXPECT_NE(nullptr, model->ModelByIndex(0)); EXPECT_EQ(model->ModelByName("nested"), model->ModelByIndex(0)); EXPECT_EQ(nullptr, model->ModelByIndex(1)); @@ -516,6 +517,7 @@ TEST(DOMRoot, LoadNestedExplicitCanonicalLink) EXPECT_TRUE(model->FrameByName("F")->ResolveAttachedToBody(body).empty()); EXPECT_EQ("nested::link2", body); + EXPECT_NE(nullptr, model->CanonicalLink()); EXPECT_EQ(model->LinkByName("nested::link2"), model->CanonicalLink()); EXPECT_EQ(model->ModelByName("nested")->LinkByName("link2"), model->CanonicalLink()); From a07b42bdeeea3e71e754f84848fd406d50a7f024 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 23 Sep 2020 13:18:13 -0500 Subject: [PATCH 05/70] Add ScopedGraph header Signed-off-by: Addisu Z. Taddese --- include/sdf/Collision.hh | 3 +- include/sdf/Frame.hh | 5 +- include/sdf/Joint.hh | 5 +- include/sdf/JointAxis.hh | 3 +- include/sdf/Light.hh | 3 +- include/sdf/Link.hh | 3 +- include/sdf/Model.hh | 3 +- include/sdf/SemanticPose.hh | 3 +- include/sdf/Sensor.hh | 3 +- include/sdf/Visual.hh | 3 +- src/Collision.cc | 6 +- src/Frame.cc | 18 +- src/FrameSemantics.cc | 339 +++++++++++++++++------------------- src/FrameSemantics.hh | 24 +-- src/Joint.cc | 17 +- src/JointAxis.cc | 9 +- src/Light.cc | 6 +- src/Link.cc | 8 +- src/Model.cc | 102 +++++++---- src/ScopedGraph.hh | 103 +++++++++++ src/SemanticPose.cc | 10 +- src/Sensor.cc | 6 +- src/Utils.hh | 20 ++- src/Visual.cc | 6 +- src/World.cc | 86 ++++----- src/ign_TEST.cc | 10 ++ src/parser.cc | 13 +- 27 files changed, 499 insertions(+), 318 deletions(-) create mode 100644 src/ScopedGraph.hh diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 275f97164..523561ac4 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -36,6 +36,7 @@ namespace sdf class Geometry; class Surface; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief A collision element descibes the collision properties associated /// with a link. This can be different from the visual properties of a link. @@ -146,7 +147,7 @@ namespace sdf /// Link::SetPoseRelativeToGraph. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index e5097e8ca..0435d3a05 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -35,6 +35,7 @@ namespace sdf class FramePrivate; struct FrameAttachedToGraph; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief A Frame element descibes the properties associated with an /// explicit frame defined in a Model or World. @@ -150,14 +151,14 @@ namespace sdf /// be called by Model::Load or World::Load. /// \param[in] _graph Weak pointer to FrameAttachedToGraph. private: void SetFrameAttachedToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Give a weak pointer to the PoseRelativeToGraph to be used /// for resolving poses. This is private and is intended to be called by /// Model::Load or World::Load. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Model::Load and World::Load to call SetPoseRelativeToGraph. friend class Model; diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 407d863c3..2c2091bc3 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -37,6 +37,7 @@ namespace sdf class JointPrivate; struct FrameAttachedToGraph; struct PoseRelativeToGraph; + template class ScopedGraph; /// \enum JointType /// \brief The set of joint types. INVALID indicates that joint type has @@ -226,14 +227,14 @@ namespace sdf /// intended to be called by Model::Load. /// \param[in] _graph Weak pointer to FrameAttachedToGraph. private: void SetFrameAttachedToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Give a weak pointer to the PoseRelativeToGraph to be used /// for resolving poses. This is private and is intended to be called by /// Model::Load. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Model::Load to call SetPoseRelativeToGraph. friend class Model; diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index b9576312e..1690da9f6 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -35,6 +35,7 @@ namespace sdf // Forward declare private data class. class JointAxisPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief Parameters related to the axis of rotation for rotational joints, /// and the axis of translation for prismatic joints. @@ -274,7 +275,7 @@ namespace sdf /// by Joint::SetPoseRelativeToGraph. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Joint::SetPoseRelativeToGraph to propagate. friend class Joint; diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index c34f33979..98214bc21 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -37,6 +37,7 @@ namespace sdf // Forward declare private data class. class LightPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \enum LightType /// \brief The set of light types. INVALID indicates that light type has @@ -278,7 +279,7 @@ namespace sdf /// Link::SetPoseRelativeToGraph or World::Load. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph or World::Load to call /// SetXmlParentName and SetPoseRelativeToGraph, diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 7b8532ac5..f342b0440 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -40,6 +40,7 @@ namespace sdf class Visual; class LinkPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE Link { @@ -230,7 +231,7 @@ namespace sdf /// Model::Load. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Model::Load to call SetPoseRelativeToGraph. friend class Model; diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index f700cca84..09c75b63a 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -38,6 +38,7 @@ namespace sdf class Link; class ModelPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE Model { @@ -285,7 +286,7 @@ namespace sdf /// \param[in] _graph Weak pointer to PoseRelativeToGraph. /// \return Error if graph pointer is invalid. private: sdf::Errors SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow World::Load to call SetPoseRelativeToGraph. friend class World; diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 6baa76035..921c799e4 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -42,6 +42,7 @@ namespace sdf // Forward declare private data class. class SemanticPosePrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \brief SemanticPose is a data structure that can be used by different /// DOM objects to resolve poses on a PoseRelativeToGraph. This object holds @@ -83,7 +84,7 @@ namespace sdf const ignition::math::Pose3d &_pose, const std::string &_relativeTo, const std::string &_defaultResolveTo, - std::weak_ptr _graph); + const sdf::ScopedGraph &_graph); /// \brief Destructor public: ~SemanticPose(); diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 33a07fa16..98fd11a16 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -40,6 +40,7 @@ namespace sdf class Magnetometer; class SensorPrivate; struct PoseRelativeToGraph; + template class ScopedGraph; /// \enum SensorType /// \brief The set of sensor types. @@ -320,7 +321,7 @@ namespace sdf /// Link::SetPoseRelativeToGraph. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index deff10053..c65491e3c 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -41,6 +41,7 @@ namespace sdf class VisualPrivate; class Geometry; struct PoseRelativeToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE Visual { @@ -173,7 +174,7 @@ namespace sdf /// Link::SetPoseRelativeToGraph. /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: void SetPoseRelativeToGraph( - std::weak_ptr _graph); + sdf::ScopedGraph _graph); /// \brief Allow Link::SetPoseRelativeToGraph to call SetXmlParentName /// and SetPoseRelativeToGraph, but Link::SetPoseRelativeToGraph is diff --git a/src/Collision.cc b/src/Collision.cc index ee98baca8..ed0071263 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -22,6 +22,8 @@ #include "sdf/Geometry.hh" #include "sdf/Surface.hh" #include "sdf/Types.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -50,7 +52,7 @@ class sdf::CollisionPrivate public: std::string xmlParentName; /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -207,7 +209,7 @@ void Collision::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Collision::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/src/Frame.cc b/src/Frame.cc index c6a5737df..2a02e6bdb 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -20,6 +20,7 @@ #include "sdf/Error.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -45,10 +46,11 @@ class sdf::FramePrivate std::string graphSourceName = ""; /// \brief Weak pointer to model's or worlds's Frame Attached-To Graph. - public: std::weak_ptr frameAttachedToGraph; + public: sdf::ScopedGraph frameAttachedToGraph; /// \brief Weak pointer to model's or world's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// TODO (addisu) Make const sdf::PoseRelativeToGraph + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -195,20 +197,20 @@ void Frame::SetPoseRelativeTo(const std::string &_frame) ///////////////////////////////////////////////// void Frame::SetFrameAttachedToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->frameAttachedToGraph = _graph; } ///////////////////////////////////////////////// void Frame::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; - auto graph = this->dataPtr->poseRelativeToGraph.lock(); + auto graph = this->dataPtr->poseRelativeToGraph; if (graph) { - this->dataPtr->graphSourceName = graph->sourceName; + this->dataPtr->graphSourceName = graph.ScopeName(); } } @@ -217,7 +219,7 @@ Errors Frame::ResolveAttachedToBody(std::string &_body) const { Errors errors; - auto graph = this->dataPtr->frameAttachedToGraph.lock(); + auto graph = this->dataPtr->frameAttachedToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -226,7 +228,7 @@ Errors Frame::ResolveAttachedToBody(std::string &_body) const } std::string body; - errors = resolveFrameAttachedToBody(body, *graph, this->dataPtr->name); + errors = resolveFrameAttachedToBody(body, graph, this->dataPtr->name); if (errors.empty()) { _body = body; diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 32d9f27a1..f21cafb3e 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -26,6 +26,7 @@ #include "sdf/World.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" namespace sdf { @@ -47,21 +48,19 @@ inline namespace SDF_VERSION_NAMESPACE { /// \return A source vertex paired with a vector of the edges leading the /// source to the starting vertex, or a NullVertex paired with an empty /// vector if a cycle or vertex with multiple incoming edges are detected. -template -std::pair &, - std::vector< ignition::math::graph::DirectedEdge > > -FindSourceVertex( - const ignition::math::graph::DirectedGraph &_graph, - const ignition::math::graph::VertexId _id, - Errors &_errors) +template +std::pair::Vertex &, + std::vector::Edge>> +FindSourceVertex(const ScopedGraph &_graph, + const ignition::math::graph::VertexId _id, Errors &_errors) { - using DirectedEdge = ignition::math::graph::DirectedEdge; - using Vertex = ignition::math::graph::Vertex; + using DirectedEdge = typename ScopedGraph::Edge; + using Vertex = typename ScopedGraph::Vertex; using VertexId = ignition::math::graph::VertexId; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; - std::reference_wrapper vertex(_graph.VertexFromId(_id)); + std::reference_wrapper vertex(_graph.Graph().VertexFromId(_id)); if (!vertex.get().Valid()) { _errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, @@ -73,7 +72,7 @@ FindSourceVertex( std::set visited; visited.insert(vertex.get().Id()); - auto incidentsTo = _graph.IncidentsTo(vertex); + auto incidentsTo = _graph.Graph().IncidentsTo(vertex); while (!incidentsTo.empty()) { if (incidentsTo.size() != 1) @@ -84,7 +83,7 @@ FindSourceVertex( return PairType(Vertex::NullVertex, EdgesType()); } auto const &edge = incidentsTo.begin()->second; - vertex = _graph.VertexFromId(edge.get().Vertices().first); + vertex = _graph.Graph().VertexFromId(edge.get().Vertices().first); edges.push_back(edge); if (visited.count(vertex.get().Id())) { @@ -93,8 +92,18 @@ FindSourceVertex( vertex.get().Name() + "]."}); return PairType(Vertex::NullVertex, EdgesType()); } + if (vertex.get().Id() == _graph.RootVertexId()) + { + // This is the source. + break; + } visited.insert(vertex.get().Id()); - incidentsTo = _graph.IncidentsTo(vertex); + incidentsTo = _graph.Graph().IncidentsTo(vertex); + } + if (vertex.get().Id() != _graph.RootVertexId()) + { + // Error, the root vertex is not the same as the the source + return PairType(Vertex::NullVertex, EdgesType()); } return PairType(vertex, edges); @@ -111,21 +120,22 @@ FindSourceVertex( /// \return A sink vertex paired with a vector of the edges leading the /// sink to the starting vertex, or a NullVertex paired with an empty /// vector if a cycle or vertex with multiple outgoing edges are detected. -template -std::pair &, - std::vector< ignition::math::graph::DirectedEdge > > +template +std::pair::Vertex &, + std::vector::Edge>> FindSinkVertex( - const ignition::math::graph::DirectedGraph &_graph, + const ScopedGraph &_graph, const ignition::math::graph::VertexId _id, Errors &_errors) { - using DirectedEdge = ignition::math::graph::DirectedEdge; - using Vertex = ignition::math::graph::Vertex; + + using DirectedEdge = typename ScopedGraph::Edge; + using Vertex = typename ScopedGraph::Vertex; using VertexId = ignition::math::graph::VertexId; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; - std::reference_wrapper vertex(_graph.VertexFromId(_id)); + std::reference_wrapper vertex(_graph.Graph().VertexFromId(_id)); if (!vertex.get().Valid()) { _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, @@ -137,7 +147,7 @@ FindSinkVertex( std::set visited; visited.insert(vertex.get().Id()); - auto incidentsFrom = _graph.IncidentsFrom(vertex); + auto incidentsFrom = _graph.Graph().IncidentsFrom(vertex); while (!incidentsFrom.empty()) { if (incidentsFrom.size() != 1) @@ -148,7 +158,7 @@ FindSinkVertex( return PairType(Vertex::NullVertex, EdgesType()); } auto const &edge = incidentsFrom.begin()->second; - vertex = _graph.VertexFromId(edge.get().Vertices().second); + vertex = _graph.Graph().VertexFromId(edge.get().Vertices().second); edges.push_back(edge); if (visited.count(vertex.get().Id())) { @@ -158,7 +168,7 @@ FindSinkVertex( return PairType(Vertex::NullVertex, EdgesType()); } visited.insert(vertex.get().Id()); - incidentsFrom = _graph.IncidentsFrom(vertex); + incidentsFrom = _graph.Graph().IncidentsFrom(vertex); } return PairType(vertex, edges); @@ -166,17 +176,15 @@ FindSinkVertex( ///////////////////////////////////////////////// Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const Model *_model) + ScopedGraph &_out, const Model *_model) { Errors errors; // add implicit model frame vertex first const std::string scopeName = "__model__"; - _out.scopeName = scopeName; + _out.SetScopeName(scopeName); auto modelFrameId = - _out.graph.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); - _out.map[scopeName] = modelFrameId; - + _out.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); if (!_model) { @@ -220,7 +228,7 @@ Errors buildFrameAttachedToGraph( for (uint64_t l = 0; l < _model->LinkCount(); ++l) { auto link = _model->LinkByIndex(l); - if (_out.map.count(link->Name()) > 0) + if (_out.Count(link->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Link with non-unique name [" + link->Name() + @@ -229,13 +237,12 @@ Errors buildFrameAttachedToGraph( continue; } auto linkId = - _out.graph.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); - _out.map[link->Name()] = linkId; + _out.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); // add edge from implicit model frame vertex to canonical link if (link == canonicalLink) { - _out.graph.AddEdge({modelFrameId, linkId}, true); + _out.AddEdge({modelFrameId, linkId}, true); } } @@ -243,7 +250,7 @@ Errors buildFrameAttachedToGraph( for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - if (_out.map.count(joint->Name()) > 0) + if (_out.Count(joint->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Joint with non-unique name [" + joint->Name() + @@ -251,16 +258,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto jointId = - _out.graph.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); - _out.map[joint->Name()] = jointId; + _out.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); } // add frame vertices for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (_out.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -268,16 +273,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); } // add nested model vertices for (uint64_t m = 0; m < _model->ModelCount(); ++m) { auto nestedModel = _model->ModelByIndex(m); - if (_out.map.count(nestedModel->Name()) > 0) + if (_out.Count(nestedModel->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Nested model with non-unique name [" + nestedModel->Name() + @@ -285,18 +288,16 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto nestedModelId = - _out.graph.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); - _out.map[nestedModel->Name()] = nestedModelId; + _out.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); } // add edges from joint to child frames for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - auto jointId = _out.map.at(joint->Name()); + auto jointId = _out.VertexIdByName(joint->Name()); auto childFrameName = joint->ChildLinkName(); - if (_out.map.count(childFrameName) != 1) + if (_out.Count(childFrameName) != 1) { errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, "Child frame with name[" + childFrameName + @@ -304,15 +305,15 @@ Errors buildFrameAttachedToGraph( "] not found in model with name[" + _model->Name() + "]."}); continue; } - auto childFrameId = _out.map.at(childFrameName); - _out.graph.AddEdge({jointId, childFrameId}, true); + auto childFrameId = _out.VertexIdByName(childFrameName); + _out.AddEdge({jointId, childFrameId}, true); } // add frame edges for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - auto frameId = _out.map.at(frame->Name()); + auto frameId = _out.VertexIdByName(frame->Name()); // look for vertex in graph that matches attached_to value std::string attachedTo = frame->AttachedTo(); if (attachedTo.empty()) @@ -320,7 +321,7 @@ Errors buildFrameAttachedToGraph( // if the attached-to name is empty, use the scope name attachedTo = scopeName; } - if (_out.map.count(attachedTo) != 1) + if (_out.Count(attachedTo) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "attached_to name[" + attachedTo + @@ -329,7 +330,7 @@ Errors buildFrameAttachedToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto attachedToId = _out.map[attachedTo]; + auto attachedToId = _out.VertexIdByName(attachedTo); bool edgeData = true; if (frame->Name() == frame->AttachedTo()) { @@ -341,7 +342,7 @@ Errors buildFrameAttachedToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({frameId, attachedToId}, edgeData); + _out.AddEdge({frameId, attachedToId}, edgeData); } return errors; @@ -349,17 +350,14 @@ Errors buildFrameAttachedToGraph( ///////////////////////////////////////////////// Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const World *_world) + ScopedGraph &_out, const World *_world) { Errors errors; // add implicit world frame vertex first const std::string scopeName = "world"; - _out.scopeName = scopeName; - auto worldFrameId = - _out.graph.AddVertex(scopeName, sdf::FrameType::WORLD).Id(); - _out.map[scopeName] = worldFrameId; - + _out.SetScopeName(scopeName); + _out.AddVertex(scopeName, sdf::FrameType::WORLD).Id(); if (!_world) { @@ -378,7 +376,7 @@ Errors buildFrameAttachedToGraph( for (uint64_t m = 0; m < _world->ModelCount(); ++m) { auto model = _world->ModelByIndex(m); - if (_out.map.count(model->Name()) > 0) + if (_out.Count(model->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Model with non-unique name [" + model->Name() + @@ -386,16 +384,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto modelId = - _out.graph.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); - _out.map[model->Name()] = modelId; + _out.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); } // add frame vertices for (uint64_t f = 0; f < _world->FrameCount(); ++f) { auto frame = _world->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (_out.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -403,23 +399,21 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); } // add frame edges for (uint64_t f = 0; f < _world->FrameCount(); ++f) { auto frame = _world->FrameByIndex(f); - auto frameId = _out.map.at(frame->Name()); + auto frameId = _out.VertexIdByName(frame->Name()); // look for vertex in graph that matches attached_to value std::string attachedTo = frame->AttachedTo(); if (attachedTo.empty()) { // if the attached-to name is empty, use the scope name attachedTo = scopeName; - if (_out.map.count(scopeName) != 1) + if (_out.Count(scopeName) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + @@ -427,7 +421,7 @@ Errors buildFrameAttachedToGraph( continue; } } - if (_out.map.count(attachedTo) != 1) + if (_out.Count(attachedTo) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "attached_to name[" + attachedTo + @@ -436,7 +430,7 @@ Errors buildFrameAttachedToGraph( "in world with name[" + _world->Name() + "]."}); continue; } - auto attachedToId = _out.map[attachedTo]; + auto attachedToId = _out.VertexIdByName(attachedTo); bool edgeData = true; if (frame->Name() == frame->AttachedTo()) { @@ -448,7 +442,7 @@ Errors buildFrameAttachedToGraph( "], causing a graph cycle " "in world with name[" + _world->Name() + "]."}); } - _out.graph.AddEdge({frameId, attachedToId}, edgeData); + _out.AddEdge({frameId, attachedToId}, edgeData); } return errors; @@ -456,7 +450,7 @@ Errors buildFrameAttachedToGraph( ///////////////////////////////////////////////// Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const Model *_model) + ScopedGraph &_out, const Model *_model) { Errors errors; @@ -474,17 +468,16 @@ Errors buildPoseRelativeToGraph( } // add implicit model frame vertex first - const std::string sourceName = "__model__"; - _out.sourceName = sourceName; + const std::string scopeName = "__model__"; + _out.SetScopeName(scopeName); auto modelFrameId = - _out.graph.AddVertex(sourceName, sdf::FrameType::MODEL).Id(); - _out.map[sourceName] = modelFrameId; + _out.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); // add link vertices and default edge if relative_to is empty for (uint64_t l = 0; l < _model->LinkCount(); ++l) { auto link = _model->LinkByIndex(l); - if (_out.map.count(link->Name()) > 0) + if (_out.Count(link->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Link with non-unique name [" + link->Name() + @@ -493,13 +486,12 @@ Errors buildPoseRelativeToGraph( continue; } auto linkId = - _out.graph.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); - _out.map[link->Name()] = linkId; + _out.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); if (link->PoseRelativeTo().empty()) { // relative_to is empty, so add edge from implicit model frame to link - _out.graph.AddEdge({modelFrameId, linkId}, link->RawPose()); + _out.AddEdge({modelFrameId, linkId}, link->RawPose()); } } @@ -507,7 +499,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - if (_out.map.count(joint->Name()) > 0) + if (_out.Count(joint->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Joint with non-unique name [" + joint->Name() + @@ -515,9 +507,7 @@ Errors buildPoseRelativeToGraph( "]."}); continue; } - auto jointId = - _out.graph.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); - _out.map[joint->Name()] = jointId; + _out.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); } // add frame vertices and default edge if both @@ -525,7 +515,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (_out.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -534,13 +524,12 @@ Errors buildPoseRelativeToGraph( continue; } auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) { // add edge from implicit model frame to frame - _out.graph.AddEdge({modelFrameId, frameId}, frame->RawPose()); + _out.AddEdge({modelFrameId, frameId}, frame->RawPose()); } } @@ -548,7 +537,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t m = 0; m < _model->ModelCount(); ++m) { auto nestedModel = _model->ModelByIndex(m); - if (_out.map.count(nestedModel->Name()) > 0) + if (_out.Count(nestedModel->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Nested model with non-unique name [" + nestedModel->Name() + @@ -557,14 +546,13 @@ Errors buildPoseRelativeToGraph( continue; } auto nestedModelId = - _out.graph.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); - _out.map[nestedModel->Name()] = nestedModelId; + _out.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); if (nestedModel->PoseRelativeTo().empty()) { // relative_to is empty, so add edge from implicit model frame // to nestedModel - _out.graph.AddEdge({modelFrameId, nestedModelId}, nestedModel->RawPose()); + _out.AddEdge({modelFrameId, nestedModelId}, nestedModel->RawPose()); } } @@ -582,10 +570,10 @@ Errors buildPoseRelativeToGraph( continue; } - auto linkId = _out.map.at(link->Name()); + auto linkId = _out.VertexIdByName(link->Name()); // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (_out.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + @@ -594,7 +582,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = _out.VertexIdByName(relativeTo); if (link->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -603,7 +591,7 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, linkId}, link->RawPose()); + _out.AddEdge({relativeToId, linkId}, link->RawPose()); } for (uint64_t j = 0; j < _model->JointCount(); ++j) @@ -617,10 +605,10 @@ Errors buildPoseRelativeToGraph( relativeTo = joint->ChildLinkName(); } - auto jointId = _out.map.at(joint->Name()); + auto jointId = _out.VertexIdByName(joint->Name()); // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (_out.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + @@ -629,7 +617,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = _out.VertexIdByName(relativeTo); if (joint->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -638,7 +626,7 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, jointId}, joint->RawPose()); + _out.AddEdge({relativeToId, jointId}, joint->RawPose()); } for (uint64_t f = 0; f < _model->FrameCount(); ++f) @@ -651,7 +639,7 @@ Errors buildPoseRelativeToGraph( continue; } - auto frameId = _out.map.at(frame->Name()); + auto frameId = _out.VertexIdByName(frame->Name()); std::string relativeTo; std::string typeForErrorMsg; ErrorCode errorCode; @@ -669,7 +657,7 @@ Errors buildPoseRelativeToGraph( } // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (_out.Count(relativeTo) != 1) { errors.push_back({errorCode, typeForErrorMsg + " name[" + relativeTo + @@ -678,7 +666,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = _out.VertexIdByName(relativeTo); if (frame->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -687,7 +675,7 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, frameId}, frame->RawPose()); + _out.AddEdge({relativeToId, frameId}, frame->RawPose()); } for (uint64_t m = 0; m < _model->ModelCount(); ++m) @@ -701,10 +689,10 @@ Errors buildPoseRelativeToGraph( continue; } - auto nestedModelId = _out.map.at(nestedModel->Name()); + auto nestedModelId = _out.VertexIdByName(nestedModel->Name()); // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (_out.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + @@ -713,7 +701,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = _out.VertexIdByName(relativeTo); if (nestedModel->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -722,15 +710,16 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, nestedModelId}, nestedModel->RawPose()); + _out.AddEdge({relativeToId, nestedModelId}, nestedModel->RawPose()); } return errors; } ///////////////////////////////////////////////// +/// TODO Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const World *_world) + ScopedGraph &_out, const World *_world) { Errors errors; @@ -749,16 +738,14 @@ Errors buildPoseRelativeToGraph( // add implicit world frame vertex first const std::string sourceName = "world"; - _out.sourceName = sourceName; - auto worldFrameId = - _out.graph.AddVertex(sourceName, sdf::FrameType::WORLD).Id(); - _out.map[sourceName] = worldFrameId; + _out.SetScopeName(sourceName); + auto worldFrameId = _out.AddVertex(sourceName, sdf::FrameType::WORLD).Id(); // add model vertices and default edge if relative_to is empty for (uint64_t m = 0; m < _world->ModelCount(); ++m) { auto model = _world->ModelByIndex(m); - if (_out.map.count(model->Name()) > 0) + if (_out.Count(model->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Model with non-unique name [" + model->Name() + @@ -767,13 +754,12 @@ Errors buildPoseRelativeToGraph( continue; } auto modelId = - _out.graph.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); - _out.map[model->Name()] = modelId; + _out.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); if (model->PoseRelativeTo().empty()) { // relative_to is empty, so add edge from implicit world frame to model - _out.graph.AddEdge({worldFrameId, modelId}, model->RawPose()); + _out.AddEdge({worldFrameId, modelId}, model->RawPose()); } } @@ -782,7 +768,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t f = 0; f < _world->FrameCount(); ++f) { auto frame = _world->FrameByIndex(f); - if (_out.map.count(frame->Name()) > 0) + if (_out.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -791,13 +777,12 @@ Errors buildPoseRelativeToGraph( continue; } auto frameId = - _out.graph.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); - _out.map[frame->Name()] = frameId; + _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) { // add edge from implicit world frame to frame - _out.graph.AddEdge({worldFrameId, frameId}, frame->RawPose()); + _out.AddEdge({worldFrameId, frameId}, frame->RawPose()); } } @@ -815,10 +800,10 @@ Errors buildPoseRelativeToGraph( continue; } - auto modelId = _out.map.at(model->Name()); + auto modelId = _out.VertexIdByName(model->Name()); // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (_out.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + @@ -827,7 +812,7 @@ Errors buildPoseRelativeToGraph( "in world with name[" + _world->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = _out.VertexIdByName(relativeTo); if (model->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -836,7 +821,7 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in world with name[" + _world->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, modelId}, model->RawPose()); + _out.AddEdge({relativeToId, modelId}, model->RawPose()); } for (uint64_t f = 0; f < _world->FrameCount(); ++f) @@ -849,7 +834,7 @@ Errors buildPoseRelativeToGraph( continue; } - auto frameId = _out.map.at(frame->Name()); + auto frameId = _out.VertexIdByName(frame->Name()); std::string relativeTo; std::string typeForErrorMsg; ErrorCode errorCode; @@ -867,7 +852,7 @@ Errors buildPoseRelativeToGraph( } // look for vertex in graph that matches relative_to value - if (_out.map.count(relativeTo) != 1) + if (_out.Count(relativeTo) != 1) { errors.push_back({errorCode, typeForErrorMsg + " name[" + relativeTo + @@ -876,7 +861,7 @@ Errors buildPoseRelativeToGraph( "in world with name[" + _world->Name() + "]."}); continue; } - auto relativeToId = _out.map[relativeTo]; + auto relativeToId = _out.VertexIdByName(relativeTo); if (frame->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -885,54 +870,54 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in world with name[" + _world->Name() + "]."}); } - _out.graph.AddEdge({relativeToId, frameId}, frame->RawPose()); + _out.AddEdge({relativeToId, frameId}, frame->RawPose()); } return errors; } ///////////////////////////////////////////////// -Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) +Errors validateFrameAttachedToGraph(const ScopedGraph &_in) { Errors errors; // Expect scopeName to be either "__model__" or "world" - if (_in.scopeName != "__model__" && _in.scopeName != "world") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error: scope frame[" + _in.scopeName + "] " + "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] " " does not match __model__ or world."}); return errors; } // Expect one vertex with name "__model__" and FrameType MODEL // or with name "world" and FrameType WORLD - auto scopeVertices = _in.graph.Vertices(_in.scopeName); + auto scopeVertices = _in.Vertices(_in.ScopeName()); if (scopeVertices.empty()) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + - _in.scopeName + "] not found in graph."}); + _in.ScopeName() + "] not found in graph."}); return errors; } else if (scopeVertices.size() > 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "more than one vertex with scope name " + _in.scopeName}); + "more than one vertex with scope name " + _in.ScopeName()}); return errors; } auto scopeVertex = scopeVertices.begin()->second.get(); sdf::FrameType scopeFrameType = scopeVertex.Data(); - if (_in.scopeName == "__model__" && scopeFrameType != sdf::FrameType::MODEL) + if (_in.ScopeName() == "__model__" && scopeFrameType != sdf::FrameType::MODEL) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "scope vertex with name __model__ should have FrameType MODEL."}); return errors; } - else if (_in.scopeName == "world" && + else if (_in.ScopeName() == "world" && scopeFrameType != sdf::FrameType::WORLD) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, @@ -942,7 +927,7 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) } // Check number of outgoing edges for each vertex - auto vertices = _in.graph.Vertices(); + auto vertices = _in.Vertices(); for (auto vertexPair : vertices) { // Vertex names should not be empty @@ -953,7 +938,7 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) "vertex with empty name detected."}); } - auto outDegree = _in.graph.OutDegree(vertexPair.first); + auto outDegree = _in.OutDegree(vertexPair.first); if (outDegree > 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, @@ -1056,7 +1041,7 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) } // check graph for cycles by finding sink from each vertex - for (auto const &namePair : _in.map) + for (auto const &namePair : _in.Map()) { std::string resolvedBody; Errors e = resolveFrameAttachedToBody(resolvedBody, _in, namePair.first); @@ -1067,47 +1052,48 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in) } ///////////////////////////////////////////////// -Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) +Errors validatePoseRelativeToGraph( + const ScopedGraph &_in) { Errors errors; - // Expect sourceName to be either "__model__" or "world" - if (_in.sourceName != "__model__" && _in.sourceName != "world") + // Expect scopeName to be either "__model__" or "world" + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error: source vertex name " + _in.sourceName + + "PoseRelativeToGraph error: source vertex name " + _in.ScopeName() + " does not match __model__ or world."}); return errors; } // Expect one vertex with name "__model__" and FrameType MODEL // or with name "world" and FrameType WORLD - auto sourceVertices = _in.graph.Vertices(_in.sourceName); + auto sourceVertices = _in.Vertices(_in.ScopeName()); if (sourceVertices.empty()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: source frame[" + - _in.sourceName + "] not found in graph."}); + _in.ScopeName() + "] not found in graph."}); return errors; } else if (sourceVertices.size() > 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " - "more than one vertex with source name " + _in.sourceName}); + "more than one vertex with source name " + _in.ScopeName()}); return errors; } auto sourceVertex = sourceVertices.begin()->second.get(); sdf::FrameType sourceFrameType = sourceVertex.Data(); - if (_in.sourceName == "__model__" && sourceFrameType != sdf::FrameType::MODEL) + if (_in.ScopeName() == "__model__" && sourceFrameType != sdf::FrameType::MODEL) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "source vertex with name __model__ should have FrameType MODEL."}); return errors; } - else if (_in.sourceName == "world" && + else if (_in.ScopeName() == "world" && sourceFrameType != sdf::FrameType::WORLD) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, @@ -1117,7 +1103,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) } // Check number of incoming edges for each vertex - auto vertices = _in.graph.Vertices(); + auto vertices = _in.Vertices(); for (auto vertexPair : vertices) { // Vertex names should not be empty @@ -1128,7 +1114,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) "vertex with empty name detected."}); } - auto inDegree = _in.graph.InDegree(vertexPair.first); + auto inDegree = _in.InDegree(vertexPair.first); if (inDegree > 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, @@ -1230,7 +1216,7 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) } // check graph for cycles by resolving pose of each vertex relative to root - for (auto const &namePair : _in.map) + for (auto const &namePair : _in.Map()) { ignition::math::Pose3d pose; Errors e = resolvePoseRelativeToRoot(pose, _in, namePair.first); @@ -1243,29 +1229,29 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in) ///////////////////////////////////////////////// Errors resolveFrameAttachedToBody( std::string &_attachedToBody, - const FrameAttachedToGraph &_in, + const ScopedGraph &_in, const std::string &_vertexName) { Errors errors; - if (_in.scopeName != "__model__" && _in.scopeName != "world") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error: scope frame[" + _in.scopeName + "] " + "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] " " does not match __model__ or world."}); return errors; } - if (_in.map.count(_vertexName) != 1) + if (_in.Count(_vertexName) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "FrameAttachedToGraph unable to find unique frame with name [" + _vertexName + "] in graph."}); return errors; } - auto vertexId = _in.map.at(_vertexName); + auto vertexId = _in.VertexIdByName(_vertexName); - auto sinkVertexEdges = FindSinkVertex(_in.graph, vertexId, errors); + auto sinkVertexEdges = FindSinkVertex(_in, vertexId, errors); auto sinkVertex = sinkVertexEdges.first; if (!errors.empty()) @@ -1281,7 +1267,7 @@ Errors resolveFrameAttachedToBody( return errors; } - if (_in.scopeName == "world" && + if (_in.ScopeName() == "world" && !(sinkVertex.Data() == FrameType::WORLD || sinkVertex.Data() == FrameType::MODEL)) { @@ -1292,7 +1278,7 @@ Errors resolveFrameAttachedToBody( return errors; } - if (_in.scopeName == "__model__") + if (_in.ScopeName() == "__model__") { if (sinkVertex.Data() == FrameType::MODEL && sinkVertex.Name() == "__model__") @@ -1322,21 +1308,21 @@ Errors resolveFrameAttachedToBody( ///////////////////////////////////////////////// Errors resolvePoseRelativeToRoot( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_vertexName) { Errors errors; - if (_graph.map.count(_vertexName) != 1) + if (_graph.Count(_vertexName) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "PoseRelativeToGraph unable to find unique frame with name [" + _vertexName + "] in graph."}); return errors; } - auto vertexId = _graph.map.at(_vertexName); + auto vertexId = _graph.VertexIdByName(_vertexName); - auto incomingVertexEdges = FindSourceVertex(_graph.graph, vertexId, errors); + auto incomingVertexEdges = FindSourceVertex(_graph, vertexId, errors); if (!errors.empty()) { @@ -1349,13 +1335,13 @@ Errors resolvePoseRelativeToRoot( "when starting from vertex with name [" + _vertexName + "]."}); return errors; } - else if (incomingVertexEdges.first.Name() != _graph.sourceName) + else if (incomingVertexEdges.first.Name() != _graph.ScopeName()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph frame with name [" + _vertexName + "] " "is disconnected; its source vertex has name [" + incomingVertexEdges.first.Name() + - "], but its source name should be " + _graph.sourceName + "."}); + "], but its source name should be " + _graph.ScopeName() + "."}); return errors; } @@ -1376,7 +1362,7 @@ Errors resolvePoseRelativeToRoot( ///////////////////////////////////////////////// Errors resolvePose( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_frameName, const std::string &_resolveTo) { @@ -1396,30 +1382,31 @@ Errors resolvePose( ///////////////////////////////////////////////// Errors updateGraphPose( - PoseRelativeToGraph &_graph, + ScopedGraph &_graph, const std::string &_frameName, const ignition::math::Pose3d &_pose) { Errors errors; - if (_graph.map.count(_frameName) != 1) + if (_graph.Count(_frameName) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "PoseRelativeToGraph unable to find unique frame with name [" + _frameName+ "] in graph."}); return errors; } - auto vertexId = _graph.map.at(_frameName); - auto incidentsTo = _graph.graph.IncidentsTo(vertexId); + auto vertexId = _graph.VertexIdByName(_frameName); + auto incidentsTo = _graph.Graph().IncidentsTo(vertexId); if (incidentsTo.size() == 1) { // There's no API to update the data of an edge, so we remove the edge and // insert a new one with the new pose. - auto &edge = incidentsTo.begin()->second; - auto tailVertexId = edge.get().Tail(); - auto headVertexId = edge.get().Head(); - _graph.graph.RemoveEdge(edge.get().Id()); - _graph.graph.AddEdge({tailVertexId, headVertexId}, _pose); + // auto &edge = incidentsTo.begin()->second; + // auto tailVertexId = edge.get().Tail(); + // auto headVertexId = edge.get().Head(); + // _graph.graph.RemoveEdge(edge.get().Id()); + // _graph.graph.AddEdge({tailVertexId, headVertexId}, _pose); + _graph.UpdateEdge(incidentsTo.begin()->second, _pose); } else if (incidentsTo.empty()) { diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 80ebf84ff..f80387146 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -18,6 +18,7 @@ #define SDF_FRAMESEMANTICS_HH_ #include +#include #include #include @@ -45,6 +46,7 @@ namespace sdf // Forward declaration. class Model; class World; + template class ScopedGraph; /// \enum FrameType /// \brief The set of frame types. INVALID indicates that frame type has @@ -109,41 +111,43 @@ namespace sdf /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const Model *_model); + ScopedGraph &_out, const Model *_model); /// \brief Build a FrameAttachedToGraph for a world. /// \param[out] _out Graph object to write. /// \param[in] _world World from which to build attached_to graph. /// \return Errors. Errors buildFrameAttachedToGraph( - FrameAttachedToGraph &_out, const World *_world); + ScopedGraph &_out, const World *_world); /// \brief Build a PoseRelativeToGraph for a model. /// \param[out] _out Graph object to write. /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const Model *_model); + ScopedGraph &_out, const Model *_model); /// \brief Build a PoseRelativeToGraph for a world. /// \param[out] _out Graph object to write. /// \param[in] _world World from which to build attached_to graph. /// \return Errors. Errors buildPoseRelativeToGraph( - PoseRelativeToGraph &_out, const World *_world); + ScopedGraph &_out, const World *_world); /// \brief Confirm that FrameAttachedToGraph is valid by checking the number /// of outbound edges for each vertex and checking for graph cycles. /// \param[in] _in Graph object to validate. /// \return Errors. - Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in); + Errors validateFrameAttachedToGraph( + const ScopedGraph &_in); /// \brief Confirm that PoseRelativeToGraph is valid by checking the number /// of outbound edges for each vertex and checking for graph cycles. /// \param[in] _in Graph object to validate. /// \return Errors. - Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in); + Errors validatePoseRelativeToGraph( + const ScopedGraph &_in); /// \brief Resolve the attached-to body for a given frame. Following the /// edges of the frame attached-to graph from a given frame must lead @@ -157,7 +161,7 @@ namespace sdf /// a link or world frame. Errors resolveFrameAttachedToBody( std::string &_attachedToBody, - const FrameAttachedToGraph &_in, + const ScopedGraph &_in, const std::string &_vertexName); /// \brief Resolve pose of a vertex relative to its outgoing ancestor @@ -168,7 +172,7 @@ namespace sdf /// \return Errors. Errors resolvePoseRelativeToRoot( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_vertexName); /// \brief Resolve pose of a frame relative to named frame. @@ -180,7 +184,7 @@ namespace sdf /// \return Errors. Errors resolvePose( ignition::math::Pose3d &_pose, - const PoseRelativeToGraph &_graph, + const ScopedGraph &_graph, const std::string &_frameName, const std::string &_resolveTo); @@ -192,7 +196,7 @@ namespace sdf /// \param[in] _pose New pose. /// \return Errors. Errors updateGraphPose( - PoseRelativeToGraph &_graph, + ScopedGraph &_graph, const std::string &_frameName, const ignition::math::Pose3d &_pose); } diff --git a/src/Joint.cc b/src/Joint.cc index 4a467cbfc..283a9bd4a 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -25,6 +25,7 @@ #include "sdf/JointAxis.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -75,10 +76,10 @@ class sdf::JointPrivate public: sdf::ElementPtr sdf; /// \brief Weak pointer to model's Frame Attached-To Graph. - public: std::weak_ptr frameAttachedToGraph; + public: sdf::ScopedGraph frameAttachedToGraph; /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -361,14 +362,14 @@ void Joint::SetPoseRelativeTo(const std::string &_frame) ///////////////////////////////////////////////// void Joint::SetFrameAttachedToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->frameAttachedToGraph = _graph; } ///////////////////////////////////////////////// void Joint::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; @@ -387,7 +388,7 @@ Errors Joint::ResolveChildLink(std::string &_link) const { Errors errors; - auto graph = this->dataPtr->frameAttachedToGraph.lock(); + auto graph = this->dataPtr->frameAttachedToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -396,7 +397,7 @@ Errors Joint::ResolveChildLink(std::string &_link) const } std::string link; - errors = resolveFrameAttachedToBody(link, *graph, this->ChildLinkName()); + errors = resolveFrameAttachedToBody(link, graph, this->ChildLinkName()); if (errors.empty()) { _link = link; @@ -417,7 +418,7 @@ Errors Joint::ResolveParentLink(std::string &_link) const return errors; } - auto graph = this->dataPtr->frameAttachedToGraph.lock(); + auto graph = this->dataPtr->frameAttachedToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -426,7 +427,7 @@ Errors Joint::ResolveParentLink(std::string &_link) const } std::string link; - errors = resolveFrameAttachedToBody(link, *graph, this->ParentLinkName()); + errors = resolveFrameAttachedToBody(link, graph, this->ParentLinkName()); if (errors.empty()) { _link = link; diff --git a/src/JointAxis.cc b/src/JointAxis.cc index f9cd705f3..4836228d0 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -24,6 +24,7 @@ #include "sdf/Error.hh" #include "sdf/JointAxis.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" using namespace sdf; @@ -86,7 +87,7 @@ class sdf::JointAxisPrivate public: std::string xmlParentName; /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -377,7 +378,7 @@ void JointAxis::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void JointAxis::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } @@ -388,7 +389,7 @@ Errors JointAxis::ResolveXyz( const std::string &_resolveTo) const { Errors errors; - auto graph = this->dataPtr->poseRelativeToGraph.lock(); + auto graph = this->dataPtr->poseRelativeToGraph; if (!graph) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -417,7 +418,7 @@ Errors JointAxis::ResolveXyz( } ignition::math::Pose3d pose; - errors = resolvePose(pose, *graph, axisExpressedIn, resolveTo); + errors = resolvePose(pose, graph, axisExpressedIn, resolveTo); if (errors.empty()) { diff --git a/src/Light.cc b/src/Light.cc index 1c65922fd..c03cd22d7 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -18,6 +18,8 @@ #include #include "sdf/Error.hh" #include "sdf/Light.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -44,7 +46,7 @@ class sdf::LightPrivate public: std::string xmlParentName; /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + public: sdf::ScopedGraph poseRelativeToGraph; /// \brief True if the light should cast shadows. public: bool castShadows = false; @@ -317,7 +319,7 @@ void Light::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Light::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/src/Link.cc b/src/Link.cc index 9ec72fc60..15003df37 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -28,6 +28,9 @@ #include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" + +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -67,7 +70,7 @@ class sdf::LinkPrivate public: bool enableWind = false; /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -374,8 +377,7 @@ void Link::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -void Link::SetPoseRelativeToGraph( - std::weak_ptr _graph) +void Link::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; diff --git a/src/Model.cc b/src/Model.cc index 62d578bac..1b872a3bf 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -27,6 +27,7 @@ #include "sdf/Model.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -77,16 +78,15 @@ class sdf::ModelPrivate public: sdf::ElementPtr sdf; /// \brief Frame Attached-To Graph constructed during Load. - public: std::shared_ptr frameAttachedToGraph; + /// TODO (addisu) More documentation + public: std::shared_ptr ownedFrameAttachedToGraph; - /// \brief Pose Relative-To Graph constructed during Load. - public: std::shared_ptr poseGraph; - - /// \brief Pose Relative-To Graph in parent (world or __model__) scope. - public: std::weak_ptr parentPoseGraph; + public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Scope name of parent Pose Relative-To Graph (world or __model__). - public: std::string parentPoseGraphScopeName; + /// \brief Pose Relative-To Graph constructed during Load. + /// TODO (addisu) More documentation + public: std::shared_ptr ownedPoseGraph; + public: sdf::ScopedGraph poseGraph; }; ///////////////////////////////////////////////// @@ -106,16 +106,24 @@ Model::~Model() Model::Model(const Model &_model) : dataPtr(new ModelPrivate(*_model.dataPtr)) { - if (_model.dataPtr->frameAttachedToGraph) + if (_model.dataPtr->ownedFrameAttachedToGraph) { - this->dataPtr->frameAttachedToGraph = + // If the model owns the frameAttachedToGraph, we need to allocate a new + // sdf::FrameAttachedToGraph object and copy it. We also need to assign the + // ScopedGraph to this object + this->dataPtr->ownedFrameAttachedToGraph = std::make_shared( - *_model.dataPtr->frameAttachedToGraph); + *_model.dataPtr->ownedFrameAttachedToGraph); + this->dataPtr->frameAttachedToGraph = + ScopedGraph( + this->dataPtr->ownedFrameAttachedToGraph); } - if (_model.dataPtr->poseGraph) + if (_model.dataPtr->ownedPoseGraph) { - this->dataPtr->poseGraph = std::make_shared( - *_model.dataPtr->poseGraph); + this->dataPtr->ownedPoseGraph = std::make_shared( + *_model.dataPtr->ownedPoseGraph); + this->dataPtr->poseGraph = + ScopedGraph(this->dataPtr->ownedPoseGraph); } for (auto &link : this->dataPtr->links) { @@ -223,10 +231,17 @@ Errors Model::Load(ElementPtr _sdf) // Set of implicit and explicit frame names in this model for tracking // name collisions std::unordered_set frameNames; + // int *subtree; + + std::function beforeLoad = [this](Model &_model) + { + // TODO (addisu) + _model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + }; // Load nested models. Errors nestedModelLoadErrors = loadUniqueRepeated(_sdf, "model", - this->dataPtr->models); + this->dataPtr->models, beforeLoad); errors.insert(errors.end(), nestedModelLoadErrors.begin(), nestedModelLoadErrors.end()); @@ -366,14 +381,19 @@ Errors Model::Load(ElementPtr _sdf) // static models. if (!this->Static()) { - this->dataPtr->frameAttachedToGraph - = std::make_shared(); + if (!this->dataPtr->frameAttachedToGraph) + { + this->dataPtr->ownedFrameAttachedToGraph = + std::make_shared(); + this->dataPtr->frameAttachedToGraph = ScopedGraph( + this->dataPtr->ownedFrameAttachedToGraph); + } Errors frameAttachedToGraphErrors = - buildFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph, this); + buildFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph, this); errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), frameAttachedToGraphErrors.end()); Errors validateFrameAttachedGraphErrors = - validateFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph); + validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), validateFrameAttachedGraphErrors.end()); for (auto &joint : this->dataPtr->joints) @@ -387,15 +407,31 @@ Errors Model::Load(ElementPtr _sdf) } // Build the PoseRelativeToGraph - this->dataPtr->poseGraph = std::make_shared(); Errors poseGraphErrors = - buildPoseRelativeToGraph(*this->dataPtr->poseGraph, this); + buildPoseRelativeToGraph(this->dataPtr->poseGraph, this); errors.insert(errors.end(), poseGraphErrors.begin(), poseGraphErrors.end()); - Errors validatePoseGraphErrors = - validatePoseRelativeToGraph(*this->dataPtr->poseGraph); - errors.insert(errors.end(), validatePoseGraphErrors.begin(), - validatePoseGraphErrors.end()); + // for (auto &model : this->dataPtr->models) + // { + // std::string prefix = model.Name() + "::"; + // poseGraphErrors = + // buildPoseRelativeToGraph(*this->dataPtr->poseGraph, &model, prefix); + // errors.insert(errors.end(), poseGraphErrors.begin(), poseGraphErrors.end()); + // } + // std::cout << "Model: " << this->Name() << " graph:" << std::endl; + // std::cout << this->dataPtr->poseGraph->graph << std::endl; + + errors.insert(errors.end(), poseGraphErrors.begin(), + poseGraphErrors.end()); + // TODO(addisu) only do this if this is the top level entity? This would be to + // improve performance + if (this->dataPtr->poseGraph.PointsTo(this->dataPtr->ownedPoseGraph)) + { + Errors validatePoseGraphErrors = + validatePoseRelativeToGraph(this->dataPtr->poseGraph); + errors.insert(errors.end(), validatePoseGraphErrors.begin(), + validatePoseGraphErrors.end()); + } for (auto &link : this->dataPtr->links) { link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); @@ -422,7 +458,7 @@ Errors Model::Load(ElementPtr _sdf) ignition::math::Pose3d X_MPf; sdf::Errors resolveErrors = - sdf::resolvePose(X_MPf, *this->dataPtr->poseGraph, + sdf::resolvePose(X_MPf, this->dataPtr->poseGraph, this->dataPtr->placementFrameName, "__model__"); if (resolveErrors.empty()) { @@ -486,7 +522,7 @@ Errors Model::Load(ElementPtr _sdf) auto X_RM = X_RPf * X_MPf.Inverse(); frame.SetRawPose(X_RM); sdf::updateGraphPose( - *this->dataPtr->poseGraph, childModelFrame->Name(), X_RM); + this->dataPtr->poseGraph, childModelFrame->Name(), X_RM); } else { @@ -771,20 +807,18 @@ void Model::SetPoseRelativeTo(const std::string &_frame) ///////////////////////////////////////////////// Errors Model::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { Errors errors; - auto graph = _graph.lock(); - if (!graph) + if (!_graph) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "Tried to set PoseRelativeToGraph with invalid pointer."}); return errors; } - this->dataPtr->parentPoseGraphScopeName = graph->sourceName; - this->dataPtr->parentPoseGraph = _graph; + this->dataPtr->poseGraph = _graph; return errors; } @@ -795,8 +829,8 @@ sdf::SemanticPose Model::SemanticPose() const return sdf::SemanticPose( this->dataPtr->pose, this->dataPtr->poseRelativeTo, - this->dataPtr->parentPoseGraphScopeName, - this->dataPtr->parentPoseGraph); + this->dataPtr->poseGraph.ScopeName(), + this->dataPtr->poseGraph); } ///////////////////////////////////////////////// diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh new file mode 100644 index 000000000..2bc4d6e79 --- /dev/null +++ b/src/ScopedGraph.hh @@ -0,0 +1,103 @@ +/* + * Copyright 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef SDF_SCOPED_GRAPH_HH +#define SDF_SCOPED_GRAPH_HH + +#include +#include +#include +#include + +#include +#include + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +template +class ScopedGraph +{ + + public: + template + struct GraphTypeExtracter + { + using Vertex = void; + using Edge = void; + }; + + public: + template + struct GraphTypeExtracter> + { + using Vertex = V; + using Edge = E; + }; + + public: using MathGraphType = typename T::GraphType; + public: using VertexId = ignition::math::graph::VertexId; + public: using VertexType = typename GraphTypeExtracter::Vertex; + public: using EdgeType = typename GraphTypeExtracter::Edge; + public: using Vertex = ignition::math::graph::Vertex; + public: using Edge = ignition::math::graph::DirectedEdge; + public: using MapType = std::unordered_map; + + public: ScopedGraph(); + // TODO: maybe _rootVertex can have a default value + public: ScopedGraph(const std::shared_ptr _graph, VertexId _rootVertex); + public: ScopedGraph(const std::shared_ptr _graph); + public: explicit operator bool() const; + + public: const MathGraphType &Graph() const; + public: const MapType &Map() const; + + // May not be needed if they can be called via Graph() + public: Vertex &AddVertex(const std::string &_name, const VertexType &); + public: Edge &AddEdge(const ignition::math::graph::VertexId_P &, const EdgeType &); + public: size_t OutDegree(const VertexId &) const; + public: size_t InDegree(const VertexId &) const; + + public: const ignition::math::graph::VertexRef_M Vertices() const; + public: const ignition::math::graph::VertexRef_M Vertices( + const std::string &) const; + + public: void UpdateEdge(std::reference_wrapper &, const EdgeType&); + + public: size_t Count(const std::string &_name) const; + public: VertexId VertexIdByName(const std::string &_name) const; + public: VertexId RootVertexId() const; + + public: bool IsTopLevel() const; + public: bool PointsTo(const std::shared_ptr _graph) const; + public: std::string SetScopeName(const std::string &_name) const; + public: std::string ScopeName() const; + + + private: std::weak_ptr graph; + + // private: std::unordered_set vertices; + private: VertexId rootVertex; + private: std::string prefix; + private: std::string scopeName; +}; + +} +} + +#endif /* SDF_SCOPED_GRAPH_HH */ diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index eefc7d51c..a6f23d59b 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -21,6 +21,7 @@ #include "sdf/Error.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" namespace sdf @@ -40,7 +41,8 @@ class SemanticPosePrivate public: std::string defaultResolveTo = ""; /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + /// TODO (addisu) Make this const + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -48,7 +50,7 @@ SemanticPose::SemanticPose( const ignition::math::Pose3d &_pose, const std::string &_relativeTo, const std::string &_defaultResolveTo, - const std::weak_ptr _graph) + const sdf::ScopedGraph &_graph) : dataPtr(std::make_unique()) { this->dataPtr->rawPose = _pose; @@ -103,7 +105,7 @@ Errors SemanticPose::Resolve( { Errors errors; - auto graph = this->dataPtr->poseRelativeToGraph.lock(); + auto graph = this->dataPtr->poseRelativeToGraph; if (!graph) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, @@ -124,7 +126,7 @@ Errors SemanticPose::Resolve( } ignition::math::Pose3d pose; - errors = resolvePose(pose, *graph, relativeTo, resolveTo); + errors = resolvePose(pose, graph, relativeTo, resolveTo); pose *= this->RawPose(); if (errors.empty()) diff --git a/src/Sensor.cc b/src/Sensor.cc index 1d3c467f3..fc588c33f 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -27,6 +27,8 @@ #include "sdf/Lidar.hh" #include "sdf/Sensor.hh" #include "sdf/Types.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -129,7 +131,7 @@ class sdf::SensorPrivate public: std::string xmlParentName; /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + public: sdf::ScopedGraph poseRelativeToGraph; /// \brief Pointer to a magnetometer. public: std::unique_ptr magnetometer; @@ -474,7 +476,7 @@ void Sensor::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Sensor::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/src/Utils.hh b/src/Utils.hh index 73740ed1d..4d81ece67 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -65,9 +65,10 @@ namespace sdf /// exists. /// \return The vector of errors. An empty vector indicates no errors were /// experienced. - template + template sdf::Errors loadUniqueRepeated(sdf::ElementPtr _sdf, - const std::string &_sdfName, std::vector &_objs) + const std::string &_sdfName, std::vector &_objs, + const std::function &_beforeLoadFunc = {}) { Errors errors; @@ -81,6 +82,10 @@ namespace sdf while (elem) { Class obj; + if (_beforeLoadFunc) + { + _beforeLoadFunc(obj); + } // Load the model and capture the errors. Errors loadErrors = obj.Load(elem); @@ -128,9 +133,10 @@ namespace sdf /// vector, unless an error is encountered during load. /// \return The vector of errors. An empty vector indicates no errors were /// experienced. - template - sdf::Errors loadRepeated(sdf::ElementPtr _sdf, - const std::string &_sdfName, std::vector &_objs) + template + sdf::Errors loadRepeated(sdf::ElementPtr _sdf, const std::string &_sdfName, + std::vector &_objs, + const std::function &_beforeLoadFunc = {}) { Errors errors; @@ -142,6 +148,10 @@ namespace sdf while (elem) { Class obj; + if (_beforeLoadFunc) + { + _beforeLoadFunc(obj); + } // Load the model and capture the errors. Errors loadErrors = obj.Load(elem); diff --git a/src/Visual.cc b/src/Visual.cc index 8d99b5f68..8c36bf673 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -21,6 +21,8 @@ #include "sdf/Types.hh" #include "sdf/Visual.hh" #include "sdf/Geometry.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -65,7 +67,7 @@ class sdf::VisualPrivate public: std::string xmlParentName; /// \brief Weak pointer to model's Pose Relative-To Graph. - public: std::weak_ptr poseRelativeToGraph; + public: sdf::ScopedGraph poseRelativeToGraph; /// \brief Visibility flags of a visual. Defaults to 0xFFFFFFFF public: uint32_t visibilityFlags = 4294967295u; @@ -276,7 +278,7 @@ void Visual::SetXmlParentName(const std::string &_xmlParentName) ///////////////////////////////////////////////// void Visual::SetPoseRelativeToGraph( - std::weak_ptr _graph) + sdf::ScopedGraph _graph) { this->dataPtr->poseRelativeToGraph = _graph; } diff --git a/src/World.cc b/src/World.cc index 38ac9c1b3..3ca43bd0f 100644 --- a/src/World.cc +++ b/src/World.cc @@ -27,6 +27,7 @@ #include "sdf/Types.hh" #include "sdf/World.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -89,10 +90,14 @@ class sdf::WorldPrivate ignition::math::Vector3d::Zero; /// \brief Frame Attached-To Graph constructed during Load. - public: std::shared_ptr frameAttachedToGraph; + // public: std::shared_ptr frameAttachedToGraph; + public: std::shared_ptr ownedFrameAttachedToGraph; + public: sdf::ScopedGraph frameAttachedToGraph; /// \brief Pose Relative-To Graph constructed during Load. - public: std::shared_ptr poseRelativeToGraph; + // public: std::shared_ptr poseRelativeToGraph; + public: sdf::PoseRelativeToGraph ownedPoseRelativeToGraph; + public: sdf::ScopedGraph poseRelativeToGraph; }; ///////////////////////////////////////////////// @@ -118,16 +123,16 @@ WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) { this->gui = std::make_unique(*(_worldPrivate.gui)); } - if (_worldPrivate.frameAttachedToGraph) - { - this->frameAttachedToGraph = std::make_shared( - *(_worldPrivate.frameAttachedToGraph)); - } - if (_worldPrivate.poseRelativeToGraph) - { - this->poseRelativeToGraph = std::make_shared( - *(_worldPrivate.poseRelativeToGraph)); - } + // if (_worldPrivate.frameAttachedToGraph) + // { + // this->frameAttachedToGraph = std::make_shared( + // *(_worldPrivate.frameAttachedToGraph)); + // } + // if (_worldPrivate.poseRelativeToGraph) + // { + // this->poseRelativeToGraph = std::make_shared( + // *(_worldPrivate.poseRelativeToGraph)); + // } if (_worldPrivate.scene) { this->scene = std::make_unique(*(_worldPrivate.scene)); @@ -152,20 +157,20 @@ World::~World() World::World(const World &_world) : dataPtr(new WorldPrivate(*_world.dataPtr)) { - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &model : this->dataPtr->models) - { - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &light : this->dataPtr->lights) - { - light.SetXmlParentName("world"); - light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } + // for (auto &frame : this->dataPtr->frames) + // { + // frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + // frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + // } + // for (auto &model : this->dataPtr->models) + // { + // model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + // } + // for (auto &light : this->dataPtr->lights) + // { + // light.SetXmlParentName("world"); + // light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + // } } ///////////////////////////////////////////////// @@ -342,34 +347,31 @@ Errors World::Load(sdf::ElementPtr _sdf) } // Build the graphs. - this->dataPtr->frameAttachedToGraph = - std::make_shared(); Errors frameAttachedToGraphErrors = - buildFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph, this); + buildFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph, this); errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), frameAttachedToGraphErrors.end()); Errors validateFrameAttachedGraphErrors = - validateFrameAttachedToGraph(*this->dataPtr->frameAttachedToGraph); + validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), validateFrameAttachedGraphErrors.end()); - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } + // for (auto &frame : this->dataPtr->frames) + // { + // frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + // } - this->dataPtr->poseRelativeToGraph = std::make_shared(); Errors poseRelativeToGraphErrors = - buildPoseRelativeToGraph(*this->dataPtr->poseRelativeToGraph, this); + buildPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph, this); errors.insert(errors.end(), poseRelativeToGraphErrors.begin(), poseRelativeToGraphErrors.end()); Errors validatePoseGraphErrors = - validatePoseRelativeToGraph(*this->dataPtr->poseRelativeToGraph); + validatePoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); errors.insert(errors.end(), validatePoseGraphErrors.begin(), validatePoseGraphErrors.end()); - for (auto &frame : this->dataPtr->frames) - { - frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } + // for (auto &frame : this->dataPtr->frames) + // { + // frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + // } for (auto &model : this->dataPtr->models) { Errors setPoseRelativeToGraphErrors = @@ -380,7 +382,7 @@ Errors World::Load(sdf::ElementPtr _sdf) for (auto &light : this->dataPtr->lights) { light.SetXmlParentName("world"); - light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + // light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); } return errors; diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index bfd92442c..6eacd7a24 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -688,6 +688,16 @@ TEST(check, SDF) std::string::npos) << output; } + + // Check an SDF file with an valid nested model cross references + { + std::string path = pathBase + "/nested_model_cross_references.sdf"; + + // Check model_invalid_placement_frame.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; + } } ///////////////////////////////////////////////// diff --git a/src/parser.cc b/src/parser.cc index 5fde25f7d..f18bb05f9 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -37,6 +37,7 @@ #include "Converter.hh" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "parser_private.hh" #include "parser_urdf.hh" @@ -1711,7 +1712,8 @@ bool checkFrameAttachedToGraph(const sdf::Root *_root) const sdf::Model *_model) -> bool { bool modelResult = true; - sdf::FrameAttachedToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildFrameAttachedToGraph(graph, _model); if (!errors.empty()) { @@ -1739,7 +1741,8 @@ bool checkFrameAttachedToGraph(const sdf::Root *_root) const sdf::World *_world) -> bool { bool worldResult = true; - sdf::FrameAttachedToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildFrameAttachedToGraph(graph, _world); if (!errors.empty()) { @@ -1792,7 +1795,8 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) const sdf::Model *_model) -> bool { bool modelResult = true; - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildPoseRelativeToGraph(graph, _model); if (!errors.empty()) { @@ -1820,7 +1824,8 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) const sdf::World *_world) -> bool { bool worldResult = true; - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildPoseRelativeToGraph(graph, _world); if (!errors.empty()) { From 612a805582d492e53d6a1d3852dd10d3dfa2b36d Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Sep 2020 11:33:40 -0500 Subject: [PATCH 06/70] Frame attached to tests passing Signed-off-by: Addisu Z. Taddese --- include/sdf/Model.hh | 7 +- include/sdf/Types.hh | 7 ++ src/FrameSemantics.cc | 195 ++++++++++++++++++++----------- src/FrameSemantics.hh | 15 ++- src/FrameSemantics_TEST.cc | 113 ++++++++++-------- src/Model.cc | 83 +++++++------ src/Root.cc | 5 +- src/ScopedGraph.hh | 230 ++++++++++++++++++++++++++++++++++--- src/Types.cc | 11 ++ src/World.cc | 93 ++++++++------- 10 files changed, 545 insertions(+), 214 deletions(-) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 09c75b63a..184638ce9 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -38,6 +38,7 @@ namespace sdf class Link; class ModelPrivate; struct PoseRelativeToGraph; + struct FrameAttachedToGraph; template class ScopedGraph; class SDFORMAT_VISIBLE Model @@ -288,7 +289,11 @@ namespace sdf private: sdf::Errors SetPoseRelativeToGraph( sdf::ScopedGraph _graph); - /// \brief Allow World::Load to call SetPoseRelativeToGraph. + private: void SetFrameAttachedToGraph( + sdf::ScopedGraph _graph); + + /// \brief Allow World::Load to call SetPoseRelativeToGraph and + /// SetFrameAttachedToGraph friend class World; /// \brief Private data pointer. diff --git a/include/sdf/Types.hh b/include/sdf/Types.hh index 607f7712f..493fcb9f9 100644 --- a/include/sdf/Types.hh +++ b/include/sdf/Types.hh @@ -94,6 +94,13 @@ namespace sdf /// \brief A vector of Error. using Errors = std::vector; + /// \brief Output operator for a collection of errors. + /// \param[in,out] _out The output stream. + /// \param[in] _err The errors to output. + /// \return Reference to the given output stream + SDFORMAT_VISIBLE std::ostream &operator<<( + std::ostream &_out, const sdf::Errors &_errs); + /// \brief Defines a color class SDFORMAT_VISIBLE Color { diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index f21cafb3e..7408fb559 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include #include "sdf/Element.hh" @@ -92,7 +93,7 @@ FindSourceVertex(const ScopedGraph &_graph, vertex.get().Name() + "]."}); return PairType(Vertex::NullVertex, EdgesType()); } - if (vertex.get().Id() == _graph.RootVertexId()) + if (vertex.get().Id() == _graph.ScopeVertexId()) { // This is the source. break; @@ -100,7 +101,7 @@ FindSourceVertex(const ScopedGraph &_graph, visited.insert(vertex.get().Id()); incidentsTo = _graph.Graph().IncidentsTo(vertex); } - if (vertex.get().Id() != _graph.RootVertexId()) + if (vertex.get().Id() != _graph.ScopeVertexId()) { // Error, the root vertex is not the same as the the source return PairType(Vertex::NullVertex, EdgesType()); @@ -184,7 +185,8 @@ Errors buildFrameAttachedToGraph( const std::string scopeName = "__model__"; _out.SetScopeName(scopeName); auto modelFrameId = - _out.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); + _out.AddScopeVertex(_model->Name(), scopeName, sdf::FrameType::MODEL) + .Id(); if (!_model) { @@ -289,6 +291,8 @@ Errors buildFrameAttachedToGraph( continue; } _out.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); + ScopedGraphchildGraph = _out; + buildFrameAttachedToGraph(childGraph, nestedModel); } // add edges from joint to child frames @@ -309,6 +313,18 @@ Errors buildFrameAttachedToGraph( _out.AddEdge({jointId, childFrameId}, true); } + // add model edges to their corresponding vertices in the model scope + for (uint64_t m = 0; m < _model->ModelCount(); ++m) + { + auto model = _model->ModelByIndex(m); + auto modelId = _out.VertexIdByName(model->Name()); + auto modelIdChildScope = _out.VertexIdByName(model->Name() + "::__model__"); + if (modelIdChildScope != ignition::math::graph::kNullId) + { + _out.AddEdge({modelId, modelIdChildScope}, true); + } + } + // add frame edges for (uint64_t f = 0; f < _model->FrameCount(); ++f) { @@ -345,6 +361,7 @@ Errors buildFrameAttachedToGraph( _out.AddEdge({frameId, attachedToId}, edgeData); } + std::cout << "Model FrameAttachedToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -357,7 +374,7 @@ Errors buildFrameAttachedToGraph( // add implicit world frame vertex first const std::string scopeName = "world"; _out.SetScopeName(scopeName); - _out.AddVertex(scopeName, sdf::FrameType::WORLD).Id(); + _out.AddScopeVertex("", scopeName, sdf::FrameType::WORLD).Id(); if (!_world) { @@ -385,6 +402,8 @@ Errors buildFrameAttachedToGraph( continue; } _out.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); + ScopedGraphchildGraph = _out; + buildFrameAttachedToGraph(childGraph, model); } // add frame vertices @@ -402,6 +421,18 @@ Errors buildFrameAttachedToGraph( _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); } + // add model edges to their corresponding vertices in the model scope + for (uint64_t m = 0; m < _world->ModelCount(); ++m) + { + auto model = _world->ModelByIndex(m); + auto modelId = _out.VertexIdByName(model->Name()); + auto modelIdChildScope = _out.VertexIdByName(model->Name() + "::__model__"); + if (modelIdChildScope != ignition::math::graph::kNullId) + { + _out.AddEdge({modelId, modelIdChildScope}, true); + } + } + // add frame edges for (uint64_t f = 0; f < _world->FrameCount(); ++f) { @@ -445,6 +476,7 @@ Errors buildFrameAttachedToGraph( _out.AddEdge({frameId, attachedToId}, edgeData); } + std::cout << "World FrameAttachedToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -467,11 +499,11 @@ Errors buildPoseRelativeToGraph( return errors; } - // add implicit model frame vertex first + // add the model frame vertex first const std::string scopeName = "__model__"; _out.SetScopeName(scopeName); auto modelFrameId = - _out.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); + _out.AddScopeVertex(_model->Name(), scopeName, sdf::FrameType::MODEL).Id(); // add link vertices and default edge if relative_to is empty for (uint64_t l = 0; l < _model->LinkCount(); ++l) @@ -548,12 +580,28 @@ Errors buildPoseRelativeToGraph( auto nestedModelId = _out.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); + ScopedGraph childGraph = _out; + buildPoseRelativeToGraph(childGraph , nestedModel); + if (nestedModel->PoseRelativeTo().empty()) { // relative_to is empty, so add edge from implicit model frame // to nestedModel _out.AddEdge({modelFrameId, nestedModelId}, nestedModel->RawPose()); } + // Add an aliasing edge from the nestedModel vertex to the + // corresponding vertex in the PoseRelativeTo graph of the nested model, + // i.e, to the ::__model__ vertex + auto scopedNestedModelId = + _out.VertexIdByName(nestedModel->Name() + "::__model__"); + if (scopedNestedModelId == ignition::math::graph::kNullId) + { + // TODO (addisu) ERROR + } + else + { + _out.AddEdge({nestedModelId, scopedNestedModelId}, {{}, true}); + } } // now that all vertices have been added to the graph, @@ -713,13 +761,13 @@ Errors buildPoseRelativeToGraph( _out.AddEdge({relativeToId, nestedModelId}, nestedModel->RawPose()); } + std::cout << "Model PoseRelativeToGraph\n" << _out.Graph() << std::endl; return errors; } ///////////////////////////////////////////////// -/// TODO Errors buildPoseRelativeToGraph( - ScopedGraph &_out, const World *_world) + ScopedGraph &_out, const World *_world) { Errors errors; @@ -739,7 +787,8 @@ Errors buildPoseRelativeToGraph( // add implicit world frame vertex first const std::string sourceName = "world"; _out.SetScopeName(sourceName); - auto worldFrameId = _out.AddVertex(sourceName, sdf::FrameType::WORLD).Id(); + auto worldFrameId = + _out.AddScopeVertex("", sourceName, sdf::FrameType::WORLD).Id(); // add model vertices and default edge if relative_to is empty for (uint64_t m = 0; m < _world->ModelCount(); ++m) @@ -756,11 +805,27 @@ Errors buildPoseRelativeToGraph( auto modelId = _out.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); + ScopedGraph childGraph = _out; + buildPoseRelativeToGraph(childGraph , model); + if (model->PoseRelativeTo().empty()) { // relative_to is empty, so add edge from implicit world frame to model _out.AddEdge({worldFrameId, modelId}, model->RawPose()); } + // Add a non-constraint edge from the model vertex to the + // corresponding vertex in the PoseRelativeTo graph of the child model, + // i.e, to the ::__model__ vertex + auto scopedModelId = + _out.VertexIdByName(model->Name() + "::__model__"); + if (scopedModelId == ignition::math::graph::kNullId) + { + // TODO (addisu) ERROR + } + else + { + _out.AddEdge({modelId, scopedModelId}, {{}, false}); + } } // add frame vertices and default edge if both @@ -873,6 +938,7 @@ Errors buildPoseRelativeToGraph( _out.AddEdge({relativeToId, frameId}, frame->RawPose()); } + std::cout << "World PoseRelativeToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -892,23 +958,15 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in // Expect one vertex with name "__model__" and FrameType MODEL // or with name "world" and FrameType WORLD - auto scopeVertices = _in.Vertices(_in.ScopeName()); - if (scopeVertices.empty()) + auto scopeVertex = _in.ScopeVertex(); + if (!scopeVertex.Valid()) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] not found in graph."}); return errors; } - else if (scopeVertices.size() > 1) - { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error, " - "more than one vertex with scope name " + _in.ScopeName()}); - return errors; - } - auto scopeVertex = scopeVertices.begin()->second.get(); sdf::FrameType scopeFrameType = scopeVertex.Data(); if (_in.ScopeName() == "__model__" && scopeFrameType != sdf::FrameType::MODEL) { @@ -930,21 +988,22 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in auto vertices = _in.Vertices(); for (auto vertexPair : vertices) { + const std::string vertexName = _in.VertexName(vertexPair.second.get()); // Vertex names should not be empty - if (vertexPair.second.get().Name().empty()) + if (vertexName.empty()) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "vertex with empty name detected."}); } - auto outDegree = _in.OutDegree(vertexPair.first); + auto outDegree = _in.Graph().OutDegree(vertexPair.first); if (outDegree > 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "too many outgoing edges at a vertex with name [" + - vertexPair.second.get().Name() + "]."}); + vertexName + "]."}); } else if (sdf::FrameType::MODEL == scopeFrameType) { @@ -953,7 +1012,7 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in case sdf::FrameType::WORLD: errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "vertex with name [" + vertexPair.second.get().Name() + "]" + + "vertex with name [" + vertexName + "]" + "should not have type WORLD in MODEL attached_to graph."}); break; case sdf::FrameType::LINK: @@ -962,20 +1021,20 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "LINK vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] should have no outgoing edges " "in MODEL attached_to graph."}); } break; case sdf::FrameType::MODEL: - if ("__model__" != vertexPair.second.get().Name()) + if (_in.ScopeVertexId() != vertexPair.second.get().Id()) { if (outDegree != 0) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "nested MODEL vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] should have no outgoing edges " "in MODEL attached_to graph."}); } @@ -989,7 +1048,7 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "Non-LINK vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] is disconnected; it should have 1 outgoing edge " + "in MODEL attached_to graph."}); } @@ -998,7 +1057,7 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " "Non-LINK vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] has " + std::to_string(outDegree) + " outgoing edges; it should only have 1 " "outgoing edge in MODEL attached_to graph."}); @@ -1011,13 +1070,6 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in // scopeFrameType must be sdf::FrameType::WORLD switch (vertexPair.second.get().Data()) { - case sdf::FrameType::JOINT: - case sdf::FrameType::LINK: - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error, " - "No JOINT or LINK vertex should be in WORLD attached_to graph."}); - break; - case sdf::FrameType::MODEL: case sdf::FrameType::WORLD: if (outDegree != 0) { @@ -1027,13 +1079,27 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in "in WORLD attached_to graph."}); } break; + case sdf::FrameType::LINK: + if (outDegree != 0) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error, " + "LINK vertex with name [" + + vertexName + + "] should have no outgoing edges " + "in WORLD attached_to graph."}); + } + break; default: if (outDegree != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "FRAME vertices in WORLD attached_to graph should have " - "1 outgoing edge."}); + "Non-LINK vertex with name [" + + vertexName + + "] has " + std::to_string(outDegree) + + " outgoing edges; it should only have 1 " + "outgoing edge in WORLD attached_to graph."}); } break; } @@ -1041,10 +1107,10 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in } // check graph for cycles by finding sink from each vertex - for (auto const &namePair : _in.Map()) + for (auto const &name : _in.VertexNames()) { std::string resolvedBody; - Errors e = resolveFrameAttachedToBody(resolvedBody, _in, namePair.first); + Errors e = resolveFrameAttachedToBody(resolvedBody, _in, name); errors.insert(errors.end(), e.begin(), e.end()); } @@ -1106,21 +1172,26 @@ Errors validatePoseRelativeToGraph( auto vertices = _in.Vertices(); for (auto vertexPair : vertices) { + const std::string vertexName = _in.VertexName(vertexPair.second.get()); // Vertex names should not be empty - if (vertexPair.second.get().Name().empty()) + if (vertexName.empty()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "vertex with empty name detected."}); } - auto inDegree = _in.InDegree(vertexPair.first); + auto incomingEdges = _in.Graph().IncidentsTo(vertexPair.first); + std::size_t inDegree = std::count_if(incomingEdges.begin(), + incomingEdges.end(), + [](const auto &_edge) { return !_edge.second.get().Data().aliasing; }); + if (inDegree > 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "too many incoming edges at a vertex with name [" + - vertexPair.second.get().Name() + "]."}); + vertexName + "]."}); } else if (sdf::FrameType::MODEL == sourceFrameType) { @@ -1129,11 +1200,13 @@ Errors validatePoseRelativeToGraph( case sdf::FrameType::WORLD: errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " - "vertex with name [" + vertexPair.second.get().Name() + "]" + + "vertex with name [" + vertexName + "]" + "should not have type WORLD in MODEL relative_to graph."}); break; case sdf::FrameType::MODEL: - if ("__model__" == vertexPair.second.get().Name()) + // TODO: What we have to check here is that if this is the scope + // vertex any incoming edge is from outside this scope. + if (_in.ScopeVertexId() == vertexPair.second.get().Id()) { if (inDegree != 0) { @@ -1153,7 +1226,7 @@ Errors validatePoseRelativeToGraph( errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "Vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] is disconnected; it should have 1 incoming edge " + "in MODEL relative_to graph."}); } @@ -1162,7 +1235,7 @@ Errors validatePoseRelativeToGraph( errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "Non-MODEL vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] has " + std::to_string(inDegree) + " incoming edges; it should only have 1 " "incoming edge in MODEL relative_to graph."}); @@ -1175,12 +1248,6 @@ Errors validatePoseRelativeToGraph( // sourceFrameType must be sdf::FrameType::WORLD switch (vertexPair.second.get().Data()) { - case sdf::FrameType::JOINT: - case sdf::FrameType::LINK: - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error, " - "No JOINT or LINK vertex should be in WORLD relative_to graph."}); - break; case sdf::FrameType::WORLD: if (inDegree != 0) { @@ -1196,7 +1263,7 @@ Errors validatePoseRelativeToGraph( errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "MODEL / FRAME vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] is disconnected; it should have 1 incoming edge " + "in WORLD relative_to graph."}); } @@ -1205,7 +1272,7 @@ Errors validatePoseRelativeToGraph( errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " "MODEL / FRAME vertex with name [" + - vertexPair.second.get().Name() + + vertexName + "] has " + std::to_string(inDegree) + " incoming edges; it should only have 1 " "incoming edge in WORLD relative_to graph."}); @@ -1216,12 +1283,12 @@ Errors validatePoseRelativeToGraph( } // check graph for cycles by resolving pose of each vertex relative to root - for (auto const &namePair : _in.Map()) - { - ignition::math::Pose3d pose; - Errors e = resolvePoseRelativeToRoot(pose, _in, namePair.first); - errors.insert(errors.end(), e.begin(), e.end()); - } + // for (auto const &name : _in.VertexNames()) + // { + // ignition::math::Pose3d pose; + // Errors e = resolvePoseRelativeToRoot(pose, _in, name); + // errors.insert(errors.end(), e.begin(), e.end()); + // } return errors; } @@ -1269,7 +1336,7 @@ Errors resolveFrameAttachedToBody( if (_in.ScopeName() == "world" && !(sinkVertex.Data() == FrameType::WORLD || - sinkVertex.Data() == FrameType::MODEL)) + sinkVertex.Data() == FrameType::LINK)) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "Graph has world scope but sink vertex named [" + @@ -1300,7 +1367,7 @@ Errors resolveFrameAttachedToBody( } } - _attachedToBody = sinkVertex.Name(); + _attachedToBody = _in.VertexName(sinkVertex); return errors; } @@ -1335,7 +1402,7 @@ Errors resolvePoseRelativeToRoot( "when starting from vertex with name [" + _vertexName + "]."}); return errors; } - else if (incomingVertexEdges.first.Name() != _graph.ScopeName()) + else if (incomingVertexEdges.first.Id() != _graph.ScopeVertex().Id()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph frame with name [" + _vertexName + "] " @@ -1348,7 +1415,7 @@ Errors resolvePoseRelativeToRoot( ignition::math::Pose3d pose; for (auto const &edge : incomingVertexEdges.second) { - pose = edge.Data() * pose; + pose = edge.Data().pose * pose; } if (errors.empty()) diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index f80387146..f88ac89e9 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -95,7 +95,20 @@ namespace sdf /// named __model__ or world. Each vertex stores its FrameType and each edge /// stores the Pose3 between those frames. using Pose3d = ignition::math::Pose3d; - using GraphType = ignition::math::graph::DirectedGraph; + struct Edge { + Edge() = default; + Edge(const Pose3d &_pose) + : pose(_pose) + { + } + Edge(const Pose3d &_pose, bool _aliasing) + : pose(_pose), aliasing(_aliasing) + { + } + Pose3d pose; + bool aliasing{false}; + }; + using GraphType = ignition::math::graph::DirectedGraph; GraphType graph; /// \brief A Map from Vertex names to Vertex Ids. diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index 262ee4c12..54dbaeac2 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -32,6 +32,7 @@ #include "sdf/sdf_config.h" #include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "test_config.h" ///////////////////////////////////////////////// @@ -43,29 +44,31 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; // Get the first model const sdf::Model *model = root.ModelByIndex(0); - sdf::FrameAttachedToGraph graph; - auto errors = sdf::buildFrameAttachedToGraph(graph, model); - EXPECT_TRUE(errors.empty()); + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, model); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_TRUE(sdf::validateFrameAttachedToGraph(graph).empty()); EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); - EXPECT_EQ(6u, graph.map.size()); - EXPECT_EQ(6u, graph.graph.Vertices().size()); - EXPECT_EQ(5u, graph.graph.Edges().size()); + EXPECT_EQ(6u, graph.Map().size()); + EXPECT_EQ(6u, graph.Graph().Vertices().size()); + EXPECT_EQ(5u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.map.count("__model__")); - EXPECT_EQ(1u, graph.map.count("L")); - EXPECT_EQ(1u, graph.map.count("F00")); - EXPECT_EQ(1u, graph.map.count("F0")); - EXPECT_EQ(1u, graph.map.count("F1")); - EXPECT_EQ(1u, graph.map.count("F2")); - EXPECT_EQ(0u, graph.map.count("invalid")); + EXPECT_EQ(1u, graph.Count("__model__")); + EXPECT_EQ(1u, graph.Count("L")); + EXPECT_EQ(1u, graph.Count("F00")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(0u, graph.Count("invalid")); std::string resolvedBody; EXPECT_TRUE( @@ -108,29 +111,34 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; // Get the first world const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(world); - sdf::FrameAttachedToGraph graph; - auto errors = sdf::buildFrameAttachedToGraph(graph, world); + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, world); EXPECT_TRUE(errors.empty()); - EXPECT_TRUE(sdf::validateFrameAttachedToGraph(graph).empty()); + std::cout << graph.Graph() << std::endl; + errors = sdf::validateFrameAttachedToGraph(graph); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); - EXPECT_EQ(6u, graph.map.size()); - EXPECT_EQ(6u, graph.graph.Vertices().size()); - EXPECT_EQ(4u, graph.graph.Edges().size()); + EXPECT_EQ(9u, graph.Map().size()); + EXPECT_EQ(9u, graph.Graph().Vertices().size()); + EXPECT_EQ(7u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.map.count("world")); - EXPECT_EQ(1u, graph.map.count("world_frame")); - EXPECT_EQ(1u, graph.map.count("F0")); - EXPECT_EQ(1u, graph.map.count("F1")); - EXPECT_EQ(1u, graph.map.count("F2")); - EXPECT_EQ(1u, graph.map.count("M1")); - EXPECT_EQ(0u, graph.map.count("invalid")); + EXPECT_EQ(1u, graph.Count("world")); + EXPECT_EQ(1u, graph.Count("world_frame")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("M1")); + EXPECT_EQ(0u, graph.Count("invalid")); std::string resolvedBody; EXPECT_TRUE( @@ -148,10 +156,10 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) EXPECT_EQ("world", resolvedBody); EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1").empty()); - EXPECT_EQ("M1", resolvedBody); + EXPECT_EQ("M1::L", resolvedBody); EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F2").empty()); - EXPECT_EQ("M1", resolvedBody); + EXPECT_EQ("M1::L", resolvedBody); // Try to resolve invalid frame name errors = sdf::resolveFrameAttachedToBody(resolvedBody, graph, "invalid"); @@ -168,19 +176,20 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) ASSERT_NE(nullptr, world); const sdf::Model *model = world->ModelByIndex(0); - sdf::FrameAttachedToGraph modelGraph; + auto ownedModelGraph = std::make_shared(); + sdf::ScopedGraph modelGraph(ownedModelGraph); errors = sdf::buildFrameAttachedToGraph(modelGraph, model); EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validateFrameAttachedToGraph(modelGraph).empty()); - EXPECT_EQ(3u, modelGraph.map.size()); - EXPECT_EQ(3u, modelGraph.graph.Vertices().size()); - EXPECT_EQ(2u, modelGraph.graph.Edges().size()); + EXPECT_EQ(3u, modelGraph.Map().size()); + EXPECT_EQ(3u, modelGraph.Graph().Vertices().size()); + EXPECT_EQ(2u, modelGraph.Graph().Edges().size()); - EXPECT_EQ(1u, modelGraph.map.count("L")); - EXPECT_EQ(1u, modelGraph.map.count("__model__")); - EXPECT_EQ(1u, modelGraph.map.count("F0")); - EXPECT_EQ(0u, modelGraph.map.count("invalid")); + EXPECT_EQ(1u, modelGraph.Count("L")); + EXPECT_EQ(1u, modelGraph.Count("__model__")); + EXPECT_EQ(1u, modelGraph.Count("F0")); + EXPECT_EQ(0u, modelGraph.Count("invalid")); EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "L").empty()); @@ -208,23 +217,24 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) // Get the first model const sdf::Model *model = root.ModelByIndex(0); - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); auto errors = sdf::buildPoseRelativeToGraph(graph, model); EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validatePoseRelativeToGraph(graph).empty()); - EXPECT_EQ(8u, graph.map.size()); - EXPECT_EQ(8u, graph.graph.Vertices().size()); - EXPECT_EQ(7u, graph.graph.Edges().size()); + EXPECT_EQ(8u, graph.Map().size()); + EXPECT_EQ(8u, graph.Graph().Vertices().size()); + EXPECT_EQ(7u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.map.count("__model__")); - EXPECT_EQ(1u, graph.map.count("P")); - EXPECT_EQ(1u, graph.map.count("C")); - EXPECT_EQ(1u, graph.map.count("J")); - EXPECT_EQ(1u, graph.map.count("F1")); - EXPECT_EQ(1u, graph.map.count("F2")); - EXPECT_EQ(1u, graph.map.count("F3")); - EXPECT_EQ(1u, graph.map.count("F4")); + EXPECT_EQ(1u, graph.Map().count("__model__")); + EXPECT_EQ(1u, graph.Map().count("P")); + EXPECT_EQ(1u, graph.Map().count("C")); + EXPECT_EQ(1u, graph.Map().count("J")); + EXPECT_EQ(1u, graph.Map().count("F1")); + EXPECT_EQ(1u, graph.Map().count("F2")); + EXPECT_EQ(1u, graph.Map().count("F3")); + EXPECT_EQ(1u, graph.Map().count("F4")); // Test resolvePoseRelativeToRoot for each frame. ignition::math::Pose3d pose; @@ -300,7 +310,8 @@ TEST(FrameSemantics, updateGraphPose) // Get the first model const sdf::Model *model = root.ModelByIndex(0); - sdf::PoseRelativeToGraph graph; + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); { sdf::Errors errors = sdf::buildPoseRelativeToGraph(graph, model); EXPECT_TRUE(errors.empty()); diff --git a/src/Model.cc b/src/Model.cc index 1b872a3bf..ae4f9ea17 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -228,15 +228,27 @@ Errors Model::Load(ElementPtr _sdf) << this->Name() << "].\n"; } + if (!this->dataPtr->poseGraph) + { + this->dataPtr->ownedPoseGraph = std::make_shared(); + this->SetPoseRelativeToGraph(this->dataPtr->ownedPoseGraph); + } + + if (!this->dataPtr->frameAttachedToGraph) + { + this->dataPtr->ownedFrameAttachedToGraph = + std::make_shared(); + this->SetFrameAttachedToGraph(this->dataPtr->ownedFrameAttachedToGraph); + } + // Set of implicit and explicit frame names in this model for tracking // name collisions std::unordered_set frameNames; - // int *subtree; - std::function beforeLoad = [this](Model &_model) { // TODO (addisu) _model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + _model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); }; // Load nested models. @@ -381,21 +393,17 @@ Errors Model::Load(ElementPtr _sdf) // static models. if (!this->Static()) { - if (!this->dataPtr->frameAttachedToGraph) + if (this->dataPtr->ownedPoseGraph) { - this->dataPtr->ownedFrameAttachedToGraph = - std::make_shared(); - this->dataPtr->frameAttachedToGraph = ScopedGraph( - this->dataPtr->ownedFrameAttachedToGraph); + Errors frameAttachedToGraphErrors = + buildFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph, this); + errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), + frameAttachedToGraphErrors.end()); + Errors validateFrameAttachedGraphErrors = + validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), + validateFrameAttachedGraphErrors.end()); } - Errors frameAttachedToGraphErrors = - buildFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph, this); - errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), - frameAttachedToGraphErrors.end()); - Errors validateFrameAttachedGraphErrors = - validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), - validateFrameAttachedGraphErrors.end()); for (auto &joint : this->dataPtr->joints) { joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); @@ -407,28 +415,13 @@ Errors Model::Load(ElementPtr _sdf) } // Build the PoseRelativeToGraph - Errors poseGraphErrors = - buildPoseRelativeToGraph(this->dataPtr->poseGraph, this); - errors.insert(errors.end(), poseGraphErrors.begin(), - poseGraphErrors.end()); - // for (auto &model : this->dataPtr->models) - // { - // std::string prefix = model.Name() + "::"; - // poseGraphErrors = - // buildPoseRelativeToGraph(*this->dataPtr->poseGraph, &model, prefix); - // errors.insert(errors.end(), poseGraphErrors.begin(), poseGraphErrors.end()); - // } - // std::cout << "Model: " << this->Name() << " graph:" << std::endl; - // std::cout << this->dataPtr->poseGraph->graph << std::endl; - - errors.insert(errors.end(), poseGraphErrors.begin(), - poseGraphErrors.end()); - // TODO(addisu) only do this if this is the top level entity? This would be to - // improve performance - if (this->dataPtr->poseGraph.PointsTo(this->dataPtr->ownedPoseGraph)) + if (this->dataPtr->ownedPoseGraph) { + Errors poseGraphErrors = + buildPoseRelativeToGraph(this->dataPtr->poseGraph, this); + errors.insert(errors.end(), poseGraphErrors.begin(), poseGraphErrors.end()); Errors validatePoseGraphErrors = - validatePoseRelativeToGraph(this->dataPtr->poseGraph); + validatePoseRelativeToGraph(this->dataPtr->poseGraph); errors.insert(errors.end(), validatePoseGraphErrors.begin(), validatePoseGraphErrors.end()); } @@ -818,10 +811,30 @@ Errors Model::SetPoseRelativeToGraph( return errors; } + // If the scoped graph doesn't point to the graph owned by this model, we + // clear the owned graph to maintain the invariant that if + // ownedPoseGraph is valid poseGraph points to it. + if (!_graph.PointsTo(this->dataPtr->ownedPoseGraph)) + { + this->dataPtr->ownedPoseGraph.reset(); + } this->dataPtr->poseGraph = _graph; return errors; } +///////////////////////////////////////////////// +void Model::SetFrameAttachedToGraph( + sdf::ScopedGraph _graph) +{ + // If the scoped graph doesn't point to the graph owned by this model, we + // clear the owned graph to maintain the invariant that if + // ownedFrameAttachedToGraph is valid frameAttachedToGraph points to it. + if (!_graph.PointsTo(this->dataPtr->ownedFrameAttachedToGraph)) + { + this->dataPtr->ownedFrameAttachedToGraph.reset(); + } + this->dataPtr->frameAttachedToGraph = _graph; +} ///////////////////////////////////////////////// sdf::SemanticPose Model::SemanticPose() const diff --git a/src/Root.cc b/src/Root.cc index 916bd879d..7898182c2 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -161,10 +161,6 @@ Errors Root::Load(SDFPtr _sdf) "World with name[" + world.Name() + "] already exists." " Each world must have a unique name. Skipping this world."}); } - else - { - this->dataPtr->worlds.push_back(std::move(world)); - } } else { @@ -173,6 +169,7 @@ Errors Root::Load(SDFPtr _sdf) errors.push_back({ErrorCode::ELEMENT_INVALID, "Failed to load a world."}); } + this->dataPtr->worlds.push_back(std::move(world)); elem = elem->GetNextElement("world"); } } diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index 2bc4d6e79..e3c71487b 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -18,10 +18,12 @@ #ifndef SDF_SCOPED_GRAPH_HH #define SDF_SCOPED_GRAPH_HH +#include #include +#include +#include #include #include -#include #include #include @@ -56,11 +58,9 @@ class ScopedGraph public: using EdgeType = typename GraphTypeExtracter::Edge; public: using Vertex = ignition::math::graph::Vertex; public: using Edge = ignition::math::graph::DirectedEdge; - public: using MapType = std::unordered_map; + public: using MapType = typename T::MapType; - public: ScopedGraph(); - // TODO: maybe _rootVertex can have a default value - public: ScopedGraph(const std::shared_ptr _graph, VertexId _rootVertex); + public: ScopedGraph() = default; public: ScopedGraph(const std::shared_ptr _graph); public: explicit operator bool() const; @@ -68,35 +68,233 @@ class ScopedGraph public: const MapType &Map() const; // May not be needed if they can be called via Graph() + public: Vertex &AddScopeVertex(const std::string &_prefix, const std::string &_name, const VertexType &_data); public: Vertex &AddVertex(const std::string &_name, const VertexType &); public: Edge &AddEdge(const ignition::math::graph::VertexId_P &, const EdgeType &); - public: size_t OutDegree(const VertexId &) const; - public: size_t InDegree(const VertexId &) const; public: const ignition::math::graph::VertexRef_M Vertices() const; public: const ignition::math::graph::VertexRef_M Vertices( const std::string &) const; + public: std::vector VertexNames() const; + public: std::string VertexName(const VertexId &_id) const; + public: std::string VertexName(const Vertex &_vertex) const; public: void UpdateEdge(std::reference_wrapper &, const EdgeType&); public: size_t Count(const std::string &_name) const; public: VertexId VertexIdByName(const std::string &_name) const; - public: VertexId RootVertexId() const; + public: Vertex ScopeVertex() const; + public: VertexId ScopeVertexId() const; - public: bool IsTopLevel() const; public: bool PointsTo(const std::shared_ptr _graph) const; - public: std::string SetScopeName(const std::string &_name) const; - public: std::string ScopeName() const; - + public: void SetScopeName(const std::string &_name); + public: const std::string &ScopeName() const; + public: std::string AddPrefix(const std::string &_name) const; + public: std::string RemovePrefix(const std::string &_name) const; - private: std::weak_ptr graph; + private: std::weak_ptr graphWeak; // private: std::unordered_set vertices; - private: VertexId rootVertex; - private: std::string prefix; - private: std::string scopeName; + private: VertexId scopeVertexId; + private: std::string prefix = {}; + private: std::string scopeName = {}; }; +///////////////////////////////////////////////// +template +ScopedGraph::ScopedGraph(const std::shared_ptr _graph): graphWeak(_graph) +{ +} + +///////////////////////////////////////////////// +template +ScopedGraph::operator bool() const +{ + return !this->graphWeak.expired(); +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::Graph() const -> const MathGraphType & +{ + return this->graphWeak.lock()->graph; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::Map() const -> const MapType & +{ + return this->graphWeak.lock()->map; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::AddScopeVertex(const std::string &_prefix, + const std::string &_name, const VertexType &_data) -> Vertex & +{ + this->prefix = this->AddPrefix(_prefix); + Vertex &vert = this->AddVertex(_name, _data); + this->scopeVertexId = vert.Id(); + return vert; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::AddVertex( + const std::string &_name, const VertexType &_data) -> Vertex & +{ + auto graph = graphWeak.lock(); + const std::string newName = this->AddPrefix(_name); + Vertex &vert = graph->graph.AddVertex(newName, _data); + graph->map[newName] = vert.Id(); + return vert; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::AddEdge( +const ignition::math::graph::VertexId_P &_vertexPair, const EdgeType &_data) -> Edge & +{ + auto graph = graphWeak.lock(); + Edge &edge = graph->graph.AddEdge(_vertexPair, _data); + return edge; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::Vertices() const + -> const ignition::math::graph::VertexRef_M +{ + auto graph = graphWeak.lock(); + return graph->graph.Vertices(); +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::Vertices(const std::string &_name) const + -> const ignition::math::graph::VertexRef_M +{ + auto graph = graphWeak.lock(); + return graph->graph.Vertices(this->AddPrefix(_name)); +} + +///////////////////////////////////////////////// +template +std::vector ScopedGraph::VertexNames() const +{ + std::vector out; + for (const auto &namePair : this->Map()) + { + out.push_back(this->RemovePrefix(namePair.first)); + } + return out; +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::VertexName(const Vertex &_vert) const +{ + return this->RemovePrefix(_vert.Name()); +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::VertexName(const VertexId &_id) const +{ + return this->VertexName(this->Graph().VertexFromId(_id)); +} + +///////////////////////////////////////////////// +template +void ScopedGraph::UpdateEdge( +std::reference_wrapper &, const EdgeType&) +{ + // TODO (addisu) +} + +///////////////////////////////////////////////// +template +void ScopedGraph::SetScopeName(const std::string &_name) +{ + this->scopeName = _name; +} + +///////////////////////////////////////////////// +template +std::size_t ScopedGraph::Count(const std::string &_name) const +{ + return this->graphWeak.lock()->map.count(this->AddPrefix(_name)); +} + +///////////////////////////////////////////////// +template +std::size_t ScopedGraph::VertexIdByName(const std::string &_name) const +{ + auto &map = this->Map(); + auto it = map.find(this->AddPrefix(_name)); + if (it != map.end()) + return it->second; + else + return ignition::math::graph::kNullId; +} + +///////////////////////////////////////////////// +template +auto ScopedGraph::ScopeVertex() const -> Vertex +{ + return this->graphWeak.lock()->graph.VertexFromId(this->scopeVertexId); +} + +///////////////////////////////////////////////// +template +std::size_t ScopedGraph::ScopeVertexId() const +{ + return this->scopeVertexId; +} + +///////////////////////////////////////////////// +template +bool ScopedGraph::PointsTo(const std::shared_ptr _graph) const +{ + return this->graphWeak.lock() == _graph; +} + +///////////////////////////////////////////////// +template +const std::string &ScopedGraph::ScopeName() const +{ + return this->scopeName; +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::AddPrefix(const std::string &_name) const +{ + if (this->prefix.empty()) + { + return _name; + } + else + { + return this->prefix + "::" + _name; + } +} + +///////////////////////////////////////////////// +template +std::string ScopedGraph::RemovePrefix(const std::string &_name) const +{ + if (!this->prefix.empty()) + { + auto ind = _name.find(this->prefix + "::"); + if (ind == 0) + { + return _name.substr(this->prefix.size() + 2); + } + } + return _name; +} + } } diff --git a/src/Types.cc b/src/Types.cc index 13df3060c..989574877 100644 --- a/src/Types.cc +++ b/src/Types.cc @@ -78,5 +78,16 @@ std::string lowercase(const std::string &_in) out[i] = std::tolower(out[i], std::locale()); return out; } + +///////////////////////////////////////////////// +std::ostream &operator<<(std::ostream &_out, const sdf::Errors &_errs) +{ + for (const auto &e : _errs) + { + _out << e << std::endl; + } + return _out; +} + } } diff --git a/src/World.cc b/src/World.cc index 3ca43bd0f..1ae05290e 100644 --- a/src/World.cc +++ b/src/World.cc @@ -35,7 +35,7 @@ using namespace sdf; class sdf::WorldPrivate { /// \brief Default constructor - public: WorldPrivate() = default; + public: WorldPrivate(); /// \brief Copy constructor /// \param[in] _worldPrivate Joint axis to move. @@ -95,11 +95,19 @@ class sdf::WorldPrivate public: sdf::ScopedGraph frameAttachedToGraph; /// \brief Pose Relative-To Graph constructed during Load. - // public: std::shared_ptr poseRelativeToGraph; - public: sdf::PoseRelativeToGraph ownedPoseRelativeToGraph; + public: std::shared_ptr ownedPoseRelativeToGraph; public: sdf::ScopedGraph poseRelativeToGraph; }; +///////////////////////////////////////////////// +WorldPrivate::WorldPrivate() + : ownedFrameAttachedToGraph(std::make_shared()) + , frameAttachedToGraph(ownedFrameAttachedToGraph) + , ownedPoseRelativeToGraph(std::make_shared()) + , poseRelativeToGraph(ownedPoseRelativeToGraph) +{ +} + ///////////////////////////////////////////////// WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) : audioDevice(_worldPrivate.audioDevice), @@ -123,16 +131,19 @@ WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) { this->gui = std::make_unique(*(_worldPrivate.gui)); } - // if (_worldPrivate.frameAttachedToGraph) - // { - // this->frameAttachedToGraph = std::make_shared( - // *(_worldPrivate.frameAttachedToGraph)); - // } - // if (_worldPrivate.poseRelativeToGraph) - // { - // this->poseRelativeToGraph = std::make_shared( - // *(_worldPrivate.poseRelativeToGraph)); - // } + if (_worldPrivate.ownedFrameAttachedToGraph) + { + this->ownedFrameAttachedToGraph = + std::make_shared( + *(_worldPrivate.ownedFrameAttachedToGraph)); + this->frameAttachedToGraph = this->ownedFrameAttachedToGraph; + } + if (_worldPrivate.ownedPoseRelativeToGraph) + { + this->ownedPoseRelativeToGraph = std::make_shared( + *(_worldPrivate.ownedPoseRelativeToGraph)); + this->poseRelativeToGraph = this->ownedPoseRelativeToGraph; + } if (_worldPrivate.scene) { this->scene = std::make_unique(*(_worldPrivate.scene)); @@ -157,20 +168,20 @@ World::~World() World::World(const World &_world) : dataPtr(new WorldPrivate(*_world.dataPtr)) { - // for (auto &frame : this->dataPtr->frames) - // { - // frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - // frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - // } - // for (auto &model : this->dataPtr->models) - // { - // model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - // } - // for (auto &light : this->dataPtr->lights) - // { - // light.SetXmlParentName("world"); - // light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - // } + for (auto &frame : this->dataPtr->frames) + { + frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } + for (auto &model : this->dataPtr->models) + { + model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } + for (auto &light : this->dataPtr->lights) + { + light.SetXmlParentName("world"); + light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } } ///////////////////////////////////////////////// @@ -270,9 +281,14 @@ Errors World::Load(sdf::ElementPtr _sdf) // name collisions std::unordered_set frameNames; + std::function beforeLoad = [this](Model &_model) + { + _model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + _model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + }; // Load all the models. Errors modelLoadErrors = loadUniqueRepeated(_sdf, "model", - this->dataPtr->models); + this->dataPtr->models, beforeLoad); errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end()); // Models are loaded first, and loadUniqueRepeated ensures there are no @@ -355,10 +371,10 @@ Errors World::Load(sdf::ElementPtr _sdf) validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), validateFrameAttachedGraphErrors.end()); - // for (auto &frame : this->dataPtr->frames) - // { - // frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - // } + for (auto &frame : this->dataPtr->frames) + { + frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } Errors poseRelativeToGraphErrors = buildPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph, this); @@ -368,21 +384,14 @@ Errors World::Load(sdf::ElementPtr _sdf) validatePoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); errors.insert(errors.end(), validatePoseGraphErrors.begin(), validatePoseGraphErrors.end()); - // for (auto &frame : this->dataPtr->frames) - // { - // frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - // } - for (auto &model : this->dataPtr->models) + for (auto &frame : this->dataPtr->frames) { - Errors setPoseRelativeToGraphErrors = - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), - setPoseRelativeToGraphErrors.end()); + frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); } for (auto &light : this->dataPtr->lights) { light.SetXmlParentName("world"); - // light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); } return errors; From f74fc33a150a904ff4bef8421949fc1c7650b0b0 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Sep 2020 12:38:06 -0500 Subject: [PATCH 07/70] FrameSemantics_TEST passing Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 20 +++++++++++++------- src/FrameSemantics_TEST.cc | 16 ++++++++-------- src/ScopedGraph.hh | 10 ++++++++-- 3 files changed, 29 insertions(+), 17 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 7408fb559..bc9c00028 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -1464,15 +1464,21 @@ Errors updateGraphPose( } auto vertexId = _graph.VertexIdByName(_frameName); auto incidentsTo = _graph.Graph().IncidentsTo(vertexId); + std::vector toRemove; + for (const auto &[id, edge] : incidentsTo) + { + if (edge.get().Data().aliasing) + { + toRemove.push_back(id); + } + } + for (const auto id : toRemove) + { + incidentsTo.erase(id); + } + if (incidentsTo.size() == 1) { - // There's no API to update the data of an edge, so we remove the edge and - // insert a new one with the new pose. - // auto &edge = incidentsTo.begin()->second; - // auto tailVertexId = edge.get().Tail(); - // auto headVertexId = edge.get().Head(); - // _graph.graph.RemoveEdge(edge.get().Id()); - // _graph.graph.AddEdge({tailVertexId, headVertexId}, _pose); _graph.UpdateEdge(incidentsTo.begin()->second, _pose); } else if (incidentsTo.empty()) diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index 54dbaeac2..e05325cf9 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -227,14 +227,14 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) EXPECT_EQ(8u, graph.Graph().Vertices().size()); EXPECT_EQ(7u, graph.Graph().Edges().size()); - EXPECT_EQ(1u, graph.Map().count("__model__")); - EXPECT_EQ(1u, graph.Map().count("P")); - EXPECT_EQ(1u, graph.Map().count("C")); - EXPECT_EQ(1u, graph.Map().count("J")); - EXPECT_EQ(1u, graph.Map().count("F1")); - EXPECT_EQ(1u, graph.Map().count("F2")); - EXPECT_EQ(1u, graph.Map().count("F3")); - EXPECT_EQ(1u, graph.Map().count("F4")); + EXPECT_EQ(1u, graph.Count("__model__")); + EXPECT_EQ(1u, graph.Count("P")); + EXPECT_EQ(1u, graph.Count("C")); + EXPECT_EQ(1u, graph.Count("J")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("F3")); + EXPECT_EQ(1u, graph.Count("F4")); // Test resolvePoseRelativeToRoot for each frame. ignition::math::Pose3d pose; diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index e3c71487b..5b56250b9 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -207,9 +207,15 @@ std::string ScopedGraph::VertexName(const VertexId &_id) const ///////////////////////////////////////////////// template void ScopedGraph::UpdateEdge( -std::reference_wrapper &, const EdgeType&) + std::reference_wrapper &_edge, const EdgeType &_data) { - // TODO (addisu) + // There's no API to update the data of an edge, so we remove the edge and + // insert a new one with the new pose. + auto tailVertexId = _edge.get().Tail(); + auto headVertexId = _edge.get().Head(); + auto &graph = this->graphWeak.lock()->graph; + graph.RemoveEdge(_edge.get().Id()); + graph.AddEdge({tailVertexId, headVertexId}, _data); } ///////////////////////////////////////////////// From 041e1c25597273e1b37d4b6b328787e26f739c16 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 25 Sep 2020 17:51:11 -0500 Subject: [PATCH 08/70] Pass ChildScope with new scope name Signed-off-by: Addisu Z. Taddese --- include/sdf/Types.hh | 3 ++ src/FrameSemantics.cc | 56 +++++++++++++++----------------- src/ScopedGraph.hh | 14 ++++++++ src/Types.cc | 7 ++++ src/World.cc | 19 +++++++---- src/ign_TEST.cc | 46 ++++++++++++++------------ test/integration/frame.cc | 13 ++++---- test/integration/model_dom.cc | 2 +- test/integration/nested_model.cc | 2 +- test/integration/root_dom.cc | 6 ++-- 10 files changed, 101 insertions(+), 67 deletions(-) diff --git a/include/sdf/Types.hh b/include/sdf/Types.hh index 493fcb9f9..bfc8cde65 100644 --- a/include/sdf/Types.hh +++ b/include/sdf/Types.hh @@ -68,6 +68,9 @@ namespace sdf std::vector split(const std::string &_str, const std::string &_splitter); + SDFORMAT_VISIBLE + bool endswith(const std::string &_a, const std::string &_b); + /// \brief Trim leading and trailing whitespace from a string. /// \param[in] _in The string to trim. /// \return A string containing the trimmed value. diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index bc9c00028..177600d0e 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -33,6 +33,15 @@ namespace sdf { inline namespace SDF_VERSION_NAMESPACE { +void printGraph(const ScopedGraph &_graph) +{ + std::cout << _graph.Graph() << std::endl; +} +void printGraph(const ScopedGraph &_graph) +{ + std::cout << _graph.Graph() << std::endl; +} + // The following two functions were originally submitted to ign-math, // but were not accepted as they were not generic enough. // For now, they will be kept here. @@ -181,6 +190,9 @@ Errors buildFrameAttachedToGraph( { Errors errors; + if (_model->Static()) + return errors; + // add implicit model frame vertex first const std::string scopeName = "__model__"; _out.SetScopeName(scopeName); @@ -207,6 +219,8 @@ Errors buildFrameAttachedToGraph( return errors; } + // TODO (addisu) Handle finding the canonical link in a nested model. + // TODO (addisu) Handle models with no links // identify canonical link const sdf::Link *canonicalLink = nullptr; if (_model->CanonicalLinkName().empty()) @@ -361,7 +375,7 @@ Errors buildFrameAttachedToGraph( _out.AddEdge({frameId, attachedToId}, edgeData); } - std::cout << "Model FrameAttachedToGraph\n" << _out.Graph() << std::endl; + // std::cout << "Model FrameAttachedToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -476,7 +490,7 @@ Errors buildFrameAttachedToGraph( _out.AddEdge({frameId, attachedToId}, edgeData); } - std::cout << "World FrameAttachedToGraph\n" << _out.Graph() << std::endl; + // std::cout << "World FrameAttachedToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -761,7 +775,7 @@ Errors buildPoseRelativeToGraph( _out.AddEdge({relativeToId, nestedModelId}, nestedModel->RawPose()); } - std::cout << "Model PoseRelativeToGraph\n" << _out.Graph() << std::endl; + // std::cout << "Model PoseRelativeToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -938,7 +952,7 @@ Errors buildPoseRelativeToGraph( _out.AddEdge({relativeToId, frameId}, frame->RawPose()); } - std::cout << "World PoseRelativeToGraph\n" << _out.Graph() << std::endl; + // std::cout << "World PoseRelativeToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -1026,22 +1040,6 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in "in MODEL attached_to graph."}); } break; - case sdf::FrameType::MODEL: - if (_in.ScopeVertexId() != vertexPair.second.get().Id()) - { - if (outDegree != 0) - { - errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error, " - "nested MODEL vertex with name [" + - vertexName + - "] should have no outgoing edges " - "in MODEL attached_to graph."}); - } - break; - } - // fall through to default case for __model__ - [[fallthrough]]; default: if (outDegree == 0) { @@ -1075,7 +1073,7 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "MODEL and WORLD vertices should have no outgoing edges " + "WORLD vertices should have no outgoing edges " "in WORLD attached_to graph."}); } break; @@ -1206,7 +1204,7 @@ Errors validatePoseRelativeToGraph( case sdf::FrameType::MODEL: // TODO: What we have to check here is that if this is the scope // vertex any incoming edge is from outside this scope. - if (_in.ScopeVertexId() == vertexPair.second.get().Id()) + if (sdf::endswith(vertexName, "__model__")) { if (inDegree != 0) { @@ -1283,12 +1281,12 @@ Errors validatePoseRelativeToGraph( } // check graph for cycles by resolving pose of each vertex relative to root - // for (auto const &name : _in.VertexNames()) - // { - // ignition::math::Pose3d pose; - // Errors e = resolvePoseRelativeToRoot(pose, _in, name); - // errors.insert(errors.end(), e.begin(), e.end()); - // } + for (auto const &name : _in.VertexNames()) + { + ignition::math::Pose3d pose; + Errors e = resolvePoseRelativeToRoot(pose, _in, name); + errors.insert(errors.end(), e.begin(), e.end()); + } return errors; } @@ -1340,7 +1338,7 @@ Errors resolveFrameAttachedToBody( { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "Graph has world scope but sink vertex named [" + - sinkVertex.Name() + "] does not have FrameType WORLD or MODEL " + sinkVertex.Name() + "] does not have FrameType WORLD or LINK " "when starting from vertex with name [" + _vertexName + "]."}); return errors; } diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index 5b56250b9..8819f58bf 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -62,6 +62,8 @@ class ScopedGraph public: ScopedGraph() = default; public: ScopedGraph(const std::shared_ptr _graph); + public: ScopedGraph ChildScope(const std::string &_name, const + std::string &_scopeName); public: explicit operator bool() const; public: const MathGraphType &Graph() const; @@ -106,6 +108,18 @@ ScopedGraph::ScopedGraph(const std::shared_ptr _graph): graphWeak(_graph) { } +///////////////////////////////////////////////// +template +ScopedGraph ScopedGraph::ChildScope( + const std::string &_name, const std::string &_scopeName) +{ + auto newScopedGraph = *this; + newScopedGraph.scopeVertexId = newScopedGraph.VertexIdByName(_name); + newScopedGraph.prefix = this->AddPrefix(_name); + newScopedGraph.scopeName = _scopeName; + return newScopedGraph; +} + ///////////////////////////////////////////////// template ScopedGraph::operator bool() const diff --git a/src/Types.cc b/src/Types.cc index 989574877..83b8d4037 100644 --- a/src/Types.cc +++ b/src/Types.cc @@ -70,6 +70,13 @@ std::string trim(const std::string &_in) return _in.substr(strBegin, strRange); } +////////////////////////////////////////////////// +bool endswith(const std::string& _a, const std::string& _b) +{ + return (_a.size() >= _b.size()) && + (_a.compare(_a.size() - _b.size(), _b.size(), _b) == 0); +} + ///////////////////////////////////////////////// std::string lowercase(const std::string &_in) { diff --git a/src/World.cc b/src/World.cc index 1ae05290e..9193a5964 100644 --- a/src/World.cc +++ b/src/World.cc @@ -281,14 +281,9 @@ Errors World::Load(sdf::ElementPtr _sdf) // name collisions std::unordered_set frameNames; - std::function beforeLoad = [this](Model &_model) - { - _model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - _model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - }; // Load all the models. - Errors modelLoadErrors = loadUniqueRepeated(_sdf, "model", - this->dataPtr->models, beforeLoad); + Errors modelLoadErrors = + loadUniqueRepeated(_sdf, "model", this->dataPtr->models); errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end()); // Models are loaded first, and loadUniqueRepeated ensures there are no @@ -388,6 +383,16 @@ Errors World::Load(sdf::ElementPtr _sdf) { frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); } + for (auto &model : this->dataPtr->models) + { + Errors setPoseRelativeToGraphErrors = + model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), + setPoseRelativeToGraphErrors.end()); + errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), + setPoseRelativeToGraphErrors.end()); + model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } for (auto &light : this->dataPtr->lights) { light.SetXmlParentName("world"); diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 6eacd7a24..0ce99bd62 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -223,8 +223,8 @@ TEST(check, SDF) // Check joint_invalid_self_child.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: FrameAttachedToGraph cycle detected, " - "already visited vertex [self]."), + EXPECT_NE(output.find("Error: FrameAttachedToGraph cycle detected, already " + "visited vertex [joint_invalid_self_child::self]."), std::string::npos) << output; } @@ -460,11 +460,13 @@ TEST(check, SDF) // Check model_frame_invalid_attached_to_cycle.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(std::string::npos, output.find( - "FrameAttachedToGraph cycle detected, already visited vertex [F1].")) + EXPECT_NE(std::string::npos, + output.find("FrameAttachedToGraph cycle detected, already visited " + "vertex [model_frame_invalid_attached_to_cycle::F1].")) << output; - EXPECT_NE(std::string::npos, output.find( - "FrameAttachedToGraph cycle detected, already visited vertex [F2].")) + EXPECT_NE(std::string::npos, + output.find("FrameAttachedToGraph cycle detected, already visited " + "vertex [model_frame_invalid_attached_to_cycle::F2].")) << output; } @@ -540,17 +542,17 @@ TEST(check, SDF) // Check an invalid SDF file with a joint that specifies a child link // within a sibling nested model using the unsupported :: syntax. - { - std::string path = pathBase +"/model_invalid_nested_joint_child.sdf"; - - // Check model_invalid_nested_joint_child.sdf - std::string output = - custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(output.find("Error: Child frame with name[M::C] specified by " - "joint with name[J] not found in model with " - "name[model_invalid_nested_joint_child]."), - std::string::npos) << output; - } + // { + // std::string path = pathBase +"/model_invalid_nested_joint_child.sdf"; + + // // Check model_invalid_nested_joint_child.sdf + // std::string output = + // custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + // EXPECT_NE(output.find("Error: Child frame with name[M::C] specified by " + // "joint with name[J] not found in model with " + // "name[model_invalid_nested_joint_child]."), + // std::string::npos) << output; + // } // Check an SDF file with joints using the relative_to attribute. // This is a valid file. @@ -628,11 +630,13 @@ TEST(check, SDF) // Check model_invalid_frame_relative_to_cycle.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - EXPECT_NE(std::string::npos, output.find( - "PoseRelativeToGraph cycle detected, already visited vertex [F1].")) + EXPECT_NE(std::string::npos, + output.find("PoseRelativeToGraph cycle detected, already visited " + "vertex [model_invalid_frame_relative_to_cycle::F1].")) << output; - EXPECT_NE(std::string::npos, output.find( - "PoseRelativeToGraph cycle detected, already visited vertex [F2].")) + EXPECT_NE(std::string::npos, + output.find("PoseRelativeToGraph cycle detected, already visited " + "vertex [model_invalid_frame_relative_to_cycle::F2].")) << output; } diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 02542f232..989710673 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -587,7 +587,8 @@ TEST(DOMFrame, LoadModelFramesAttachedToNestedModel) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; // Get the first model const sdf::Model *model = root.ModelByIndex(0); @@ -622,9 +623,9 @@ TEST(DOMFrame, LoadModelFramesAttachedToNestedModel) std::string body; EXPECT_TRUE(model->FrameByName("F1")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("nested_model", body); + EXPECT_EQ("nested_model::nested_link", body); EXPECT_TRUE(model->FrameByName("F2")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("nested_model", body); + EXPECT_EQ("nested_model::nested_link", body); } ///////////////////////////////////////////////// @@ -691,7 +692,7 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo) EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty()); EXPECT_EQ("world", body); EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("M1", body); + EXPECT_EQ("M1::L", body); } ///////////////////////////////////////////////// @@ -1109,7 +1110,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidRelativeTo) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(11u, errors.size()); + EXPECT_EQ(15u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -1175,7 +1176,7 @@ TEST(DOMFrame, WorldIncludeModel) ASSERT_NE(nullptr, model); ignition::math::Pose3d modelPose; sdf::Errors resolveErrors = model->SemanticPose().Resolve(modelPose); - EXPECT_TRUE(resolveErrors.empty()); + EXPECT_TRUE(resolveErrors.empty()) << resolveErrors; EXPECT_EQ(expectedPoses[i], modelPose); } } diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 299100509..cbb3e9c24 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -69,7 +69,7 @@ TEST(DOMModel, NoLinks) sdf::Root root; auto errors = root.Load(testFile); EXPECT_FALSE(errors.empty()); - ASSERT_EQ(4u, errors.size()); + ASSERT_EQ(3u, errors.size()); EXPECT_EQ(sdf::ErrorCode::MODEL_WITHOUT_LINK, errors[0].Code()); EXPECT_TRUE(errors[0].Message().find("model must have at least one link") != std::string::npos); diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 72d961f21..8da384fc1 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -859,7 +859,7 @@ class PlacementFrame: public ::testing::Test { std::cout << this->root.Element()->ToString("") << std::endl; } - EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; this->world = this->root.WorldByIndex(0); ASSERT_NE(nullptr, world); diff --git a/test/integration/root_dom.cc b/test/integration/root_dom.cc index e23775591..f6f06af24 100644 --- a/test/integration/root_dom.cc +++ b/test/integration/root_dom.cc @@ -61,7 +61,8 @@ TEST(DOMRoot, Load) sdf::Root root; EXPECT_EQ(0u, root.WorldCount()); - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_EQ(SDF_PROTOCOL_VERSION, root.Version()); EXPECT_EQ(1u, root.WorldCount()); EXPECT_TRUE(root.WorldByIndex(0) != nullptr); @@ -83,7 +84,8 @@ TEST(DOMRoot, LoadMultipleModels) "root_multiple_models.sdf"); sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_EQ(3u, root.ModelCount()); EXPECT_EQ("robot1", root.ModelByIndex(0)->Name()); From b0bf73b53c7d88ec68be7e1fecfa1d01cd46e959 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 30 Sep 2020 22:59:10 -0500 Subject: [PATCH 09/70] Add command line tool to generate graph Signed-off-by: Addisu Z. Taddese --- src/cmd/cmdsdformat.rb.in | 8 +++++ src/ign.cc | 63 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 71 insertions(+) diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 4d354c3b9..9743bfd5b 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -41,6 +41,7 @@ COMMANDS = { 'sdf' => " -k [ --check ] arg Check if an SDFormat file is valid.\n" + " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + " -p [ --print ] arg Print converted arg.\n" + + " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph.\n" + COMMON_OPTIONS } @@ -77,6 +78,10 @@ class Cmd 'Print converted arg') do |arg| options['print'] = arg end + opts.on('-g arg', '--graph type', String, + 'Print PoseRelativeTo or FrameAttachedTo graph') do |graph_type| + options['graph'] = {:type => graph_type} + end end begin opt_parser.parse!(args) @@ -156,6 +161,9 @@ class Cmd elsif options.key?('print') Importer.extern 'int cmdPrint(const char *)' exit(Importer.cmdPrint(File.expand_path(options['print']))) + elsif options.key?('graph') + Importer.extern 'int cmdGraph(const char *, const char *)' + exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1]))) else puts 'Command error: I do not have an implementation '\ 'for this command.' diff --git a/src/ign.cc b/src/ign.cc index 90262276b..2eb2e9664 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -24,6 +25,8 @@ #include "sdf/parser.hh" #include "sdf/system_util.hh" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "ign.hh" ////////////////////////////////////////////////// @@ -157,3 +160,63 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path) return 0; } + +////////////////////////////////////////////////// +// cppcheck-suppress unusedFunction +extern "C" SDFORMAT_VISIBLE int cmdGraph( + const char *_graphType, const char *_path) +{ + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + + if (std::strcmp(_graphType, "pose") == 0) + { + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + if (root.WorldCount() > 0) + { + errors = sdf::buildPoseRelativeToGraph(graph, root.WorldByIndex(0)); + } + else if (root.ModelCount() > 0) + { + errors = sdf::buildPoseRelativeToGraph(graph, root.ModelByIndex(0)); + } + + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + std::cout << graph.Graph() << std::endl; + } + else if (std::strcmp(_graphType, "frame") == 0) + { + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + if (root.WorldCount() > 0) + { + errors = sdf::buildFrameAttachedToGraph(graph, root.WorldByIndex(0)); + } + else if (root.ModelCount() > 0) + { + errors = sdf::buildFrameAttachedToGraph(graph, root.ModelByIndex(0)); + } + + if (!errors.empty()) + { + std::cerr << errors << std::endl; + } + std::cout << graph.Graph() << std::endl; + } + + return 0; +} From 3fe002bc6c26dd6fa65d342078bb7c8c2c76e06c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 30 Sep 2020 23:00:38 -0500 Subject: [PATCH 10/70] Avoid using the RawPose of the DOM object when the object is a frame (implicit or explicit) Instead use the edges in the pose graph. This allows us to update only the pose graph when handling placement frames. Signed-off-by: Addisu Z. Taddese --- include/sdf/SemanticPose.hh | 7 +++++++ src/Frame.cc | 1 + src/Joint.cc | 1 + src/Link.cc | 1 + src/Model.cc | 1 + src/SemanticPose.cc | 28 ++++++++++++++++++++++++++-- 6 files changed, 37 insertions(+), 2 deletions(-) diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 921c799e4..a93324e49 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -86,6 +86,13 @@ namespace sdf const std::string &_defaultResolveTo, const sdf::ScopedGraph &_graph); + private: SemanticPose( + const std::string &_name, + const ignition::math::Pose3d &_pose, + const std::string &_relativeTo, + const std::string &_defaultResolveTo, + const sdf::ScopedGraph &_graph); + /// \brief Destructor public: ~SemanticPose(); diff --git a/src/Frame.cc b/src/Frame.cc index 2a02e6bdb..84f7558d1 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -245,6 +245,7 @@ sdf::SemanticPose Frame::SemanticPose() const relativeTo = this->dataPtr->attachedTo; } return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, relativeTo, this->dataPtr->graphSourceName, diff --git a/src/Joint.cc b/src/Joint.cc index 283a9bd4a..aad53dee7 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -439,6 +439,7 @@ Errors Joint::ResolveParentLink(std::string &_link) const sdf::SemanticPose Joint::SemanticPose() const { return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, this->ChildLinkName(), diff --git a/src/Link.cc b/src/Link.cc index 15003df37..883e9bcb7 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -408,6 +408,7 @@ void Link::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) sdf::SemanticPose Link::SemanticPose() const { return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, "__model__", diff --git a/src/Model.cc b/src/Model.cc index ca1fbd108..2b09dd050 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -884,6 +884,7 @@ void Model::SetFrameAttachedToGraph( sdf::SemanticPose Model::SemanticPose() const { return sdf::SemanticPose( + this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, this->dataPtr->poseGraph.ScopeName(), diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index a6f23d59b..37a617b5f 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -31,6 +31,8 @@ inline namespace SDF_VERSION_NAMESPACE { /// \brief Private data for the SemanticPose class. class SemanticPosePrivate { + public: std::string name = ""; + /// \brief Raw pose of the SemanticPose object. public: ignition::math::Pose3d rawPose = ignition::math::Pose3d::Zero; @@ -47,18 +49,33 @@ class SemanticPosePrivate ///////////////////////////////////////////////// SemanticPose::SemanticPose( + const std::string &_name, const ignition::math::Pose3d &_pose, const std::string &_relativeTo, const std::string &_defaultResolveTo, const sdf::ScopedGraph &_graph) : dataPtr(std::make_unique()) { + this->dataPtr->name = _name; this->dataPtr->rawPose = _pose; this->dataPtr->relativeTo = _relativeTo; this->dataPtr->defaultResolveTo = _defaultResolveTo; this->dataPtr->poseRelativeToGraph = _graph; } +///////////////////////////////////////////////// +SemanticPose::SemanticPose( + const ignition::math::Pose3d &_pose, + const std::string &_relativeTo, + const std::string &_defaultResolveTo, + const sdf::ScopedGraph &_graph) + : dataPtr(std::make_unique()) +{ + this->dataPtr->rawPose = _pose; + this->dataPtr->relativeTo = _relativeTo; + this->dataPtr->defaultResolveTo = _defaultResolveTo; + this->dataPtr->poseRelativeToGraph = _graph; +} ///////////////////////////////////////////////// SemanticPose::~SemanticPose() = default; @@ -126,8 +143,15 @@ Errors SemanticPose::Resolve( } ignition::math::Pose3d pose; - errors = resolvePose(pose, graph, relativeTo, resolveTo); - pose *= this->RawPose(); + if (this->dataPtr->name.empty()) + { + errors = resolvePose(pose, graph, relativeTo, resolveTo); + pose *= this->RawPose(); + } + else + { + errors = resolvePose(pose, graph, this->dataPtr->name, resolveTo); + } if (errors.empty()) { From dc5f941a92129b3294cce84c5ba828611214e4b7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 30 Sep 2020 23:03:15 -0500 Subject: [PATCH 11/70] Handle placement frames with new PoseRelativeTo construction scheme The PoseRelativeTo used to be constructed in each nested model's Load funcition. Now, it's only constructed at the outer most model. Because of this decoupling and Since the model is `const` when the graph is constructed, placement frames are handled differently. Instead of updating the raw pose of the model, the edge connecting the model frame to it's relative_to frame is modified to take into account the placement frame. Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 301 ++++++++++++------ src/FrameSemantics.hh | 2 + src/Model.cc | 201 +++++------- src/parser.cc | 23 +- test/integration/model_dom.cc | 4 +- test/integration/nested_model.cc | 211 ++++++------ .../nested_model_with_frames_expected.sdf | 122 +++---- ...evel_nested_model_with_frames_expected.sdf | 128 ++++---- 8 files changed, 524 insertions(+), 468 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index db608a59c..e1f8694a9 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -203,15 +203,14 @@ Errors buildFrameAttachedToGraph( { Errors errors; - if (_model->Static()) - return errors; - // add implicit model frame vertex first const std::string scopeName = "__model__"; _out.SetScopeName(scopeName); + + auto frameType = + _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; auto modelFrameId = - _out.AddScopeVertex(_model->Name(), scopeName, sdf::FrameType::MODEL) - .Id(); + _out.AddScopeVertex(_model->Name(), scopeName, frameType).Id(); if (!_model) { @@ -225,7 +224,8 @@ Errors buildFrameAttachedToGraph( "Invalid model element in sdf::Model."}); return errors; } - else if (_model->LinkCount() < 1 && _model->ModelCount() < 1) + else if (_model->LinkCount() < 1 && _model->ModelCount() < 1 && + !_model->Static()) { errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, "A model must have at least one link."}); @@ -236,7 +236,7 @@ Errors buildFrameAttachedToGraph( auto canonicalLinkAndName = modelCanonicalLinkAndRelativeName(_model); const sdf::Link *canonicalLink = canonicalLinkAndName.first; const std::string canonicalLinkName = canonicalLinkAndName.second; - if (nullptr == canonicalLink) + if (nullptr == canonicalLink && !_model->Static()) { if (canonicalLinkName.empty()) { @@ -252,20 +252,6 @@ Errors buildFrameAttachedToGraph( // return early 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) { @@ -278,14 +264,13 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto linkId = - _out.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); + _out.AddVertex(link->Name(), sdf::FrameType::LINK); - // add edge from implicit model frame vertex to canonical link - if (link == canonicalLink) - { - _out.AddEdge({modelFrameId, linkId}, true); - } + // // add edge from implicit model frame vertex to canonical link + // if (!_model->Static() && link == canonicalLink) + // { + // _out.AddEdge({modelFrameId, linkId}, true); + // } } // add joint vertices @@ -330,9 +315,12 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - _out.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); + auto modelFrameType = + _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; + _out.AddVertex(nestedModel->Name(), modelFrameType).Id(); ScopedGraphchildGraph = _out; - buildFrameAttachedToGraph(childGraph, nestedModel); + auto nestedErrors = buildFrameAttachedToGraph(childGraph, nestedModel); + errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); } // add edges from joint to child frames @@ -401,6 +389,42 @@ Errors buildFrameAttachedToGraph( _out.AddEdge({frameId, attachedToId}, edgeData); } + + // Identify if the canonical link is in a nested model. + if (!_model->Static() && canonicalLink != nullptr) + { + // 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.VertexIdByName(canonicalLinkName); + _out.AddEdge({modelFrameId, linkId}, true); + } + + if (_model->Static()) + { + // If the model is static, add an edge from the model frame to the global + // root vertex + // TODO (addisu) Ensure that the global root vertex has an ID of 0. + // _out.AddEdge({modelFrameId, 0}, true); + } + // add edge from implicit model frame vertex to canonical link + // if (!_model->Static() && canonicalLink == nullptr) + // { + // for (uint64_t m = 0; m < _model->ModelCount(); ++m) + // { + // sdf::Errors sinkErrors; + // auto vertexId = _out.VertexIdByName(_model->ModelByIndex(m)->Name()); + // auto sinkVertexEdges = FindSinkVertex(_out, vertexId, sinkErrors); + // if (sinkErrors.empty()) + // { + // auto sinkVertex = sinkVertexEdges.first; + // _out.AddEdge({modelFrameId, vertexId}, true); + // break; + // } + // } + // } + // std::cout << "Model FrameAttachedToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -441,9 +465,12 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - _out.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); + auto modelFrameType = + model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; + _out.AddVertex(model->Name(), modelFrameType).Id(); ScopedGraphchildGraph = _out; - buildFrameAttachedToGraph(childGraph, model); + auto modelErrors = buildFrameAttachedToGraph(childGraph, model); + errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); } // add frame vertices @@ -621,14 +648,8 @@ Errors buildPoseRelativeToGraph( _out.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); ScopedGraph childGraph = _out; - buildPoseRelativeToGraph(childGraph , nestedModel); - - if (nestedModel->PoseRelativeTo().empty()) - { - // relative_to is empty, so add edge from implicit model frame - // to nestedModel - _out.AddEdge({modelFrameId, nestedModelId}, nestedModel->RawPose()); - } + auto nestedErrors = buildPoseRelativeToGraph(childGraph , nestedModel); + errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); // Add an aliasing edge from the nestedModel vertex to the // corresponding vertex in the PoseRelativeTo graph of the nested model, // i.e, to the ::__model__ vertex @@ -640,7 +661,8 @@ Errors buildPoseRelativeToGraph( } else { - _out.AddEdge({nestedModelId, scopedNestedModelId}, {{}, true}); + auto &edge = _out.AddEdge({nestedModelId, scopedNestedModelId}, {{}, false}); + edge.SetWeight(0); } } @@ -770,35 +792,67 @@ Errors buildPoseRelativeToGraph( { auto nestedModel = _model->ModelByIndex(m); - // check if we've already added a default edge - const std::string relativeTo = nestedModel->PoseRelativeTo(); - if (relativeTo.empty()) - { - continue; - } - auto nestedModelId = _out.VertexIdByName(nestedModel->Name()); + // relative_to is empty, so add edge from implicit model frame + // to nestedModel + auto relativeToId = modelFrameId; - // look for vertex in graph that matches relative_to value - if (_out.Count(relativeTo) != 1) + const std::string &relativeTo = nestedModel->PoseRelativeTo(); + if (!relativeTo.empty()) { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by nested model with name[" + nestedModel->Name() + - "] does not match a nested model, link, joint, or frame name " - "in model with name[" + _model->Name() + "]."}); - continue; + // look for vertex in graph that matches relative_to value + if (_out.Count(relativeTo) != 1) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "relative_to name[" + relativeTo + + "] specified by nested model with name[" + nestedModel->Name() + + "] does not match a nested model, link, joint, or frame name " + "in model with name[" + _model->Name() + "]."}); + continue; + } + + relativeToId = _out.VertexIdByName(relativeTo); + if (nestedModel->Name() == relativeTo) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, + "relative_to name[" + relativeTo + + "] is identical to nested model name[" + nestedModel->Name() + + "], causing a graph cycle " + "in model with name[" + _model->Name() + "]."}); + } } - auto relativeToId = _out.VertexIdByName(relativeTo); - if (nestedModel->Name() == relativeTo) + + ignition::math::Pose3d X_RM = nestedModel->RawPose(); + // If the model has a placement frame, calculate the necessary pose to use + // TODO (addisu) docs + if (!nestedModel->PlacementFrameName().empty()) { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to nested model name[" + nestedModel->Name() + - "], causing a graph cycle " - "in model with name[" + _model->Name() + "]."}); + ignition::math::Pose3d X_MPf; + + sdf::Errors resolveErrors = sdf::resolvePoseRelativeToRoot(X_MPf, + _out.ChildScope(nestedModel->Name(), "__model__"), + nestedModel->PlacementFrameName()); + if (resolveErrors.empty()) + { + // Before this update, i.e, as specified in the SDFormat, the model pose + // (X_RPf) is the pose of the placement frame (Pf) relative to a frame + // (R) in the parent scope of the model. However, when this model (M) is + // inserted into a pose graph of the parent scope, only the pose (X_RM) + // of the __model__ frame can be used. Thus, the model pose has to be + // updated to X_RM. + // + // Note that X_RPf is the raw pose specified in //model/pose before this + // update. + const auto &X_RPf = nestedModel->RawPose(); + X_RM = X_RPf * X_MPf.Inverse(); + } + else + { + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + } } - _out.AddEdge({relativeToId, nestedModelId}, nestedModel->RawPose()); + + _out.AddEdge({relativeToId, nestedModelId}, X_RM); } // std::cout << "Model PoseRelativeToGraph\n" << _out.Graph() << std::endl; @@ -846,14 +900,10 @@ Errors buildPoseRelativeToGraph( _out.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); ScopedGraph childGraph = _out; - buildPoseRelativeToGraph(childGraph , model); + auto modelErrors = buildPoseRelativeToGraph(childGraph , model); + errors.insert(errors.end(), modelErrors .begin(), modelErrors .end()); - if (model->PoseRelativeTo().empty()) - { - // relative_to is empty, so add edge from implicit world frame to model - _out.AddEdge({worldFrameId, modelId}, model->RawPose()); - } - // Add a non-constraint edge from the model vertex to the + // Add an aliasing edge from the model vertex to the // corresponding vertex in the PoseRelativeTo graph of the child model, // i.e, to the ::__model__ vertex auto scopedModelId = @@ -864,7 +914,8 @@ Errors buildPoseRelativeToGraph( } else { - _out.AddEdge({modelId, scopedModelId}, {{}, false}); + auto &edge = _out.AddEdge({modelId, scopedModelId}, {{}, false}); + edge.SetWeight(0); } } @@ -898,35 +949,67 @@ Errors buildPoseRelativeToGraph( { auto model = _world->ModelByIndex(m); - // check if we've already added a default edge - const std::string relativeTo = model->PoseRelativeTo(); - if (relativeTo.empty()) - { - continue; - } - auto modelId = _out.VertexIdByName(model->Name()); + // relative_to is empty, so add edge from implicit model frame + // to world + auto relativeToId = worldFrameId; - // look for vertex in graph that matches relative_to value - if (_out.Count(relativeTo) != 1) + // check if we've already added a default edge + const std::string &relativeTo = model->PoseRelativeTo(); + if (!relativeTo.empty()) { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "relative_to name[" + relativeTo + - "] specified by model with name[" + model->Name() + - "] does not match a model or frame name " - "in world with name[" + _world->Name() + "]."}); - continue; + // look for vertex in graph that matches relative_to value + if (_out.Count(relativeTo) != 1) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "relative_to name[" + relativeTo + + "] specified by model with name[" + model->Name() + + "] does not match a model or frame name " + "in world with name[" + _world->Name() + "]."}); + continue; + } + + relativeToId = _out.VertexIdByName(relativeTo); + if (model->Name() == relativeTo) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, + "relative_to name[" + relativeTo + + "] is identical to model name[" + model->Name() + + "], causing a graph cycle " + "in world with name[" + _world->Name() + "]."}); + } } - auto relativeToId = _out.VertexIdByName(relativeTo); - if (model->Name() == relativeTo) + + ignition::math::Pose3d X_RM = model->RawPose(); + // If the model has a placement frame, calculate the necessary pose to use + // TODO (addisu) docs + if (!model->PlacementFrameName().empty()) { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, - "relative_to name[" + relativeTo + - "] is identical to model name[" + model->Name() + - "], causing a graph cycle " - "in world with name[" + _world->Name() + "]."}); + ignition::math::Pose3d X_MPf; + + sdf::Errors resolveErrors = sdf::resolvePoseRelativeToRoot(X_MPf, + _out.ChildScope(model->Name(), "__model__"), + model->PlacementFrameName()); + if (resolveErrors.empty()) + { + // Before this update, i.e, as specified in the SDFormat, the model pose + // (X_RPf) is the pose of the placement frame (Pf) relative to a frame + // (R) in the parent scope of the model. However, when this model (M) is + // inserted into a pose graph of the parent scope, only the pose (X_RM) + // of the __model__ frame can be used. Thus, the model pose has to be + // updated to X_RM. + // + // Note that X_RPf is the raw pose specified in //model/pose before this + // update. + const auto &X_RPf = model->RawPose(); + X_RM = X_RPf * X_MPf.Inverse(); + } + else + { + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + } } - _out.AddEdge({relativeToId, modelId}, model->RawPose()); + _out.AddEdge({relativeToId, modelId}, X_RM); } for (uint64_t f = 0; f < _world->FrameCount(); ++f) @@ -1008,11 +1091,14 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in } sdf::FrameType scopeFrameType = scopeVertex.Data(); - if (_in.ScopeName() == "__model__" && scopeFrameType != sdf::FrameType::MODEL) + if (_in.ScopeName() == "__model__" && + (scopeFrameType != sdf::FrameType::MODEL && + scopeFrameType != sdf::FrameType::STATIC_MODEL)) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "scope vertex with name __model__ should have FrameType MODEL."}); + "scope vertex with name __model__ should have FrameType MODEL or " + "STATIC_MODEL."}); return errors; } else if (_in.ScopeName() == "world" && @@ -1108,12 +1194,14 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "LINK vertex with name [" + + "LINK/STATIC_MODEL vertex with name [" + vertexName + "] should have no outgoing edges " "in WORLD attached_to graph."}); } break; + case sdf::FrameType::STATIC_MODEL: + break; default: if (outDegree != 1) { @@ -1230,7 +1318,8 @@ Errors validatePoseRelativeToGraph( case sdf::FrameType::MODEL: // TODO: What we have to check here is that if this is the scope // vertex any incoming edge is from outside this scope. - if (sdf::endswith(vertexName, "__model__")) + // if (sdf::endswith(vertexName, "__model__")) + if (_in.ScopeVertexId() == vertexPair.first) { if (inDegree != 0) { @@ -1360,12 +1449,14 @@ Errors resolveFrameAttachedToBody( if (_in.ScopeName() == "world" && !(sinkVertex.Data() == FrameType::WORLD || - sinkVertex.Data() == FrameType::LINK)) + sinkVertex.Data() == FrameType::STATIC_MODEL || + sinkVertex.Data() == FrameType::LINK)) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "Graph has world scope but sink vertex named [" + - sinkVertex.Name() + "] does not have FrameType WORLD or LINK " - "when starting from vertex with name [" + _vertexName + "]."}); + "Graph has world scope but sink vertex named [" + sinkVertex.Name() + + "] does not have FrameType WORLD, LINK or STATIC_MODEL" + "when starting from vertex with name [" + + _vertexName + "]."}); return errors; } @@ -1381,11 +1472,11 @@ Errors resolveFrameAttachedToBody( return errors; } else if (sinkVertex.Data() != FrameType::LINK && - sinkVertex.Data() != FrameType::MODEL) + sinkVertex.Data() != FrameType::STATIC_MODEL) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "Graph has __model__ scope but sink vertex named [" + - sinkVertex.Name() + "] does not have FrameType LINK OR MODEL " + sinkVertex.Name() + "] does not have FrameType LINK OR STATIC_MODEL " "when starting from vertex with name [" + _vertexName + "]."}); return errors; } diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index f88ac89e9..7015278d9 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -67,6 +67,8 @@ namespace sdf /// \brief An explicit frame. FRAME = 4, + + STATIC_MODEL = 5, }; /// \brief Data structure for frame attached_to graphs for Model or World. diff --git a/src/Model.cc b/src/Model.cc index 2b09dd050..27ad0ed14 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include #include #include @@ -228,32 +229,35 @@ Errors Model::Load(ElementPtr _sdf) << this->Name() << "].\n"; } - if (!this->dataPtr->poseGraph) + bool modelFile = false; + + // if (!this->dataPtr->poseGraph) + auto parentElem = this->dataPtr->sdf->GetParent(); + if (parentElem && parentElem->GetName() == "sdf") { + // std::cout << "Creating owned graphs for " << this->Name() << std::endl; this->dataPtr->ownedPoseGraph = std::make_shared(); this->SetPoseRelativeToGraph(this->dataPtr->ownedPoseGraph); - } - - if (!this->dataPtr->frameAttachedToGraph) - { this->dataPtr->ownedFrameAttachedToGraph = - std::make_shared(); + std::make_shared(); this->SetFrameAttachedToGraph(this->dataPtr->ownedFrameAttachedToGraph); + modelFile = true; } // Set of implicit and explicit frame names in this model for tracking // name collisions std::unordered_set frameNames; - std::function beforeLoad = [this](Model &_model) - { - // TODO (addisu) - _model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - _model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - }; + + // std::function beforeLoad = [this](Model &_model) + // { + // // TODO (addisu) + // _model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + // _model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + // }; // Load nested models. Errors nestedModelLoadErrors = loadUniqueRepeated(_sdf, "model", - this->dataPtr->models, beforeLoad); + this->dataPtr->models); errors.insert(errors.end(), nestedModelLoadErrors.begin(), nestedModelLoadErrors.end()); @@ -394,7 +398,7 @@ Errors Model::Load(ElementPtr _sdf) // static models. if (!this->Static()) { - if (this->dataPtr->ownedPoseGraph) + if (modelFile) { Errors frameAttachedToGraphErrors = buildFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph, this); @@ -404,19 +408,23 @@ Errors Model::Load(ElementPtr _sdf) validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), validateFrameAttachedGraphErrors.end()); - } - for (auto &joint : this->dataPtr->joints) - { - joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + for (auto &joint : this->dataPtr->joints) + { + joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } + for (auto &model : this->dataPtr->models) + { + model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } } } // Build the PoseRelativeToGraph - if (this->dataPtr->ownedPoseGraph) + if (modelFile) { Errors poseGraphErrors = buildPoseRelativeToGraph(this->dataPtr->poseGraph, this); @@ -425,109 +433,28 @@ Errors Model::Load(ElementPtr _sdf) validatePoseRelativeToGraph(this->dataPtr->poseGraph); errors.insert(errors.end(), validatePoseGraphErrors.begin(), validatePoseGraphErrors.end()); - } - for (auto &link : this->dataPtr->links) - { - link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &model : this->dataPtr->models) - { - Errors setPoseRelativeToGraphErrors = - model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), - setPoseRelativeToGraphErrors.end()); - } - for (auto &joint : this->dataPtr->joints) - { - joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - - // Update the model pose to account for the placement frame. - if (!this->dataPtr->placementFrameName.empty()) - { - ignition::math::Pose3d X_MPf; - - sdf::Errors resolveErrors = - sdf::resolvePose(X_MPf, this->dataPtr->poseGraph, - this->dataPtr->placementFrameName, "__model__"); - if (resolveErrors.empty()) + for (auto &link : this->dataPtr->links) { - // Before this update, i.e, as specified in the SDFormat, the model pose - // (X_RPf) is the pose of the placement frame (Pf) relative to a frame (R) - // in the parent scope of the model. However, when this model (M) is - // inserted into a pose graph of the parent scope, only the pose (X_RM) - // of the __model__ frame can be used. Thus, the model pose has to be - // updated to X_RM. - // - // Note that X_RPf is the raw pose specified in //model/pose before this - // update. - const auto &X_RPf = this->dataPtr->pose; - auto &X_RM = this->dataPtr->pose; - X_RM = X_RPf * X_MPf.Inverse(); + link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); } - else + for (auto &model : this->dataPtr->models) { - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + Errors setPoseRelativeToGraphErrors = + model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), + setPoseRelativeToGraphErrors.end()); } - } - - // The placement_frame attributes of included child models are currently lost - // during parsing because the parser expands the included models into the - // parent model. As a temporary workardound to preserve this information, the - // parser injects ::__placement_frame__ for every included child - // model. This serves as an identifier that the model frame of the child model - // should be treated as if it has its placement_frame attribute set. - // This will not be necessary when included nested models work as directly - // nested models. See - // https://github.com/osrf/sdformat/issues/319#issuecomment-665214004 - // TODO (addisu) Remove placementFrameIdentifier once PR addressing - // https://github.com/osrf/sdformat/issues/284 lands - for (auto &frame : this->dataPtr->frames) - { - auto placementSubstrInd = frame.Name().rfind("::__placement_frame__"); - if (placementSubstrInd != std::string::npos) + for (auto &joint : this->dataPtr->joints) { - const std::string childModelName = - frame.Name().substr(0, placementSubstrInd); - // Find the model frame associated with this placement frame - const Frame *childModelFrame = - this->FrameByName(childModelName + "::__model__"); - if (nullptr != childModelFrame) - { - // The RawPose of the child model frame is the desired pose of the - // placement frame relative to a frame (R) in the scope of the parent - // model. We don't need to resolve the pose because the relative_to - // frame remains unchanged after the pose update here. i.e, the updated - // pose of the child model frame will still be relative to R. - const auto &X_RPf = childModelFrame->RawPose(); - - ignition::math::Pose3d X_MPf; - sdf::Errors resolveErrors = - frame.SemanticPose().Resolve(X_MPf, childModelFrame->Name()); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - - // We need to update childModelFrame's pose relative to the parent - // frame as well as the corresponding edge in the pose graph because - // just updating childModelFrame doesn't update the pose graph. - auto X_RM = X_RPf * X_MPf.Inverse(); - frame.SetRawPose(X_RM); - sdf::updateGraphPose( - this->dataPtr->poseGraph, childModelFrame->Name(), X_RM); - } - else - { - errors.push_back({ErrorCode::MODEL_PLACEMENT_FRAME_INVALID, - "Found a __placement_frame__ for child model with name [" + - childModelName + "], but the model does not exist in the" + - " parent model with name [" + this->Name() + "]"}); - } + joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetPoseRelativeToGraph(this->dataPtr->poseGraph); } } + return errors; } @@ -858,12 +785,34 @@ Errors Model::SetPoseRelativeToGraph( // If the scoped graph doesn't point to the graph owned by this model, we // clear the owned graph to maintain the invariant that if // ownedPoseGraph is valid poseGraph points to it. + // TODO (addisu) This may not be needed if (!_graph.PointsTo(this->dataPtr->ownedPoseGraph)) { this->dataPtr->ownedPoseGraph.reset(); } this->dataPtr->poseGraph = _graph; + auto childPoseGraph = + this->dataPtr->poseGraph.ChildScope(this->Name(), "__model__"); + for (auto &link : this->dataPtr->links) + { + link.SetPoseRelativeToGraph(childPoseGraph ); + } + for (auto &model : this->dataPtr->models) + { + Errors setPoseRelativeToGraphErrors = + model.SetPoseRelativeToGraph(childPoseGraph ); + errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), + setPoseRelativeToGraphErrors.end()); + } + for (auto &joint : this->dataPtr->joints) + { + joint.SetPoseRelativeToGraph(childPoseGraph ); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetPoseRelativeToGraph(childPoseGraph ); + } return errors; } ///////////////////////////////////////////////// @@ -877,7 +826,23 @@ void Model::SetFrameAttachedToGraph( { this->dataPtr->ownedFrameAttachedToGraph.reset(); } + this->dataPtr->frameAttachedToGraph = _graph; + + auto childFrameAttachedToGraph = + this->dataPtr->frameAttachedToGraph.ChildScope(this->Name(), "__model__"); + for (auto &joint : this->dataPtr->joints) + { + joint.SetFrameAttachedToGraph(childFrameAttachedToGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetFrameAttachedToGraph(childFrameAttachedToGraph); + } + for (auto &model : this->dataPtr->models) + { + model.SetFrameAttachedToGraph(childFrameAttachedToGraph); + } } ///////////////////////////////////////////////// diff --git a/src/parser.cc b/src/parser.cc index f18bb05f9..f902cefea 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1067,21 +1067,14 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) } } - if (_sdf->GetName() == "model") - { - addNestedModel(_sdf, includeSDF->Root(), _errors); - } - else - { - includeSDF->Root()->GetFirstElement()->SetParent(_sdf); - _sdf->InsertElement(includeSDF->Root()->GetFirstElement()); - // TODO: This was used to store the included filename so that when - // a world is saved, the included model's SDF is not stored in the - // world file. This highlights the need to make model inclusion - // a core feature of SDF, and not a hack that that parser handles - // includeSDF->Root()->GetFirstElement()->SetInclude( - // elemXml->Attribute("filename")); - } + includeSDF->Root()->GetFirstElement()->SetParent(_sdf); + _sdf->InsertElement(includeSDF->Root()->GetFirstElement()); + // TODO: This was used to store the included filename so that when + // a world is saved, the included model's SDF is not stored in the + // world file. This highlights the need to make model inclusion + // a core feature of SDF, and not a hack that that parser handles + // includeSDF->Root()->GetFirstElement()->SetInclude( + // elemXml->Attribute("filename")); continue; } diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index f5086a803..99f3b580f 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -70,7 +70,7 @@ TEST(DOMModel, NoLinks) sdf::Root root; auto errors = root.Load(testFile); EXPECT_FALSE(errors.empty()); - ASSERT_EQ(3u, errors.size()); + ASSERT_EQ(4u, errors.size()); EXPECT_EQ(sdf::ErrorCode::MODEL_WITHOUT_LINK, errors[0].Code()); EXPECT_TRUE(errors[0].Message().find("model must have at least one link") != std::string::npos); @@ -538,7 +538,7 @@ TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(10u, errors.size()); + EXPECT_EQ(5u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::MODEL_WITHOUT_LINK); EXPECT_NE(std::string::npos, errors[0].Message().find("A model must have at least one link")); diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 8da384fc1..394ff1a63 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -324,8 +324,8 @@ TEST(NestedModel, NestedInclude) // each model has 3 links, and the link names of the nested models have // been transformed EXPECT_EQ(3u, model1->LinkCount()); - EXPECT_EQ(3u, model2->LinkCount()); - EXPECT_EQ(3u, model3->LinkCount()); + EXPECT_EQ(0u, model2->LinkCount()); + EXPECT_EQ(0u, model3->LinkCount()); const sdf::Link *baseLink1 = model1->LinkByName("base"); const sdf::Link *baseLink2 = model2->LinkByName(name + "::base"); const sdf::Link *baseLink3 = model3->LinkByName(name + "_14::base"); @@ -353,24 +353,24 @@ TEST(NestedModel, NestedInclude) EXPECT_EQ(lowerLink3->RawPose(), lowerLink1->RawPose()); EXPECT_EQ(upperLink2->RawPose(), upperLink1->RawPose()); EXPECT_EQ(upperLink3->RawPose(), upperLink1->RawPose()); - // expect //pose/@relative_to to contain the name of the nested model frame - // injected during parsing. model1 is not nested, so it's links should have - // empty //pose/@relative_to - EXPECT_TRUE(baseLink1->PoseRelativeTo().empty()); - EXPECT_EQ(name + "::__model__", baseLink2->PoseRelativeTo()); - EXPECT_EQ(name + "_14::__model__", baseLink3->PoseRelativeTo()); - EXPECT_TRUE(lowerLink1->PoseRelativeTo().empty()); - EXPECT_EQ(name + "::__model__", lowerLink2->PoseRelativeTo()); - EXPECT_EQ(name + "_14::__model__", lowerLink3->PoseRelativeTo()); - EXPECT_TRUE(upperLink1->PoseRelativeTo().empty()); - EXPECT_EQ(name + "::__model__", upperLink2->PoseRelativeTo()); - EXPECT_EQ(name + "_14::__model__", upperLink3->PoseRelativeTo()); + // // expect //pose/@relative_to to contain the name of the nested model frame + // // injected during parsing. model1 is not nested, so it's links should have + // // empty //pose/@relative_to + // EXPECT_TRUE(baseLink1->PoseRelativeTo().empty()); + // EXPECT_EQ(name + "::__model__", baseLink2->PoseRelativeTo()); + // EXPECT_EQ(name + "_14::__model__", baseLink3->PoseRelativeTo()); + // EXPECT_TRUE(lowerLink1->PoseRelativeTo().empty()); + // EXPECT_EQ(name + "::__model__", lowerLink2->PoseRelativeTo()); + // EXPECT_EQ(name + "_14::__model__", lowerLink3->PoseRelativeTo()); + // EXPECT_TRUE(upperLink1->PoseRelativeTo().empty()); + // EXPECT_EQ(name + "::__model__", upperLink2->PoseRelativeTo()); + // EXPECT_EQ(name + "_14::__model__", upperLink3->PoseRelativeTo()); // each model has 2 joints, and the joint names of the nested models have // been transformed EXPECT_EQ(2u, model1->JointCount()); - EXPECT_EQ(2u, model2->JointCount()); - EXPECT_EQ(2u, model3->JointCount()); + EXPECT_EQ(0u, model2->JointCount()); + EXPECT_EQ(0u, model3->JointCount()); auto *lowerJoint1 = model1->JointByName("lower_joint"); auto *lowerJoint2 = model2->JointByName(name + "::lower_joint"); auto *lowerJoint3 = model3->JointByName(name + "_14::lower_joint"); @@ -407,8 +407,8 @@ TEST(NestedModel, NestedInclude) // //axis/xyz/@expressed_in == "__model__" inside the nested model EXPECT_EQ(ignition::math::Vector3d::UnitX, lowerAxis3->Xyz()); EXPECT_EQ(ignition::math::Vector3d::UnitX, upperAxis3->Xyz()); - EXPECT_EQ(name + "_14::__model__", lowerAxis3->XyzExpressedIn()); - EXPECT_EQ(name + "_14::__model__", upperAxis3->XyzExpressedIn()); + // EXPECT_EQ(name + "_14::__model__", lowerAxis3->XyzExpressedIn()); + // EXPECT_EQ(name + "_14::__model__", upperAxis3->XyzExpressedIn()); } ////////////////////////////////////////////////// @@ -427,6 +427,7 @@ TEST(NestedModel, NestedModelWithFrames) << "" << "" << " " + << " " << " " << " " + modelPath + "" << " M1" @@ -452,6 +453,9 @@ TEST(NestedModel, NestedModelWithFrames) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + const sdf::Frame *modelFrame = parentModel->FrameByIndex(0); + ASSERT_NE(nullptr, modelFrame ); + using ignition::math::Pose3d; using ignition::math::Vector3d; // Expected poses for frames, links and joints after nesting. @@ -463,55 +467,49 @@ TEST(NestedModel, NestedModelWithFrames) Vector3d joint1AxisExpVector = frame2ExpPose.Rot() * Vector3d::UnitZ; Vector3d joint1Axis2ExpVector = frame2ExpPose.Rot() * Vector3d::UnitX; - const auto *frame1 = parentModel->FrameByName("M1::F1"); - ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose().Resolve(frame1Pose).empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + EXPECT_TRUE(modelFrame->SemanticPose().Resolve(frame1Pose, "M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); - const auto *frame2 = parentModel->FrameByName("M1::F2"); - ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose).empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(modelFrame->SemanticPose().Resolve(frame2Pose, "M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose).empty()); - EXPECT_EQ(link1ExpPose, link1Pose); + EXPECT_TRUE(modelFrame->SemanticPose().Resolve(link1Pose, "M1::L1").empty()); + EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); const auto *visual1 = link1->VisualByName("V1"); ASSERT_NE(nullptr, visual1); - EXPECT_EQ("M1::F2", visual1->PoseRelativeTo()); + EXPECT_EQ("F2", visual1->PoseRelativeTo()); const auto *collision1 = link1->CollisionByName("C1"); ASSERT_NE(nullptr, collision1); - EXPECT_EQ("M1::__model__", collision1->PoseRelativeTo()); + EXPECT_EQ("__model__", collision1->PoseRelativeTo()); - const auto *link2 = parentModel->LinkByName("M1::L2"); - ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose).empty()); - EXPECT_EQ(link2ExpPose, link2Pose); + EXPECT_TRUE(modelFrame->SemanticPose().Resolve(link2Pose, "M1::L2").empty()); + EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, "__model__").empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose); + EXPECT_TRUE(modelFrame->SemanticPose().Resolve(joint1Pose, "M1::J1").empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); const auto joint1Axis = joint1->Axis(0); ASSERT_NE(nullptr, joint1Axis); Vector3d joint1AxisVector; EXPECT_TRUE(joint1Axis->ResolveXyz(joint1AxisVector, "__model__").empty()); - EXPECT_EQ(joint1AxisExpVector, joint1AxisVector); + EXPECT_EQ(joint1AxisExpVector, model1Pose.Rot() * joint1AxisVector); const auto joint1Axis2 = joint1->Axis(1); ASSERT_NE(nullptr, joint1Axis2); Vector3d joint1Axis2Vector; EXPECT_TRUE(joint1Axis2->ResolveXyz(joint1Axis2Vector, "__model__").empty()); - EXPECT_EQ(joint1Axis2ExpVector, joint1Axis2Vector); + EXPECT_EQ(joint1Axis2ExpVector, model1Pose.Rot() * joint1Axis2Vector); } // Function to remove any element in //joint/axis that is not @@ -526,12 +524,39 @@ static void removeNoneXyz(const sdf::ElementPtr &_elem) } } - for (auto el : toRemove) + for (const auto &el : toRemove) { _elem->RemoveChild(el); } } +void prepareModelForDirectComparison(sdf::ElementPtr _modelElem) +{ + if (_modelElem->HasElement("joint")) + { + for (auto joint = _modelElem->GetElement("joint"); joint; + joint = joint->GetNextElement("joint")) + { + if (joint->HasElement("axis")) + { + removeNoneXyz(joint->GetElement("axis")); + } + if (joint->HasElement("axis2")) + { + removeNoneXyz(joint->GetElement("axis2")); + } + } + } + if (_modelElem->HasElement("model")) + { + for (auto elem = _modelElem->GetElement("model"); elem; + elem = elem->GetNextElement("model")) + { + prepareModelForDirectComparison(elem); + } + } +} + // Remove elements in world that are not model for comparison with // expectation. Also remove any element in //joint/axis that is not void prepareForDirectComparison(sdf::ElementPtr _worldElem) @@ -546,21 +571,7 @@ void prepareForDirectComparison(sdf::ElementPtr _worldElem) } else { - if (elem->HasElement("joint")) - { - for (auto joint = elem->GetElement("joint"); joint; - joint = joint->GetNextElement("joint")) - { - if (joint->HasElement("axis")) - { - removeNoneXyz(joint->GetElement("axis")); - } - if (joint->HasElement("axis2")) - { - removeNoneXyz(joint->GetElement("axis2")); - } - } - } + prepareModelForDirectComparison(elem); } } @@ -691,6 +702,7 @@ TEST(NestedModel, NestedModelWithSiblingFrames) << "" << "" << " " + << " " << " " << " " << testFramePose << "" << " " @@ -718,12 +730,13 @@ TEST(NestedModel, NestedModelWithSiblingFrames) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + const sdf::Frame *parentModelFrame = parentModel->FrameByIndex(0); + ASSERT_NE(nullptr, parentModelFrame); - const sdf::Frame *nestedModel1Frame = - parentModel->FrameByName("M1::__model__"); - ASSERT_NE(nullptr, nestedModel1Frame); + const sdf::Model *nestedModel1 = parentModel->ModelByName("M1"); + ASSERT_NE(nullptr, nestedModel1 ); - EXPECT_EQ("testFrame", nestedModel1Frame->PoseRelativeTo()); + EXPECT_EQ("testFrame", nestedModel1->PoseRelativeTo()); using ignition::math::Pose3d; using ignition::math::Vector3d; @@ -738,38 +751,33 @@ TEST(NestedModel, NestedModelWithSiblingFrames) Pose3d nestedModel1FramePose; EXPECT_TRUE( - nestedModel1Frame->SemanticPose().Resolve(nestedModel1FramePose).empty()); + nestedModel1->SemanticPose().Resolve(nestedModel1FramePose).empty()); EXPECT_EQ(nestedModel1FrameExpPose, nestedModel1FramePose); - const auto *frame1 = parentModel->FrameByName("M1::F1"); - ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose().Resolve(frame1Pose).empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + EXPECT_TRUE( + parentModelFrame->SemanticPose().Resolve(frame1Pose, "M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); - const auto *frame2 = parentModel->FrameByName("M1::F2"); - ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose).empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE( + parentModelFrame->SemanticPose().Resolve(frame2Pose, "M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); - const auto *link1 = parentModel->LinkByName("M1::L1"); - ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose).empty()); - EXPECT_EQ(link1ExpPose, link1Pose); + EXPECT_TRUE( + parentModelFrame->SemanticPose().Resolve(link1Pose, "M1::L1").empty()); + EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); - const auto *link2 = parentModel->LinkByName("M1::L2"); - ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose).empty()); - EXPECT_EQ(link2ExpPose, link2Pose); + EXPECT_TRUE( + parentModelFrame->SemanticPose().Resolve(link2Pose, "M1::L2").empty()); + EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); - const auto *joint1 = parentModel->JointByName("M1::J1"); - ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, "__model__").empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose); + EXPECT_TRUE( + parentModelFrame->SemanticPose().Resolve(joint1Pose, "M1::J1").empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); } ////////////////////////////////////////////////// @@ -784,11 +792,13 @@ TEST(NestedModel, NestedFrameOnlyModel) const ignition::math::Pose3d model1Pose(10, 0, 0, 0, 0, IGN_PI/2); std::ostringstream stream; - std::string version = "1.7"; + std::string version = "1.8"; stream << "" << "" << " " + << " true" + << " " << " " << " " + MODEL_PATH + "" << " M1" @@ -804,9 +814,7 @@ TEST(NestedModel, NestedFrameOnlyModel) sdf::Root root; sdf::Errors errors = root.Load(sdfParsed); - for (auto e : errors) - std::cout << e.Message() << std::endl; - EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; const sdf::World *world = root.WorldByIndex(0); ASSERT_NE(nullptr, world); @@ -814,23 +822,24 @@ TEST(NestedModel, NestedFrameOnlyModel) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + const sdf::Frame *parentModelFrame = parentModel->FrameByIndex(0); + ASSERT_NE(nullptr, parentModelFrame); + using ignition::math::Pose3d; using ignition::math::Vector3d; // Expected poses for frames after nesting. Pose3d frame1ExpPose = model1Pose * Pose3d(0, 0, 0, IGN_PI, 0, 0); Pose3d frame2ExpPose = frame1ExpPose * Pose3d(0, 0, 0, 0, IGN_PI, 0); - const auto *frame1 = parentModel->FrameByName("M1::F1"); - ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose().Resolve(frame1Pose).empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + EXPECT_TRUE( + parentModelFrame->SemanticPose().Resolve(frame1Pose, "M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); - const auto *frame2 = parentModel->FrameByName("M1::F2"); - ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose).empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE( + parentModelFrame->SemanticPose().Resolve(frame2Pose, "M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); } class PlacementFrame: public ::testing::Test @@ -851,14 +860,10 @@ class PlacementFrame: public ::testing::Test return sdf::filesystem::append(modelRootPath, _file); }); sdf::Errors errors = this->root.Load(testModelPath); - for (const auto &e : errors) - { - std::cout << e.Message() << std::endl; - } - if (!errors.empty()) - { - std::cout << this->root.Element()->ToString("") << std::endl; - } + // if (!errors.empty()) + // { + // std::cout << this->root.Element()->ToString("") << std::endl; + // } EXPECT_TRUE(errors.empty()) << errors; this->world = this->root.WorldByIndex(0); @@ -940,13 +945,13 @@ class PlacementFrame: public ::testing::Test Pose3d testFramePose; { - sdf::Errors errors = - testFrame->SemanticPose().Resolve(testFramePose, "__model__"); - EXPECT_TRUE(errors.empty()) << errors[0].Message(); + sdf::Errors errors = parentModel->SemanticPose().Resolve( + testFramePose, _parentModelName + "::" + _testFrameName); + EXPECT_TRUE(errors.empty()) << errors; } // The expected pose of the test frame is precisely the placement pose - EXPECT_EQ(placementPose, testFramePose); + EXPECT_EQ(placementPose, testFramePose.Inverse()); } protected: sdf::Root root; diff --git a/test/integration/nested_model_with_frames_expected.sdf b/test/integration/nested_model_with_frames_expected.sdf index 6e3f1dcbe..84d53f7d2 100644 --- a/test/integration/nested_model_with_frames_expected.sdf +++ b/test/integration/nested_model_with_frames_expected.sdf @@ -2,67 +2,67 @@ - - 10 0 0 0 -0 1.5708 - - - 0 0 0 1.5708 -0 0 - - - 0 0 0 0 0.785398 0 - - - 0 0 0 0 -0 0 - - 0 0 0 0 -0 0 - - - 1 - - - - - 0 0 0 0 -0 0 - - - 1 - - - - - - 1 0 0 0 -0 0 - - - 0 1 0 0 -0 0 - - - 0 0 1 0 -0 0 - - - 0 0 0 0 -0 0 - M1::L1 - M1::L2 - - 0 0 1 - - - 1 0 0 - - - - 0 0 1 0 -0 0 - M1::L2 - M1::L3 - - 0 0 1 - - - - 1 0 1 0 -0 0 - M1::L3 - M1::L4 - + + + 0 0 0 1.5708 -0 0 + + + 0 0 0 0 0.785398 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 1 + + + + + 0 0 0 0 -0 0 + + + 1 + + + + + + 1 0 0 0 -0 0 + + + 0 1 0 0 -0 0 + + + 0 0 1 0 -0 0 + + + 0 0 0 0 -0 0 + L1 + L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 -0 0 + L2 + L3 + + 0 0 1 + + + + 1 0 1 0 -0 0 + L3 + L4 + + 10 0 0 0 -0 1.5708 + diff --git a/test/integration/two_level_nested_model_with_frames_expected.sdf b/test/integration/two_level_nested_model_with_frames_expected.sdf index f96fae7ba..dcc26cf14 100644 --- a/test/integration/two_level_nested_model_with_frames_expected.sdf +++ b/test/integration/two_level_nested_model_with_frames_expected.sdf @@ -2,70 +2,70 @@ - - 10 0 0 0 -0 1.5708 - - - 0 10 0 1.5708 -0 0 - - - 0 0 0 1.5708 -0 0 - - - 0 0 0 0 0.785398 0 - - - 0 0 0 0 -0 0 - - 0 0 0 0 -0 0 - - - 1 - - - - - 0 0 0 0 -0 0 - - - 1 - - - - - - 1 0 0 0 -0 0 - - - 0 1 0 0 -0 0 - - - 0 0 1 0 -0 0 - - - 0 0 0 0 -0 0 - M1::test_model_with_frames::L1 - M1::test_model_with_frames::L2 - - 0 0 1 - - - 1 0 0 - - - - 0 0 1 0 -0 0 - M1::test_model_with_frames::L2 - M1::test_model_with_frames::L3 - - 0 0 1 - - - - 1 0 1 0 -0 0 - M1::test_model_with_frames::L3 - M1::test_model_with_frames::L4 - + + + + 0 0 0 1.5708 -0 0 + + + 0 0 0 0 0.785398 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 1 + + + + + 0 0 0 0 -0 0 + + + 1 + + + + + + 1 0 0 0 -0 0 + + + 0 1 0 0 -0 0 + + + 0 0 1 0 -0 0 + + + 0 0 0 0 -0 0 + L1 + L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 -0 0 + L2 + L3 + + 0 0 1 + + + + 1 0 1 0 -0 0 + L3 + L4 + + 0 10 0 1.5708 -0 0 + + 10 0 0 0 -0 1.5708 + From 46ff7c0e21dc6ffdbf3b253ab1400b4a09db3ff4 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 1 Oct 2020 01:08:06 -0500 Subject: [PATCH 12/70] Add __root__ scope The __root__ vertex is the root node of both world and model PoseRelativeTo graphs. It corresponds to the `` tag in SDFormat files. Having this node makes it possible to keep the a model's pose and (possibly accounting for placement_frame) information in the edge from the _root__ vertex to the model vertex. Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 159 +++++++++++++----- src/FrameSemantics.hh | 4 +- src/FrameSemantics_TEST.cc | 8 +- src/Model.cc | 11 +- src/ScopedGraph.hh | 11 +- src/parser.cc | 2 + test/integration/model_dom.cc | 20 +++ test/integration/nested_model.cc | 1 - .../model_with_placement_frame_attribute.sdf | 12 ++ 9 files changed, 168 insertions(+), 60 deletions(-) create mode 100644 test/sdf/model_with_placement_frame_attribute.sdf diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index e1f8694a9..810b9cbe4 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -81,6 +81,12 @@ FindSourceVertex(const ScopedGraph &_graph, return PairType(Vertex::NullVertex, EdgesType()); } + if (_id == _graph.ScopeVertexId()) + { + // This is the source. + return PairType(vertex, EdgesType()); + } + std::set visited; visited.insert(vertex.get().Id()); @@ -425,7 +431,6 @@ Errors buildFrameAttachedToGraph( // } // } - // std::cout << "Model FrameAttachedToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -543,13 +548,12 @@ Errors buildFrameAttachedToGraph( _out.AddEdge({frameId, attachedToId}, edgeData); } - // std::cout << "World FrameAttachedToGraph\n" << _out.Graph() << std::endl; return errors; } ///////////////////////////////////////////////// Errors buildPoseRelativeToGraph( - ScopedGraph &_out, const Model *_model) + ScopedGraph &_out, const Model *_model, bool _root) { Errors errors; @@ -566,17 +570,39 @@ Errors buildPoseRelativeToGraph( return errors; } + ScopedGraph::VertexId rootId = ignition::math::graph::kNullId; + ScopedGraph::VertexId modelId = ignition::math::graph::kNullId; + ScopedGraph::VertexId modelFrameId = ignition::math::graph::kNullId; // add the model frame vertex first - const std::string scopeName = "__model__"; - _out.SetScopeName(scopeName); - auto modelFrameId = - _out.AddScopeVertex(_model->Name(), scopeName, sdf::FrameType::MODEL).Id(); + ScopedGraph outModel; + if (_root) + { + rootId = _out.AddScopeVertex("", "__root__", sdf::FrameType::ROOT).Id(); + const std::string scopeName = "__root__"; + _out.SetScopeName(scopeName); + modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); + + outModel = _out.ChildScope(_model->Name(), "__model__"); + // modelFrameId = outModel.AddScopeVertex(_model->Name(), "__model__", sdf::FrameType::MODEL).Id(); + modelFrameId = outModel.AddVertex("__model__", sdf::FrameType::MODEL).Id(); + } + else + { + outModel = _out; + const std::string scopeName = "__model__"; + outModel.SetScopeName(scopeName); + modelFrameId = outModel.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); + } + if (_root) + { + outModel.AddEdge({modelId, modelFrameId}, {}); + } // add link vertices and default edge if relative_to is empty for (uint64_t l = 0; l < _model->LinkCount(); ++l) { auto link = _model->LinkByIndex(l); - if (_out.Count(link->Name()) > 0) + if (outModel.Count(link->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Link with non-unique name [" + link->Name() + @@ -585,12 +611,12 @@ Errors buildPoseRelativeToGraph( continue; } auto linkId = - _out.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); + outModel.AddVertex(link->Name(), sdf::FrameType::LINK).Id(); if (link->PoseRelativeTo().empty()) { // relative_to is empty, so add edge from implicit model frame to link - _out.AddEdge({modelFrameId, linkId}, link->RawPose()); + outModel.AddEdge({modelFrameId, linkId}, link->RawPose()); } } @@ -598,7 +624,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - if (_out.Count(joint->Name()) > 0) + if (outModel.Count(joint->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Joint with non-unique name [" + joint->Name() + @@ -606,7 +632,7 @@ Errors buildPoseRelativeToGraph( "]."}); continue; } - _out.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); + outModel.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); } // add frame vertices and default edge if both @@ -614,7 +640,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - if (_out.Count(frame->Name()) > 0) + if (outModel.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -623,12 +649,12 @@ Errors buildPoseRelativeToGraph( continue; } auto frameId = - _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); + outModel.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); if (frame->PoseRelativeTo().empty() && frame->AttachedTo().empty()) { // add edge from implicit model frame to frame - _out.AddEdge({modelFrameId, frameId}, frame->RawPose()); + outModel.AddEdge({modelFrameId, frameId}, frame->RawPose()); } } @@ -636,7 +662,7 @@ Errors buildPoseRelativeToGraph( for (uint64_t m = 0; m < _model->ModelCount(); ++m) { auto nestedModel = _model->ModelByIndex(m); - if (_out.Count(nestedModel->Name()) > 0) + if (outModel.Count(nestedModel->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Nested model with non-unique name [" + nestedModel->Name() + @@ -645,23 +671,25 @@ Errors buildPoseRelativeToGraph( continue; } auto nestedModelId = - _out.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); + outModel.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); - ScopedGraph childGraph = _out; - auto nestedErrors = buildPoseRelativeToGraph(childGraph , nestedModel); + ScopedGraph childGraph = + outModel.ChildScope(nestedModel->Name(), "__model__"); + auto nestedErrors = buildPoseRelativeToGraph(childGraph, nestedModel, false); errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); // Add an aliasing edge from the nestedModel vertex to the // corresponding vertex in the PoseRelativeTo graph of the nested model, // i.e, to the ::__model__ vertex auto scopedNestedModelId = - _out.VertexIdByName(nestedModel->Name() + "::__model__"); + outModel.VertexIdByName(nestedModel->Name() + "::__model__"); if (scopedNestedModelId == ignition::math::graph::kNullId) { // TODO (addisu) ERROR } else { - auto &edge = _out.AddEdge({nestedModelId, scopedNestedModelId}, {{}, false}); + auto &edge = + outModel.AddEdge({nestedModelId, scopedNestedModelId}, {{}, false}); edge.SetWeight(0); } } @@ -680,10 +708,10 @@ Errors buildPoseRelativeToGraph( continue; } - auto linkId = _out.VertexIdByName(link->Name()); + auto linkId = outModel.VertexIdByName(link->Name()); // look for vertex in graph that matches relative_to value - if (_out.Count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + @@ -692,7 +720,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.VertexIdByName(relativeTo); + auto relativeToId = outModel.VertexIdByName(relativeTo); if (link->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -701,7 +729,7 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.AddEdge({relativeToId, linkId}, link->RawPose()); + outModel.AddEdge({relativeToId, linkId}, link->RawPose()); } for (uint64_t j = 0; j < _model->JointCount(); ++j) @@ -715,10 +743,10 @@ Errors buildPoseRelativeToGraph( relativeTo = joint->ChildLinkName(); } - auto jointId = _out.VertexIdByName(joint->Name()); + auto jointId = outModel.VertexIdByName(joint->Name()); // look for vertex in graph that matches relative_to value - if (_out.Count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + @@ -727,7 +755,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.VertexIdByName(relativeTo); + auto relativeToId = outModel.VertexIdByName(relativeTo); if (joint->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -736,7 +764,7 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.AddEdge({relativeToId, jointId}, joint->RawPose()); + outModel.AddEdge({relativeToId, jointId}, joint->RawPose()); } for (uint64_t f = 0; f < _model->FrameCount(); ++f) @@ -749,7 +777,7 @@ Errors buildPoseRelativeToGraph( continue; } - auto frameId = _out.VertexIdByName(frame->Name()); + auto frameId = outModel.VertexIdByName(frame->Name()); std::string relativeTo; std::string typeForErrorMsg; ErrorCode errorCode; @@ -767,7 +795,7 @@ Errors buildPoseRelativeToGraph( } // look for vertex in graph that matches relative_to value - if (_out.Count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({errorCode, typeForErrorMsg + " name[" + relativeTo + @@ -776,7 +804,7 @@ Errors buildPoseRelativeToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto relativeToId = _out.VertexIdByName(relativeTo); + auto relativeToId = outModel.VertexIdByName(relativeTo); if (frame->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -785,14 +813,14 @@ Errors buildPoseRelativeToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.AddEdge({relativeToId, frameId}, frame->RawPose()); + outModel.AddEdge({relativeToId, frameId}, frame->RawPose()); } for (uint64_t m = 0; m < _model->ModelCount(); ++m) { auto nestedModel = _model->ModelByIndex(m); - auto nestedModelId = _out.VertexIdByName(nestedModel->Name()); + auto nestedModelId = outModel.VertexIdByName(nestedModel->Name()); // relative_to is empty, so add edge from implicit model frame // to nestedModel auto relativeToId = modelFrameId; @@ -801,7 +829,7 @@ Errors buildPoseRelativeToGraph( if (!relativeTo.empty()) { // look for vertex in graph that matches relative_to value - if (_out.Count(relativeTo) != 1) + if (outModel.Count(relativeTo) != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "relative_to name[" + relativeTo + @@ -811,7 +839,7 @@ Errors buildPoseRelativeToGraph( continue; } - relativeToId = _out.VertexIdByName(relativeTo); + relativeToId = outModel.VertexIdByName(relativeTo); if (nestedModel->Name() == relativeTo) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, @@ -830,7 +858,7 @@ Errors buildPoseRelativeToGraph( ignition::math::Pose3d X_MPf; sdf::Errors resolveErrors = sdf::resolvePoseRelativeToRoot(X_MPf, - _out.ChildScope(nestedModel->Name(), "__model__"), + outModel.ChildScope(nestedModel->Name(), "__model__"), nestedModel->PlacementFrameName()); if (resolveErrors.empty()) { @@ -852,10 +880,43 @@ Errors buildPoseRelativeToGraph( } } - _out.AddEdge({relativeToId, nestedModelId}, X_RM); + outModel.AddEdge({relativeToId, nestedModelId}, X_RM); } - // std::cout << "Model PoseRelativeToGraph\n" << _out.Graph() << std::endl; + if (_root) + { + auto rootToModel = outModel.AddEdge({rootId, modelId}, {}); + ignition::math::Pose3d X_RM = _model->RawPose(); + // If the model has a placement frame, calculate the necessary pose to use + // TODO (addisu) docs + if (!_model->PlacementFrameName().empty()) + { + ignition::math::Pose3d X_MPf; + + sdf::Errors resolveErrors = sdf::resolvePoseRelativeToRoot( + X_MPf, outModel, _model->PlacementFrameName()); + if (resolveErrors.empty()) + { + // Before this update, i.e, as specified in the SDFormat, the model pose + // (X_RPf) is the pose of the placement frame (Pf) relative to a frame + // (R) in the parent scope of the model. However, when this model (M) is + // inserted into a pose graph of the parent scope, only the pose (X_RM) + // of the __model__ frame can be used. Thus, the model pose has to be + // updated to X_RM. + // + // Note that X_RPf is the raw pose specified in //model/pose before this + // update. + const auto &X_RPf = _model->RawPose(); + X_RM = X_RPf * X_MPf.Inverse(); + } + else + { + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + } + } + + outModel.UpdateEdge(rootToModel, X_RM); + } return errors; } @@ -878,12 +939,14 @@ Errors buildPoseRelativeToGraph( return errors; } + auto rootId = _out.AddVertex("__root__", sdf::FrameType::ROOT).Id(); // add implicit world frame vertex first const std::string sourceName = "world"; _out.SetScopeName(sourceName); auto worldFrameId = _out.AddScopeVertex("", sourceName, sdf::FrameType::WORLD).Id(); + _out.AddEdge({rootId, worldFrameId}, {}); // add model vertices and default edge if relative_to is empty for (uint64_t m = 0; m < _world->ModelCount(); ++m) { @@ -899,8 +962,9 @@ Errors buildPoseRelativeToGraph( auto modelId = _out.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); - ScopedGraph childGraph = _out; - auto modelErrors = buildPoseRelativeToGraph(childGraph , model); + ScopedGraph childGraph = + _out.ChildScope(model->Name(), "__model__"); + auto modelErrors = buildPoseRelativeToGraph(childGraph , model, false); errors.insert(errors.end(), modelErrors .begin(), modelErrors .end()); // Add an aliasing edge from the model vertex to the @@ -1061,7 +1125,6 @@ Errors buildPoseRelativeToGraph( _out.AddEdge({relativeToId, frameId}, frame->RawPose()); } - // std::cout << "World PoseRelativeToGraph\n" << _out.Graph() << std::endl; return errors; } @@ -1236,7 +1299,7 @@ Errors validatePoseRelativeToGraph( Errors errors; // Expect scopeName to be either "__model__" or "world" - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && _in.ScopeName() != "__root__") { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: source vertex name " + _in.ScopeName() + @@ -1292,6 +1355,10 @@ Errors validatePoseRelativeToGraph( "PoseRelativeToGraph error, " "vertex with empty name detected."}); } + if (vertexName == "__root__") + { + continue; + } auto incomingEdges = _in.Graph().IncidentsTo(vertexPair.first); std::size_t inDegree = std::count_if(incomingEdges.begin(), @@ -1362,11 +1429,11 @@ Errors validatePoseRelativeToGraph( switch (vertexPair.second.get().Data()) { case sdf::FrameType::WORLD: - if (inDegree != 0) + if (inDegree != 1) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " - "WORLD vertices should have no incoming edges " + "WORLD vertices should have 1 incoming edge " "in WORLD relative_to graph."}); } break; @@ -1398,6 +1465,8 @@ Errors validatePoseRelativeToGraph( // check graph for cycles by resolving pose of each vertex relative to root for (auto const &name : _in.VertexNames()) { + if (name == "__root__") + continue; ignition::math::Pose3d pose; Errors e = resolvePoseRelativeToRoot(pose, _in, name); errors.insert(errors.end(), e.begin(), e.end()); diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 7015278d9..79e25b446 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -69,6 +69,8 @@ namespace sdf FRAME = 4, STATIC_MODEL = 5, + + ROOT = 6, }; /// \brief Data structure for frame attached_to graphs for Model or World. @@ -140,7 +142,7 @@ namespace sdf /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. Errors buildPoseRelativeToGraph( - ScopedGraph &_out, const Model *_model); + ScopedGraph &_out, const Model *_model, bool _root = true); /// \brief Build a PoseRelativeToGraph for a world. /// \param[out] _out Graph object to write. diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index e05325cf9..27f2d8fcd 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -222,10 +222,11 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) auto errors = sdf::buildPoseRelativeToGraph(graph, model); EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validatePoseRelativeToGraph(graph).empty()); + graph = graph.ChildScope(model->Name(), "__model__"); - EXPECT_EQ(8u, graph.Map().size()); - EXPECT_EQ(8u, graph.Graph().Vertices().size()); - EXPECT_EQ(7u, graph.Graph().Edges().size()); + EXPECT_EQ(10u, graph.Map().size()); + EXPECT_EQ(10u, graph.Graph().Vertices().size()); + EXPECT_EQ(9u, graph.Graph().Edges().size()); EXPECT_EQ(1u, graph.Count("__model__")); EXPECT_EQ(1u, graph.Count("P")); @@ -322,6 +323,7 @@ TEST(FrameSemantics, updateGraphPose) EXPECT_EQ(Pose3d::Zero, model->RawPose()); + graph = graph.ChildScope(model->Name(), "__model__"); { Pose3d pose; EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty()); diff --git a/src/Model.cc b/src/Model.cc index 27ad0ed14..bbe1adfd6 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -433,24 +433,27 @@ Errors Model::Load(ElementPtr _sdf) validatePoseRelativeToGraph(this->dataPtr->poseGraph); errors.insert(errors.end(), validatePoseGraphErrors.begin(), validatePoseGraphErrors.end()); + + auto childPoseGraph = + this->dataPtr->poseGraph.ChildScope(this->Name(), "__model__"); for (auto &link : this->dataPtr->links) { - link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + link.SetPoseRelativeToGraph(childPoseGraph); } for (auto &model : this->dataPtr->models) { Errors setPoseRelativeToGraphErrors = - model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + model.SetPoseRelativeToGraph(childPoseGraph); errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), setPoseRelativeToGraphErrors.end()); } for (auto &joint : this->dataPtr->joints) { - joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + joint.SetPoseRelativeToGraph(childPoseGraph); } for (auto &frame : this->dataPtr->frames) { - frame.SetPoseRelativeToGraph(this->dataPtr->poseGraph); + frame.SetPoseRelativeToGraph(childPoseGraph); } } diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index 8819f58bf..825c6cb97 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -81,7 +81,7 @@ class ScopedGraph public: std::string VertexName(const VertexId &_id) const; public: std::string VertexName(const Vertex &_vertex) const; - public: void UpdateEdge(std::reference_wrapper &, const EdgeType&); + public: void UpdateEdge(const Edge &, const EdgeType&); public: size_t Count(const std::string &_name) const; public: VertexId VertexIdByName(const std::string &_name) const; @@ -220,15 +220,14 @@ std::string ScopedGraph::VertexName(const VertexId &_id) const ///////////////////////////////////////////////// template -void ScopedGraph::UpdateEdge( - std::reference_wrapper &_edge, const EdgeType &_data) +void ScopedGraph::UpdateEdge(const Edge &_edge, const EdgeType &_data) { // There's no API to update the data of an edge, so we remove the edge and // insert a new one with the new pose. - auto tailVertexId = _edge.get().Tail(); - auto headVertexId = _edge.get().Head(); + auto tailVertexId = _edge.Tail(); + auto headVertexId = _edge.Head(); auto &graph = this->graphWeak.lock()->graph; - graph.RemoveEdge(_edge.get().Id()); + graph.RemoveEdge(_edge.Id()); graph.AddEdge({tailVertexId, headVertexId}, _data); } diff --git a/src/parser.cc b/src/parser.cc index f902cefea..05cec39c8 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1808,6 +1808,7 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) << error.Message() << std::endl; } + std::cout << graph.Graph() << std::endl; modelResult = false; } return modelResult; @@ -1837,6 +1838,7 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) << error.Message() << std::endl; } + std::cout << graph.Graph() << std::endl; worldResult = false; } return worldResult; diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 99f3b580f..0b66b713c 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -525,6 +525,26 @@ TEST(DOMRoot, LoadNestedExplicitCanonicalLink) EXPECT_EQ("link2", model->CanonicalLink()->Name()); } +TEST(DOMRoot, ModelPlacementFrameAttribute) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_with_placement_frame_attribute.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + auto *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) { diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 394ff1a63..78afdd8ad 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -1018,4 +1018,3 @@ TEST_F(PlacementFrame, ModelPlacementFrameAttribute) "model_with_placement_frame_and_pose_relative_to", "L4"); } -// TODO (addisu) Add NestedModelPlacementFrameAttribute tests diff --git a/test/sdf/model_with_placement_frame_attribute.sdf b/test/sdf/model_with_placement_frame_attribute.sdf new file mode 100644 index 000000000..6b5df9340 --- /dev/null +++ b/test/sdf/model_with_placement_frame_attribute.sdf @@ -0,0 +1,12 @@ + + + + 0 0 10 0 0 0 + + 1 0 0 0 0 0 + + + 0 2 0 0 0 0 + + + From 5fd9abcc844dc75fd61909987b8e2d6552ad6f04 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 1 Oct 2020 12:25:17 -0500 Subject: [PATCH 13/70] Revert changes in loadUniqueRepeated Signed-off-by: Addisu Z. Taddese --- src/Utils.hh | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/Utils.hh b/src/Utils.hh index 4d81ece67..be6ca1432 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -67,8 +67,7 @@ namespace sdf /// experienced. template sdf::Errors loadUniqueRepeated(sdf::ElementPtr _sdf, - const std::string &_sdfName, std::vector &_objs, - const std::function &_beforeLoadFunc = {}) + const std::string &_sdfName, std::vector &_objs) { Errors errors; @@ -82,10 +81,6 @@ namespace sdf while (elem) { Class obj; - if (_beforeLoadFunc) - { - _beforeLoadFunc(obj); - } // Load the model and capture the errors. Errors loadErrors = obj.Load(elem); From 565dffb57560c696219d8cddb5f45f55d5d17667 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 1 Oct 2020 12:55:17 -0500 Subject: [PATCH 14/70] Make ScopedGraph use pointer semantics Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 164 +++++++++++++++++++++++----------- src/FrameSemantics.hh | 5 +- src/FrameSemantics_TEST.cc | 15 ++-- src/Model.cc | 63 ++++--------- src/ScopedGraph.hh | 102 +++++++++++---------- test/integration/joint_dom.cc | 6 +- test/integration/model_dom.cc | 3 +- 7 files changed, 199 insertions(+), 159 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 810b9cbe4..4b15be7d2 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -205,19 +205,45 @@ std::pair ///////////////////////////////////////////////// Errors buildFrameAttachedToGraph( - ScopedGraph &_out, const Model *_model) + ScopedGraph &_out, const Model *_model, bool _root) { Errors errors; - // add implicit model frame vertex first - const std::string scopeName = "__model__"; - _out.SetScopeName(scopeName); - auto frameType = _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; - auto modelFrameId = - _out.AddScopeVertex(_model->Name(), scopeName, frameType).Id(); + auto rootId = ignition::math::graph::kNullId; + auto modelId = ignition::math::graph::kNullId; + auto modelFrameId = ignition::math::graph::kNullId; + // TODO(addisu) + // ScopedGraph outModel = _out; + ScopedGraph outModel; + if (_root) + { + rootId = + _out.AddScopeVertex("", "__root__", sdf::FrameType::STATIC_MODEL).Id(); + const std::string scopeName = "__model__"; + _out.SetScopeName(scopeName); + modelId = _out.AddVertex(_model->Name(), frameType).Id(); + + outModel = _out.ChildScope(_model->Name(), "__model__"); + // modelFrameId = outModel.AddScopeVertex(_model->Name(), "__model__", sdf::FrameType::MODEL).Id(); + modelFrameId = outModel.AddVertex("__model__", frameType).Id(); + } + else + { + outModel = _out; + const std::string scopeName = "__model__"; + // TODO (addisu) May not need SetScopeName + outModel.SetScopeName(scopeName); + modelFrameId = _out.AddVertex(scopeName, frameType).Id(); + } + + if (_root) + { + outModel.AddEdge({modelId, modelFrameId}, true); + } + const std::string scopeName = "__model__"; if (!_model) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -262,7 +288,7 @@ Errors buildFrameAttachedToGraph( for (uint64_t l = 0; l < _model->LinkCount(); ++l) { auto link = _model->LinkByIndex(l); - if (_out.Count(link->Name()) > 0) + if (outModel.Count(link->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Link with non-unique name [" + link->Name() + @@ -270,12 +296,12 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - _out.AddVertex(link->Name(), sdf::FrameType::LINK); + outModel.AddVertex(link->Name(), sdf::FrameType::LINK); // // add edge from implicit model frame vertex to canonical link // if (!_model->Static() && link == canonicalLink) // { - // _out.AddEdge({modelFrameId, linkId}, true); + // outModel.AddEdge({modelFrameId, linkId}, true); // } } @@ -283,7 +309,7 @@ Errors buildFrameAttachedToGraph( for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - if (_out.Count(joint->Name()) > 0) + if (outModel.Count(joint->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Joint with non-unique name [" + joint->Name() + @@ -291,14 +317,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - _out.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); + outModel.AddVertex(joint->Name(), sdf::FrameType::JOINT).Id(); } // add frame vertices for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - if (_out.Count(frame->Name()) > 0) + if (outModel.Count(frame->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Frame with non-unique name [" + frame->Name() + @@ -306,14 +332,14 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); + outModel.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); } // add nested model vertices for (uint64_t m = 0; m < _model->ModelCount(); ++m) { auto nestedModel = _model->ModelByIndex(m); - if (_out.Count(nestedModel->Name()) > 0) + if (outModel.Count(nestedModel->Name()) > 0) { errors.push_back({ErrorCode::DUPLICATE_NAME, "Nested model with non-unique name [" + nestedModel->Name() + @@ -323,19 +349,33 @@ Errors buildFrameAttachedToGraph( } auto modelFrameType = _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; - _out.AddVertex(nestedModel->Name(), modelFrameType).Id(); - ScopedGraphchildGraph = _out; - auto nestedErrors = buildFrameAttachedToGraph(childGraph, nestedModel); + auto nestedModelId = + outModel.AddVertex(nestedModel->Name(), modelFrameType).Id(); + ScopedGraph childGraph = + outModel.ChildScope(nestedModel->Name(), "__model__"); + auto nestedErrors = + buildFrameAttachedToGraph(childGraph, nestedModel, false); errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); + auto scopedNestedModelId = + outModel.VertexIdByName(nestedModel->Name() + "::__model__"); + if (scopedNestedModelId == ignition::math::graph::kNullId) + { + // TODO (addisu) ERROR + } + else + { + auto &edge = outModel.AddEdge({nestedModelId, scopedNestedModelId}, true); + edge.SetWeight(0); + } } // add edges from joint to child frames for (uint64_t j = 0; j < _model->JointCount(); ++j) { auto joint = _model->JointByIndex(j); - auto jointId = _out.VertexIdByName(joint->Name()); + auto jointId = outModel.VertexIdByName(joint->Name()); auto childFrameName = joint->ChildLinkName(); - if (_out.Count(childFrameName) != 1) + if (outModel.Count(childFrameName) != 1) { errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, "Child frame with name[" + childFrameName + @@ -343,27 +383,27 @@ Errors buildFrameAttachedToGraph( "] not found in model with name[" + _model->Name() + "]."}); continue; } - auto childFrameId = _out.VertexIdByName(childFrameName); - _out.AddEdge({jointId, childFrameId}, true); + auto childFrameId = outModel.VertexIdByName(childFrameName); + outModel.AddEdge({jointId, childFrameId}, true); } // add model edges to their corresponding vertices in the model scope - for (uint64_t m = 0; m < _model->ModelCount(); ++m) - { - auto model = _model->ModelByIndex(m); - auto modelId = _out.VertexIdByName(model->Name()); - auto modelIdChildScope = _out.VertexIdByName(model->Name() + "::__model__"); - if (modelIdChildScope != ignition::math::graph::kNullId) - { - _out.AddEdge({modelId, modelIdChildScope}, true); - } - } + // for (uint64_t m = 0; m < _model->ModelCount(); ++m) + // { + // auto model = _model->ModelByIndex(m); + // auto nestedModelId = outModel.VertexIdByName(model->Name()); + // auto modelIdChildScope = outModel.VertexIdByName(model->Name() + "::__model__"); + // if (modelIdChildScope != ignition::math::graph::kNullId) + // { + // outModel.AddEdge({nestedModelId, modelIdChildScope}, true); + // } + // } // add frame edges for (uint64_t f = 0; f < _model->FrameCount(); ++f) { auto frame = _model->FrameByIndex(f); - auto frameId = _out.VertexIdByName(frame->Name()); + auto frameId = outModel.VertexIdByName(frame->Name()); // look for vertex in graph that matches attached_to value std::string attachedTo = frame->AttachedTo(); if (attachedTo.empty()) @@ -371,7 +411,7 @@ Errors buildFrameAttachedToGraph( // if the attached-to name is empty, use the scope name attachedTo = scopeName; } - if (_out.Count(attachedTo) != 1) + if (outModel.Count(attachedTo) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "attached_to name[" + attachedTo + @@ -380,7 +420,7 @@ Errors buildFrameAttachedToGraph( "in model with name[" + _model->Name() + "]."}); continue; } - auto attachedToId = _out.VertexIdByName(attachedTo); + auto attachedToId = outModel.VertexIdByName(attachedTo); bool edgeData = true; if (frame->Name() == frame->AttachedTo()) { @@ -392,7 +432,7 @@ Errors buildFrameAttachedToGraph( "], causing a graph cycle " "in model with name[" + _model->Name() + "]."}); } - _out.AddEdge({frameId, attachedToId}, edgeData); + outModel.AddEdge({frameId, attachedToId}, edgeData); } @@ -403,8 +443,8 @@ Errors buildFrameAttachedToGraph( // here with an edge from __model__. // The nested canonical link name should be a nested name // relative to _model, delimited by "::". - auto linkId = _out.VertexIdByName(canonicalLinkName); - _out.AddEdge({modelFrameId, linkId}, true); + auto linkId = outModel.VertexIdByName(canonicalLinkName); + outModel.AddEdge({modelFrameId, linkId}, true); } if (_model->Static()) @@ -412,7 +452,7 @@ Errors buildFrameAttachedToGraph( // If the model is static, add an edge from the model frame to the global // root vertex // TODO (addisu) Ensure that the global root vertex has an ID of 0. - // _out.AddEdge({modelFrameId, 0}, true); + // outModel.AddEdge({modelFrameId, 0}, true); } // add edge from implicit model frame vertex to canonical link // if (!_model->Static() && canonicalLink == nullptr) @@ -420,12 +460,12 @@ Errors buildFrameAttachedToGraph( // for (uint64_t m = 0; m < _model->ModelCount(); ++m) // { // sdf::Errors sinkErrors; - // auto vertexId = _out.VertexIdByName(_model->ModelByIndex(m)->Name()); - // auto sinkVertexEdges = FindSinkVertex(_out, vertexId, sinkErrors); + // auto vertexId = outModel.VertexIdByName(_model->ModelByIndex(m)->Name()); + // auto sinkVertexEdges = FindSinkVertex(outModel, vertexId, sinkErrors); // if (sinkErrors.empty()) // { // auto sinkVertex = sinkVertexEdges.first; - // _out.AddEdge({modelFrameId, vertexId}, true); + // outModel.AddEdge({modelFrameId, vertexId}, true); // break; // } // } @@ -473,8 +513,9 @@ Errors buildFrameAttachedToGraph( auto modelFrameType = model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; _out.AddVertex(model->Name(), modelFrameType).Id(); - ScopedGraphchildGraph = _out; - auto modelErrors = buildFrameAttachedToGraph(childGraph, model); + ScopedGraph childGraph = + _out.ChildScope(model->Name(), "__model__"); + auto modelErrors = buildFrameAttachedToGraph(childGraph, model, false); errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); } @@ -570,10 +611,12 @@ Errors buildPoseRelativeToGraph( return errors; } - ScopedGraph::VertexId rootId = ignition::math::graph::kNullId; - ScopedGraph::VertexId modelId = ignition::math::graph::kNullId; - ScopedGraph::VertexId modelFrameId = ignition::math::graph::kNullId; + auto rootId = ignition::math::graph::kNullId; + auto modelId = ignition::math::graph::kNullId; + auto modelFrameId = ignition::math::graph::kNullId; // add the model frame vertex first + // TODO(addisu) + // ScopedGraph outModel = _out; ScopedGraph outModel; if (_root) { @@ -590,6 +633,7 @@ Errors buildPoseRelativeToGraph( { outModel = _out; const std::string scopeName = "__model__"; + // TODO (addisu) May not need SetScopeName outModel.SetScopeName(scopeName); modelFrameId = outModel.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); } @@ -1134,7 +1178,8 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in Errors errors; // Expect scopeName to be either "__model__" or "world" - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && + _in.ScopeName() != "__root__") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] " @@ -1185,6 +1230,9 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in "FrameAttachedToGraph error, " "vertex with empty name detected."}); } + // TODO (addisu) Handle root vertices properly + if (vertexName == "__root__") + continue; auto outDegree = _in.Graph().OutDegree(vertexPair.first); if (outDegree > 1) @@ -1194,7 +1242,8 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in "too many outgoing edges at a vertex with name [" + vertexName + "]."}); } - else if (sdf::FrameType::MODEL == scopeFrameType) + else if (sdf::FrameType::MODEL == scopeFrameType || + sdf::FrameType::STATIC_MODEL == scopeFrameType) { switch (vertexPair.second.get().Data()) { @@ -1215,6 +1264,17 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in "in MODEL attached_to graph."}); } break; + case sdf::FrameType::STATIC_MODEL: + // if (outDegree != 0) + // { + // errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + // "FrameAttachedToGraph error, " + // "LINK vertex with name [" + + // vertexName + + // "] should have no outgoing edges " + // "in MODEL attached_to graph."}); + // } + break; default: if (outDegree == 0) { @@ -1264,6 +1324,7 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in } break; case sdf::FrameType::STATIC_MODEL: + case sdf::FrameType::ROOT: break; default: if (outDegree != 1) @@ -1299,7 +1360,8 @@ Errors validatePoseRelativeToGraph( Errors errors; // Expect scopeName to be either "__model__" or "world" - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && _in.ScopeName() != "__root__") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && + _in.ScopeName() != "__root__") { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: source vertex name " + _in.ScopeName() + @@ -1483,7 +1545,7 @@ Errors resolveFrameAttachedToBody( { Errors errors; - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && _in.ScopeName() != "__root__") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] " diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 79e25b446..5924a291e 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -68,6 +68,7 @@ namespace sdf /// \brief An explicit frame. FRAME = 4, + /// \brief An implicit model frame. STATIC_MODEL = 5, ROOT = 6, @@ -127,8 +128,8 @@ namespace sdf /// \param[out] _out Graph object to write. /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. - Errors buildFrameAttachedToGraph( - ScopedGraph &_out, const Model *_model); + Errors buildFrameAttachedToGraph(ScopedGraph &_out, + const Model *_model, bool _root = true); /// \brief Build a FrameAttachedToGraph for a world. /// \param[out] _out Graph object to write. diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index 27f2d8fcd..f7afdbf1a 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -58,9 +58,10 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model) EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); - EXPECT_EQ(6u, graph.Map().size()); - EXPECT_EQ(6u, graph.Graph().Vertices().size()); - EXPECT_EQ(5u, graph.Graph().Edges().size()); + graph = graph.ChildScope(model->Name(), "__model__"); + EXPECT_EQ(8u, graph.Map().size()); + EXPECT_EQ(8u, graph.Graph().Vertices().size()); + EXPECT_EQ(6u, graph.Graph().Edges().size()); EXPECT_EQ(1u, graph.Count("__model__")); EXPECT_EQ(1u, graph.Count("L")); @@ -182,9 +183,11 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validateFrameAttachedToGraph(modelGraph).empty()); - EXPECT_EQ(3u, modelGraph.Map().size()); - EXPECT_EQ(3u, modelGraph.Graph().Vertices().size()); - EXPECT_EQ(2u, modelGraph.Graph().Edges().size()); + modelGraph = modelGraph.ChildScope(model->Name(), "__model__"); + + EXPECT_EQ(5u, modelGraph.Map().size()); + EXPECT_EQ(5u, modelGraph.Graph().Vertices().size()); + EXPECT_EQ(3u, modelGraph.Graph().Edges().size()); EXPECT_EQ(1u, modelGraph.Count("L")); EXPECT_EQ(1u, modelGraph.Count("__model__")); diff --git a/src/Model.cc b/src/Model.cc index bbe1adfd6..dc02ea6b6 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -237,10 +237,11 @@ Errors Model::Load(ElementPtr _sdf) { // std::cout << "Creating owned graphs for " << this->Name() << std::endl; this->dataPtr->ownedPoseGraph = std::make_shared(); - this->SetPoseRelativeToGraph(this->dataPtr->ownedPoseGraph); + this->dataPtr->poseGraph = this->dataPtr->ownedPoseGraph; this->dataPtr->ownedFrameAttachedToGraph = std::make_shared(); - this->SetFrameAttachedToGraph(this->dataPtr->ownedFrameAttachedToGraph); + this->dataPtr->frameAttachedToGraph = + this->dataPtr->ownedFrameAttachedToGraph; modelFile = true; } @@ -248,13 +249,6 @@ Errors Model::Load(ElementPtr _sdf) // name collisions std::unordered_set frameNames; - // std::function beforeLoad = [this](Model &_model) - // { - // // TODO (addisu) - // _model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - // _model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - // }; - // Load nested models. Errors nestedModelLoadErrors = loadUniqueRepeated(_sdf, "model", this->dataPtr->models); @@ -408,18 +402,8 @@ Errors Model::Load(ElementPtr _sdf) validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), validateFrameAttachedGraphErrors.end()); - for (auto &joint : this->dataPtr->joints) - { - joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - for (auto &model : this->dataPtr->models) - { - model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } + + this->SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); } } @@ -434,27 +418,7 @@ Errors Model::Load(ElementPtr _sdf) errors.insert(errors.end(), validatePoseGraphErrors.begin(), validatePoseGraphErrors.end()); - auto childPoseGraph = - this->dataPtr->poseGraph.ChildScope(this->Name(), "__model__"); - for (auto &link : this->dataPtr->links) - { - link.SetPoseRelativeToGraph(childPoseGraph); - } - for (auto &model : this->dataPtr->models) - { - Errors setPoseRelativeToGraphErrors = - model.SetPoseRelativeToGraph(childPoseGraph); - errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), - setPoseRelativeToGraphErrors.end()); - } - for (auto &joint : this->dataPtr->joints) - { - joint.SetPoseRelativeToGraph(childPoseGraph); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetPoseRelativeToGraph(childPoseGraph); - } + this->SetPoseRelativeToGraph(this->dataPtr->poseGraph); } @@ -799,22 +763,22 @@ Errors Model::SetPoseRelativeToGraph( this->dataPtr->poseGraph.ChildScope(this->Name(), "__model__"); for (auto &link : this->dataPtr->links) { - link.SetPoseRelativeToGraph(childPoseGraph ); + link.SetPoseRelativeToGraph(childPoseGraph); } for (auto &model : this->dataPtr->models) { Errors setPoseRelativeToGraphErrors = - model.SetPoseRelativeToGraph(childPoseGraph ); + model.SetPoseRelativeToGraph(childPoseGraph); errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), setPoseRelativeToGraphErrors.end()); } for (auto &joint : this->dataPtr->joints) { - joint.SetPoseRelativeToGraph(childPoseGraph ); + joint.SetPoseRelativeToGraph(childPoseGraph); } for (auto &frame : this->dataPtr->frames) { - frame.SetPoseRelativeToGraph(childPoseGraph ); + frame.SetPoseRelativeToGraph(childPoseGraph); } return errors; } @@ -851,11 +815,16 @@ void Model::SetFrameAttachedToGraph( ///////////////////////////////////////////////// sdf::SemanticPose Model::SemanticPose() const { + std::string scopeName = ""; + if (this->dataPtr->poseGraph) + { + scopeName = this->dataPtr->poseGraph.ScopeName(); + } return sdf::SemanticPose( this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, - this->dataPtr->poseGraph.ScopeName(), + scopeName, this->dataPtr->poseGraph); } diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index 825c6cb97..bbb78229f 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -25,28 +25,37 @@ #include #include -#include #include +#include namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { +struct ScopedGraphData +{ + // std::unordered_set vertices; + ignition::math::graph::VertexId scopeVertexId { + ignition::math::graph::kNullId}; + + std::string prefix {}; + + std::string scopeName {}; +}; + template class ScopedGraph { - public: - template - struct GraphTypeExtracter + public: template + struct GraphTypeExtracter { using Vertex = void; using Edge = void; }; - public: - template - struct GraphTypeExtracter> + public: template + struct GraphTypeExtracter> { using Vertex = V; using Edge = E; @@ -59,52 +68,44 @@ class ScopedGraph public: using Vertex = ignition::math::graph::Vertex; public: using Edge = ignition::math::graph::DirectedEdge; public: using MapType = typename T::MapType; - public: ScopedGraph() = default; public: ScopedGraph(const std::shared_ptr _graph); - public: ScopedGraph ChildScope(const std::string &_name, const - std::string &_scopeName); + public: ScopedGraph ChildScope( + const std::string &_name, const std::string &_scopeName); public: explicit operator bool() const; - public: const MathGraphType &Graph() const; public: const MapType &Map() const; - // May not be needed if they can be called via Graph() - public: Vertex &AddScopeVertex(const std::string &_prefix, const std::string &_name, const VertexType &_data); + public: Vertex &AddScopeVertex(const std::string &_prefix, + const std::string &_name, const VertexType &_data); public: Vertex &AddVertex(const std::string &_name, const VertexType &); - public: Edge &AddEdge(const ignition::math::graph::VertexId_P &, const EdgeType &); - + public: Edge &AddEdge( + const ignition::math::graph::VertexId_P &, const EdgeType &); public: const ignition::math::graph::VertexRef_M Vertices() const; public: const ignition::math::graph::VertexRef_M Vertices( - const std::string &) const; + const std::string &) const; public: std::vector VertexNames() const; public: std::string VertexName(const VertexId &_id) const; public: std::string VertexName(const Vertex &_vertex) const; - - public: void UpdateEdge(const Edge &, const EdgeType&); - + public: void UpdateEdge(const Edge &, const EdgeType &); public: size_t Count(const std::string &_name) const; public: VertexId VertexIdByName(const std::string &_name) const; public: Vertex ScopeVertex() const; public: VertexId ScopeVertexId() const; - public: bool PointsTo(const std::shared_ptr _graph) const; public: void SetScopeName(const std::string &_name); public: const std::string &ScopeName() const; public: std::string AddPrefix(const std::string &_name) const; public: std::string RemovePrefix(const std::string &_name) const; - private: std::weak_ptr graphWeak; - - // private: std::unordered_set vertices; - private: VertexId scopeVertexId; - private: std::string prefix = {}; - private: std::string scopeName = {}; + private: std::shared_ptr dataPtr; }; ///////////////////////////////////////////////// template -ScopedGraph::ScopedGraph(const std::shared_ptr _graph): graphWeak(_graph) +ScopedGraph::ScopedGraph(const std::shared_ptr _graph) + : graphWeak(_graph) + , dataPtr(std::make_shared()) { } @@ -114,9 +115,10 @@ ScopedGraph ScopedGraph::ChildScope( const std::string &_name, const std::string &_scopeName) { auto newScopedGraph = *this; - newScopedGraph.scopeVertexId = newScopedGraph.VertexIdByName(_name); - newScopedGraph.prefix = this->AddPrefix(_name); - newScopedGraph.scopeName = _scopeName; + newScopedGraph.dataPtr = std::make_shared(); + newScopedGraph.dataPtr->scopeVertexId = this->VertexIdByName(_name); + newScopedGraph.dataPtr->prefix = this->AddPrefix(_name); + newScopedGraph.dataPtr->scopeName = _scopeName; return newScopedGraph; } @@ -124,7 +126,7 @@ ScopedGraph ScopedGraph::ChildScope( template ScopedGraph::operator bool() const { - return !this->graphWeak.expired(); + return !this->graphWeak.expired() && (this->dataPtr != nullptr); } ///////////////////////////////////////////////// @@ -146,9 +148,9 @@ template auto ScopedGraph::AddScopeVertex(const std::string &_prefix, const std::string &_name, const VertexType &_data) -> Vertex & { - this->prefix = this->AddPrefix(_prefix); + this->dataPtr->prefix = this->AddPrefix(_prefix); Vertex &vert = this->AddVertex(_name, _data); - this->scopeVertexId = vert.Id(); + this->dataPtr->scopeVertexId = vert.Id(); return vert; } @@ -167,7 +169,8 @@ auto ScopedGraph::AddVertex( ///////////////////////////////////////////////// template auto ScopedGraph::AddEdge( -const ignition::math::graph::VertexId_P &_vertexPair, const EdgeType &_data) -> Edge & + const ignition::math::graph::VertexId_P &_vertexPair, const EdgeType &_data) + -> Edge & { auto graph = graphWeak.lock(); Edge &edge = graph->graph.AddEdge(_vertexPair, _data); @@ -231,11 +234,18 @@ void ScopedGraph::UpdateEdge(const Edge &_edge, const EdgeType &_data) graph.AddEdge({tailVertexId, headVertexId}, _data); } +///////////////////////////////////////////////// +template +const std::string &ScopedGraph::ScopeName() const +{ + return this->dataPtr->scopeName; +} + ///////////////////////////////////////////////// template void ScopedGraph::SetScopeName(const std::string &_name) { - this->scopeName = _name; + this->dataPtr->scopeName = _name; } ///////////////////////////////////////////////// @@ -261,14 +271,15 @@ std::size_t ScopedGraph::VertexIdByName(const std::string &_name) const template auto ScopedGraph::ScopeVertex() const -> Vertex { - return this->graphWeak.lock()->graph.VertexFromId(this->scopeVertexId); + return this->graphWeak.lock()->graph.VertexFromId( + this->dataPtr->scopeVertexId); } ///////////////////////////////////////////////// template std::size_t ScopedGraph::ScopeVertexId() const { - return this->scopeVertexId; + return this->dataPtr->scopeVertexId; } ///////////////////////////////////////////////// @@ -278,24 +289,17 @@ bool ScopedGraph::PointsTo(const std::shared_ptr _graph) const return this->graphWeak.lock() == _graph; } -///////////////////////////////////////////////// -template -const std::string &ScopedGraph::ScopeName() const -{ - return this->scopeName; -} - ///////////////////////////////////////////////// template std::string ScopedGraph::AddPrefix(const std::string &_name) const { - if (this->prefix.empty()) + if (this->dataPtr->prefix.empty()) { return _name; } else { - return this->prefix + "::" + _name; + return this->dataPtr->prefix + "::" + _name; } } @@ -303,12 +307,12 @@ std::string ScopedGraph::AddPrefix(const std::string &_name) const template std::string ScopedGraph::RemovePrefix(const std::string &_name) const { - if (!this->prefix.empty()) + if (!this->dataPtr->prefix.empty()) { - auto ind = _name.find(this->prefix + "::"); + auto ind = _name.find(this->dataPtr->prefix + "::"); if (ind == 0) { - return _name.substr(this->prefix.size() + 2); + return _name.substr(this->dataPtr->prefix.size() + 2); } } return _name; diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index cb711c1b1..c4bff160d 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -564,9 +564,9 @@ TEST(DOMJoint, LoadInvalidChild) "found")); EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR); EXPECT_NE(std::string::npos, - errors[1].Message().find( - "FrameAttachedToGraph error, Non-LINK vertex with name [joint] is " - "disconnected")); + errors[1].Message().find("FrameAttachedToGraph error, Non-LINK vertex " + "with name [joint_invalid_child::joint] is " + "disconnected")); // errors[2] // errors[3] // errors[4] diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 0b66b713c..19a959926 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -70,7 +70,7 @@ TEST(DOMModel, NoLinks) sdf::Root root; auto errors = root.Load(testFile); EXPECT_FALSE(errors.empty()); - ASSERT_EQ(4u, errors.size()); + ASSERT_EQ(5u, errors.size()); EXPECT_EQ(sdf::ErrorCode::MODEL_WITHOUT_LINK, errors[0].Code()); EXPECT_TRUE(errors[0].Message().find("model must have at least one link") != std::string::npos); @@ -79,6 +79,7 @@ TEST(DOMModel, NoLinks) std::string::npos); // errors[2] // errors[3] + // errors[4] } ///////////////////////////////////////////////// From 4cefd7936c991881b6d55f09f4136c3ce173cb56 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 6 Oct 2020 13:19:41 -0500 Subject: [PATCH 15/70] Cleanup Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 68 ++++++++++++++--------------------- test/integration/model_dom.cc | 4 +-- 2 files changed, 28 insertions(+), 44 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 4b15be7d2..b5a21d4c5 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -209,41 +209,6 @@ Errors buildFrameAttachedToGraph( { Errors errors; - auto frameType = - _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; - - auto rootId = ignition::math::graph::kNullId; - auto modelId = ignition::math::graph::kNullId; - auto modelFrameId = ignition::math::graph::kNullId; - // TODO(addisu) - // ScopedGraph outModel = _out; - ScopedGraph outModel; - if (_root) - { - rootId = - _out.AddScopeVertex("", "__root__", sdf::FrameType::STATIC_MODEL).Id(); - const std::string scopeName = "__model__"; - _out.SetScopeName(scopeName); - modelId = _out.AddVertex(_model->Name(), frameType).Id(); - - outModel = _out.ChildScope(_model->Name(), "__model__"); - // modelFrameId = outModel.AddScopeVertex(_model->Name(), "__model__", sdf::FrameType::MODEL).Id(); - modelFrameId = outModel.AddVertex("__model__", frameType).Id(); - } - else - { - outModel = _out; - const std::string scopeName = "__model__"; - // TODO (addisu) May not need SetScopeName - outModel.SetScopeName(scopeName); - modelFrameId = _out.AddVertex(scopeName, frameType).Id(); - } - - if (_root) - { - outModel.AddEdge({modelId, modelFrameId}, true); - } - const std::string scopeName = "__model__"; if (!_model) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -264,6 +229,26 @@ Errors buildFrameAttachedToGraph( return errors; } + auto frameType = + _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; + + auto modelId = ignition::math::graph::kNullId; + ScopedGraph outModel = _out; + if (_root) + { + _out.AddScopeVertex("", "__root__", sdf::FrameType::STATIC_MODEL); + _out.SetScopeName("__model__"); + modelId = _out.AddVertex(_model->Name(), frameType).Id(); + outModel = _out.ChildScope(_model->Name(), "__model__"); + } + + const std::string scopeName = "__model__"; + const auto modelFrameId = outModel.AddVertex(scopeName, frameType).Id(); + + if (_root) + { + outModel.AddEdge({modelId, modelFrameId}, true); + } // identify canonical link, which may be nested auto canonicalLinkAndName = modelCanonicalLinkAndRelativeName(_model); const sdf::Link *canonicalLink = canonicalLinkAndName.first; @@ -480,11 +465,6 @@ Errors buildFrameAttachedToGraph( { Errors errors; - // add implicit world frame vertex first - const std::string scopeName = "world"; - _out.SetScopeName(scopeName); - _out.AddScopeVertex("", scopeName, sdf::FrameType::WORLD).Id(); - if (!_world) { errors.push_back({ErrorCode::ELEMENT_INVALID, @@ -498,6 +478,11 @@ Errors buildFrameAttachedToGraph( return errors; } + // add implicit world frame vertex first + const std::string scopeName = "world"; + _out.SetScopeName(scopeName); + _out.AddScopeVertex("", scopeName, sdf::FrameType::WORLD).Id(); + // add model vertices for (uint64_t m = 0; m < _world->ModelCount(); ++m) { @@ -1545,7 +1530,8 @@ Errors resolveFrameAttachedToBody( { Errors errors; - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && _in.ScopeName() != "__root__") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && + _in.ScopeName() != "__root__") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] " diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 19a959926..ec1045e57 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -70,7 +70,7 @@ TEST(DOMModel, NoLinks) sdf::Root root; auto errors = root.Load(testFile); EXPECT_FALSE(errors.empty()); - ASSERT_EQ(5u, errors.size()); + ASSERT_EQ(3u, errors.size()); EXPECT_EQ(sdf::ErrorCode::MODEL_WITHOUT_LINK, errors[0].Code()); EXPECT_TRUE(errors[0].Message().find("model must have at least one link") != std::string::npos); @@ -78,8 +78,6 @@ TEST(DOMModel, NoLinks) EXPECT_TRUE(errors[1].Message().find("model must have at least one link") != std::string::npos); // errors[2] - // errors[3] - // errors[4] } ///////////////////////////////////////////////// From 446a5366599ec30af8c7b403b252f4e4796fe536 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 15:39:22 -0500 Subject: [PATCH 16/70] Remove `__root__` scope name The `__root__` vertex still exits, but has either a `__model__` scope or a `world` scope. Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 244 +++++++++------------------------- src/Model.cc | 10 +- src/ScopedGraph.hh | 9 +- test/integration/model_dom.cc | 2 +- 4 files changed, 74 insertions(+), 191 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index b5a21d4c5..7a6a791ea 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -232,23 +232,23 @@ Errors buildFrameAttachedToGraph( auto frameType = _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; - auto modelId = ignition::math::graph::kNullId; - ScopedGraph outModel = _out; + const std::string scopeName = "__model__"; + + auto rootId = ignition::math::graph::kNullId; if (_root) { - _out.AddScopeVertex("", "__root__", sdf::FrameType::STATIC_MODEL); - _out.SetScopeName("__model__"); - modelId = _out.AddVertex(_model->Name(), frameType).Id(); - outModel = _out.ChildScope(_model->Name(), "__model__"); + rootId = _out.AddScopeVertex( + "", "__root__", scopeName, sdf::FrameType::STATIC_MODEL).Id(); } - const std::string scopeName = "__model__"; + const auto modelId = _out.AddVertex(_model->Name(), frameType).Id(); + + auto outModel = _out.ChildScope(_model->Name(), "__model__"); const auto modelFrameId = outModel.AddVertex(scopeName, frameType).Id(); - if (_root) - { - outModel.AddEdge({modelId, modelFrameId}, true); - } + auto &edge = outModel.AddEdge({modelId, modelFrameId}, true); + edge.SetWeight(0); + // identify canonical link, which may be nested auto canonicalLinkAndName = modelCanonicalLinkAndRelativeName(_model); const sdf::Link *canonicalLink = canonicalLinkAndName.first; @@ -282,12 +282,6 @@ Errors buildFrameAttachedToGraph( continue; } outModel.AddVertex(link->Name(), sdf::FrameType::LINK); - - // // add edge from implicit model frame vertex to canonical link - // if (!_model->Static() && link == canonicalLink) - // { - // outModel.AddEdge({modelFrameId, linkId}, true); - // } } // add joint vertices @@ -332,26 +326,8 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto modelFrameType = - _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; - auto nestedModelId = - outModel.AddVertex(nestedModel->Name(), modelFrameType).Id(); - ScopedGraph childGraph = - outModel.ChildScope(nestedModel->Name(), "__model__"); - auto nestedErrors = - buildFrameAttachedToGraph(childGraph, nestedModel, false); + auto nestedErrors = buildFrameAttachedToGraph(outModel, nestedModel, false); errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); - auto scopedNestedModelId = - outModel.VertexIdByName(nestedModel->Name() + "::__model__"); - if (scopedNestedModelId == ignition::math::graph::kNullId) - { - // TODO (addisu) ERROR - } - else - { - auto &edge = outModel.AddEdge({nestedModelId, scopedNestedModelId}, true); - edge.SetWeight(0); - } } // add edges from joint to child frames @@ -372,18 +348,6 @@ Errors buildFrameAttachedToGraph( outModel.AddEdge({jointId, childFrameId}, true); } - // add model edges to their corresponding vertices in the model scope - // for (uint64_t m = 0; m < _model->ModelCount(); ++m) - // { - // auto model = _model->ModelByIndex(m); - // auto nestedModelId = outModel.VertexIdByName(model->Name()); - // auto modelIdChildScope = outModel.VertexIdByName(model->Name() + "::__model__"); - // if (modelIdChildScope != ignition::math::graph::kNullId) - // { - // outModel.AddEdge({nestedModelId, modelIdChildScope}, true); - // } - // } - // add frame edges for (uint64_t f = 0; f < _model->FrameCount(); ++f) { @@ -437,24 +401,15 @@ Errors buildFrameAttachedToGraph( // If the model is static, add an edge from the model frame to the global // root vertex // TODO (addisu) Ensure that the global root vertex has an ID of 0. - // outModel.AddEdge({modelFrameId, 0}, true); - } - // add edge from implicit model frame vertex to canonical link - // if (!_model->Static() && canonicalLink == nullptr) - // { - // for (uint64_t m = 0; m < _model->ModelCount(); ++m) - // { - // sdf::Errors sinkErrors; - // auto vertexId = outModel.VertexIdByName(_model->ModelByIndex(m)->Name()); - // auto sinkVertexEdges = FindSinkVertex(outModel, vertexId, sinkErrors); - // if (sinkErrors.empty()) - // { - // auto sinkVertex = sinkVertexEdges.first; - // outModel.AddEdge({modelFrameId, vertexId}, true); - // break; - // } - // } - // } + // if (_root) + // { + // outModel.AddEdge({modelFrameId, rootId}, true); + // } + // else + // { + // outModel.AddEdge({modelFrameId, 0}, true); + // } + } return errors; } @@ -480,8 +435,7 @@ Errors buildFrameAttachedToGraph( // add implicit world frame vertex first const std::string scopeName = "world"; - _out.SetScopeName(scopeName); - _out.AddScopeVertex("", scopeName, sdf::FrameType::WORLD).Id(); + _out.AddScopeVertex("", scopeName, scopeName, sdf::FrameType::WORLD); // add model vertices for (uint64_t m = 0; m < _world->ModelCount(); ++m) @@ -495,12 +449,7 @@ Errors buildFrameAttachedToGraph( "]."}); continue; } - auto modelFrameType = - model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; - _out.AddVertex(model->Name(), modelFrameType).Id(); - ScopedGraph childGraph = - _out.ChildScope(model->Name(), "__model__"); - auto modelErrors = buildFrameAttachedToGraph(childGraph, model, false); + auto modelErrors = buildFrameAttachedToGraph(_out, model, false); errors.insert(errors.end(), modelErrors.begin(), modelErrors.end()); } @@ -519,18 +468,6 @@ Errors buildFrameAttachedToGraph( _out.AddVertex(frame->Name(), sdf::FrameType::FRAME).Id(); } - // add model edges to their corresponding vertices in the model scope - for (uint64_t m = 0; m < _world->ModelCount(); ++m) - { - auto model = _world->ModelByIndex(m); - auto modelId = _out.VertexIdByName(model->Name()); - auto modelIdChildScope = _out.VertexIdByName(model->Name() + "::__model__"); - if (modelIdChildScope != ignition::math::graph::kNullId) - { - _out.AddEdge({modelId, modelIdChildScope}, true); - } - } - // add frame edges for (uint64_t f = 0; f < _world->FrameCount(); ++f) { @@ -596,37 +533,25 @@ Errors buildPoseRelativeToGraph( return errors; } - auto rootId = ignition::math::graph::kNullId; - auto modelId = ignition::math::graph::kNullId; - auto modelFrameId = ignition::math::graph::kNullId; + const std::string scopeName = "__model__"; + auto rootId = ignition::math::graph::kNullId; // add the model frame vertex first - // TODO(addisu) - // ScopedGraph outModel = _out; - ScopedGraph outModel; if (_root) { - rootId = _out.AddScopeVertex("", "__root__", sdf::FrameType::ROOT).Id(); - const std::string scopeName = "__root__"; - _out.SetScopeName(scopeName); - modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); - - outModel = _out.ChildScope(_model->Name(), "__model__"); - // modelFrameId = outModel.AddScopeVertex(_model->Name(), "__model__", sdf::FrameType::MODEL).Id(); - modelFrameId = outModel.AddVertex("__model__", sdf::FrameType::MODEL).Id(); - } - else - { - outModel = _out; - const std::string scopeName = "__model__"; - // TODO (addisu) May not need SetScopeName - outModel.SetScopeName(scopeName); - modelFrameId = outModel.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); + rootId = _out.AddScopeVertex( + "", "__root__", scopeName, sdf::FrameType::STATIC_MODEL) + .Id(); } + auto modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); + + auto outModel = _out.ChildScope(_model->Name(), scopeName); + auto modelFrameId = outModel.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); + // Add an aliasing edge from the model vertex to the + // corresponding vertex in the PoseRelativeTo graph of the child model, + // i.e, to the ::__model__ vertex + auto &edge = _out.AddEdge({modelId, modelFrameId}, {{}, false}); + edge.SetWeight(0); - if (_root) - { - outModel.AddEdge({modelId, modelFrameId}, {}); - } // add link vertices and default edge if relative_to is empty for (uint64_t l = 0; l < _model->LinkCount(); ++l) { @@ -699,28 +624,9 @@ Errors buildPoseRelativeToGraph( "]."}); continue; } - auto nestedModelId = - outModel.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id(); - ScopedGraph childGraph = - outModel.ChildScope(nestedModel->Name(), "__model__"); - auto nestedErrors = buildPoseRelativeToGraph(childGraph, nestedModel, false); + auto nestedErrors = buildPoseRelativeToGraph(outModel, nestedModel, false); errors.insert(errors.end(), nestedErrors.begin(), nestedErrors.end()); - // Add an aliasing edge from the nestedModel vertex to the - // corresponding vertex in the PoseRelativeTo graph of the nested model, - // i.e, to the ::__model__ vertex - auto scopedNestedModelId = - outModel.VertexIdByName(nestedModel->Name() + "::__model__"); - if (scopedNestedModelId == ignition::math::graph::kNullId) - { - // TODO (addisu) ERROR - } - else - { - auto &edge = - outModel.AddEdge({nestedModelId, scopedNestedModelId}, {{}, false}); - edge.SetWeight(0); - } } // now that all vertices have been added to the graph, @@ -731,7 +637,7 @@ Errors buildPoseRelativeToGraph( auto link = _model->LinkByIndex(l); // check if we've already added a default edge - const std::string relativeTo = link->PoseRelativeTo(); + const std::string &relativeTo = link->PoseRelativeTo(); if (relativeTo.empty()) { continue; @@ -968,12 +874,12 @@ Errors buildPoseRelativeToGraph( return errors; } - auto rootId = _out.AddVertex("__root__", sdf::FrameType::ROOT).Id(); + auto rootId = _out.AddVertex("__root__", sdf::FrameType::WORLD).Id(); // add implicit world frame vertex first const std::string sourceName = "world"; - _out.SetScopeName(sourceName); auto worldFrameId = - _out.AddScopeVertex("", sourceName, sdf::FrameType::WORLD).Id(); + _out.AddScopeVertex("", sourceName, sourceName, sdf::FrameType::WORLD) + .Id(); _out.AddEdge({rootId, worldFrameId}, {}); // add model vertices and default edge if relative_to is empty @@ -988,28 +894,9 @@ Errors buildPoseRelativeToGraph( "]."}); continue; } - auto modelId = - _out.AddVertex(model->Name(), sdf::FrameType::MODEL).Id(); - ScopedGraph childGraph = - _out.ChildScope(model->Name(), "__model__"); - auto modelErrors = buildPoseRelativeToGraph(childGraph , model, false); + auto modelErrors = buildPoseRelativeToGraph(_out , model, false); errors.insert(errors.end(), modelErrors .begin(), modelErrors .end()); - - // Add an aliasing edge from the model vertex to the - // corresponding vertex in the PoseRelativeTo graph of the child model, - // i.e, to the ::__model__ vertex - auto scopedModelId = - _out.VertexIdByName(model->Name() + "::__model__"); - if (scopedModelId == ignition::math::graph::kNullId) - { - // TODO (addisu) ERROR - } - else - { - auto &edge = _out.AddEdge({modelId, scopedModelId}, {{}, false}); - edge.SetWeight(0); - } } // add frame vertices and default edge if both @@ -1163,8 +1050,7 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in Errors errors; // Expect scopeName to be either "__model__" or "world" - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && - _in.ScopeName() != "__root__") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] " @@ -1185,8 +1071,8 @@ Errors validateFrameAttachedToGraph(const ScopedGraph &_in sdf::FrameType scopeFrameType = scopeVertex.Data(); if (_in.ScopeName() == "__model__" && - (scopeFrameType != sdf::FrameType::MODEL && - scopeFrameType != sdf::FrameType::STATIC_MODEL)) + scopeFrameType != sdf::FrameType::MODEL && + scopeFrameType != sdf::FrameType::STATIC_MODEL) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " @@ -1345,8 +1231,7 @@ Errors validatePoseRelativeToGraph( Errors errors; // Expect scopeName to be either "__model__" or "world" - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && - _in.ScopeName() != "__root__") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: source vertex name " + _in.ScopeName() + @@ -1356,25 +1241,19 @@ Errors validatePoseRelativeToGraph( // Expect one vertex with name "__model__" and FrameType MODEL // or with name "world" and FrameType WORLD - auto sourceVertices = _in.Vertices(_in.ScopeName()); - if (sourceVertices.empty()) + auto sourceVertex = _in.ScopeVertex(); + if (!sourceVertex.Valid()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: source frame[" + _in.ScopeName() + "] not found in graph."}); return errors; } - else if (sourceVertices.size() > 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error, " - "more than one vertex with source name " + _in.ScopeName()}); - return errors; - } - auto sourceVertex = sourceVertices.begin()->second.get(); sdf::FrameType sourceFrameType = sourceVertex.Data(); - if (_in.ScopeName() == "__model__" && sourceFrameType != sdf::FrameType::MODEL) + if (_in.ScopeName() == "__model__" && + sourceFrameType != sdf::FrameType::MODEL && + sourceFrameType != sdf::FrameType::STATIC_MODEL) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " @@ -1530,8 +1409,7 @@ Errors resolveFrameAttachedToBody( { Errors errors; - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world" && - _in.ScopeName() != "__root__") + if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] " @@ -1667,13 +1545,17 @@ Errors resolvePose( { Errors errors = resolvePoseRelativeToRoot(_pose, _graph, _frameName); - ignition::math::Pose3d poseR; - Errors errorsR = resolvePoseRelativeToRoot(poseR, _graph, _resolveTo); - errors.insert(errors.end(), errorsR.begin(), errorsR.end()); - - if (errors.empty()) + // If the resolveTo is empty, we're resolving to the Root, so we're done + if (!_resolveTo.empty()) { - _pose = poseR.Inverse() * _pose; + ignition::math::Pose3d poseR; + Errors errorsR = resolvePoseRelativeToRoot(poseR, _graph, _resolveTo); + errors.insert(errors.end(), errorsR.begin(), errorsR.end()); + + if (errors.empty()) + { + _pose = poseR.Inverse() * _pose; + } } return errors; diff --git a/src/Model.cc b/src/Model.cc index dc02ea6b6..38c0e3388 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -815,16 +815,14 @@ void Model::SetFrameAttachedToGraph( ///////////////////////////////////////////////// sdf::SemanticPose Model::SemanticPose() const { - std::string scopeName = ""; - if (this->dataPtr->poseGraph) - { - scopeName = this->dataPtr->poseGraph.ScopeName(); - } + // Pass an empty string for the defaultResolveTo argument so that it can + // resolve to the scope vertex regardless of whether this model is a + // standalone model or part of a world. return sdf::SemanticPose( this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, - scopeName, + "", this->dataPtr->poseGraph); } diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index bbb78229f..bee2e8e8d 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -77,7 +77,8 @@ class ScopedGraph public: const MapType &Map() const; // May not be needed if they can be called via Graph() public: Vertex &AddScopeVertex(const std::string &_prefix, - const std::string &_name, const VertexType &_data); + const std::string &_name, const std::string &_scopeName, + const VertexType &_data); public: Vertex &AddVertex(const std::string &_name, const VertexType &); public: Edge &AddEdge( const ignition::math::graph::VertexId_P &, const EdgeType &); @@ -146,11 +147,13 @@ auto ScopedGraph::Map() const -> const MapType & ///////////////////////////////////////////////// template auto ScopedGraph::AddScopeVertex(const std::string &_prefix, - const std::string &_name, const VertexType &_data) -> Vertex & + const std::string &_name, const std::string &_scopeName, + const VertexType &_data) -> Vertex & { this->dataPtr->prefix = this->AddPrefix(_prefix); Vertex &vert = this->AddVertex(_name, _data); this->dataPtr->scopeVertexId = vert.Id(); + this->SetScopeName(_scopeName); return vert; } @@ -159,7 +162,7 @@ template auto ScopedGraph::AddVertex( const std::string &_name, const VertexType &_data) -> Vertex & { - auto graph = graphWeak.lock(); + auto graph = this->graphWeak.lock(); const std::string newName = this->AddPrefix(_name); Vertex &vert = graph->graph.AddVertex(newName, _data); graph->map[newName] = vert.Id(); diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index ec1045e57..30794b040 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -155,7 +155,7 @@ TEST(DOMRoot, NestedModel) // Load the SDF file sdf::Root root; auto errors = root.Load(testFile); - EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_EQ(1u, root.ModelCount()); From 023aea5544487e59b84dca035f71eb5cdf8a8d6c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 18:52:23 -0500 Subject: [PATCH 17/70] SemanticPose::Resolve and JointAxis::ResolveXyz to a SemanticPose object to set the scope Signed-off-by: Addisu Z. Taddese --- include/sdf/JointAxis.hh | 12 ++++++ include/sdf/SemanticPose.hh | 7 ++++ src/FrameSemantics.cc | 69 ++++++++++++++++++++++++++++++++ src/FrameSemantics.hh | 11 +++++ src/JointAxis.cc | 55 +++++++++++++++++++++++++ src/SemanticPose.cc | 50 +++++++++++++++++------ test/integration/nested_model.cc | 54 ++++++++++++++++--------- 7 files changed, 227 insertions(+), 31 deletions(-) diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index 1690da9f6..cc3074cce 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -34,6 +34,7 @@ namespace sdf // Forward declare private data class. class JointAxisPrivate; + class SemanticPose; struct PoseRelativeToGraph; template class ScopedGraph; @@ -258,6 +259,17 @@ namespace sdf ignition::math::Vector3d &_xyz, const std::string &_resolveTo = "") const; + public: Errors ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::SemanticPose &_scope, + const std::string &_resolveTo) const; + + // private: Errors ResolveXyzImpl( + // ignition::math::Vector3d &_xyz, + // sdf::ScopedGraph _graph, + // const std::string &_expressedIn, + // const std::string &_resolveTo) const; + /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index a93324e49..9ab7f0213 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -73,6 +73,10 @@ namespace sdf public: Errors Resolve(ignition::math::Pose3d &_pose, const std::string &_resolveTo = "") const; + public: Errors Resolve(ignition::math::Pose3d &_pose, + const SemanticPose &_scope, + const std::string &_resolveTo) const; + /// \brief Private constructor. /// \param[in] _pose Raw pose of object. /// \param[in] _relativeTo Name of frame in graph relative-to which the @@ -93,6 +97,8 @@ namespace sdf const std::string &_defaultResolveTo, const sdf::ScopedGraph &_graph); + private: const sdf::ScopedGraph & + PoseRelativeToGraph() const; /// \brief Destructor public: ~SemanticPose(); @@ -122,6 +128,7 @@ namespace sdf friend class Model; friend class Sensor; friend class Visual; + friend class JointAxis; /// \brief Private data pointer. private: std::unique_ptr dataPtr; diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 7a6a791ea..b8e5a2069 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -1535,6 +1535,75 @@ Errors resolvePoseRelativeToRoot( return errors; } +///////////////////////////////////////////////// +Errors resolvePoseRelativeToRoot( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_vertexId) +{ + Errors errors; + + auto incomingVertexEdges = FindSourceVertex(_graph, _vertexId, errors); + + if (!errors.empty()) + { + return errors; + } + else if (!incomingVertexEdges.first.Valid()) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph unable to find path to source vertex " + "when starting from vertex with id [" + std::to_string(_vertexId) + "]."}); + return errors; + } + else if (incomingVertexEdges.first.Id() != _graph.ScopeVertex().Id()) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph frame with name [" + std::to_string(_vertexId) + "] " + "is disconnected; its source vertex has name [" + + incomingVertexEdges.first.Name() + + "], but its source name should be " + _graph.ScopeName() + "."}); + return errors; + } + + ignition::math::Pose3d pose; + for (auto const &edge : incomingVertexEdges.second) + { + pose = edge.Data().pose * pose; + } + + if (errors.empty()) + { + _pose = pose; + } + + return errors; +} + +///////////////////////////////////////////////// +Errors resolvePose(ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_frameVertexId, + const ignition::math::graph::VertexId &_resolveToVertexId) +{ + Errors errors = resolvePoseRelativeToRoot(_pose, _graph, _frameVertexId); + + // If the resolveTo is empty, we're resolving to the Root, so we're done + if (_resolveToVertexId != ignition::math::graph::kNullId) + { + ignition::math::Pose3d poseR; + Errors errorsR = + resolvePoseRelativeToRoot(poseR, _graph, _resolveToVertexId); + errors.insert(errors.end(), errorsR.begin(), errorsR.end()); + + if (errors.empty()) + { + _pose = poseR.Inverse() * _pose; + } + } + + return errors; +} ///////////////////////////////////////////////// Errors resolvePose( diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 5924a291e..08e7250af 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -193,6 +193,11 @@ namespace sdf const ScopedGraph &_graph, const std::string &_vertexName); + Errors resolvePoseRelativeToRoot( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_vertexId); + /// \brief Resolve pose of a frame relative to named frame. /// \param[out] _pose Pose object to write. /// \param[in] _graph PoseRelativeToGraph to read from. @@ -206,6 +211,12 @@ namespace sdf const std::string &_frameName, const std::string &_resolveTo); + Errors resolvePose( + ignition::math::Pose3d &_pose, + const ScopedGraph &_graph, + const ignition::math::graph::VertexId &_frameVertexId, + const ignition::math::graph::VertexId &_resolveToVertexId); + /// \brief Update the pose of a frame in the pose graph. This updates the /// content of the edge incident to the vertex identified by the given /// _frameName. diff --git a/src/JointAxis.cc b/src/JointAxis.cc index 4836228d0..5e74dbd2c 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -23,6 +23,8 @@ #include "sdf/Assert.hh" #include "sdf/Error.hh" #include "sdf/JointAxis.hh" +#include "sdf/SemanticPose.hh" + #include "FrameSemantics.hh" #include "ScopedGraph.hh" @@ -428,6 +430,59 @@ Errors JointAxis::ResolveXyz( return errors; } +///////////////////////////////////////////////// +Errors JointAxis::ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::SemanticPose &_scope, + const std::string &_resolveTo) const +{ + Errors errors; + const auto &graph = this->dataPtr->poseRelativeToGraph; + const auto &scopeGraph = _scope.PoseRelativeToGraph(); + if (!graph || !scopeGraph) + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "JointAxis has invalid pointer to PoseRelativeToGraph."}); + return errors; + } + if (this->dataPtr->xmlParentName.empty()) + { + errors.push_back({ErrorCode::ELEMENT_INVALID, + "JointAxis has invalid name of xml parent object."}); + return errors; + } + + // JointAxis is not in the graph, but its XyzExpressedIn() name should be. + // If XyzExpressedIn() is empty, use the name of the xml parent object. + std::string axisExpressedIn = this->XyzExpressedIn(); + if (axisExpressedIn.empty()) + { + axisExpressedIn = this->dataPtr->xmlParentName; + } + + auto axisExpressedInVertexId = graph.VertexIdByName(axisExpressedIn); + + const ignition::math::graph::VertexId resolveToVertexId = [&] + { + if (_resolveTo.empty()) + { + return graph.VertexIdByName(this->dataPtr->xmlParentName); + } + return scopeGraph.VertexIdByName(_resolveTo); + }(); + + ignition::math::Pose3d pose; + errors = + resolvePose(pose, scopeGraph, axisExpressedInVertexId, resolveToVertexId); + + if (errors.empty()) + { + _xyz = pose.Rot() * this->Xyz(); + } + + return errors; +} + ///////////////////////////////////////////////// sdf::ElementPtr JointAxis::Element() const { diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index 37a617b5f..4e9a4dc5b 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -103,6 +103,13 @@ SemanticPose &SemanticPose::operator=(const SemanticPose &_semanticPose) return *this; } +///////////////////////////////////////////////// +const sdf::ScopedGraph & +SemanticPose::PoseRelativeToGraph() const +{ + return this->dataPtr->poseRelativeToGraph; +} + ///////////////////////////////////////////////// const ignition::math::Pose3d &SemanticPose::RawPose() const { @@ -114,43 +121,60 @@ const std::string &SemanticPose::RelativeTo() const { return this->dataPtr->relativeTo; } +///////////////////////////////////////////////// +Errors SemanticPose::Resolve( + ignition::math::Pose3d &_pose, + const std::string &_resolveTo) const +{ + return this->Resolve(_pose, *this, _resolveTo); +} ///////////////////////////////////////////////// Errors SemanticPose::Resolve( ignition::math::Pose3d &_pose, + const SemanticPose &_scope, const std::string &_resolveTo) const { Errors errors; auto graph = this->dataPtr->poseRelativeToGraph; - if (!graph) + const auto &scopeGraph = _scope.PoseRelativeToGraph(); + if (!graph || !scopeGraph) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "SemanticPose has invalid pointer to PoseRelativeToGraph."}); return errors; } - std::string relativeTo = this->dataPtr->relativeTo; - if (relativeTo.empty()) + const ignition::math::graph::VertexId resolveToVertexId = [&] { - relativeTo = this->dataPtr->defaultResolveTo; - } - - std::string resolveTo = _resolveTo; - if (resolveTo.empty()) - { - resolveTo = this->dataPtr->defaultResolveTo; - } + if (_resolveTo.empty()) + { + if (this->dataPtr->defaultResolveTo.empty()) + return graph.ScopeVertex().Id(); + else + return graph.VertexIdByName(this->dataPtr->defaultResolveTo); + } + return scopeGraph.VertexIdByName(_resolveTo); + }(); ignition::math::Pose3d pose; if (this->dataPtr->name.empty()) { - errors = resolvePose(pose, graph, relativeTo, resolveTo); + std::string relativeTo = this->dataPtr->relativeTo; + if (relativeTo.empty()) + { + relativeTo = this->dataPtr->defaultResolveTo; + } + auto relativeToVertexId = graph.VertexIdByName(relativeTo); + errors = + resolvePose(pose, scopeGraph, relativeToVertexId, resolveToVertexId); pose *= this->RawPose(); } else { - errors = resolvePose(pose, graph, this->dataPtr->name, resolveTo); + const auto frameVertexId = graph.VertexIdByName(this->dataPtr->name); + errors = resolvePose(pose, scopeGraph, frameVertexId, resolveToVertexId); } if (errors.empty()) diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 78afdd8ad..86df81a8a 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -15,6 +15,7 @@ * */ +#include #include #include #include @@ -427,7 +428,6 @@ TEST(NestedModel, NestedModelWithFrames) << "" << "" << " " - << " " << " " << " " + modelPath + "" << " M1" @@ -453,8 +453,8 @@ TEST(NestedModel, NestedModelWithFrames) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); - const sdf::Frame *modelFrame = parentModel->FrameByIndex(0); - ASSERT_NE(nullptr, modelFrame ); + const sdf::Model *childModel = parentModel->ModelByIndex(0); + ASSERT_NE(nullptr, childModel); using ignition::math::Pose3d; using ignition::math::Vector3d; @@ -467,19 +467,31 @@ TEST(NestedModel, NestedModelWithFrames) Vector3d joint1AxisExpVector = frame2ExpPose.Rot() * Vector3d::UnitZ; Vector3d joint1Axis2ExpVector = frame2ExpPose.Rot() * Vector3d::UnitX; + const auto *frame1 = parentModel->FrameByName("M1::F1"); + ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(modelFrame->SemanticPose().Resolve(frame1Pose, "M1::F1").empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); + EXPECT_TRUE( + frame1->SemanticPose() + .Resolve(frame1Pose, parentModel->SemanticPose(), parentModel->Name()) + .empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose); + + const auto *frame2 = childModel->FrameByName("F2"); + ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(modelFrame->SemanticPose().Resolve(frame2Pose, "M1::F2").empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); + EXPECT_TRUE(frame2->SemanticPose() + .Resolve(frame2Pose, childModel->SemanticPose(), "__model__") + .empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose); - const auto *link1 = parentModel->LinkByName("M1::L1"); + const auto *link1 = childModel->LinkByName("L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(modelFrame->SemanticPose().Resolve(link1Pose, "M1::L1").empty()); - EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); + EXPECT_TRUE(link1->SemanticPose() + .Resolve(link1Pose, childModel->SemanticPose(), "__model__") + .empty()); + EXPECT_EQ(link1ExpPose, link1Pose); const auto *visual1 = link1->VisualByName("V1"); ASSERT_NE(nullptr, visual1); @@ -490,26 +502,32 @@ TEST(NestedModel, NestedModelWithFrames) EXPECT_EQ("__model__", collision1->PoseRelativeTo()); Pose3d link2Pose; - EXPECT_TRUE(modelFrame->SemanticPose().Resolve(link2Pose, "M1::L2").empty()); + EXPECT_TRUE(parentModel->SemanticPose() + .Resolve(link2Pose, "ParentModel::M1::L2") + .empty()); EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(modelFrame->SemanticPose().Resolve(joint1Pose, "M1::J1").empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); + EXPECT_TRUE(joint1->SemanticPose() + .Resolve(joint1Pose, childModel->SemanticPose(), "__model__") + .empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose); const auto joint1Axis = joint1->Axis(0); ASSERT_NE(nullptr, joint1Axis); Vector3d joint1AxisVector; - EXPECT_TRUE(joint1Axis->ResolveXyz(joint1AxisVector, "__model__").empty()); - EXPECT_EQ(joint1AxisExpVector, model1Pose.Rot() * joint1AxisVector); - + EXPECT_TRUE(joint1Axis->ResolveXyz( + joint1AxisVector, childModel->SemanticPose(), "__model__").empty()); + EXPECT_EQ(joint1AxisExpVector, joint1AxisVector); const auto joint1Axis2 = joint1->Axis(1); ASSERT_NE(nullptr, joint1Axis2); Vector3d joint1Axis2Vector; - EXPECT_TRUE(joint1Axis2->ResolveXyz(joint1Axis2Vector, "__model__").empty()); - EXPECT_EQ(joint1Axis2ExpVector, model1Pose.Rot() * joint1Axis2Vector); + EXPECT_TRUE(joint1Axis2 + ->ResolveXyz(joint1Axis2Vector, parentModel->SemanticPose(), + parentModel->Name()).empty()); + EXPECT_EQ(joint1Axis2ExpVector, joint1Axis2Vector); } // Function to remove any element in //joint/axis that is not From 71e24bb94d3f86b0ff76db0e8177dadc354217b5 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 20:02:22 -0500 Subject: [PATCH 18/70] Add missing file Signed-off-by: Addisu Z. Taddese --- test/sdf/nested_model_cross_references.sdf | 34 ++++++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 test/sdf/nested_model_cross_references.sdf diff --git a/test/sdf/nested_model_cross_references.sdf b/test/sdf/nested_model_cross_references.sdf new file mode 100644 index 000000000..2590f650a --- /dev/null +++ b/test/sdf/nested_model_cross_references.sdf @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From f97e00cc971f712a21b6055d0c3e1d2c5857cf40 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 20:02:38 -0500 Subject: [PATCH 19/70] Remove aliasing edges, there is no need for them. Also remove updateGraphPose Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 65 +++-------------------------- src/FrameSemantics.hh | 27 +----------- src/FrameSemantics_TEST.cc | 85 -------------------------------------- 3 files changed, 6 insertions(+), 171 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index b8e5a2069..0ba005b8e 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -549,8 +549,8 @@ Errors buildPoseRelativeToGraph( // Add an aliasing edge from the model vertex to the // corresponding vertex in the PoseRelativeTo graph of the child model, // i.e, to the ::__model__ vertex - auto &edge = _out.AddEdge({modelId, modelFrameId}, {{}, false}); - edge.SetWeight(0); + // auto &edge = _out.AddEdge({modelId, modelFrameId}, {{}, false}); + _out.AddEdge({modelId, modelFrameId}, {}); // add link vertices and default edge if relative_to is empty for (uint64_t l = 0; l < _model->LinkCount(); ++l) @@ -1286,10 +1286,7 @@ Errors validatePoseRelativeToGraph( continue; } - auto incomingEdges = _in.Graph().IncidentsTo(vertexPair.first); - std::size_t inDegree = std::count_if(incomingEdges.begin(), - incomingEdges.end(), - [](const auto &_edge) { return !_edge.second.get().Data().aliasing; }); + std::size_t inDegree = _in.Graph().InDegree(vertexPair.first); if (inDegree > 1) { @@ -1525,7 +1522,7 @@ Errors resolvePoseRelativeToRoot( ignition::math::Pose3d pose; for (auto const &edge : incomingVertexEdges.second) { - pose = edge.Data().pose * pose; + pose = edge.Data() * pose; } if (errors.empty()) @@ -1569,7 +1566,7 @@ Errors resolvePoseRelativeToRoot( ignition::math::Pose3d pose; for (auto const &edge : incomingVertexEdges.second) { - pose = edge.Data().pose * pose; + pose = edge.Data() * pose; } if (errors.empty()) @@ -1630,57 +1627,5 @@ Errors resolvePose( return errors; } -///////////////////////////////////////////////// -Errors updateGraphPose( - ScopedGraph &_graph, - const std::string &_frameName, - const ignition::math::Pose3d &_pose) -{ - Errors errors; - - if (_graph.Count(_frameName) != 1) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, - "PoseRelativeToGraph unable to find unique frame with name [" + - _frameName+ "] in graph."}); - return errors; - } - auto vertexId = _graph.VertexIdByName(_frameName); - auto incidentsTo = _graph.Graph().IncidentsTo(vertexId); - std::vector toRemove; - for (const auto &[id, edge] : incidentsTo) - { - if (edge.get().Data().aliasing) - { - toRemove.push_back(id); - } - } - for (const auto id : toRemove) - { - incidentsTo.erase(id); - } - - if (incidentsTo.size() == 1) - { - _graph.UpdateEdge(incidentsTo.begin()->second, _pose); - } - else if (incidentsTo.empty()) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error: no incoming edge to " - "vertex [" + _frameName + "]."}); - } - else - { - // This is an error because there should be only one way to define the pose - // of a frame. This would be like a frame having two //pose elements with - // different //pose/@relative_to attributes. - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error: multiple incoming edges to " - "vertex [" + _frameName + "]."}); - } - - return errors; -} } } diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 08e7250af..779444355 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -100,20 +100,7 @@ namespace sdf /// named __model__ or world. Each vertex stores its FrameType and each edge /// stores the Pose3 between those frames. using Pose3d = ignition::math::Pose3d; - struct Edge { - Edge() = default; - Edge(const Pose3d &_pose) - : pose(_pose) - { - } - Edge(const Pose3d &_pose, bool _aliasing) - : pose(_pose), aliasing(_aliasing) - { - } - Pose3d pose; - bool aliasing{false}; - }; - using GraphType = ignition::math::graph::DirectedGraph; + using GraphType = ignition::math::graph::DirectedGraph; GraphType graph; /// \brief A Map from Vertex names to Vertex Ids. @@ -216,18 +203,6 @@ namespace sdf const ScopedGraph &_graph, const ignition::math::graph::VertexId &_frameVertexId, const ignition::math::graph::VertexId &_resolveToVertexId); - - /// \brief Update the pose of a frame in the pose graph. This updates the - /// content of the edge incident to the vertex identified by the given - /// _frameName. - /// \param[in] _graph PoseRelativeToGraph to update. - /// \param[in] _frameName Name of frame whose pose is to be updated. - /// \param[in] _pose New pose. - /// \return Errors. - Errors updateGraphPose( - ScopedGraph &_graph, - const std::string &_frameName, - const ignition::math::Pose3d &_pose); } } #endif diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index f7afdbf1a..b04260d33 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -300,88 +300,3 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) "PoseRelativeToGraph unable to find unique frame with name [" "invalid] in graph.")); } - -///////////////////////////////////////////////// -TEST(FrameSemantics, updateGraphPose) -{ - const std::string testFile = sdf::filesystem::append( - PROJECT_SOURCE_PATH, "test", "sdf", "model_link_relative_to.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); - - auto ownedGraph = std::make_shared(); - sdf::ScopedGraph graph(ownedGraph); - { - sdf::Errors errors = sdf::buildPoseRelativeToGraph(graph, model); - EXPECT_TRUE(errors.empty()); - EXPECT_TRUE(sdf::validatePoseRelativeToGraph(graph).empty()); - } - - using ignition::math::Pose3d; - - EXPECT_EQ(Pose3d::Zero, model->RawPose()); - - graph = graph.ChildScope(model->Name(), "__model__"); - { - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty()); - EXPECT_EQ(Pose3d(1, 0, 0, 0, IGN_PI_2, 0), pose); - } - { - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "__model__").empty()); - EXPECT_EQ(Pose3d(1, 0, -3, 0, IGN_PI_2, 0), pose); - } - // Try updating the pose of a frame that doesn't exist - { - sdf::Errors errors = sdf::updateGraphPose(graph, "nonexistent", {}); - ASSERT_FALSE(errors.empty()); - EXPECT_EQ(sdf::ErrorCode::POSE_RELATIVE_TO_INVALID, errors[0].Code()); - } - - // Update L1's pose in the graph. This will be relative to __model__ - Pose3d l1NewPose(0, 5, 0, 0, 0, 0); - EXPECT_TRUE(sdf::updateGraphPose(graph, "L1", l1NewPose).empty()); - - { - // L1 relative to __model__ is l1NewPose - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty()); - EXPECT_EQ(l1NewPose, pose); - } - { - // Since L3's pose is given relative to L1, updating L1 affects L3 - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "__model__").empty()); - EXPECT_EQ(l1NewPose * Pose3d(3, 0, 0, 0, 0, 0), pose); - } - - // Update L3's pose. This will be relative to L1 - Pose3d l3NewPose(0, 0, 2, 0, 0, 0); - EXPECT_TRUE(sdf::updateGraphPose(graph, "L3", l3NewPose).empty()); - - { - // L3 relative to L1 is l3NewPose - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "L1").empty()); - EXPECT_EQ(l3NewPose, pose); - } - { - // L1's pose shouldn't have changed - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty()); - EXPECT_EQ(l1NewPose, pose); - } - { - // The new pose of L3 relative to __model__ is affected by the new relative - // poses of L1 and L3 - Pose3d pose; - EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "__model__").empty()); - EXPECT_EQ(l1NewPose * l3NewPose, pose); - } -} From dd835a6137e4408d83b7d1424d4c20b0ffef48ee Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 20:22:37 -0500 Subject: [PATCH 20/70] Refactor Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 66 +++++++++++-------------------------------- 1 file changed, 17 insertions(+), 49 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 0ba005b8e..b3aa4082b 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -1494,43 +1494,9 @@ Errors resolvePoseRelativeToRoot( _vertexName + "] in graph."}); return errors; } - auto vertexId = _graph.VertexIdByName(_vertexName); - auto incomingVertexEdges = FindSourceVertex(_graph, vertexId, errors); - - if (!errors.empty()) - { - return errors; - } - else if (!incomingVertexEdges.first.Valid()) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph unable to find path to source vertex " - "when starting from vertex with name [" + _vertexName + "]."}); - return errors; - } - else if (incomingVertexEdges.first.Id() != _graph.ScopeVertex().Id()) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph frame with name [" + _vertexName + "] " - "is disconnected; its source vertex has name [" + - incomingVertexEdges.first.Name() + - "], but its source name should be " + _graph.ScopeName() + "."}); - return errors; - } - - ignition::math::Pose3d pose; - for (auto const &edge : incomingVertexEdges.second) - { - pose = edge.Data() * pose; - } - - if (errors.empty()) - { - _pose = pose; - } - - return errors; + return resolvePoseRelativeToRoot( + _pose, _graph, _graph.VertexIdByName(_vertexName)); } ///////////////////////////////////////////////// Errors resolvePoseRelativeToRoot( @@ -1609,22 +1575,24 @@ Errors resolvePose( const std::string &_frameName, const std::string &_resolveTo) { - Errors errors = resolvePoseRelativeToRoot(_pose, _graph, _frameName); - - // If the resolveTo is empty, we're resolving to the Root, so we're done - if (!_resolveTo.empty()) + Errors errors; + if (_graph.Count(_frameName) != 1) { - ignition::math::Pose3d poseR; - Errors errorsR = resolvePoseRelativeToRoot(poseR, _graph, _resolveTo); - errors.insert(errors.end(), errorsR.begin(), errorsR.end()); - - if (errors.empty()) - { - _pose = poseR.Inverse() * _pose; - } + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "PoseRelativeToGraph unable to find unique frame with name [" + + _frameName + "] in graph."}); + return errors; + } + if (_graph.Count(_resolveTo) != 1) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, + "PoseRelativeToGraph unable to find unique frame with name [" + + _resolveTo + "] in graph."}); + return errors; } - return errors; + return resolvePose(_pose, _graph, _graph.VertexIdByName(_frameName), + _graph.VertexIdByName(_resolveTo)); } } From 50edaec0d3fd4e32583285474900016c2b56d2b3 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 20:37:39 -0500 Subject: [PATCH 21/70] Cleanup Signed-off-by: Addisu Z. Taddese --- test/integration/nested_model.cc | 74 ++++++++++++++++---------------- 1 file changed, 36 insertions(+), 38 deletions(-) diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 86df81a8a..b55aaf240 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -15,7 +15,6 @@ * */ -#include #include #include #include @@ -354,18 +353,6 @@ TEST(NestedModel, NestedInclude) EXPECT_EQ(lowerLink3->RawPose(), lowerLink1->RawPose()); EXPECT_EQ(upperLink2->RawPose(), upperLink1->RawPose()); EXPECT_EQ(upperLink3->RawPose(), upperLink1->RawPose()); - // // expect //pose/@relative_to to contain the name of the nested model frame - // // injected during parsing. model1 is not nested, so it's links should have - // // empty //pose/@relative_to - // EXPECT_TRUE(baseLink1->PoseRelativeTo().empty()); - // EXPECT_EQ(name + "::__model__", baseLink2->PoseRelativeTo()); - // EXPECT_EQ(name + "_14::__model__", baseLink3->PoseRelativeTo()); - // EXPECT_TRUE(lowerLink1->PoseRelativeTo().empty()); - // EXPECT_EQ(name + "::__model__", lowerLink2->PoseRelativeTo()); - // EXPECT_EQ(name + "_14::__model__", lowerLink3->PoseRelativeTo()); - // EXPECT_TRUE(upperLink1->PoseRelativeTo().empty()); - // EXPECT_EQ(name + "::__model__", upperLink2->PoseRelativeTo()); - // EXPECT_EQ(name + "_14::__model__", upperLink3->PoseRelativeTo()); // each model has 2 joints, and the joint names of the nested models have // been transformed @@ -408,8 +395,6 @@ TEST(NestedModel, NestedInclude) // //axis/xyz/@expressed_in == "__model__" inside the nested model EXPECT_EQ(ignition::math::Vector3d::UnitX, lowerAxis3->Xyz()); EXPECT_EQ(ignition::math::Vector3d::UnitX, upperAxis3->Xyz()); - // EXPECT_EQ(name + "_14::__model__", lowerAxis3->XyzExpressedIn()); - // EXPECT_EQ(name + "_14::__model__", upperAxis3->XyzExpressedIn()); } ////////////////////////////////////////////////// @@ -477,15 +462,18 @@ TEST(NestedModel, NestedModelWithFrames) .empty()); EXPECT_EQ(frame1ExpPose, frame1Pose); - const auto *frame2 = childModel->FrameByName("F2"); + const auto *frame2 = parentModel->FrameByName("M1::F2"); ASSERT_NE(nullptr, frame2); + // Use childModel's SemanticPose with the resolveTo argument of "__model__". + // This should have the same result as using parentModel's SemanticPose with + // the resolveTo argument of parentModel->Name() Pose3d frame2Pose; EXPECT_TRUE(frame2->SemanticPose() .Resolve(frame2Pose, childModel->SemanticPose(), "__model__") .empty()); EXPECT_EQ(frame2ExpPose, frame2Pose); - const auto *link1 = childModel->LinkByName("L1"); + const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; EXPECT_TRUE(link1->SemanticPose() @@ -501,11 +489,13 @@ TEST(NestedModel, NestedModelWithFrames) ASSERT_NE(nullptr, collision1); EXPECT_EQ("__model__", collision1->PoseRelativeTo()); + const auto *link2 = parentModel->LinkByName("M1::L2"); + ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(parentModel->SemanticPose() - .Resolve(link2Pose, "ParentModel::M1::L2") + EXPECT_TRUE(link2->SemanticPose() + .Resolve(link2Pose, childModel->SemanticPose(), "__model__") .empty()); - EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); + EXPECT_EQ(link2ExpPose, link2Pose); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); @@ -521,6 +511,7 @@ TEST(NestedModel, NestedModelWithFrames) EXPECT_TRUE(joint1Axis->ResolveXyz( joint1AxisVector, childModel->SemanticPose(), "__model__").empty()); EXPECT_EQ(joint1AxisExpVector, joint1AxisVector); + const auto joint1Axis2 = joint1->Axis(1); ASSERT_NE(nullptr, joint1Axis2); Vector3d joint1Axis2Vector; @@ -752,7 +743,7 @@ TEST(NestedModel, NestedModelWithSiblingFrames) ASSERT_NE(nullptr, parentModelFrame); const sdf::Model *nestedModel1 = parentModel->ModelByName("M1"); - ASSERT_NE(nullptr, nestedModel1 ); + ASSERT_NE(nullptr, nestedModel1); EXPECT_EQ("testFrame", nestedModel1->PoseRelativeTo()); @@ -816,7 +807,6 @@ TEST(NestedModel, NestedFrameOnlyModel) << "" << " " << " true" - << " " << " " << " " + MODEL_PATH + "" << " M1" @@ -840,24 +830,29 @@ TEST(NestedModel, NestedFrameOnlyModel) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); - const sdf::Frame *parentModelFrame = parentModel->FrameByIndex(0); - ASSERT_NE(nullptr, parentModelFrame); - using ignition::math::Pose3d; using ignition::math::Vector3d; // Expected poses for frames after nesting. Pose3d frame1ExpPose = model1Pose * Pose3d(0, 0, 0, IGN_PI, 0, 0); Pose3d frame2ExpPose = frame1ExpPose * Pose3d(0, 0, 0, 0, IGN_PI, 0); + const auto *frame1 = parentModel->FrameByName("M1::F1"); + ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; EXPECT_TRUE( - parentModelFrame->SemanticPose().Resolve(frame1Pose, "M1::F1").empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); + frame1->SemanticPose() + .Resolve(frame1Pose, parentModel->SemanticPose(), parentModel->Name()) + .empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose); + const auto *frame2 = parentModel->FrameByName("M1::F2"); + ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; EXPECT_TRUE( - parentModelFrame->SemanticPose().Resolve(frame2Pose, "M1::F2").empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); + frame2->SemanticPose() + .Resolve(frame2Pose, parentModel->SemanticPose(), parentModel->Name()) + .empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose); } class PlacementFrame: public ::testing::Test @@ -878,11 +873,15 @@ class PlacementFrame: public ::testing::Test return sdf::filesystem::append(modelRootPath, _file); }); sdf::Errors errors = this->root.Load(testModelPath); - // if (!errors.empty()) - // { - // std::cout << this->root.Element()->ToString("") << std::endl; - // } - EXPECT_TRUE(errors.empty()) << errors; + for (const auto &e : errors) + { + std::cout << e.Message() << std::endl; + } + if (!errors.empty()) + { + std::cout << this->root.Element()->ToString("") << std::endl; + } + EXPECT_TRUE(errors.empty()); this->world = this->root.WorldByIndex(0); ASSERT_NE(nullptr, world); @@ -963,13 +962,13 @@ class PlacementFrame: public ::testing::Test Pose3d testFramePose; { - sdf::Errors errors = parentModel->SemanticPose().Resolve( - testFramePose, _parentModelName + "::" + _testFrameName); + sdf::Errors errors = testFrame->SemanticPose().Resolve( + testFramePose, parentModel->SemanticPose(), _parentModelName); EXPECT_TRUE(errors.empty()) << errors; } // The expected pose of the test frame is precisely the placement pose - EXPECT_EQ(placementPose, testFramePose.Inverse()); + EXPECT_EQ(placementPose, testFramePose); } protected: sdf::Root root; @@ -1035,4 +1034,3 @@ TEST_F(PlacementFrame, ModelPlacementFrameAttribute) this->TestExpectedWorldPose( "model_with_placement_frame_and_pose_relative_to", "L4"); } - From 9b6c99e9a0a79888d4ce7dc89025327702aa0293 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 21:19:10 -0500 Subject: [PATCH 22/70] Add Resolve function that takes DOM objects Signed-off-by: Addisu Z. Taddese --- include/sdf/JointAxis.hh | 21 ++++++++++++ include/sdf/SemanticPose.hh | 13 +++++++ src/FrameSemantics.cc | 3 +- src/JointAxis.cc | 36 +++++++++++++++++++ src/SemanticPose.cc | 38 +++++++++++++++++++- test/integration/nested_model.cc | 59 +++++++++++++++----------------- 6 files changed, 137 insertions(+), 33 deletions(-) diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index cc3074cce..cee1ceea1 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -35,6 +35,10 @@ namespace sdf // Forward declare private data class. class JointAxisPrivate; class SemanticPose; + class Frame; + class Joint; + class Link; + class Model; struct PoseRelativeToGraph; template class ScopedGraph; @@ -264,6 +268,23 @@ namespace sdf const sdf::SemanticPose &_scope, const std::string &_resolveTo) const; + + public: Errors ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::Frame &_resolveTo) const; + + public: Errors ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::Joint &_resolveTo) const; + + public: Errors ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::Link &_resolveTo) const; + + public: Errors ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::Model &_resolveTo) const; + // private: Errors ResolveXyzImpl( // ignition::math::Vector3d &_xyz, // sdf::ScopedGraph _graph, diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 9ab7f0213..b1cae00fb 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -40,6 +40,10 @@ namespace sdf // // Forward declare private data class. + class Model; + class Link; + class Frame; + class Joint; class SemanticPosePrivate; struct PoseRelativeToGraph; template class ScopedGraph; @@ -77,6 +81,15 @@ namespace sdf const SemanticPose &_scope, const std::string &_resolveTo) const; + public: Errors Resolve(ignition::math::Pose3d &_pose, + const sdf::Frame &_resolveTo) const; + public: Errors Resolve(ignition::math::Pose3d &_pose, + const sdf::Joint &_resolveTo) const; + public: Errors Resolve(ignition::math::Pose3d &_pose, + const sdf::Link &_resolveTo) const; + public: Errors Resolve(ignition::math::Pose3d &_pose, + const sdf::Model &_resolveTo) const; + /// \brief Private constructor. /// \param[in] _pose Raw pose of object. /// \param[in] _relativeTo Name of frame in graph relative-to which the diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index b3aa4082b..777f0a0a8 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -1045,7 +1045,8 @@ Errors buildPoseRelativeToGraph( } ///////////////////////////////////////////////// -Errors validateFrameAttachedToGraph(const ScopedGraph &_in) +Errors validateFrameAttachedToGraph( + const ScopedGraph &_in) { Errors errors; diff --git a/src/JointAxis.cc b/src/JointAxis.cc index 5e74dbd2c..3a55a091c 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -22,7 +22,11 @@ #include "sdf/Assert.hh" #include "sdf/Error.hh" +#include "sdf/Frame.hh" +#include "sdf/Joint.hh" #include "sdf/JointAxis.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" #include "sdf/SemanticPose.hh" #include "FrameSemantics.hh" @@ -430,6 +434,38 @@ Errors JointAxis::ResolveXyz( return errors; } +///////////////////////////////////////////////// +Errors JointAxis::ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::Frame &_resolveTo) const +{ + return this->ResolveXyz(_xyz, _resolveTo.SemanticPose(), _resolveTo.Name()); +} + +///////////////////////////////////////////////// +Errors JointAxis::ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::Joint &_resolveTo) const +{ + return this->ResolveXyz(_xyz, _resolveTo.SemanticPose(), _resolveTo.Name()); +} + +///////////////////////////////////////////////// +Errors JointAxis::ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::Link &_resolveTo) const +{ + return this->ResolveXyz(_xyz, _resolveTo.SemanticPose(), _resolveTo.Name()); +} + +///////////////////////////////////////////////// +Errors JointAxis::ResolveXyz( + ignition::math::Vector3d &_xyz, + const sdf::Model &_resolveTo) const +{ + return this->ResolveXyz(_xyz, _resolveTo.SemanticPose(), _resolveTo.Name()); +} + ///////////////////////////////////////////////// Errors JointAxis::ResolveXyz( ignition::math::Vector3d &_xyz, diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index 4e9a4dc5b..675956439 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -17,8 +17,12 @@ #include #include #include "sdf/Assert.hh" -#include "sdf/SemanticPose.hh" #include "sdf/Error.hh" +#include "sdf/Frame.hh" +#include "sdf/Joint.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" +#include "sdf/SemanticPose.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" @@ -129,6 +133,38 @@ Errors SemanticPose::Resolve( return this->Resolve(_pose, *this, _resolveTo); } +///////////////////////////////////////////////// +Errors SemanticPose::Resolve( + ignition::math::Pose3d &_pose, + const sdf::Frame &_resolveTo) const +{ + return this->Resolve(_pose, _resolveTo.SemanticPose(), _resolveTo.Name()); +} + +///////////////////////////////////////////////// +Errors SemanticPose::Resolve( + ignition::math::Pose3d &_pose, + const sdf::Joint &_resolveTo) const +{ + return this->Resolve(_pose, _resolveTo.SemanticPose(), _resolveTo.Name()); +} + +///////////////////////////////////////////////// +Errors SemanticPose::Resolve( + ignition::math::Pose3d &_pose, + const sdf::Link &_resolveTo) const +{ + return this->Resolve(_pose, _resolveTo.SemanticPose(), _resolveTo.Name()); +} + +///////////////////////////////////////////////// +Errors SemanticPose::Resolve( + ignition::math::Pose3d &_pose, + const sdf::Model &_resolveTo) const +{ + return this->Resolve(_pose, _resolveTo.SemanticPose(), _resolveTo.Name()); +} + ///////////////////////////////////////////////// Errors SemanticPose::Resolve( ignition::math::Pose3d &_pose, diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index b55aaf240..a21a83de4 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -464,9 +464,9 @@ TEST(NestedModel, NestedModelWithFrames) const auto *frame2 = parentModel->FrameByName("M1::F2"); ASSERT_NE(nullptr, frame2); - // Use childModel's SemanticPose with the resolveTo argument of "__model__". - // This should have the same result as using parentModel's SemanticPose with - // the resolveTo argument of parentModel->Name() + // Test using childModel's SemanticPose with the resolveTo argument of + // "__model__". This should have the same result as using parentModel's + // SemanticPose with the resolveTo argument of parentModel->Name() Pose3d frame2Pose; EXPECT_TRUE(frame2->SemanticPose() .Resolve(frame2Pose, childModel->SemanticPose(), "__model__") @@ -476,9 +476,7 @@ TEST(NestedModel, NestedModelWithFrames) const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose() - .Resolve(link1Pose, childModel->SemanticPose(), "__model__") - .empty()); + EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose, *parentModel).empty()); EXPECT_EQ(link1ExpPose, link1Pose); const auto *visual1 = link1->VisualByName("V1"); @@ -492,32 +490,25 @@ TEST(NestedModel, NestedModelWithFrames) const auto *link2 = parentModel->LinkByName("M1::L2"); ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose() - .Resolve(link2Pose, childModel->SemanticPose(), "__model__") - .empty()); + EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose, *parentModel).empty()); EXPECT_EQ(link2ExpPose, link2Pose); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose() - .Resolve(joint1Pose, childModel->SemanticPose(), "__model__") - .empty()); + EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, *parentModel).empty()); EXPECT_EQ(joint1ExpPose, joint1Pose); const auto joint1Axis = joint1->Axis(0); ASSERT_NE(nullptr, joint1Axis); Vector3d joint1AxisVector; - EXPECT_TRUE(joint1Axis->ResolveXyz( - joint1AxisVector, childModel->SemanticPose(), "__model__").empty()); + EXPECT_TRUE(joint1Axis->ResolveXyz(joint1AxisVector, *parentModel).empty()); EXPECT_EQ(joint1AxisExpVector, joint1AxisVector); const auto joint1Axis2 = joint1->Axis(1); ASSERT_NE(nullptr, joint1Axis2); Vector3d joint1Axis2Vector; - EXPECT_TRUE(joint1Axis2 - ->ResolveXyz(joint1Axis2Vector, parentModel->SemanticPose(), - parentModel->Name()).empty()); + EXPECT_TRUE(joint1Axis2->ResolveXyz(joint1Axis2Vector, *parentModel).empty()); EXPECT_EQ(joint1Axis2ExpVector, joint1Axis2Vector); } @@ -763,30 +754,36 @@ TEST(NestedModel, NestedModelWithSiblingFrames) nestedModel1->SemanticPose().Resolve(nestedModel1FramePose).empty()); EXPECT_EQ(nestedModel1FrameExpPose, nestedModel1FramePose); + const auto *frame1 = parentModel->FrameByName("M1::F1"); + ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; EXPECT_TRUE( - parentModelFrame->SemanticPose().Resolve(frame1Pose, "M1::F1").empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); + frame1->SemanticPose().Resolve(frame1Pose, *parentModel).empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose); + const auto *frame2 = parentModel->FrameByName("M1::F2"); + ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE( - parentModelFrame->SemanticPose().Resolve(frame2Pose, "M1::F2").empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); + EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose, *parentModel).empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose); + const auto *link1 = parentModel->LinkByName("M1::L1"); + ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE( - parentModelFrame->SemanticPose().Resolve(link1Pose, "M1::L1").empty()); - EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); + EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose, *parentModel).empty()); + EXPECT_EQ(link1ExpPose, link1Pose); + const auto *link2 = parentModel->LinkByName("M1::L2"); + ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE( - parentModelFrame->SemanticPose().Resolve(link2Pose, "M1::L2").empty()); - EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); + EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose, *parentModel).empty()); + EXPECT_EQ(link2ExpPose, link2Pose); + const auto *joint1 = parentModel->JointByName("M1::J1"); + ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE( - parentModelFrame->SemanticPose().Resolve(joint1Pose, "M1::J1").empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); + EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, *parentModel).empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose); } ////////////////////////////////////////////////// From 1cecc0c401cc704e600d1f9b7599774ca39fb0b5 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 22:05:04 -0500 Subject: [PATCH 23/70] Add test for placement attribute with nested link Signed-off-by: Addisu Z. Taddese --- test/integration/model_dom.cc | 22 +++++++++++++++++++ ..._with_nested_placement_frame_attribute.sdf | 14 ++++++++++++ 2 files changed, 36 insertions(+) create mode 100644 test/sdf/model_with_nested_placement_frame_attribute.sdf diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 30794b040..775912cd0 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -524,6 +524,7 @@ TEST(DOMRoot, LoadNestedExplicitCanonicalLink) EXPECT_EQ("link2", model->CanonicalLink()->Name()); } +///////////////////////////////////////////////// TEST(DOMRoot, ModelPlacementFrameAttribute) { const std::string testFile = @@ -544,6 +545,27 @@ TEST(DOMRoot, ModelPlacementFrameAttribute) EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); } +///////////////////////////////////////////////// +TEST(DOMRoot, ModelNestedPlacementFrameAttribute) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_with_nested_placement_frame_attribute.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + auto *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) { diff --git a/test/sdf/model_with_nested_placement_frame_attribute.sdf b/test/sdf/model_with_nested_placement_frame_attribute.sdf new file mode 100644 index 000000000..c7d9b6206 --- /dev/null +++ b/test/sdf/model_with_nested_placement_frame_attribute.sdf @@ -0,0 +1,14 @@ + + + + 0 0 10 0 0 0 + + 1 0 0 0 0 0 + + + + 0 2 0 0 0 0 + + + + From eb78dbce63dcceaf8db4a13c431e717f8435f1f3 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 22:12:11 -0500 Subject: [PATCH 24/70] Codecheck Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 13 ++++++------- src/FrameSemantics.hh | 6 +++--- src/ScopedGraph.hh | 3 +-- src/Types.cc | 1 - 4 files changed, 10 insertions(+), 13 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 777f0a0a8..3dc73a83a 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -146,7 +146,6 @@ FindSinkVertex( const ignition::math::graph::VertexId _id, Errors &_errors) { - using DirectedEdge = typename ScopedGraph::Edge; using Vertex = typename ScopedGraph::Vertex; using VertexId = ignition::math::graph::VertexId; @@ -1517,16 +1516,17 @@ Errors resolvePoseRelativeToRoot( { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph unable to find path to source vertex " - "when starting from vertex with id [" + std::to_string(_vertexId) + "]."}); + "when starting from vertex with id [" + + std::to_string(_vertexId) + "]."}); return errors; } else if (incomingVertexEdges.first.Id() != _graph.ScopeVertex().Id()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph frame with name [" + std::to_string(_vertexId) + "] " - "is disconnected; its source vertex has name [" + - incomingVertexEdges.first.Name() + - "], but its source name should be " + _graph.ScopeName() + "."}); + "PoseRelativeToGraph frame with name [" + std::to_string(_vertexId) + + "] is disconnected; its source vertex has name [" + + incomingVertexEdges.first.Name() + + "], but its source name should be " + _graph.ScopeName() + "."}); return errors; } @@ -1595,6 +1595,5 @@ Errors resolvePose( return resolvePose(_pose, _graph, _graph.VertexIdByName(_frameName), _graph.VertexIdByName(_resolveTo)); } - } } diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 779444355..4015f2b45 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -116,7 +116,7 @@ namespace sdf /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. Errors buildFrameAttachedToGraph(ScopedGraph &_out, - const Model *_model, bool _root = true); + const Model *_model, bool _root = true); /// \brief Build a FrameAttachedToGraph for a world. /// \param[out] _out Graph object to write. @@ -129,8 +129,8 @@ namespace sdf /// \param[out] _out Graph object to write. /// \param[in] _model Model from which to build attached_to graph. /// \return Errors. - Errors buildPoseRelativeToGraph( - ScopedGraph &_out, const Model *_model, bool _root = true); + Errors buildPoseRelativeToGraph(ScopedGraph &_out, + const Model *_model, bool _root = true); /// \brief Build a PoseRelativeToGraph for a world. /// \param[out] _out Graph object to write. diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index bee2e8e8d..ad4305acf 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -46,7 +47,6 @@ struct ScopedGraphData template class ScopedGraph { - public: template struct GraphTypeExtracter { @@ -320,7 +320,6 @@ std::string ScopedGraph::RemovePrefix(const std::string &_name) const } return _name; } - } } diff --git a/src/Types.cc b/src/Types.cc index 83b8d4037..fe22d3a17 100644 --- a/src/Types.cc +++ b/src/Types.cc @@ -95,6 +95,5 @@ std::ostream &operator<<(std::ostream &_out, const sdf::Errors &_errs) } return _out; } - } } From 2cbe572f8259bd760beffdd8a9fdb360d16b5638 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 7 Oct 2020 22:41:41 -0500 Subject: [PATCH 25/70] Fix macOS build error Signed-off-by: Addisu Z. Taddese --- src/ScopedGraph.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index ad4305acf..3673ab2ef 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -260,7 +260,7 @@ std::size_t ScopedGraph::Count(const std::string &_name) const ///////////////////////////////////////////////// template -std::size_t ScopedGraph::VertexIdByName(const std::string &_name) const +auto ScopedGraph::VertexIdByName(const std::string &_name) const -> VertexId { auto &map = this->Map(); auto it = map.find(this->AddPrefix(_name)); @@ -280,7 +280,7 @@ auto ScopedGraph::ScopeVertex() const -> Vertex ///////////////////////////////////////////////// template -std::size_t ScopedGraph::ScopeVertexId() const +auto ScopedGraph::ScopeVertexId() const -> VertexId { return this->dataPtr->scopeVertexId; } From 67da6d8fd390f6c4c2899df7f1d20bae22e481de Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 8 Oct 2020 10:10:03 -0500 Subject: [PATCH 26/70] Add nested model pose relative_to test Signed-off-by: Addisu Z. Taddese --- test/integration/nested_model.cc | 62 +++++++++++++++++++ .../model_relative_to_nested_reference.sdf | 52 ++++++++++++++++ 2 files changed, 114 insertions(+) create mode 100644 test/sdf/model_relative_to_nested_reference.sdf diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index a21a83de4..5ea7d1149 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -1031,3 +1031,65 @@ TEST_F(PlacementFrame, ModelPlacementFrameAttribute) this->TestExpectedWorldPose( "model_with_placement_frame_and_pose_relative_to", "L4"); } + +////////////////////////////////////////////////// +TEST(NestedReference, PoseRelativeTo) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_relative_to_nested_reference.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + + using Pose = ignition::math::Pose3d; + // Get the first model + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + // The child models starting from M2 all reference an entity inside M1. Check + // that resolving poses works when referencing such entities. + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M2"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_EQ(Pose(1, 0, -2, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M3"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_EQ(Pose(1, 1, -3, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M4"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_EQ(Pose(2, 0, -4, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M5"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_EQ(Pose(2, 0, -6, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M6"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_EQ(Pose(1, 1, -6, IGN_PI_2, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Model *testModel = model->ModelByName("M7"); + ASSERT_NE(nullptr, testModel); + EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose); + } +} diff --git a/test/sdf/model_relative_to_nested_reference.sdf b/test/sdf/model_relative_to_nested_reference.sdf new file mode 100644 index 000000000..f895b69ec --- /dev/null +++ b/test/sdf/model_relative_to_nested_reference.sdf @@ -0,0 +1,52 @@ + + + + + + 1 0 0 0 1.5707963267948966 0 + + 0 1 0 0 0 0 + + + L1 + L2 + + + 0 0 1 0 0 0 + + + 1 0 0 0 0 0 + + + 0 1 0 1.5707963267948966 0 0 + + 1 0 0 0 -1.5707963267948966 0 + + + + + 2 0 0 0 0 0 + + + + 3 0 0 0 0 0 + + + + 4 0 0 0 0 0 + + + + 5 0 0 0 0 0 + + + + 6 0 0 0 0 0 + + + + 7 0 0 0 0 0 + + + + From db47ba2c3feabc9f8f3b81fd6a79942f95260c86 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 8 Oct 2020 11:04:53 -0500 Subject: [PATCH 27/70] Add nested model frame attached_to test Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics_TEST.cc | 243 +++++++++++++++++++- src/parser.cc | 31 ++- test/sdf/model_nested_frame_attached_to.sdf | 22 ++ test/sdf/world_nested_frame_attached_to.sdf | 27 +++ 4 files changed, 320 insertions(+), 3 deletions(-) create mode 100644 test/sdf/model_nested_frame_attached_to.sdf create mode 100644 test/sdf/world_nested_frame_attached_to.sdf diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index b04260d33..85f07ca36 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -123,7 +123,6 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) sdf::ScopedGraph graph(ownedGraph); errors = sdf::buildFrameAttachedToGraph(graph, world); EXPECT_TRUE(errors.empty()); - std::cout << graph.Graph() << std::endl; errors = sdf::validateFrameAttachedToGraph(graph); EXPECT_TRUE(errors.empty()) << errors; EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); @@ -300,3 +299,245 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) "PoseRelativeToGraph unable to find unique frame with name [" "invalid] in graph.")); } + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, buildFrameAttachedToGraph_Model) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_nested_frame_attached_to.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + // Get the first model + const sdf::Model *model = root.ModelByIndex(0); + + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, model); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_TRUE(sdf::validateFrameAttachedToGraph(graph).empty()); + EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); + EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); + + graph = graph.ChildScope(model->Name(), "__model__"); + EXPECT_EQ(1u, graph.Count("__model__")); + EXPECT_EQ(1u, graph.Count("L")); + EXPECT_EQ(1u, graph.Count("M1")); + EXPECT_EQ(1u, graph.Count("M1::__model__")); + EXPECT_EQ(1u, graph.Count("M1::L")); + EXPECT_EQ(1u, graph.Count("M1::M2")); + EXPECT_EQ(1u, graph.Count("M1::M2::L")); + EXPECT_EQ(1u, graph.Count("M1::F")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("F3")); + EXPECT_EQ(1u, graph.Count("F4")); + EXPECT_EQ(0u, graph.Count("invalid")); + + std::string resolvedBody; + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "__model__").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "L").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::__model__") + .empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::L").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2::L").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::F").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F0").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F2").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F3").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F4").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + + // Try to resolve invalid frame name + errors = sdf::resolveFrameAttachedToBody(resolvedBody, graph, "invalid"); + for (auto &e : errors) + std::cerr << e.Message() << std::endl; + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "FrameAttachedToGraph unable to find unique frame with name [" + "invalid] in graph.")); +} + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, buildFrameAttachedToGraph_World) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_nested_frame_attached_to.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + // Get the first world + const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(world); + + auto ownedGraph = std::make_shared(); + sdf::ScopedGraph graph(ownedGraph); + errors = sdf::buildFrameAttachedToGraph(graph, world); + EXPECT_TRUE(errors.empty()); + errors = sdf::validateFrameAttachedToGraph(graph); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); + EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); + + EXPECT_EQ(1u, graph.Count("world")); + EXPECT_EQ(1u, graph.Count("world_frame")); + EXPECT_EQ(1u, graph.Count("M1")); + EXPECT_EQ(1u, graph.Count("M1::__model__")); + EXPECT_EQ(1u, graph.Count("M1::L")); + EXPECT_EQ(1u, graph.Count("M1::M2")); + EXPECT_EQ(1u, graph.Count("M1::M2::__model__")); + EXPECT_EQ(1u, graph.Count("M1::M2::L")); + EXPECT_EQ(1u, graph.Count("M1::F0")); + EXPECT_EQ(1u, graph.Count("F0")); + EXPECT_EQ(1u, graph.Count("F1")); + EXPECT_EQ(1u, graph.Count("F2")); + EXPECT_EQ(1u, graph.Count("F3")); + EXPECT_EQ(1u, graph.Count("F4")); + EXPECT_EQ(1u, graph.Count("F5")); + EXPECT_EQ(1u, graph.Count("F6")); + EXPECT_EQ(0u, graph.Count("invalid")); + + std::string resolvedBody; + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "world").empty()); + EXPECT_EQ("world", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody( + resolvedBody, graph, "world_frame").empty()); + EXPECT_EQ("world", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::__model__") + .empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::L").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2::__model__") + .empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::M2::L").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "M1::F0").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F0").empty()); + EXPECT_EQ("world", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F1").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F2").empty()); + EXPECT_EQ("M1::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F3").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F4").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F5").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, graph, "F6").empty()); + EXPECT_EQ("M1::M2::L", resolvedBody); + + // Try to resolve invalid frame name + errors = sdf::resolveFrameAttachedToBody(resolvedBody, graph, "invalid"); + for (auto &e : errors) + std::cerr << e.Message() << std::endl; + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "FrameAttachedToGraph unable to find unique frame with name [" + "invalid] in graph.")); + + // Create graph from embedded model + ASSERT_NE(nullptr, world); + const sdf::Model *model = world->ModelByIndex(0); + + auto ownedModelGraph = std::make_shared(); + sdf::ScopedGraph modelGraph(ownedModelGraph); + errors = sdf::buildFrameAttachedToGraph(modelGraph, model); + EXPECT_TRUE(errors.empty()); + EXPECT_TRUE(sdf::validateFrameAttachedToGraph(modelGraph).empty()); + + modelGraph = modelGraph.ChildScope(model->Name(), "__model__"); + + EXPECT_EQ(1u, modelGraph.Count("__model__")); + EXPECT_EQ(1u, modelGraph.Count("L")); + EXPECT_EQ(1u, modelGraph.Count("M2")); + EXPECT_EQ(1u, modelGraph.Count("M2::__model__")); + EXPECT_EQ(1u, modelGraph.Count("M2::L")); + EXPECT_EQ(1u, modelGraph.Count("F0")); + EXPECT_EQ(0u, modelGraph.Count("invalid")); + + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody( + resolvedBody, modelGraph, "__model__").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "L").empty()); + EXPECT_EQ("L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "M2").empty()); + EXPECT_EQ("M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "M2::__model__") + .empty()); + EXPECT_EQ("M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "M2::L").empty()); + EXPECT_EQ("M2::L", resolvedBody); + EXPECT_TRUE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F0").empty()); + EXPECT_EQ("M2::L", resolvedBody); +} diff --git a/src/parser.cc b/src/parser.cc index 05cec39c8..5800fb774 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1583,6 +1583,34 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) auto checkWorldFrameAttachedToNames = []( const sdf::World *_world) -> bool { + auto findNameInWorld = [](const sdf::World *_inWorld, + const std::string &_name) -> bool { + if (_inWorld->ModelNameExists(_name) || + _inWorld->FrameNameExists(_name)) + { + return true; + } + + for (uint64_t m = 0; m < _inWorld->ModelCount(); ++m) + { + const auto *model = _inWorld->ModelByIndex(m); + auto index = _name.find(model->Name() + "::"); + std::string nameToCheck = _name; + if (index != std::string::npos) + { + nameToCheck = _name.substr(index + model->Name().size() + 2); + } + if (model->LinkNameExists(nameToCheck) || + model->ModelNameExists(nameToCheck) || + model->JointNameExists(nameToCheck) || + model->FrameNameExists(nameToCheck)) + { + return true; + } + } + return false; + }; + bool worldResult = true; for (uint64_t f = 0; f < _world->FrameCount(); ++f) { @@ -1606,8 +1634,7 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) << std::endl; worldResult = false; } - else if (!_world->ModelNameExists(attachedTo) && - !_world->FrameNameExists(attachedTo)) + else if (!findNameInWorld(_world, attachedTo)) { std::cerr << "Error: attached_to name[" << attachedTo << "] specified by frame with name[" << frame->Name() diff --git a/test/sdf/model_nested_frame_attached_to.sdf b/test/sdf/model_nested_frame_attached_to.sdf new file mode 100644 index 000000000..6722b8f99 --- /dev/null +++ b/test/sdf/model_nested_frame_attached_to.sdf @@ -0,0 +1,22 @@ + + + + + + + + + + + + L + F + + + + + + + + + diff --git a/test/sdf/world_nested_frame_attached_to.sdf b/test/sdf/world_nested_frame_attached_to.sdf new file mode 100644 index 000000000..1dc64fa72 --- /dev/null +++ b/test/sdf/world_nested_frame_attached_to.sdf @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + L + F0 + + + + + + + + + + + + From 28e2c61969c859713284a8c60977da56c7b03a96 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 8 Oct 2020 11:56:03 -0500 Subject: [PATCH 28/70] Add test for joints that reference entities in nested models Signed-off-by: Addisu Z. Taddese --- src/ign_TEST.cc | 11 +++++ src/parser.cc | 3 +- test/integration/joint_dom.cc | 65 +++++++++++++++++++++++++ test/sdf/joint_nested_parent_child.sdf | 67 ++++++++++++++++++++++++++ 4 files changed, 145 insertions(+), 1 deletion(-) create mode 100644 test/sdf/joint_nested_parent_child.sdf diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 602499dcf..02bd543e6 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -573,6 +573,17 @@ TEST(check, SDF) EXPECT_EQ("Valid.\n", output) << output; } + // Check an SDF file with nested_models using nested links/frames as joint + // parent or child frames. + // This is a valid file. + { + std::string path = pathBase +"/joint_nested_parent_child.sdf"; + + // Check model_nested_model_relative_to.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 with a joint that specifies a child link // within a sibling nested model using the unsupported :: syntax. // { diff --git a/src/parser.cc b/src/parser.cc index 5800fb774..a82862355 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1930,7 +1930,8 @@ bool checkJointParentChildLinkNames(const sdf::Root *_root) if (!_model->LinkNameExists(childName) && !_model->JointNameExists(childName) && - !_model->FrameNameExists(childName)) + !_model->FrameNameExists(childName) && + !_model->ModelNameExists(childName)) { std::cerr << "Error: child frame with name[" << childName << "] specified by joint with name[" << joint->Name() diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index c4bff160d..b763acc40 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -766,3 +766,68 @@ TEST(DOMJoint, LoadURDFJointPoseRelativeTo) model->JointByName("joint12")->Axis()->ResolveXyz(vec3, "joint12").empty()); EXPECT_EQ(Vector3(0, 1.0, 0), vec3); } + +///////////////////////////////////////////////// +TEST(DOMJoint, LoadJointNestedParentChild) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "joint_nested_parent_child.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + using Pose = ignition::math::Pose3d; + + // Get the first model + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + { + const sdf::Joint *j1 = model->JointByName("J1"); + ASSERT_NE(nullptr, j1); + EXPECT_EQ("M1::L1", j1->ParentLinkName()); + EXPECT_EQ("L1", j1->ChildLinkName()); + Pose pose; + EXPECT_TRUE(j1->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 0, 9, 0, IGN_PI_2, 0), pose); + } + { + const sdf::Joint *j2 = model->JointByName("J2"); + ASSERT_NE(nullptr, j2); + EXPECT_EQ("F1", j2->ParentLinkName()); + EXPECT_EQ("L1", j2->ChildLinkName()); + Pose pose; + EXPECT_TRUE(j2->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, 1, 10, 0, IGN_PI_2, 0), pose); + } + { + const sdf::Joint *j3 = model->JointByName("J3"); + ASSERT_NE(nullptr, j3); + EXPECT_EQ("L1", j3->ParentLinkName()); + EXPECT_EQ("M1::L2", j3->ChildLinkName()); + Pose pose; + EXPECT_TRUE(j3->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 1, 1, 0, 0, 0), pose); + } + { + const sdf::Joint *j4 = model->JointByName("J4"); + ASSERT_NE(nullptr, j4); + EXPECT_EQ("L1", j4->ParentLinkName()); + EXPECT_EQ("M1::F1", j4->ChildLinkName()); + Pose pose; + EXPECT_TRUE(j4->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(1, 0, 1, 0, 0, 0), pose); + } + { + const sdf::Joint *j5 = model->JointByName("J5"); + ASSERT_NE(nullptr, j5); + EXPECT_EQ("L1", j5->ParentLinkName()); + EXPECT_EQ("M1::M2", j5->ChildLinkName()); + Pose pose; + EXPECT_TRUE(j5->SemanticPose().Resolve(pose, "__model__").empty()); + EXPECT_EQ(Pose(0, -1, 1, IGN_PI_2, 0, 0), pose); + } +} diff --git a/test/sdf/joint_nested_parent_child.sdf b/test/sdf/joint_nested_parent_child.sdf new file mode 100644 index 000000000..de16f001d --- /dev/null +++ b/test/sdf/joint_nested_parent_child.sdf @@ -0,0 +1,67 @@ + + + + + + + 0 0 1 0 0 0 + + + 1 1 0 0 0 0 + + + 1 0 0 0 0 0 + + + 0 0 1 1.570796326790 0 0 + + + + + + 0 0 10 0 1.57079632679 0 + + + + 1 0 0 0 0 0 + + + + + 1 0 0 0 0 0 + M1::L1 + L1 + + + + 0 1 0 0 0 0 + F1 + L1 + + + + 0 0 1 0 0 0 + L1 + M1::L2 + + + + + 0 0 1 0 0 0 + L1 + M1::F1 + + + + + 0 0 1 0 0 0 + L1 + M1::M2 + + + From b36c1d2e89936fc2a58738773f41bfe3b88b1dbb Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 9 Oct 2020 15:12:38 -0500 Subject: [PATCH 29/70] Add API documentation for ScopedGraph, refactor Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 21 +-- src/FrameSemantics.hh | 2 + src/FrameSemantics_TEST.cc | 10 +- src/Model.cc | 8 +- src/ScopedGraph.hh | 253 ++++++++++++++++++++++++++++--------- src/World.cc | 4 +- 6 files changed, 219 insertions(+), 79 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 3dc73a83a..f45286901 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -242,7 +242,7 @@ Errors buildFrameAttachedToGraph( const auto modelId = _out.AddVertex(_model->Name(), frameType).Id(); - auto outModel = _out.ChildScope(_model->Name(), "__model__"); + auto outModel = _out.ChildModelScope(_model->Name()); const auto modelFrameId = outModel.AddVertex(scopeName, frameType).Id(); auto &edge = outModel.AddEdge({modelId, modelFrameId}, true); @@ -543,7 +543,7 @@ Errors buildPoseRelativeToGraph( } auto modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); - auto outModel = _out.ChildScope(_model->Name(), scopeName); + auto outModel = _out.ChildModelScope(_model->Name()); auto modelFrameId = outModel.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); // Add an aliasing edge from the model vertex to the // corresponding vertex in the PoseRelativeTo graph of the child model, @@ -792,7 +792,7 @@ Errors buildPoseRelativeToGraph( ignition::math::Pose3d X_MPf; sdf::Errors resolveErrors = sdf::resolvePoseRelativeToRoot(X_MPf, - outModel.ChildScope(nestedModel->Name(), "__model__"), + outModel.ChildModelScope(nestedModel->Name()), nestedModel->PlacementFrameName()); if (resolveErrors.empty()) { @@ -819,6 +819,9 @@ Errors buildPoseRelativeToGraph( if (_root) { + // We have to add this edge now with an identity pose to be able to call + // sdf::resolvePoseRelativeToRoot. We will later update the edge after the + // pose is calculated. auto rootToModel = outModel.AddEdge({rootId, modelId}, {}); ignition::math::Pose3d X_RM = _model->RawPose(); // If the model has a placement frame, calculate the necessary pose to use @@ -967,7 +970,7 @@ Errors buildPoseRelativeToGraph( ignition::math::Pose3d X_MPf; sdf::Errors resolveErrors = sdf::resolvePoseRelativeToRoot(X_MPf, - _out.ChildScope(model->Name(), "__model__"), + _out.ChildModelScope(model->Name()), model->PlacementFrameName()); if (resolveErrors.empty()) { @@ -1090,10 +1093,10 @@ Errors validateFrameAttachedToGraph( } // Check number of outgoing edges for each vertex - auto vertices = _in.Vertices(); + auto vertices = _in.Graph().Vertices(); for (auto vertexPair : vertices) { - const std::string vertexName = _in.VertexName(vertexPair.second.get()); + const std::string vertexName = _in.VertexLocalName(vertexPair.second.get()); // Vertex names should not be empty if (vertexName.empty()) { @@ -1270,10 +1273,10 @@ Errors validatePoseRelativeToGraph( } // Check number of incoming edges for each vertex - auto vertices = _in.Vertices(); + auto vertices = _in.Graph().Vertices(); for (auto vertexPair : vertices) { - const std::string vertexName = _in.VertexName(vertexPair.second.get()); + const std::string vertexName = _in.VertexLocalName(vertexPair.second.get()); // Vertex names should not be empty if (vertexName.empty()) { @@ -1474,7 +1477,7 @@ Errors resolveFrameAttachedToBody( } } - _attachedToBody = _in.VertexName(sinkVertex); + _attachedToBody = _in.VertexLocalName(sinkVertex); return errors; } diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 4015f2b45..05423bdeb 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -43,10 +43,12 @@ namespace sdf // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // +#ifndef DOXYGEN_SHOULD_SKIP_THIS // Forward declaration. class Model; class World; template class ScopedGraph; +#endif /// \enum FrameType /// \brief The set of frame types. INVALID indicates that frame type has diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index 85f07ca36..a8e6f29e1 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -58,7 +58,7 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model) EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); - graph = graph.ChildScope(model->Name(), "__model__"); + graph = graph.ChildModelScope(model->Name()); EXPECT_EQ(8u, graph.Map().size()); EXPECT_EQ(8u, graph.Graph().Vertices().size()); EXPECT_EQ(6u, graph.Graph().Edges().size()); @@ -182,7 +182,7 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World) EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validateFrameAttachedToGraph(modelGraph).empty()); - modelGraph = modelGraph.ChildScope(model->Name(), "__model__"); + modelGraph = modelGraph.ChildModelScope(model->Name()); EXPECT_EQ(5u, modelGraph.Map().size()); EXPECT_EQ(5u, modelGraph.Graph().Vertices().size()); @@ -224,7 +224,7 @@ TEST(FrameSemantics, buildPoseRelativeToGraph) auto errors = sdf::buildPoseRelativeToGraph(graph, model); EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validatePoseRelativeToGraph(graph).empty()); - graph = graph.ChildScope(model->Name(), "__model__"); + graph = graph.ChildModelScope(model->Name()); EXPECT_EQ(10u, graph.Map().size()); EXPECT_EQ(10u, graph.Graph().Vertices().size()); @@ -323,7 +323,7 @@ TEST(NestedFrameSemantics, buildFrameAttachedToGraph_Model) EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root)); EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root)); - graph = graph.ChildScope(model->Name(), "__model__"); + graph = graph.ChildModelScope(model->Name()); EXPECT_EQ(1u, graph.Count("__model__")); EXPECT_EQ(1u, graph.Count("L")); EXPECT_EQ(1u, graph.Count("M1")); @@ -510,7 +510,7 @@ TEST(NestedFrameSemantics, buildFrameAttachedToGraph_World) EXPECT_TRUE(errors.empty()); EXPECT_TRUE(sdf::validateFrameAttachedToGraph(modelGraph).empty()); - modelGraph = modelGraph.ChildScope(model->Name(), "__model__"); + modelGraph = modelGraph.ChildModelScope(model->Name()); EXPECT_EQ(1u, modelGraph.Count("__model__")); EXPECT_EQ(1u, modelGraph.Count("L")); diff --git a/src/Model.cc b/src/Model.cc index 38c0e3388..301c1b58a 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -237,11 +237,11 @@ Errors Model::Load(ElementPtr _sdf) { // std::cout << "Creating owned graphs for " << this->Name() << std::endl; this->dataPtr->ownedPoseGraph = std::make_shared(); - this->dataPtr->poseGraph = this->dataPtr->ownedPoseGraph; + this->dataPtr->poseGraph = ScopedGraph(this->dataPtr->ownedPoseGraph); this->dataPtr->ownedFrameAttachedToGraph = std::make_shared(); this->dataPtr->frameAttachedToGraph = - this->dataPtr->ownedFrameAttachedToGraph; + ScopedGraph(this->dataPtr->ownedFrameAttachedToGraph); modelFile = true; } @@ -760,7 +760,7 @@ Errors Model::SetPoseRelativeToGraph( this->dataPtr->poseGraph = _graph; auto childPoseGraph = - this->dataPtr->poseGraph.ChildScope(this->Name(), "__model__"); + this->dataPtr->poseGraph.ChildModelScope(this->Name()); for (auto &link : this->dataPtr->links) { link.SetPoseRelativeToGraph(childPoseGraph); @@ -797,7 +797,7 @@ void Model::SetFrameAttachedToGraph( this->dataPtr->frameAttachedToGraph = _graph; auto childFrameAttachedToGraph = - this->dataPtr->frameAttachedToGraph.ChildScope(this->Name(), "__model__"); + this->dataPtr->frameAttachedToGraph.ChildModelScope(this->Name()); for (auto &joint : this->dataPtr->joints) { joint.SetFrameAttachedToGraph(childFrameAttachedToGraph); diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index 3673ab2ef..eb2deba10 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -20,33 +20,76 @@ #include #include -#include #include -#include -#include +#include #include #include #include +#include "sdf/sdf_config.h" + namespace sdf { // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { + +/// \brief Data structure that holds information associated with each scope struct ScopedGraphData { - // std::unordered_set vertices; + /// \brief The vertex ID of the designated scope vertex ignition::math::graph::VertexId scopeVertexId { ignition::math::graph::kNullId}; + /// \brief The prefix for all names under this scope std::string prefix {}; + /// \brief The name of this scope. Either world or __model__ std::string scopeName {}; }; +// Forward declarations for static_assert +class PoseRelativeToGraph; +class FrameAttachedToGraph; + +/// \brief The ScopedGraph allows manipulating FrameAttachedTo and +/// PoseRelativeTo graphs within a smaller scope such as the scope of a model +/// inside a world graph. +/// +/// Each valid instance of ScopedGraph has a pointer to either a +/// FrameAttachedToGraph or a PoseRelativeToGraph. In addition, each instance +/// contains the following: +/// - The prefix of the scope, which is empty for a world scope and the +/// name of a model for a model scope. +/// - The scope name of the scope, which can be either "world" or "__model__". +/// - ID of the vertex on which the scope is anchored. For +/// PoseRelativeToGraph's, this vertex is the root of the sub tree represented +/// by the ScopedGraph instance. For FrameAttachedToGraph, this vertex only +/// represents the scope boundary and does not imply that it's the root of the +/// sub tree because these graphs can have multiple disconnected trees. +/// +/// When a vertex is added to the graph via ScopedGraph, the prefix is prepended +/// to the input name of the vertex. The name of the vertex with the prefix is +/// known as the absolute name of the vertex and the name of the vertex without +/// the prefix is known as the local name of the vertex. +/// +/// \remark ScopedGraph has pointer semantic. i.e, a ScopedGraph object and its +/// copy point to the same underlying data and modifying one will affect the +/// other. +/// +/// \tparam T Either FrameAttachedTo or PoseRelativeTo graph template class ScopedGraph { + static_assert( + std::is_same_v || + std::is_same_v, + "Template parameter has to be either sdf::PoseRelativeToGraph or " + "sdf::FrameAttachedToGraph"); + + /// \brief template for extracting the graph type from the template parameter. + /// \tparam G ignition::math::DirectedGraph type + /// \return Vertex and Edge types of G public: template struct GraphTypeExtracter { @@ -54,6 +97,8 @@ class ScopedGraph using Edge = void; }; + /// \brief Specialization for GraphTypeExtracter on + /// ignition::math::DirectedGraph public: template struct GraphTypeExtracter> { @@ -61,6 +106,7 @@ class ScopedGraph using Edge = E; }; + // Type aliases public: using MathGraphType = typename T::GraphType; public: using VertexId = ignition::math::graph::VertexId; public: using VertexType = typename GraphTypeExtracter::Vertex; @@ -68,43 +114,144 @@ class ScopedGraph public: using Vertex = ignition::math::graph::Vertex; public: using Edge = ignition::math::graph::DirectedEdge; public: using MapType = typename T::MapType; + + /// \brief Default constructor. The constructed object is invalid as it + /// doesn't point to any graph. public: ScopedGraph() = default; - public: ScopedGraph(const std::shared_ptr _graph); - public: ScopedGraph ChildScope( - const std::string &_name, const std::string &_scopeName); + + /// \brief Constructor. The constructed object holds a weak pointer to the + /// passed in graph. + /// \param[in] _graph A shared pointer to PoseRelativeTo or FrameAttachedTo + /// graph. + public: explicit ScopedGraph(const std::shared_ptr &_graph); + + /// \brief Creates a scope anchored at an existing child model vertex. + /// \param[in] _name Name of child model vertex. The prefix of the current + /// scope will be prepended to this name before searching for the vertex that + /// matches the name. The new scope will have a new prefix formed by appending + /// _name to the existing prefix of the current scope. + /// \return A new child scope. + public: ScopedGraph ChildModelScope(const std::string &_name); + + /// \brief Checks if the scope points to a valid graph. + /// \return True if the scope points to a valid graph. public: explicit operator bool() const; + + /// \brief Immutable reference to the underlying PoseRelativeTo::graph or + /// FrameAttachedTo::graph. public: const MathGraphType &Graph() const; + + /// \brief Immutable reference to the underlying PoseRelativeTo::map or + /// FrameAttachedTo::map. public: const MapType &Map() const; - // May not be needed if they can be called via Graph() + + /// \brief Adds a scope vertex to the graph. This creates a new + /// scope by updating the prefix of the current scope, adding a new scope + /// vertex to the graph, and setting a new scope name. + /// \param[in] _prefix The new prefix of the scope. + /// \param[in] _name The Name of the scope vertex to be added. The full name + /// of the vertex will be newPrefix::_name, where newPrefix is the prefix + /// obtained by adding _prefix to the existing prefix of the scope. public: Vertex &AddScopeVertex(const std::string &_prefix, const std::string &_name, const std::string &_scopeName, const VertexType &_data); - public: Vertex &AddVertex(const std::string &_name, const VertexType &); - public: Edge &AddEdge( - const ignition::math::graph::VertexId_P &, const EdgeType &); - public: const ignition::math::graph::VertexRef_M Vertices() const; - public: const ignition::math::graph::VertexRef_M Vertices( - const std::string &) const; + + /// \brief Adds a vertex to the graph. + /// \param[in] _name The local name of the vertex. The absolute name of the + /// vertex will be _prefix::_name. + /// \param[in] _data Vertex data. + /// \return The newly created vertex. + public: Vertex &AddVertex(const std::string &_name, const VertexType &_data); + + /// \brief Adds an edge to the graph. + /// \param[in] _vertexPair Pair of vertex IDs between which the edge will be + /// created. + /// \param[in] _data Edge data. + /// \return The newly created edge. + public: Edge &AddEdge(const ignition::math::graph::VertexId_P &_vertexPair, + const EdgeType &_data); + + /// \brief Gets all the local names of the vertices in the current scope. + /// \return A list of vertex names in the current scope. public: std::vector VertexNames() const; - public: std::string VertexName(const VertexId &_id) const; - public: std::string VertexName(const Vertex &_vertex) const; - public: void UpdateEdge(const Edge &, const EdgeType &); + + /// \brief Get the local name of a vertex. + /// \param[in] _id ID of the vertex. + /// \return The local name of the vertex. If the vertex was not found in the + /// graph, "__null__" will be returned. + public: std::string VertexLocalName(const VertexId &_id) const; + + /// \brief Get the local name of a vertex. + /// \param[in] _vertex Vertex object. + /// \return The local name of the vertex. If the vertex was not found in the + /// graph, "__null__" will be returned. + public: std::string VertexLocalName(const Vertex &_vertex) const; + + /// \brief Update the information contained by an edge + /// \remark Since this function has to remove the edge from the graph and add + /// a new one, the original edge will have a new ID. + /// \param[in] _edge The edge to update. + /// \param[in] _data The new data. + public: void UpdateEdge(Edge &_edge, const EdgeType &_data); + + /// \brief Count the number of vertices with a given local name in the scope. + /// \param[in] _name Local name query + /// \return Number of vertices that have the given local name in the scope. public: size_t Count(const std::string &_name) const; + + /// \brief Get the vertex ID of a vertex with a give local name. + /// \param[in] _name Local name of the vertex + /// \return Vertex ID of the vertex if found in this scope of the graph. + /// Otherwise, kNullId is returned. public: VertexId VertexIdByName(const std::string &_name) const; - public: Vertex ScopeVertex() const; + + /// \brief Get the scope vertex. + /// \return Immutable reference to the scope vertex of this scope. + public: const Vertex &ScopeVertex() const; + + /// \brief Get the scope vertex ID. + /// \return Immutable reference to the scope vertex of this scope. public: VertexId ScopeVertexId() const; - public: bool PointsTo(const std::shared_ptr _graph) const; + + /// \brief Check if the graph on which this scope is based is the same as the + /// input graph. + /// \param[in] _graph Graph object to check. + /// \return True if this scope points to the same graph as the input. + public: bool PointsTo(const std::shared_ptr &_graph) const; + + /// \brief Set the scope name. + /// \param[in] _name New scope name. public: void SetScopeName(const std::string &_name); + + /// \brief Get the current scope name. + /// \return The current scope name. public: const std::string &ScopeName() const; + + /// \brief Add the current prefix to the given name. + /// \param[in] _name Input name. + /// \return The name with the prefix prepended. public: std::string AddPrefix(const std::string &_name) const; - public: std::string RemovePrefix(const std::string &_name) const; + + /// \brief Find the current prefix in the given name. If the prefix is found, + /// remove it. + /// \param[in] _name Input name. + /// \return The name with the prefix removed and true, if the prefix is found. + /// Otherwise, the original name and false + public: std::pair FindAndRemovePrefix( + const std::string &_name) const; + + /// \brief Weak pointer to either a FrameAttachedToGraph or + /// PoseRelativeToGraph. private: std::weak_ptr graphWeak; + + /// \brief Shared pointer to the scope's data. A shared_ptr is used because + /// this data has to be shared among many DOM objects. private: std::shared_ptr dataPtr; }; ///////////////////////////////////////////////// template -ScopedGraph::ScopedGraph(const std::shared_ptr _graph) +ScopedGraph::ScopedGraph(const std::shared_ptr &_graph) : graphWeak(_graph) , dataPtr(std::make_shared()) { @@ -112,14 +259,13 @@ ScopedGraph::ScopedGraph(const std::shared_ptr _graph) ///////////////////////////////////////////////// template -ScopedGraph ScopedGraph::ChildScope( - const std::string &_name, const std::string &_scopeName) +ScopedGraph ScopedGraph::ChildModelScope(const std::string &_name) { auto newScopedGraph = *this; newScopedGraph.dataPtr = std::make_shared(); newScopedGraph.dataPtr->scopeVertexId = this->VertexIdByName(_name); newScopedGraph.dataPtr->prefix = this->AddPrefix(_name); - newScopedGraph.dataPtr->scopeName = _scopeName; + newScopedGraph.dataPtr->scopeName = "__model__"; return newScopedGraph; } @@ -180,24 +326,6 @@ auto ScopedGraph::AddEdge( return edge; } -///////////////////////////////////////////////// -template -auto ScopedGraph::Vertices() const - -> const ignition::math::graph::VertexRef_M -{ - auto graph = graphWeak.lock(); - return graph->graph.Vertices(); -} - -///////////////////////////////////////////////// -template -auto ScopedGraph::Vertices(const std::string &_name) const - -> const ignition::math::graph::VertexRef_M -{ - auto graph = graphWeak.lock(); - return graph->graph.Vertices(this->AddPrefix(_name)); -} - ///////////////////////////////////////////////// template std::vector ScopedGraph::VertexNames() const @@ -205,28 +333,32 @@ std::vector ScopedGraph::VertexNames() const std::vector out; for (const auto &namePair : this->Map()) { - out.push_back(this->RemovePrefix(namePair.first)); + const auto &idNamePair = this->FindAndRemovePrefix(namePair.first); + if (idNamePair.second) + { + out.push_back(idNamePair.first); + } } return out; } ///////////////////////////////////////////////// template -std::string ScopedGraph::VertexName(const Vertex &_vert) const +std::string ScopedGraph::VertexLocalName(const Vertex &_vert) const { - return this->RemovePrefix(_vert.Name()); + return this->FindAndRemovePrefix(_vert.Name()).first; } ///////////////////////////////////////////////// template -std::string ScopedGraph::VertexName(const VertexId &_id) const +std::string ScopedGraph::VertexLocalName(const VertexId &_id) const { - return this->VertexName(this->Graph().VertexFromId(_id)); + return this->VertexLocalName(this->Graph().VertexFromId(_id)); } ///////////////////////////////////////////////// template -void ScopedGraph::UpdateEdge(const Edge &_edge, const EdgeType &_data) +void ScopedGraph::UpdateEdge(Edge &_edge, const EdgeType &_data) { // There's no API to update the data of an edge, so we remove the edge and // insert a new one with the new pose. @@ -234,7 +366,7 @@ void ScopedGraph::UpdateEdge(const Edge &_edge, const EdgeType &_data) auto headVertexId = _edge.Head(); auto &graph = this->graphWeak.lock()->graph; graph.RemoveEdge(_edge.Id()); - graph.AddEdge({tailVertexId, headVertexId}, _data); + _edge = graph.AddEdge({tailVertexId, headVertexId}, _data); } ///////////////////////////////////////////////// @@ -272,7 +404,7 @@ auto ScopedGraph::VertexIdByName(const std::string &_name) const -> VertexId ///////////////////////////////////////////////// template -auto ScopedGraph::ScopeVertex() const -> Vertex +auto ScopedGraph::ScopeVertex() const -> const Vertex & { return this->graphWeak.lock()->graph.VertexFromId( this->dataPtr->scopeVertexId); @@ -287,7 +419,7 @@ auto ScopedGraph::ScopeVertexId() const -> VertexId ///////////////////////////////////////////////// template -bool ScopedGraph::PointsTo(const std::shared_ptr _graph) const +bool ScopedGraph::PointsTo(const std::shared_ptr &_graph) const { return this->graphWeak.lock() == _graph; } @@ -308,17 +440,20 @@ std::string ScopedGraph::AddPrefix(const std::string &_name) const ///////////////////////////////////////////////// template -std::string ScopedGraph::RemovePrefix(const std::string &_name) const +std::pair +ScopedGraph::FindAndRemovePrefix(const std::string &_name) const { - if (!this->dataPtr->prefix.empty()) + if (this->dataPtr->prefix.empty()) { - auto ind = _name.find(this->dataPtr->prefix + "::"); - if (ind == 0) - { - return _name.substr(this->dataPtr->prefix.size() + 2); - } + return std::make_pair(_name, true); + } + + if (0 == + _name.compare(0, this->dataPtr->prefix.size(), this->dataPtr->prefix)) + { + return std::make_pair(_name.substr(this->dataPtr->prefix.size() + 2), true); } - return _name; + return std::make_pair(_name, false); } } } diff --git a/src/World.cc b/src/World.cc index 9193a5964..5808fb1ea 100644 --- a/src/World.cc +++ b/src/World.cc @@ -136,13 +136,13 @@ WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) this->ownedFrameAttachedToGraph = std::make_shared( *(_worldPrivate.ownedFrameAttachedToGraph)); - this->frameAttachedToGraph = this->ownedFrameAttachedToGraph; + this->frameAttachedToGraph = ScopedGraph(this->ownedFrameAttachedToGraph); } if (_worldPrivate.ownedPoseRelativeToGraph) { this->ownedPoseRelativeToGraph = std::make_shared( *(_worldPrivate.ownedPoseRelativeToGraph)); - this->poseRelativeToGraph = this->ownedPoseRelativeToGraph; + this->poseRelativeToGraph = ScopedGraph(this->ownedPoseRelativeToGraph); } if (_worldPrivate.scene) { From e50ae0ba23954a22926a84c972c2e048ac9ea77f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 9 Oct 2020 17:12:52 -0500 Subject: [PATCH 30/70] Add test for placement_frame element that references nested frames Signed-off-by: Addisu Z. Taddese --- .../model.sdf} | 3 + test/integration/model_dom.cc | 21 --- test/integration/nested_model.cc | 153 ++++++++++++++++++ 3 files changed, 156 insertions(+), 21 deletions(-) rename test/{sdf/model_with_nested_placement_frame_attribute.sdf => integration/model/model_with_nested_placement_frame_attribute/model.sdf} (82%) diff --git a/test/sdf/model_with_nested_placement_frame_attribute.sdf b/test/integration/model/model_with_nested_placement_frame_attribute/model.sdf similarity index 82% rename from test/sdf/model_with_nested_placement_frame_attribute.sdf rename to test/integration/model/model_with_nested_placement_frame_attribute/model.sdf index c7d9b6206..51ce83b22 100644 --- a/test/sdf/model_with_nested_placement_frame_attribute.sdf +++ b/test/integration/model/model_with_nested_placement_frame_attribute/model.sdf @@ -9,6 +9,9 @@ 0 2 0 0 0 0 + + 0 0 3 0 0 0 + diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 775912cd0..fddbf0c4c 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -545,27 +545,6 @@ TEST(DOMRoot, ModelPlacementFrameAttribute) EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); } -///////////////////////////////////////////////// -TEST(DOMRoot, ModelNestedPlacementFrameAttribute) -{ - const std::string testFile = - sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", - "model_with_nested_placement_frame_attribute.sdf"); - - // Load the SDF file - sdf::Root root; - sdf::Errors errors = root.Load(testFile); - EXPECT_TRUE(errors.empty()) << errors; - - auto *model = root.ModelByIndex(0); - ASSERT_NE(nullptr, model); - - ignition::math::Pose3d pose; - errors = model->SemanticPose().Resolve(pose); - EXPECT_TRUE(errors.empty()) << errors; - EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); -} - ///////////////////////////////////////////////// TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) { diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 5ea7d1149..6538cc7b6 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -1093,3 +1093,156 @@ TEST(NestedReference, PoseRelativeTo) EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose); } } + +///////////////////////////////////////////////// +TEST(NestedReference, PlacementFrameAttribute) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", + "model", "model_with_nested_placement_frame_attribute", "model.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + auto *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); +} + +///////////////////////////////////////////////// +TEST(NestedReference, PlacementFrameElement) +{ + const std::string modelRootPath = sdf::filesystem::append( + PROJECT_SOURCE_PATH, "test", "integration", "model"); + + sdf::setFindCallback([&](const std::string &_file) + { + return sdf::filesystem::append(modelRootPath, _file); + }); + + // Test with //world/include without overriding the placement_frame + { + std::ostringstream stream; + stream << R"( + + + + model_with_nested_placement_frame_attribute + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); + } + + // Test with //world/include overriding the placement_frame + { + const ignition::math::Pose3d placementPose(0, 10, 0, IGN_PI_2, 0, 0); + std::ostringstream stream; + stream << R"( + + + + model_with_nested_placement_frame_attribute + child_model::link3 + )" << placementPose << R"(" + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + auto *link = model->LinkByName("child_model::link3"); + ASSERT_NE(nullptr, link); + ignition::math::Pose3d pose; + errors = link->SemanticPose().Resolve(pose, model->SemanticPose(), "world"); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(placementPose, pose); + } + + + // Test with //model/include without overriding the placement_frame + { + std::ostringstream stream; + stream << R"( + + + + model_with_nested_placement_frame_attribute + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *parentModel = root.ModelByIndex(0); + ASSERT_NE(nullptr, parentModel ); + auto *model = parentModel ->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + ignition::math::Pose3d pose; + errors = model->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, -2, 10, 0, 0, 0), pose); + } + + // Test with //model/include overriding the placement_frame + { + const ignition::math::Pose3d placementPose(0, 10, 0, IGN_PI_2, 0, 0); + std::ostringstream stream; + stream << R"( + + + + model_with_nested_placement_frame_attribute + child_model::link3 + )" << placementPose << R"(" + + + )"; + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + EXPECT_TRUE(errors.empty()) << errors; + + auto *parentModel = root.ModelByIndex(0); + ASSERT_NE(nullptr, parentModel ); + auto *model = parentModel ->ModelByIndex(0); + ASSERT_NE(nullptr, model); + + auto *link = model->LinkByName("child_model::link3"); + ASSERT_NE(nullptr, link); + ignition::math::Pose3d pose; + errors = + link->SemanticPose().Resolve(pose, model->SemanticPose(), "__model__"); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(placementPose, pose); + } +} From d5551bc9b9a49a7bdf20d5a7d238552001ea4283 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 9 Oct 2020 17:52:31 -0500 Subject: [PATCH 31/70] Add test for empty models that contain a nested static model Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 87 +++++++++++++------------- src/FrameSemantics_TEST.cc | 31 +++++++++ test/integration/model_dom.cc | 5 +- test/integration/nested_model.cc | 6 +- test/sdf/model_nested_static_model.sdf | 11 ++++ 5 files changed, 90 insertions(+), 50 deletions(-) create mode 100644 test/sdf/model_nested_static_model.sdf diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index f45286901..cac9aad1f 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -248,26 +248,6 @@ Errors buildFrameAttachedToGraph( auto &edge = outModel.AddEdge({modelId, modelFrameId}, true); edge.SetWeight(0); - // 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 && !_model->Static()) - { - 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 - return errors; - } // add link vertices for (uint64_t l = 0; l < _model->LinkCount(); ++l) { @@ -383,31 +363,50 @@ Errors buildFrameAttachedToGraph( outModel.AddEdge({frameId, attachedToId}, edgeData); } - - // Identify if the canonical link is in a nested model. - if (!_model->Static() && canonicalLink != nullptr) - { - // 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 = outModel.VertexIdByName(canonicalLinkName); - outModel.AddEdge({modelFrameId, linkId}, true); - } - - if (_model->Static()) + // identify canonical link, which may be nested + auto canonicalLinkAndName = modelCanonicalLinkAndRelativeName(_model); + const sdf::Link *canonicalLink = canonicalLinkAndName.first; + const std::string canonicalLinkName = canonicalLinkAndName.second; + if (!_model->Static()) { - // If the model is static, add an edge from the model frame to the global - // root vertex - // TODO (addisu) Ensure that the global root vertex has an ID of 0. - // if (_root) - // { - // outModel.AddEdge({modelFrameId, rootId}, true); - // } - // else - // { - // outModel.AddEdge({modelFrameId, 0}, true); - // } + if (nullptr == canonicalLink) + { + if (canonicalLinkName.empty()) + { + if (_model->ModelCount() == 0) + { + errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, + "A model must have at least one link."}); + } + else + { + // The canonical link was not found, but the model could have a + // decendant that has a static model, so simply create an edge to the + // first model and let the attached_to frame resolution take care of + // finding the canonical link + auto firstChildModelId = + outModel.VertexIdByName(_model->ModelByIndex(0)->Name()); + outModel.AddEdge({modelFrameId, firstChildModelId }, true); + } + } + else + { + errors.push_back({ErrorCode::MODEL_CANONICAL_LINK_INVALID, + "canonical_link with name[" + canonicalLinkName + + "] not found in model with name[" + _model->Name() + "]."}); + } + // return early + return errors; + } + else + { + // 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 = outModel.VertexIdByName(canonicalLinkName); + outModel.AddEdge({modelFrameId, linkId}, true); + } } return errors; diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index a8e6f29e1..fb3704312 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -541,3 +541,34 @@ TEST(NestedFrameSemantics, buildFrameAttachedToGraph_World) sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F0").empty()); EXPECT_EQ("M2::L", resolvedBody); } + +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, ModelWithoutLinksWithNestedStaticModel) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "model_nested_static_model.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; + + const sdf::Model *model = root.ModelByIndex(0); + ASSERT_NE(nullptr, model); + + auto ownedModelGraph = std::make_shared(); + sdf::ScopedGraph modelGraph(ownedModelGraph); + errors = sdf::buildFrameAttachedToGraph(modelGraph, model); + EXPECT_TRUE(errors.empty()) << errors; + errors = sdf::validateFrameAttachedToGraph(modelGraph); + EXPECT_TRUE(errors.empty()) << errors; + + std::string resolvedBody; + errors = sdf::resolveFrameAttachedToBody( + resolvedBody, modelGraph, "model_nested_static_model"); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ("model_nested_static_model::child_model::static_model::__model__", + resolvedBody); +} + diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index fddbf0c4c..0e9ffac3b 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -546,7 +546,7 @@ TEST(DOMRoot, ModelPlacementFrameAttribute) } ///////////////////////////////////////////////// -TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) +TEST(DOMRoot, LoadInvalidNestedModelWithoutLinks) { const std::string testFile = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", @@ -558,7 +558,7 @@ TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(5u, errors.size()); + EXPECT_EQ(7u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::MODEL_WITHOUT_LINK); EXPECT_NE(std::string::npos, errors[0].Message().find("A model must have at least one link")); @@ -574,4 +574,3 @@ TEST(DOMJoint, LoadInvalidNestedModelWithoutLinks) // errors[8] // errors[9] } - diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 6538cc7b6..4dece9dd7 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -1203,8 +1203,8 @@ TEST(NestedReference, PlacementFrameElement) EXPECT_TRUE(errors.empty()) << errors; auto *parentModel = root.ModelByIndex(0); - ASSERT_NE(nullptr, parentModel ); - auto *model = parentModel ->ModelByIndex(0); + ASSERT_NE(nullptr, parentModel); + auto *model = parentModel->ModelByIndex(0); ASSERT_NE(nullptr, model); ignition::math::Pose3d pose; @@ -1233,7 +1233,7 @@ TEST(NestedReference, PlacementFrameElement) EXPECT_TRUE(errors.empty()) << errors; auto *parentModel = root.ModelByIndex(0); - ASSERT_NE(nullptr, parentModel ); + ASSERT_NE(nullptr, parentModel); auto *model = parentModel ->ModelByIndex(0); ASSERT_NE(nullptr, model); diff --git a/test/sdf/model_nested_static_model.sdf b/test/sdf/model_nested_static_model.sdf new file mode 100644 index 000000000..3bdedec3d --- /dev/null +++ b/test/sdf/model_nested_static_model.sdf @@ -0,0 +1,11 @@ + + + + + + true + + + + + From d51f340859c071fb328c0cddac152512c0d829e5 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 9 Oct 2020 20:05:31 -0500 Subject: [PATCH 32/70] Resolve SemanticPose objects relative to other SemanticPose objects Signed-off-by: Addisu Z. Taddese --- include/sdf/JointAxis.hh | 15 +------- include/sdf/SemanticPose.hh | 24 ++++++++----- include/sdf/Sensor.hh | 1 + src/JointAxis.cc | 49 ++++++++++++-------------- src/Model.cc | 5 ++- src/SemanticPose.cc | 57 ++++++++++++++++-------------- test/integration/nested_model.cc | 59 +++++++++++++++++++++++--------- 7 files changed, 116 insertions(+), 94 deletions(-) diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index cee1ceea1..1b74a05fa 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -268,22 +268,9 @@ namespace sdf const sdf::SemanticPose &_scope, const std::string &_resolveTo) const; - - public: Errors ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::Frame &_resolveTo) const; - - public: Errors ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::Joint &_resolveTo) const; - - public: Errors ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::Link &_resolveTo) const; - public: Errors ResolveXyz( ignition::math::Vector3d &_xyz, - const sdf::Model &_resolveTo) const; + const sdf::SemanticPose &_scope) const; // private: Errors ResolveXyzImpl( // ignition::math::Vector3d &_xyz, diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index b1cae00fb..9479af1cd 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -67,6 +67,20 @@ namespace sdf /// \return The name of the pose relative-to frame. public: const std::string &RelativeTo() const; + /// \brief Get the name of the coordinate frame to which this object's pose + /// will be resolved when the _resolveTo argument is empty. An empty value + /// indicates that the pose will be resolved to to the default parent + /// object. + /// \return The name of the default resolve-to frame. + public: const std::string &DefaultResolveTo() const; + + /// \brief Get the name of the vertex in the PoseRelativeToGraph associated + /// with this object. An empty value indicates that this object is not + /// directly mapped to a vertex, as would be the case for sdf::Collection + /// objects, for example. + /// \return The name of the the vertex assocated with this object. + public: const std::string &VertexName() const; + /// \brief Resolve pose of this object with respect to another named frame. /// If there are any errors resolving the pose, the output will not be /// modified. @@ -78,17 +92,11 @@ namespace sdf const std::string &_resolveTo = "") const; public: Errors Resolve(ignition::math::Pose3d &_pose, - const SemanticPose &_scope, + const SemanticPose &_semPose, const std::string &_resolveTo) const; public: Errors Resolve(ignition::math::Pose3d &_pose, - const sdf::Frame &_resolveTo) const; - public: Errors Resolve(ignition::math::Pose3d &_pose, - const sdf::Joint &_resolveTo) const; - public: Errors Resolve(ignition::math::Pose3d &_pose, - const sdf::Link &_resolveTo) const; - public: Errors Resolve(ignition::math::Pose3d &_pose, - const sdf::Model &_resolveTo) const; + const SemanticPose &_semPose) const; /// \brief Private constructor. /// \param[in] _pose Raw pose of object. diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 98fd11a16..ded5c94cd 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -23,6 +23,7 @@ #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" #include "sdf/Types.hh" +#include "sdf/sdf_config.h" #include "sdf/system_util.hh" namespace sdf diff --git a/src/JointAxis.cc b/src/JointAxis.cc index 3a55a091c..fd94b333a 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -437,44 +437,37 @@ Errors JointAxis::ResolveXyz( ///////////////////////////////////////////////// Errors JointAxis::ResolveXyz( ignition::math::Vector3d &_xyz, - const sdf::Frame &_resolveTo) const + const sdf::SemanticPose &_semPose) const { - return this->ResolveXyz(_xyz, _resolveTo.SemanticPose(), _resolveTo.Name()); -} - -///////////////////////////////////////////////// -Errors JointAxis::ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::Joint &_resolveTo) const -{ - return this->ResolveXyz(_xyz, _resolveTo.SemanticPose(), _resolveTo.Name()); -} - -///////////////////////////////////////////////// -Errors JointAxis::ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::Link &_resolveTo) const -{ - return this->ResolveXyz(_xyz, _resolveTo.SemanticPose(), _resolveTo.Name()); -} - -///////////////////////////////////////////////// -Errors JointAxis::ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::Model &_resolveTo) const -{ - return this->ResolveXyz(_xyz, _resolveTo.SemanticPose(), _resolveTo.Name()); + if (!_semPose.VertexName().empty()) + { + return this->ResolveXyz(_xyz, _semPose, _semPose.VertexName()); + } + else + { + std::string relativeTo = _semPose.RelativeTo(); + if (relativeTo.empty()) + { + relativeTo = _semPose.DefaultResolveTo(); + } + sdf::Errors errors = this->ResolveXyz(_xyz, _semPose, relativeTo); + if (errors.empty()) + { + _xyz = _semPose.RawPose().Rot().Inverse() * _xyz; + } + return errors; + } } ///////////////////////////////////////////////// Errors JointAxis::ResolveXyz( ignition::math::Vector3d &_xyz, - const sdf::SemanticPose &_scope, + const sdf::SemanticPose &_semPose, const std::string &_resolveTo) const { Errors errors; const auto &graph = this->dataPtr->poseRelativeToGraph; - const auto &scopeGraph = _scope.PoseRelativeToGraph(); + const auto &scopeGraph = _semPose.PoseRelativeToGraph(); if (!graph || !scopeGraph) { errors.push_back({ErrorCode::ELEMENT_INVALID, diff --git a/src/Model.cc b/src/Model.cc index 301c1b58a..278f0e0c0 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -88,6 +88,8 @@ class sdf::ModelPrivate /// TODO (addisu) More documentation public: std::shared_ptr ownedPoseGraph; public: sdf::ScopedGraph poseGraph; + /// \brief Scope name of parent Pose Relative-To Graph (world or __model__). + public: std::string parentPoseGraphScopeName; }; ///////////////////////////////////////////////// @@ -758,6 +760,7 @@ Errors Model::SetPoseRelativeToGraph( this->dataPtr->ownedPoseGraph.reset(); } this->dataPtr->poseGraph = _graph; + this->dataPtr->parentPoseGraphScopeName = _graph.ScopeName(); auto childPoseGraph = this->dataPtr->poseGraph.ChildModelScope(this->Name()); @@ -822,7 +825,7 @@ sdf::SemanticPose Model::SemanticPose() const this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, - "", + this->dataPtr->parentPoseGraphScopeName, this->dataPtr->poseGraph); } diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index 675956439..941edc322 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -125,56 +125,55 @@ const std::string &SemanticPose::RelativeTo() const { return this->dataPtr->relativeTo; } -///////////////////////////////////////////////// -Errors SemanticPose::Resolve( - ignition::math::Pose3d &_pose, - const std::string &_resolveTo) const -{ - return this->Resolve(_pose, *this, _resolveTo); -} ///////////////////////////////////////////////// -Errors SemanticPose::Resolve( - ignition::math::Pose3d &_pose, - const sdf::Frame &_resolveTo) const +const std::string &SemanticPose::DefaultResolveTo() const { - return this->Resolve(_pose, _resolveTo.SemanticPose(), _resolveTo.Name()); + return this->dataPtr->defaultResolveTo; } ///////////////////////////////////////////////// Errors SemanticPose::Resolve( ignition::math::Pose3d &_pose, - const sdf::Joint &_resolveTo) const + const std::string &_resolveTo) const { - return this->Resolve(_pose, _resolveTo.SemanticPose(), _resolveTo.Name()); + return this->Resolve(_pose, *this, _resolveTo); } -///////////////////////////////////////////////// Errors SemanticPose::Resolve( ignition::math::Pose3d &_pose, - const sdf::Link &_resolveTo) const + const SemanticPose &_semPose) const { - return this->Resolve(_pose, _resolveTo.SemanticPose(), _resolveTo.Name()); -} - -///////////////////////////////////////////////// -Errors SemanticPose::Resolve( - ignition::math::Pose3d &_pose, - const sdf::Model &_resolveTo) const -{ - return this->Resolve(_pose, _resolveTo.SemanticPose(), _resolveTo.Name()); + if (!_semPose.VertexName().empty()) + { + return this->Resolve(_pose, _semPose, _semPose.VertexName()); + } + else + { + std::string relativeTo = _semPose.RelativeTo(); + if (relativeTo.empty()) + { + relativeTo = _semPose.DefaultResolveTo(); + } + sdf::Errors errors = this->Resolve(_pose, _semPose, relativeTo); + if (errors.empty()) + { + _pose = _semPose.RawPose().Inverse() * _pose; + } + return errors; + } } ///////////////////////////////////////////////// Errors SemanticPose::Resolve( ignition::math::Pose3d &_pose, - const SemanticPose &_scope, + const SemanticPose &_semPose, const std::string &_resolveTo) const { Errors errors; auto graph = this->dataPtr->poseRelativeToGraph; - const auto &scopeGraph = _scope.PoseRelativeToGraph(); + const auto &scopeGraph = _semPose.PoseRelativeToGraph(); if (!graph || !scopeGraph) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, @@ -220,5 +219,11 @@ Errors SemanticPose::Resolve( return errors; } + +///////////////////////////////////////////////// +const std::string &SemanticPose::VertexName() const +{ + return this->dataPtr->name; +} } // inline namespace } // namespace sdf diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 4dece9dd7..d7bc642f8 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -476,7 +476,9 @@ TEST(NestedModel, NestedModelWithFrames) const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose, *parentModel).empty()); + EXPECT_TRUE(link1->SemanticPose() + .Resolve(link1Pose, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(link1ExpPose, link1Pose); const auto *visual1 = link1->VisualByName("V1"); @@ -490,25 +492,33 @@ TEST(NestedModel, NestedModelWithFrames) const auto *link2 = parentModel->LinkByName("M1::L2"); ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose, *parentModel).empty()); + EXPECT_TRUE(link2->SemanticPose() + .Resolve(link2Pose, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(link2ExpPose, link2Pose); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, *parentModel).empty()); + EXPECT_TRUE(joint1->SemanticPose() + .Resolve(joint1Pose, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(joint1ExpPose, joint1Pose); const auto joint1Axis = joint1->Axis(0); ASSERT_NE(nullptr, joint1Axis); Vector3d joint1AxisVector; - EXPECT_TRUE(joint1Axis->ResolveXyz(joint1AxisVector, *parentModel).empty()); + EXPECT_TRUE( + joint1Axis->ResolveXyz(joint1AxisVector, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(joint1AxisExpVector, joint1AxisVector); const auto joint1Axis2 = joint1->Axis(1); ASSERT_NE(nullptr, joint1Axis2); Vector3d joint1Axis2Vector; - EXPECT_TRUE(joint1Axis2->ResolveXyz(joint1Axis2Vector, *parentModel).empty()); + EXPECT_TRUE( + joint1Axis2->ResolveXyz(joint1Axis2Vector, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(joint1Axis2ExpVector, joint1Axis2Vector); } @@ -757,32 +767,41 @@ TEST(NestedModel, NestedModelWithSiblingFrames) const auto *frame1 = parentModel->FrameByName("M1::F1"); ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE( - frame1->SemanticPose().Resolve(frame1Pose, *parentModel).empty()); + EXPECT_TRUE(frame1->SemanticPose() + .Resolve(frame1Pose, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(frame1ExpPose, frame1Pose); const auto *frame2 = parentModel->FrameByName("M1::F2"); ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose().Resolve(frame2Pose, *parentModel).empty()); + EXPECT_TRUE(frame2->SemanticPose() + .Resolve(frame2Pose, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(frame2ExpPose, frame2Pose); const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose().Resolve(link1Pose, *parentModel).empty()); + EXPECT_TRUE(link1->SemanticPose() + .Resolve(link1Pose, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(link1ExpPose, link1Pose); const auto *link2 = parentModel->LinkByName("M1::L2"); ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose().Resolve(link2Pose, *parentModel).empty()); + EXPECT_TRUE(link2->SemanticPose() + .Resolve(link2Pose, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(link2ExpPose, link2Pose); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose().Resolve(joint1Pose, *parentModel).empty()); + EXPECT_TRUE(joint1->SemanticPose() + .Resolve(joint1Pose, parentModel->SemanticPose()) + .empty()); EXPECT_EQ(joint1ExpPose, joint1Pose); } @@ -1054,42 +1073,48 @@ TEST(NestedReference, PoseRelativeTo) Pose pose; const sdf::Model *testModel = model->ModelByName("M2"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_TRUE( + testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); EXPECT_EQ(Pose(1, 0, -2, 0, IGN_PI_2, 0), pose); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M3"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_TRUE( + testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); EXPECT_EQ(Pose(1, 1, -3, 0, IGN_PI_2, 0), pose); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M4"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_TRUE( + testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); EXPECT_EQ(Pose(2, 0, -4, 0, IGN_PI_2, 0), pose); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M5"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_TRUE( + testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); EXPECT_EQ(Pose(2, 0, -6, 0, IGN_PI_2, 0), pose); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M6"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_TRUE( + testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); EXPECT_EQ(Pose(1, 1, -6, IGN_PI_2, IGN_PI_2, 0), pose); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M7"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE(testModel->SemanticPose().Resolve(pose, *model).empty()); + EXPECT_TRUE( + testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose); } } From 4d86a91334a0002dc3862df09d8ec0a0aa187a4a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 9 Oct 2020 20:06:48 -0500 Subject: [PATCH 33/70] Add missing file Signed-off-by: Addisu Z. Taddese --- .../model.config | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 test/integration/model/model_with_nested_placement_frame_attribute/model.config diff --git a/test/integration/model/model_with_nested_placement_frame_attribute/model.config b/test/integration/model/model_with_nested_placement_frame_attribute/model.config new file mode 100644 index 000000000..4514d14ba --- /dev/null +++ b/test/integration/model/model_with_nested_placement_frame_attribute/model.config @@ -0,0 +1,7 @@ + + + + model_with_nested_placement_frame_attribute + model.sdf + + From 7d34a5ef6222c17b5e40ccf4a4b536334dcef9c8 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 13 Oct 2020 22:42:09 -0500 Subject: [PATCH 34/70] Revert the SemanticPose::Resolve API that takes SemanticPose objects This proved to be problematic since the graph contained in the input SemanticPose can be at a different scope than the graph in the current object. Thus it becomes necessary to find the least common ancestor vertex between two vertices in the graph. This is feasible, but it would be best to do it in a separate PR. Signed-off-by: Addisu Z. Taddese --- include/sdf/JointAxis.hh | 20 ---- include/sdf/SemanticPose.hh | 24 ----- src/FrameSemantics.cc | 37 ++++--- src/JointAxis.cc | 84 ---------------- src/Model.cc | 7 +- src/ScopedGraph.hh | 31 +++--- src/SemanticPose.cc | 93 +++--------------- test/integration/nested_model.cc | 161 ++++++++++++------------------- 8 files changed, 117 insertions(+), 340 deletions(-) diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index 1b74a05fa..1690da9f6 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -34,11 +34,6 @@ namespace sdf // Forward declare private data class. class JointAxisPrivate; - class SemanticPose; - class Frame; - class Joint; - class Link; - class Model; struct PoseRelativeToGraph; template class ScopedGraph; @@ -263,21 +258,6 @@ namespace sdf ignition::math::Vector3d &_xyz, const std::string &_resolveTo = "") const; - public: Errors ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::SemanticPose &_scope, - const std::string &_resolveTo) const; - - public: Errors ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::SemanticPose &_scope) const; - - // private: Errors ResolveXyzImpl( - // ignition::math::Vector3d &_xyz, - // sdf::ScopedGraph _graph, - // const std::string &_expressedIn, - // const std::string &_resolveTo) const; - /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 9479af1cd..7062f5f2d 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -67,20 +67,6 @@ namespace sdf /// \return The name of the pose relative-to frame. public: const std::string &RelativeTo() const; - /// \brief Get the name of the coordinate frame to which this object's pose - /// will be resolved when the _resolveTo argument is empty. An empty value - /// indicates that the pose will be resolved to to the default parent - /// object. - /// \return The name of the default resolve-to frame. - public: const std::string &DefaultResolveTo() const; - - /// \brief Get the name of the vertex in the PoseRelativeToGraph associated - /// with this object. An empty value indicates that this object is not - /// directly mapped to a vertex, as would be the case for sdf::Collection - /// objects, for example. - /// \return The name of the the vertex assocated with this object. - public: const std::string &VertexName() const; - /// \brief Resolve pose of this object with respect to another named frame. /// If there are any errors resolving the pose, the output will not be /// modified. @@ -91,13 +77,6 @@ namespace sdf public: Errors Resolve(ignition::math::Pose3d &_pose, const std::string &_resolveTo = "") const; - public: Errors Resolve(ignition::math::Pose3d &_pose, - const SemanticPose &_semPose, - const std::string &_resolveTo) const; - - public: Errors Resolve(ignition::math::Pose3d &_pose, - const SemanticPose &_semPose) const; - /// \brief Private constructor. /// \param[in] _pose Raw pose of object. /// \param[in] _relativeTo Name of frame in graph relative-to which the @@ -118,8 +97,6 @@ namespace sdf const std::string &_defaultResolveTo, const sdf::ScopedGraph &_graph); - private: const sdf::ScopedGraph & - PoseRelativeToGraph() const; /// \brief Destructor public: ~SemanticPose(); @@ -149,7 +126,6 @@ namespace sdf friend class Model; friend class Sensor; friend class Visual; - friend class JointAxis; /// \brief Private data pointer. private: std::unique_ptr dataPtr; diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index cac9aad1f..715681585 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -37,6 +37,7 @@ inline namespace SDF_VERSION_NAMESPACE { void printGraph(const ScopedGraph &_graph) { + std::stringstream ss; std::cout << _graph.Graph() << std::endl; } void printGraph(const ScopedGraph &_graph) @@ -236,14 +237,16 @@ Errors buildFrameAttachedToGraph( auto rootId = ignition::math::graph::kNullId; if (_root) { - rootId = _out.AddScopeVertex( - "", "__root__", scopeName, sdf::FrameType::STATIC_MODEL).Id(); + _out = _out.AddScopeVertex( + "", "__root__", scopeName, sdf::FrameType::STATIC_MODEL); + rootId = _out.ScopeVertexId(); } const auto modelId = _out.AddVertex(_model->Name(), frameType).Id(); - auto outModel = _out.ChildModelScope(_model->Name()); - const auto modelFrameId = outModel.AddVertex(scopeName, frameType).Id(); + auto outModel = + _out.AddScopeVertex(_model->Name(), scopeName, scopeName, frameType); + const auto modelFrameId = outModel.ScopeVertexId(); auto &edge = outModel.AddEdge({modelId, modelFrameId}, true); edge.SetWeight(0); @@ -433,7 +436,7 @@ Errors buildFrameAttachedToGraph( // add implicit world frame vertex first const std::string scopeName = "world"; - _out.AddScopeVertex("", scopeName, scopeName, sdf::FrameType::WORLD); + _out = _out.AddScopeVertex("", scopeName, scopeName, sdf::FrameType::WORLD); // add model vertices for (uint64_t m = 0; m < _world->ModelCount(); ++m) @@ -536,14 +539,15 @@ Errors buildPoseRelativeToGraph( // add the model frame vertex first if (_root) { - rootId = _out.AddScopeVertex( - "", "__root__", scopeName, sdf::FrameType::STATIC_MODEL) - .Id(); + _out = _out.AddScopeVertex( + "", "__root__", scopeName, sdf::FrameType::STATIC_MODEL); + rootId = _out.ScopeVertexId(); } auto modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); + auto outModel = _out.AddScopeVertex( + _model->Name(), scopeName, scopeName, sdf::FrameType::MODEL); + auto modelFrameId = outModel.ScopeVertexId(); - auto outModel = _out.ChildModelScope(_model->Name()); - auto modelFrameId = outModel.AddVertex(scopeName, sdf::FrameType::MODEL).Id(); // Add an aliasing edge from the model vertex to the // corresponding vertex in the PoseRelativeTo graph of the child model, // i.e, to the ::__model__ vertex @@ -874,13 +878,14 @@ Errors buildPoseRelativeToGraph( "Invalid world element in sdf::World."}); return errors; } - - auto rootId = _out.AddVertex("__root__", sdf::FrameType::WORLD).Id(); // add implicit world frame vertex first const std::string sourceName = "world"; - auto worldFrameId = - _out.AddScopeVertex("", sourceName, sourceName, sdf::FrameType::WORLD) - .Id(); + + _out = _out.AddScopeVertex("", "__root__", sourceName, sdf::FrameType::WORLD); + auto rootId = _out.ScopeVertexId(); + + _out = _out.AddScopeVertex("", sourceName, sourceName, sdf::FrameType::WORLD); + auto worldFrameId = _out.ScopeVertexId(); _out.AddEdge({rootId, worldFrameId}, {}); // add model vertices and default edge if relative_to is empty @@ -1448,7 +1453,7 @@ Errors resolveFrameAttachedToBody( { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "Graph has world scope but sink vertex named [" + sinkVertex.Name() + - "] does not have FrameType WORLD, LINK or STATIC_MODEL" + "] does not have FrameType WORLD, LINK or STATIC_MODEL " "when starting from vertex with name [" + _vertexName + "]."}); return errors; diff --git a/src/JointAxis.cc b/src/JointAxis.cc index fd94b333a..4836228d0 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -22,13 +22,7 @@ #include "sdf/Assert.hh" #include "sdf/Error.hh" -#include "sdf/Frame.hh" -#include "sdf/Joint.hh" #include "sdf/JointAxis.hh" -#include "sdf/Link.hh" -#include "sdf/Model.hh" -#include "sdf/SemanticPose.hh" - #include "FrameSemantics.hh" #include "ScopedGraph.hh" @@ -434,84 +428,6 @@ Errors JointAxis::ResolveXyz( return errors; } -///////////////////////////////////////////////// -Errors JointAxis::ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::SemanticPose &_semPose) const -{ - if (!_semPose.VertexName().empty()) - { - return this->ResolveXyz(_xyz, _semPose, _semPose.VertexName()); - } - else - { - std::string relativeTo = _semPose.RelativeTo(); - if (relativeTo.empty()) - { - relativeTo = _semPose.DefaultResolveTo(); - } - sdf::Errors errors = this->ResolveXyz(_xyz, _semPose, relativeTo); - if (errors.empty()) - { - _xyz = _semPose.RawPose().Rot().Inverse() * _xyz; - } - return errors; - } -} - -///////////////////////////////////////////////// -Errors JointAxis::ResolveXyz( - ignition::math::Vector3d &_xyz, - const sdf::SemanticPose &_semPose, - const std::string &_resolveTo) const -{ - Errors errors; - const auto &graph = this->dataPtr->poseRelativeToGraph; - const auto &scopeGraph = _semPose.PoseRelativeToGraph(); - if (!graph || !scopeGraph) - { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "JointAxis has invalid pointer to PoseRelativeToGraph."}); - return errors; - } - if (this->dataPtr->xmlParentName.empty()) - { - errors.push_back({ErrorCode::ELEMENT_INVALID, - "JointAxis has invalid name of xml parent object."}); - return errors; - } - - // JointAxis is not in the graph, but its XyzExpressedIn() name should be. - // If XyzExpressedIn() is empty, use the name of the xml parent object. - std::string axisExpressedIn = this->XyzExpressedIn(); - if (axisExpressedIn.empty()) - { - axisExpressedIn = this->dataPtr->xmlParentName; - } - - auto axisExpressedInVertexId = graph.VertexIdByName(axisExpressedIn); - - const ignition::math::graph::VertexId resolveToVertexId = [&] - { - if (_resolveTo.empty()) - { - return graph.VertexIdByName(this->dataPtr->xmlParentName); - } - return scopeGraph.VertexIdByName(_resolveTo); - }(); - - ignition::math::Pose3d pose; - errors = - resolvePose(pose, scopeGraph, axisExpressedInVertexId, resolveToVertexId); - - if (errors.empty()) - { - _xyz = pose.Rot() * this->Xyz(); - } - - return errors; -} - ///////////////////////////////////////////////// sdf::ElementPtr JointAxis::Element() const { diff --git a/src/Model.cc b/src/Model.cc index 278f0e0c0..245666b48 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -89,7 +89,7 @@ class sdf::ModelPrivate public: std::shared_ptr ownedPoseGraph; public: sdf::ScopedGraph poseGraph; /// \brief Scope name of parent Pose Relative-To Graph (world or __model__). - public: std::string parentPoseGraphScopeName; + public: std::string poseGraphScopeVertexName; }; ///////////////////////////////////////////////// @@ -760,7 +760,8 @@ Errors Model::SetPoseRelativeToGraph( this->dataPtr->ownedPoseGraph.reset(); } this->dataPtr->poseGraph = _graph; - this->dataPtr->parentPoseGraphScopeName = _graph.ScopeName(); + this->dataPtr->poseGraphScopeVertexName = + _graph.VertexLocalName(_graph.ScopeVertexId()); auto childPoseGraph = this->dataPtr->poseGraph.ChildModelScope(this->Name()); @@ -825,7 +826,7 @@ sdf::SemanticPose Model::SemanticPose() const this->dataPtr->name, this->dataPtr->pose, this->dataPtr->poseRelativeTo, - this->dataPtr->parentPoseGraphScopeName, + this->dataPtr->poseGraphScopeVertexName, this->dataPtr->poseGraph); } diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index eb2deba10..de9b58aba 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -152,7 +152,7 @@ class ScopedGraph /// \param[in] _name The Name of the scope vertex to be added. The full name /// of the vertex will be newPrefix::_name, where newPrefix is the prefix /// obtained by adding _prefix to the existing prefix of the scope. - public: Vertex &AddScopeVertex(const std::string &_prefix, + public: ScopedGraph AddScopeVertex(const std::string &_prefix, const std::string &_name, const std::string &_scopeName, const VertexType &_data); @@ -210,7 +210,7 @@ class ScopedGraph public: const Vertex &ScopeVertex() const; /// \brief Get the scope vertex ID. - /// \return Immutable reference to the scope vertex of this scope. + /// \return The ID of the scope vertex of this scope. public: VertexId ScopeVertexId() const; /// \brief Check if the graph on which this scope is based is the same as the @@ -263,8 +263,9 @@ ScopedGraph ScopedGraph::ChildModelScope(const std::string &_name) { auto newScopedGraph = *this; newScopedGraph.dataPtr = std::make_shared(); - newScopedGraph.dataPtr->scopeVertexId = this->VertexIdByName(_name); newScopedGraph.dataPtr->prefix = this->AddPrefix(_name); + newScopedGraph.dataPtr->scopeVertexId = + newScopedGraph.VertexIdByName("__model__"); newScopedGraph.dataPtr->scopeName = "__model__"; return newScopedGraph; } @@ -292,15 +293,17 @@ auto ScopedGraph::Map() const -> const MapType & ///////////////////////////////////////////////// template -auto ScopedGraph::AddScopeVertex(const std::string &_prefix, +ScopedGraph ScopedGraph::AddScopeVertex(const std::string &_prefix, const std::string &_name, const std::string &_scopeName, - const VertexType &_data) -> Vertex & + const VertexType &_data) { - this->dataPtr->prefix = this->AddPrefix(_prefix); - Vertex &vert = this->AddVertex(_name, _data); - this->dataPtr->scopeVertexId = vert.Id(); - this->SetScopeName(_scopeName); - return vert; + auto newScopedGraph = *this; + newScopedGraph.dataPtr = std::make_shared(); + newScopedGraph.dataPtr->prefix = this->AddPrefix(_prefix); + Vertex &vert = newScopedGraph.AddVertex(_name, _data); + newScopedGraph.dataPtr->scopeVertexId = vert.Id(); + newScopedGraph.SetScopeName(_scopeName); + return newScopedGraph; } ///////////////////////////////////////////////// @@ -448,10 +451,12 @@ ScopedGraph::FindAndRemovePrefix(const std::string &_name) const return std::make_pair(_name, true); } - if (0 == - _name.compare(0, this->dataPtr->prefix.size(), this->dataPtr->prefix)) + // Convenient alias + const std::string &prefix = this->dataPtr->prefix; + if (_name.size() > prefix.size() + 2 && + (0 == _name.compare(0, prefix.size(), prefix))) { - return std::make_pair(_name.substr(this->dataPtr->prefix.size() + 2), true); + return std::make_pair(_name.substr(prefix.size() + 2), true); } return std::make_pair(_name, false); } diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index 941edc322..37a617b5f 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -17,12 +17,8 @@ #include #include #include "sdf/Assert.hh" -#include "sdf/Error.hh" -#include "sdf/Frame.hh" -#include "sdf/Joint.hh" -#include "sdf/Link.hh" -#include "sdf/Model.hh" #include "sdf/SemanticPose.hh" +#include "sdf/Error.hh" #include "sdf/Types.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" @@ -107,13 +103,6 @@ SemanticPose &SemanticPose::operator=(const SemanticPose &_semanticPose) return *this; } -///////////////////////////////////////////////// -const sdf::ScopedGraph & -SemanticPose::PoseRelativeToGraph() const -{ - return this->dataPtr->poseRelativeToGraph; -} - ///////////////////////////////////////////////// const ignition::math::Pose3d &SemanticPose::RawPose() const { @@ -126,90 +115,42 @@ const std::string &SemanticPose::RelativeTo() const return this->dataPtr->relativeTo; } -///////////////////////////////////////////////// -const std::string &SemanticPose::DefaultResolveTo() const -{ - return this->dataPtr->defaultResolveTo; -} - -///////////////////////////////////////////////// -Errors SemanticPose::Resolve( - ignition::math::Pose3d &_pose, - const std::string &_resolveTo) const -{ - return this->Resolve(_pose, *this, _resolveTo); -} - -Errors SemanticPose::Resolve( - ignition::math::Pose3d &_pose, - const SemanticPose &_semPose) const -{ - if (!_semPose.VertexName().empty()) - { - return this->Resolve(_pose, _semPose, _semPose.VertexName()); - } - else - { - std::string relativeTo = _semPose.RelativeTo(); - if (relativeTo.empty()) - { - relativeTo = _semPose.DefaultResolveTo(); - } - sdf::Errors errors = this->Resolve(_pose, _semPose, relativeTo); - if (errors.empty()) - { - _pose = _semPose.RawPose().Inverse() * _pose; - } - return errors; - } -} - ///////////////////////////////////////////////// Errors SemanticPose::Resolve( ignition::math::Pose3d &_pose, - const SemanticPose &_semPose, const std::string &_resolveTo) const { Errors errors; auto graph = this->dataPtr->poseRelativeToGraph; - const auto &scopeGraph = _semPose.PoseRelativeToGraph(); - if (!graph || !scopeGraph) + if (!graph) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "SemanticPose has invalid pointer to PoseRelativeToGraph."}); return errors; } - const ignition::math::graph::VertexId resolveToVertexId = [&] + std::string relativeTo = this->dataPtr->relativeTo; + if (relativeTo.empty()) { - if (_resolveTo.empty()) - { - if (this->dataPtr->defaultResolveTo.empty()) - return graph.ScopeVertex().Id(); - else - return graph.VertexIdByName(this->dataPtr->defaultResolveTo); - } - return scopeGraph.VertexIdByName(_resolveTo); - }(); + relativeTo = this->dataPtr->defaultResolveTo; + } + + std::string resolveTo = _resolveTo; + if (resolveTo.empty()) + { + resolveTo = this->dataPtr->defaultResolveTo; + } ignition::math::Pose3d pose; if (this->dataPtr->name.empty()) { - std::string relativeTo = this->dataPtr->relativeTo; - if (relativeTo.empty()) - { - relativeTo = this->dataPtr->defaultResolveTo; - } - auto relativeToVertexId = graph.VertexIdByName(relativeTo); - errors = - resolvePose(pose, scopeGraph, relativeToVertexId, resolveToVertexId); + errors = resolvePose(pose, graph, relativeTo, resolveTo); pose *= this->RawPose(); } else { - const auto frameVertexId = graph.VertexIdByName(this->dataPtr->name); - errors = resolvePose(pose, scopeGraph, frameVertexId, resolveToVertexId); + errors = resolvePose(pose, graph, this->dataPtr->name, resolveTo); } if (errors.empty()) @@ -219,11 +160,5 @@ Errors SemanticPose::Resolve( return errors; } - -///////////////////////////////////////////////// -const std::string &SemanticPose::VertexName() const -{ - return this->dataPtr->name; -} } // inline namespace } // namespace sdf diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index d7bc642f8..18e19acf1 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -452,34 +452,20 @@ TEST(NestedModel, NestedModelWithFrames) Vector3d joint1AxisExpVector = frame2ExpPose.Rot() * Vector3d::UnitZ; Vector3d joint1Axis2ExpVector = frame2ExpPose.Rot() * Vector3d::UnitX; - const auto *frame1 = parentModel->FrameByName("M1::F1"); - ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; + auto parentSemPose = parentModel->SemanticPose(); + EXPECT_TRUE(parentSemPose.Resolve(frame1Pose, "ParentModel::M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); - EXPECT_TRUE( - frame1->SemanticPose() - .Resolve(frame1Pose, parentModel->SemanticPose(), parentModel->Name()) - .empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); - - const auto *frame2 = parentModel->FrameByName("M1::F2"); - ASSERT_NE(nullptr, frame2); - // Test using childModel's SemanticPose with the resolveTo argument of - // "__model__". This should have the same result as using parentModel's - // SemanticPose with the resolveTo argument of parentModel->Name() Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose() - .Resolve(frame2Pose, childModel->SemanticPose(), "__model__") - .empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame2Pose, "ParentModel::M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose() - .Resolve(link1Pose, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(link1ExpPose, link1Pose); + EXPECT_TRUE(parentSemPose.Resolve(link1Pose, "ParentModel::M1::L1").empty()); + EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); const auto *visual1 = link1->VisualByName("V1"); ASSERT_NE(nullptr, visual1); @@ -492,34 +478,26 @@ TEST(NestedModel, NestedModelWithFrames) const auto *link2 = parentModel->LinkByName("M1::L2"); ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose() - .Resolve(link2Pose, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(link2ExpPose, link2Pose); + EXPECT_TRUE(parentSemPose.Resolve(link2Pose, "ParentModel::M1::L2").empty()); + EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose() - .Resolve(joint1Pose, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose); + EXPECT_TRUE(parentSemPose.Resolve(joint1Pose, "ParentModel::M1::J1").empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); const auto joint1Axis = joint1->Axis(0); ASSERT_NE(nullptr, joint1Axis); Vector3d joint1AxisVector; - EXPECT_TRUE( - joint1Axis->ResolveXyz(joint1AxisVector, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(joint1AxisExpVector, joint1AxisVector); + EXPECT_TRUE(joint1Axis->ResolveXyz(joint1AxisVector, "__model__").empty()); + EXPECT_EQ(joint1AxisExpVector, model1Pose.Rot() * joint1AxisVector); const auto joint1Axis2 = joint1->Axis(1); ASSERT_NE(nullptr, joint1Axis2); Vector3d joint1Axis2Vector; - EXPECT_TRUE( - joint1Axis2->ResolveXyz(joint1Axis2Vector, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(joint1Axis2ExpVector, joint1Axis2Vector); + EXPECT_TRUE(joint1Axis2->ResolveXyz(joint1Axis2Vector, "__model__").empty()); + EXPECT_EQ(joint1Axis2ExpVector, model1Pose.Rot() * joint1Axis2Vector); } // Function to remove any element in //joint/axis that is not @@ -764,45 +742,37 @@ TEST(NestedModel, NestedModelWithSiblingFrames) nestedModel1->SemanticPose().Resolve(nestedModel1FramePose).empty()); EXPECT_EQ(nestedModel1FrameExpPose, nestedModel1FramePose); + auto parentSemPose = parentModel->SemanticPose(); + const auto *frame1 = parentModel->FrameByName("M1::F1"); ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE(frame1->SemanticPose() - .Resolve(frame1Pose, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame1Pose, "ParentModel::M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); const auto *frame2 = parentModel->FrameByName("M1::F2"); ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE(frame2->SemanticPose() - .Resolve(frame2Pose, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame2Pose, "ParentModel::M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); const auto *link1 = parentModel->LinkByName("M1::L1"); ASSERT_NE(nullptr, link1); Pose3d link1Pose; - EXPECT_TRUE(link1->SemanticPose() - .Resolve(link1Pose, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(link1ExpPose, link1Pose); + EXPECT_TRUE(parentSemPose.Resolve(link1Pose, "ParentModel::M1::L1").empty()); + EXPECT_EQ(link1ExpPose, link1Pose.Inverse()); const auto *link2 = parentModel->LinkByName("M1::L2"); ASSERT_NE(nullptr, link2); Pose3d link2Pose; - EXPECT_TRUE(link2->SemanticPose() - .Resolve(link2Pose, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(link2ExpPose, link2Pose); + EXPECT_TRUE(parentSemPose.Resolve(link2Pose, "ParentModel::M1::L2").empty()); + EXPECT_EQ(link2ExpPose, link2Pose.Inverse()); const auto *joint1 = parentModel->JointByName("M1::J1"); ASSERT_NE(nullptr, joint1); Pose3d joint1Pose; - EXPECT_TRUE(joint1->SemanticPose() - .Resolve(joint1Pose, parentModel->SemanticPose()) - .empty()); - EXPECT_EQ(joint1ExpPose, joint1Pose); + EXPECT_TRUE(parentSemPose.Resolve(joint1Pose, "ParentModel::M1::J1").empty()); + EXPECT_EQ(joint1ExpPose, joint1Pose.Inverse()); } ////////////////////////////////////////////////// @@ -846,29 +816,21 @@ TEST(NestedModel, NestedFrameOnlyModel) const sdf::Model *parentModel = world->ModelByIndex(0); ASSERT_NE(nullptr, parentModel); + auto parentSemPose = parentModel->SemanticPose(); + using ignition::math::Pose3d; using ignition::math::Vector3d; // Expected poses for frames after nesting. Pose3d frame1ExpPose = model1Pose * Pose3d(0, 0, 0, IGN_PI, 0, 0); Pose3d frame2ExpPose = frame1ExpPose * Pose3d(0, 0, 0, 0, IGN_PI, 0); - const auto *frame1 = parentModel->FrameByName("M1::F1"); - ASSERT_NE(nullptr, frame1); Pose3d frame1Pose; - EXPECT_TRUE( - frame1->SemanticPose() - .Resolve(frame1Pose, parentModel->SemanticPose(), parentModel->Name()) - .empty()); - EXPECT_EQ(frame1ExpPose, frame1Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame1Pose, "ParentModel::M1::F1").empty()); + EXPECT_EQ(frame1ExpPose, frame1Pose.Inverse()); - const auto *frame2 = parentModel->FrameByName("M1::F2"); - ASSERT_NE(nullptr, frame2); Pose3d frame2Pose; - EXPECT_TRUE( - frame2->SemanticPose() - .Resolve(frame2Pose, parentModel->SemanticPose(), parentModel->Name()) - .empty()); - EXPECT_EQ(frame2ExpPose, frame2Pose); + EXPECT_TRUE(parentSemPose.Resolve(frame2Pose, "ParentModel::M1::F2").empty()); + EXPECT_EQ(frame2ExpPose, frame2Pose.Inverse()); } class PlacementFrame: public ::testing::Test @@ -978,13 +940,13 @@ class PlacementFrame: public ::testing::Test Pose3d testFramePose; { - sdf::Errors errors = testFrame->SemanticPose().Resolve( - testFramePose, parentModel->SemanticPose(), _parentModelName); + sdf::Errors errors = parentModel->SemanticPose().Resolve( + testFramePose, parentModel->Name() + "::" + _testFrameName); EXPECT_TRUE(errors.empty()) << errors; } // The expected pose of the test frame is precisely the placement pose - EXPECT_EQ(placementPose, testFramePose); + EXPECT_EQ(placementPose, testFramePose.Inverse()); } protected: sdf::Root root; @@ -1067,55 +1029,50 @@ TEST(NestedReference, PoseRelativeTo) const sdf::Model *model = root.ModelByIndex(0); ASSERT_NE(nullptr, model); + auto parentSemPose = model->SemanticPose(); // The child models starting from M2 all reference an entity inside M1. Check // that resolving poses works when referencing such entities. { Pose pose; const sdf::Model *testModel = model->ModelByName("M2"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE( - testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); - EXPECT_EQ(Pose(1, 0, -2, 0, IGN_PI_2, 0), pose); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M2").empty()); + EXPECT_EQ(Pose(1, 0, -2, 0, IGN_PI_2, 0), pose.Inverse()); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M3"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE( - testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); - EXPECT_EQ(Pose(1, 1, -3, 0, IGN_PI_2, 0), pose); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M3").empty()); + EXPECT_EQ(Pose(1, 1, -3, 0, IGN_PI_2, 0), pose.Inverse()); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M4"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE( - testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); - EXPECT_EQ(Pose(2, 0, -4, 0, IGN_PI_2, 0), pose); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M4").empty()); + EXPECT_EQ(Pose(2, 0, -4, 0, IGN_PI_2, 0), pose.Inverse()); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M5"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE( - testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); - EXPECT_EQ(Pose(2, 0, -6, 0, IGN_PI_2, 0), pose); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M5").empty()); + EXPECT_EQ(Pose(2, 0, -6, 0, IGN_PI_2, 0), pose.Inverse()); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M6"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE( - testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); - EXPECT_EQ(Pose(1, 1, -6, IGN_PI_2, IGN_PI_2, 0), pose); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M6").empty()); + EXPECT_EQ(Pose(1, 1, -6, IGN_PI_2, IGN_PI_2, 0), pose.Inverse()); } { Pose pose; const sdf::Model *testModel = model->ModelByName("M7"); ASSERT_NE(nullptr, testModel); - EXPECT_TRUE( - testModel->SemanticPose().Resolve(pose, model->SemanticPose()).empty()); - EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose); + EXPECT_TRUE(parentSemPose.Resolve(pose, "parent_model::M7").empty()); + EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose.Inverse()); } } @@ -1185,6 +1142,7 @@ TEST(NestedReference, PlacementFrameElement) stream << R"( + model_with_nested_placement_frame_attribute child_model::link3 @@ -1199,15 +1157,14 @@ TEST(NestedReference, PlacementFrameElement) auto *world = root.WorldByIndex(0); ASSERT_NE(nullptr, world); - auto *model = world->ModelByIndex(0); - ASSERT_NE(nullptr, model); - auto *link = model->LinkByName("child_model::link3"); - ASSERT_NE(nullptr, link); + auto *world_frame = world->FrameByName("world_frame"); + ASSERT_NE(nullptr, world_frame); ignition::math::Pose3d pose; - errors = link->SemanticPose().Resolve(pose, model->SemanticPose(), "world"); + errors = world_frame->SemanticPose().Resolve( + pose, "model_placement_frame::child_model::link3"); EXPECT_TRUE(errors.empty()) << errors; - EXPECT_EQ(placementPose, pose); + EXPECT_EQ(placementPose, pose.Inverse()); } @@ -1245,6 +1202,7 @@ TEST(NestedReference, PlacementFrameElement) stream << R"( + model_with_nested_placement_frame_attribute child_model::link3 @@ -1262,12 +1220,13 @@ TEST(NestedReference, PlacementFrameElement) auto *model = parentModel ->ModelByIndex(0); ASSERT_NE(nullptr, model); + auto *parentModelFrame = parentModel->FrameByName("parent_model_frame"); auto *link = model->LinkByName("child_model::link3"); ASSERT_NE(nullptr, link); ignition::math::Pose3d pose; - errors = - link->SemanticPose().Resolve(pose, model->SemanticPose(), "__model__"); + errors = parentModelFrame->SemanticPose().Resolve( + pose, "model_placement_frame::child_model::link3"); EXPECT_TRUE(errors.empty()) << errors; - EXPECT_EQ(placementPose, pose); + EXPECT_EQ(placementPose, pose.Inverse()); } } From 116867280f93d3f9ec9ec18b8d0388e7b123a9b9 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 13 Oct 2020 23:07:43 -0500 Subject: [PATCH 35/70] Remove addNestedModel Signed-off-by: Addisu Z. Taddese --- include/sdf/parser.hh | 25 ----- src/parser.cc | 177 -------------------------------- src/parser_TEST.cc | 234 ------------------------------------------ 3 files changed, 436 deletions(-) diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index d17bdcdb8..5e33a8ef7 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -185,31 +185,6 @@ namespace sdf SDFORMAT_VISIBLE std::string getModelFilePath(const std::string &_modelDirPath); - /// \brief Copy the contents of the first model element from one ElementPtr - /// to another ElementPtr, prepending the copied model name with `::` to - /// link and joint names, and apply the model pose to the copied link poses. - /// If //xyz/@expressed_in == "__model__" for the axes of any copied joints, - /// then apply the model pose rotation to those joint axes. - /// \param[in] _sdf ElementPtr for model into which the elements will be - /// copied. - /// \param[in] _includeSDF The first model element from this ElementPtr will - /// be copied to _sdf with the mentioned name and pose transformations. - SDFORMAT_VISIBLE - void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF); - - /// \brief Copy the contents of the first model element from one ElementPtr - /// to another ElementPtr, prepending the copied model name with `::` to - /// link and joint names, and apply the model pose to the copied link poses. - /// If //xyz/@expressed_in == "__model__" for the axes of any copied joints, - /// then apply the model pose rotation to those joint axes. - /// \param[in] _sdf ElementPtr for model into which the elements will be - /// copied. - /// \param[in] _includeSDF The first model element from this ElementPtr will - /// be copied to _sdf with the mentioned name and pose transformations. - /// \param[out] _errors Any errors will be appended to this variable. - SDFORMAT_VISIBLE - void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors); - /// \brief Convert an SDF file to a specific SDF version. /// \param[in] _filename Name of the SDF file to convert. /// \param[in] _version Version to convert _filename to. diff --git a/src/parser.cc b/src/parser.cc index a82862355..85394b465 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1148,26 +1148,6 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) return true; } -///////////////////////////////////////////////// -static void replace_all(std::string &_str, - const std::string &_from, - const std::string &_to) -{ - if (_from.empty()) - { - return; - } - size_t start_pos = 0; - while ((start_pos = _str.find(_from, start_pos)) != std::string::npos) - { - _str.replace(start_pos, _from.length(), _to); - // We need to advance our starting position beyond what we - // just replaced to deal with the case where the '_to' string - // happens to contain a piece of '_from'. - start_pos += _to.length(); - } -} - ///////////////////////////////////////////////// void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml, @@ -1227,163 +1207,6 @@ void copyChildren(ElementPtr _sdf, } } -///////////////////////////////////////////////// -void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF) -{ - Errors errors; - addNestedModel(_sdf, _includeSDF, errors); - for (const auto &e : errors) - { - sdferr << e << '\n'; - } -} - -///////////////////////////////////////////////// -void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors) -{ - ElementPtr modelPtr = _includeSDF->GetElement("model"); - ElementPtr elem = modelPtr->GetFirstElement(); - std::map replace; - - ignition::math::Pose3d modelPose = - modelPtr->Get("pose"); - - std::string modelName = modelPtr->Get("name"); - - // Inject a frame to represent the nested __model__ frame. - ElementPtr nestedModelFrame = _sdf->AddElement("frame"); - const std::string nestedModelFrameName = modelName + "::__model__"; - nestedModelFrame->GetAttribute("name")->Set(nestedModelFrameName); - - replace["__model__"] = nestedModelFrameName; - - if (modelPtr->GetAttribute("placement_frame")->GetSet()) - { - const std::string placementFrameName = - modelPtr->GetAttribute("placement_frame")->GetAsString(); - - if (!placementFrameName.empty()) - { - // The placementFrameShimElement frame is added as a way to inject the - // placement frame information into expanded nested models. This will not - // be necessary when included nested models work as directly nested - // models. See - // https://github.com/osrf/sdformat/issues/319#issuecomment-665214004 - // TODO (addisu) Remove placementFrameIdentifier once PR addressing - // https://github.com/osrf/sdformat/issues/284 lands - ElementPtr placementFrameShimElement = _sdf->AddElement("frame"); - placementFrameShimElement->GetAttribute("name")->Set( - modelName + "::__placement_frame__"); - placementFrameShimElement->GetAttribute("attached_to") - ->Set(modelName + "::" + placementFrameName); - } - } - - std::string canonicalLinkName = ""; - if (modelPtr->GetAttribute("canonical_link")->GetSet()) - { - canonicalLinkName = modelPtr->GetAttribute("canonical_link")->GetAsString(); - } - else if (modelPtr->HasElement("link")) - { - canonicalLinkName = - modelPtr->GetElement("link")->GetAttribute("name")->GetAsString(); - } - nestedModelFrame->GetAttribute("attached_to") - ->Set(modelName + "::" + canonicalLinkName); - - ElementPtr nestedModelFramePose = nestedModelFrame->AddElement("pose"); - nestedModelFramePose->Set(modelPose); - - // Set the nestedModelFrame's //pose/@relative_to to the frame used in - // //include/pose/@relative_to. - std::string modelPoseRelativeTo = ""; - if (modelPtr->HasElement("pose")) - { - modelPoseRelativeTo = - modelPtr->GetElement("pose")->Get("relative_to"); - } - - // If empty, use "__model__", since leaving it empty would make it - // relative_to the canonical link frame specified in //frame/@attached_to. - if (modelPoseRelativeTo.empty()) - { - modelPoseRelativeTo = "__model__"; - } - - nestedModelFramePose->GetAttribute("relative_to")->Set(modelPoseRelativeTo); - - while (elem) - { - if ((elem->GetName() == "link") || - (elem->GetName() == "joint") || - (elem->GetName() == "frame")) - { - std::string elemName = elem->Get("name"); - std::string newName = modelName + "::" + elemName; - replace[elemName] = newName; - } - - if ((elem->GetName() == "link")) - { - // Add a pose element even if the element doesn't originally have one - auto elemPose = elem->GetElement("pose"); - - // If //pose/@relative_to is empty, explicitly set it to the name - // of the nested model frame. - auto relativeTo = elemPose->GetAttribute("relative_to"); - if (relativeTo->GetAsString().empty()) - { - relativeTo->Set(nestedModelFrameName); - } - - // If //pose/@relative_to is set, let the replacement step handle it. - } - else if (elem->GetName() == "frame") - { - // If //frame/@attached_to is empty, explicitly set it to the name - // of the nested model frame. - auto attachedTo = elem->GetAttribute("attached_to"); - if (attachedTo->GetAsString().empty()) - { - attachedTo->Set(nestedModelFrameName); - } - - // If //frame/@attached_to is set, let the replacement step handle it. - } - elem = elem->GetNextElement(); - } - - std::string str = _includeSDF->ToString(""); - for (std::map::iterator iter = replace.begin(); - iter != replace.end(); ++iter) - { - replace_all(str, std::string("\"") + iter->first + "\"", - std::string("\"") + iter->second + "\""); - replace_all(str, std::string("'") + iter->first + "'", - std::string("'") + iter->second + "'"); - replace_all(str, std::string(">") + iter->first + "<", - std::string(">") + iter->second + "<"); - } - - _includeSDF->ClearElements(); - readString(str, _includeSDF, _errors); - - elem = _includeSDF->GetElement("model")->GetFirstElement(); - ElementPtr nextElem; - while (elem) - { - nextElem = elem->GetNextElement(); - - if (elem->GetName() != "pose") - { - elem->SetParent(_sdf); - _sdf->InsertElement(elem); - } - elem = nextElem; - } -} - ///////////////////////////////////////////////// bool convertFile(const std::string &_filename, const std::string &_version, SDFPtr _sdf) diff --git a/src/parser_TEST.cc b/src/parser_TEST.cc index d41355beb..f9649d9fc 100644 --- a/src/parser_TEST.cc +++ b/src/parser_TEST.cc @@ -110,240 +110,6 @@ TEST(Parser, readFileConversions) } } -///////////////////////////////////////////////// -TEST(Parser, addNestedModel) -{ - auto getIncludedModelSdfString = []( - const std::string &_version, - const std::string &_expressedIn = "", - const std::string &_canonicalLink = "") -> std::string - { - std::string modelAttributeString = ""; - if (!_canonicalLink.empty()) - { - modelAttributeString = " canonical_link='" + _canonicalLink + "'"; - } - std::ostringstream stream; - stream - << "" - << "" - << " 0 0 10 0 0 1.57" - << " " - << " " - << " " - << " parent" - << " child" - << " "; - if (_expressedIn.empty()) - { - stream << "1 0 0"; - } - else - { - stream << "1 0 0"; - } - stream - << " " - << " " - << "" - << ""; - return stream.str(); - }; - - auto checkNestedModel = []( - sdf::ElementPtr _elem, const std::string &_expressedIn = "", - const std::string &_canonicalLink = "included::parent") - { - EXPECT_TRUE(_elem->HasElement("frame")); - sdf::ElementPtr nestedModelFrame = _elem->GetElement("frame"); - EXPECT_EQ(nullptr, nestedModelFrame->GetNextElement("frame")); - - sdf::ElementPtr link1 = _elem->GetElement("link"); - sdf::ElementPtr link2 = link1->GetNextElement("link"); - EXPECT_EQ(nullptr, link2->GetNextElement("link")); - - EXPECT_TRUE(_elem->HasElement("joint")); - sdf::ElementPtr joint = _elem->GetElement("joint"); - EXPECT_EQ(nullptr, joint->GetNextElement("joint")); - - EXPECT_EQ( - "included::__model__", nestedModelFrame->Get("name")); - EXPECT_EQ("included::parent", link1->Get("name")); - EXPECT_EQ("included::child", link2->Get("name")); - EXPECT_EQ("included::joint", joint->Get("name")); - EXPECT_EQ("included::parent", joint->Get("parent")); - EXPECT_EQ("included::child", joint->Get("child")); - - EXPECT_EQ(_canonicalLink, - nestedModelFrame->Get("attached_to")); - using ignition::math::Pose3d; - const Pose3d pose(0, 0, 10, 0, 0, 1.57); - EXPECT_EQ(pose, nestedModelFrame->Get("pose")); - - EXPECT_FALSE(_elem->HasElement("pose")); - EXPECT_EQ("included::__model__", - link1->GetElement("pose")->Get("relative_to")); - EXPECT_EQ("included::__model__", - link2->GetElement("pose")->Get("relative_to")); - EXPECT_EQ(Pose3d::Zero, link1->Get("pose")); - EXPECT_EQ(Pose3d::Zero, link2->Get("pose")); - EXPECT_EQ(Pose3d::Zero, joint->Get("pose")); - - EXPECT_TRUE(joint->HasElement("axis")); - sdf::ElementPtr axis = joint->GetElement("axis"); - EXPECT_TRUE(axis->HasElement("xyz")); - sdf::ElementPtr xyz = axis->GetElement("xyz"); - - EXPECT_EQ(_expressedIn, xyz->Get("expressed_in")); - EXPECT_EQ( - ignition::math::Vector3d::UnitX, xyz->Get()); - }; - - // insert as 1.4, expect rotation of //joint/axis/xyz - { - const std::string version = "1.4"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version), sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - // Expect //joint/axis/xyz[@expressed_in] = "included::__model__" because - // it is the default behavior 1.4 - checkNestedModel(elem, "included::__model__"); - } - - // insert as 1.5, expect no change to //joint/axis/xyz - { - const std::string version = "1.5"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version), sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - checkNestedModel(elem); - } - - // insert as 1.7, expressed_in=__model__ - // expect rotation of //joint/axis/xyz - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "__model__"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - checkNestedModel(elem, "included::__model__"); - } - - // insert as 1.7, expressed_in=child - // expect no change to //joint/axis/xyz - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "child"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_TRUE(errors.empty()); - - checkNestedModel(elem, "included::child"); - - // test coverage for addNestedModel without returning Errors - sdf::ElementPtr elem2 = std::make_shared(); - sdf::initFile("model.sdf", elem2); - sdf::addNestedModel(elem2, sdf->Root()); - } - - // insert as 1.7, expressed_in=parent - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "parent"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_EQ(0u, errors.size()); - - checkNestedModel(elem, "included::parent"); - - // test coverage for addNestedModel without returning Errors - sdf::ElementPtr elem2 = std::make_shared(); - sdf::initFile("model.sdf", elem2); - sdf::addNestedModel(elem2, sdf->Root()); - } - - // insert as 1.7, canonicalLink = child - { - const std::string version = "1.7"; - sdf::Errors errors; - sdf::SDFPtr sdf = InitSDF(); - EXPECT_TRUE( - sdf::readString(getIncludedModelSdfString(version, "parent", "child"), - sdf, errors)); - EXPECT_TRUE(errors.empty()); - EXPECT_EQ(SDF_PROTOCOL_VERSION, sdf->Root()->Get("version")); - EXPECT_EQ(version, sdf->OriginalVersion()); - EXPECT_EQ(version, sdf->Root()->OriginalVersion()); - - sdf::ElementPtr elem = std::make_shared(); - sdf::initFile("model.sdf", elem); - - sdf::addNestedModel(elem, sdf->Root(), errors); - EXPECT_EQ(0u, errors.size()); - - checkNestedModel(elem, "included::parent", "included::child"); - } -} - ///////////////////////////////////////////////// TEST(Parser, NameUniqueness) { From 2482b745b2054278ae6eace53f229136eee1d73e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 13 Oct 2020 23:56:34 -0500 Subject: [PATCH 36/70] Add more documentation, cleanup Signed-off-by: Addisu Z. Taddese --- src/Collision.cc | 2 +- src/Frame.cc | 4 ++-- src/FrameSemantics.cc | 3 --- src/Joint.cc | 4 ++-- src/JointAxis.cc | 2 +- src/Light.cc | 2 +- src/Link.cc | 2 +- src/Model.cc | 13 ++++++++++--- src/SemanticPose.cc | 2 +- src/Sensor.cc | 2 +- src/Visual.cc | 2 +- src/World.cc | 7 ++++++- 12 files changed, 27 insertions(+), 18 deletions(-) diff --git a/src/Collision.cc b/src/Collision.cc index ed0071263..733b7cba9 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -51,7 +51,7 @@ class sdf::CollisionPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. + /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; }; diff --git a/src/Frame.cc b/src/Frame.cc index 84f7558d1..59347dd24 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -45,10 +45,10 @@ class sdf::FramePrivate /// \brief Name of graph source. std::string graphSourceName = ""; - /// \brief Weak pointer to model's or worlds's Frame Attached-To Graph. + /// \brief Scoped Frame Attached-To graph at the parent model or world scope. public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Weak pointer to model's or world's Pose Relative-To Graph. + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. /// TODO (addisu) Make const sdf::PoseRelativeToGraph public: sdf::ScopedGraph poseRelativeToGraph; }; diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 715681585..d0cd6820b 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -789,7 +789,6 @@ Errors buildPoseRelativeToGraph( ignition::math::Pose3d X_RM = nestedModel->RawPose(); // If the model has a placement frame, calculate the necessary pose to use - // TODO (addisu) docs if (!nestedModel->PlacementFrameName().empty()) { ignition::math::Pose3d X_MPf; @@ -828,7 +827,6 @@ Errors buildPoseRelativeToGraph( auto rootToModel = outModel.AddEdge({rootId, modelId}, {}); ignition::math::Pose3d X_RM = _model->RawPose(); // If the model has a placement frame, calculate the necessary pose to use - // TODO (addisu) docs if (!_model->PlacementFrameName().empty()) { ignition::math::Pose3d X_MPf; @@ -968,7 +966,6 @@ Errors buildPoseRelativeToGraph( ignition::math::Pose3d X_RM = model->RawPose(); // If the model has a placement frame, calculate the necessary pose to use - // TODO (addisu) docs if (!model->PlacementFrameName().empty()) { ignition::math::Pose3d X_MPf; diff --git a/src/Joint.cc b/src/Joint.cc index aad53dee7..e1b29bac5 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -75,10 +75,10 @@ class sdf::JointPrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Weak pointer to model's Frame Attached-To Graph. + /// \brief Scoped Frame Attached-To graph at the parent model scope public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Weak pointer to model's Pose Relative-To Graph. + /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; }; diff --git a/src/JointAxis.cc b/src/JointAxis.cc index 4836228d0..e7b410974 100644 --- a/src/JointAxis.cc +++ b/src/JointAxis.cc @@ -86,7 +86,7 @@ class sdf::JointAxisPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. + /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; }; diff --git a/src/Light.cc b/src/Light.cc index c03cd22d7..98b8c453c 100644 --- a/src/Light.cc +++ b/src/Light.cc @@ -45,7 +45,7 @@ class sdf::LightPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. public: sdf::ScopedGraph poseRelativeToGraph; /// \brief True if the light should cast shadows. diff --git a/src/Link.cc b/src/Link.cc index 883e9bcb7..cb8b58365 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -69,7 +69,7 @@ class sdf::LinkPrivate /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; - /// \brief Weak pointer to model's Pose Relative-To Graph. + /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; }; diff --git a/src/Model.cc b/src/Model.cc index 245666b48..aee1e564d 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -79,15 +79,23 @@ class sdf::ModelPrivate public: sdf::ElementPtr sdf; /// \brief Frame Attached-To Graph constructed during Load. - /// TODO (addisu) More documentation + /// This would only be allocated if this model is a top level model, i.e, a + /// model file. public: std::shared_ptr ownedFrameAttachedToGraph; + /// \brief Scoped Frame Attached-To graph that can either point to a graph + /// owned by this model or a subgraph of a parent entity. public: sdf::ScopedGraph frameAttachedToGraph; /// \brief Pose Relative-To Graph constructed during Load. - /// TODO (addisu) More documentation + /// This would only be allocated if this model is a top level model, i.e, a + /// model file. public: std::shared_ptr ownedPoseGraph; + + /// \brief Scoped Pose Relative-To graph that can either point to a graph + /// owned by this model or a subgraph of a parent entity. public: sdf::ScopedGraph poseGraph; + /// \brief Scope name of parent Pose Relative-To Graph (world or __model__). public: std::string poseGraphScopeVertexName; }; @@ -754,7 +762,6 @@ Errors Model::SetPoseRelativeToGraph( // If the scoped graph doesn't point to the graph owned by this model, we // clear the owned graph to maintain the invariant that if // ownedPoseGraph is valid poseGraph points to it. - // TODO (addisu) This may not be needed if (!_graph.PointsTo(this->dataPtr->ownedPoseGraph)) { this->dataPtr->ownedPoseGraph.reset(); diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc index 37a617b5f..83e86bfa2 100644 --- a/src/SemanticPose.cc +++ b/src/SemanticPose.cc @@ -42,7 +42,7 @@ class SemanticPosePrivate /// \brief Name of the default frame to resolve to. public: std::string defaultResolveTo = ""; - /// \brief Weak pointer to model's Pose Relative-To Graph. + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. /// TODO (addisu) Make this const public: sdf::ScopedGraph poseRelativeToGraph; }; diff --git a/src/Sensor.cc b/src/Sensor.cc index fc588c33f..60f892431 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -130,7 +130,7 @@ class sdf::SensorPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. + /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; /// \brief Pointer to a magnetometer. diff --git a/src/Visual.cc b/src/Visual.cc index 8c36bf673..ac7c4ae47 100644 --- a/src/Visual.cc +++ b/src/Visual.cc @@ -66,7 +66,7 @@ class sdf::VisualPrivate /// \brief Name of xml parent object. public: std::string xmlParentName; - /// \brief Weak pointer to model's Pose Relative-To Graph. + /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; /// \brief Visibility flags of a visual. Defaults to 0xFFFFFFFF diff --git a/src/World.cc b/src/World.cc index 5808fb1ea..0475961e6 100644 --- a/src/World.cc +++ b/src/World.cc @@ -90,12 +90,17 @@ class sdf::WorldPrivate ignition::math::Vector3d::Zero; /// \brief Frame Attached-To Graph constructed during Load. - // public: std::shared_ptr frameAttachedToGraph; public: std::shared_ptr ownedFrameAttachedToGraph; + + /// \brief Scoped Frame Attached-To graph that points to a graph owned + /// by this world. public: sdf::ScopedGraph frameAttachedToGraph; /// \brief Pose Relative-To Graph constructed during Load. public: std::shared_ptr ownedPoseRelativeToGraph; + + /// \brief Scoped Pose Relative-To graph that points to a graph owned by this + /// world. public: sdf::ScopedGraph poseRelativeToGraph; }; From c99545f9e2930e4a2c8912f5f0ae0e8e1998f005 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 14 Oct 2020 00:44:59 -0500 Subject: [PATCH 37/70] Validate __root__ vertices Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 74 +++++++++++++++++++++++++++++-------------- 1 file changed, 51 insertions(+), 23 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index d0cd6820b..bf50ac83b 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -248,7 +248,10 @@ Errors buildFrameAttachedToGraph( _out.AddScopeVertex(_model->Name(), scopeName, scopeName, frameType); const auto modelFrameId = outModel.ScopeVertexId(); + // Add an aliasing edge from the model vertex to the + // corresponding vertex in the FrameAttachedTo graph of the child model, auto &edge = outModel.AddEdge({modelId, modelFrameId}, true); + // Set the edge weight to 0 to indicate that this is an aliasing edge. edge.SetWeight(0); // add link vertices @@ -550,9 +553,10 @@ Errors buildPoseRelativeToGraph( // Add an aliasing edge from the model vertex to the // corresponding vertex in the PoseRelativeTo graph of the child model, - // i.e, to the ::__model__ vertex - // auto &edge = _out.AddEdge({modelId, modelFrameId}, {{}, false}); - _out.AddEdge({modelId, modelFrameId}, {}); + // i.e, to the ::__model__ vertex. + auto &edge = _out.AddEdge({modelId, modelFrameId}, {}); + // Set the edge weight to 0 to indicate that this is an aliasing edge. + edge.SetWeight(0); // add link vertices and default edge if relative_to is empty for (uint64_t l = 0; l < _model->LinkCount(); ++l) @@ -1105,11 +1109,8 @@ Errors validateFrameAttachedToGraph( "FrameAttachedToGraph error, " "vertex with empty name detected."}); } - // TODO (addisu) Handle root vertices properly - if (vertexName == "__root__") - continue; - auto outDegree = _in.Graph().OutDegree(vertexPair.first); + if (outDegree > 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, @@ -1117,6 +1118,18 @@ Errors validateFrameAttachedToGraph( "too many outgoing edges at a vertex with name [" + vertexName + "]."}); } + else if (vertexName == "__root__" ) + { + if (outDegree != 0) + { + std::string graphType = + scopeFrameType == sdf::FrameType::MODEL ? "MODEL" : "WORLD"; + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error," + " __root__ should have no outgoing edged in " + + graphType + " relative_to graph."}); + } + } else if (sdf::FrameType::MODEL == scopeFrameType || sdf::FrameType::STATIC_MODEL == scopeFrameType) { @@ -1140,15 +1153,25 @@ Errors validateFrameAttachedToGraph( } break; case sdf::FrameType::STATIC_MODEL: - // if (outDegree != 0) - // { - // errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - // "FrameAttachedToGraph error, " - // "LINK vertex with name [" + - // vertexName + - // "] should have no outgoing edges " - // "in MODEL attached_to graph."}); - // } + if (outDegree != 0) + { + auto edges = _in.Graph().IncidentsFrom(vertexPair.first); + // Filter out alias edges (edge with a weight of 0) + auto outDegreeNoAliases = std::count_if( + edges.begin(), edges.end(), [](const auto &_edge) + { + return _edge.second.get().Weight() >= 1.0; + }); + if (outDegreeNoAliases) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error, " + "STATIC_MODEL vertex with name [" + + vertexName + + "] should have no outgoing edges " + "in MODEL attached_to graph."}); + } + } break; default: if (outDegree == 0) @@ -1285,10 +1308,6 @@ Errors validatePoseRelativeToGraph( "PoseRelativeToGraph error, " "vertex with empty name detected."}); } - if (vertexName == "__root__") - { - continue; - } std::size_t inDegree = _in.Graph().InDegree(vertexPair.first); @@ -1299,6 +1318,18 @@ Errors validatePoseRelativeToGraph( "too many incoming edges at a vertex with name [" + vertexName + "]."}); } + else if (vertexName == "__root__" ) + { + if (inDegree != 0) + { + std::string graphType = + sourceFrameType == sdf::FrameType::MODEL ? "MODEL" : "WORLD"; + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph error," + " __root__ vertex should have no incoming edges in " + + graphType + " relative_to graph."}); + } + } else if (sdf::FrameType::MODEL == sourceFrameType) { switch (vertexPair.second.get().Data()) @@ -1310,9 +1341,6 @@ Errors validatePoseRelativeToGraph( "should not have type WORLD in MODEL relative_to graph."}); break; case sdf::FrameType::MODEL: - // TODO: What we have to check here is that if this is the scope - // vertex any incoming edge is from outside this scope. - // if (sdf::endswith(vertexName, "__model__")) if (_in.ScopeVertexId() == vertexPair.first) { if (inDegree != 0) From 1c0749432a644b52941032ade81aa61afbf2da97 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 14 Oct 2020 01:05:45 -0500 Subject: [PATCH 38/70] Add more error cases in validate* functions Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 39 ++++++++++++++++++++++++++++----------- src/FrameSemantics.hh | 4 +--- 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index bf50ac83b..dc58fb607 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -543,7 +543,7 @@ Errors buildPoseRelativeToGraph( if (_root) { _out = _out.AddScopeVertex( - "", "__root__", scopeName, sdf::FrameType::STATIC_MODEL); + "", "__root__", scopeName, sdf::FrameType::MODEL); rootId = _out.ScopeVertexId(); } auto modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); @@ -1066,8 +1066,8 @@ Errors validateFrameAttachedToGraph( return errors; } - // Expect one vertex with name "__model__" and FrameType MODEL - // or with name "world" and FrameType WORLD + // Expect the scope vertex to be valid and check that the scope name matches + // the frame type. auto scopeVertex = _in.ScopeVertex(); if (!scopeVertex.Valid()) { @@ -1123,10 +1123,10 @@ Errors validateFrameAttachedToGraph( if (outDegree != 0) { std::string graphType = - scopeFrameType == sdf::FrameType::MODEL ? "MODEL" : "WORLD"; + scopeFrameType == sdf::FrameType::WORLD ? "WORLD" : "MODEL"; errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error," - " __root__ should have no outgoing edged in " + + " __root__ should have no outgoing edges in " + graphType + " relative_to graph."}); } } @@ -1222,7 +1222,25 @@ Errors validateFrameAttachedToGraph( } break; case sdf::FrameType::STATIC_MODEL: - case sdf::FrameType::ROOT: + if (outDegree != 0) + { + auto edges = _in.Graph().IncidentsFrom(vertexPair.first); + // Filter out alias edges (edge with a weight of 0) + auto outDegreeNoAliases = std::count_if( + edges.begin(), edges.end(), [](const auto &_edge) + { + return _edge.second.get().Weight() >= 1.0; + }); + if (outDegreeNoAliases) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error, " + "STATIC_MODEL vertex with name [" + + vertexName + + "] should have no outgoing edges " + "in WORLD attached_to graph."}); + } + } break; default: if (outDegree != 1) @@ -1266,8 +1284,8 @@ Errors validatePoseRelativeToGraph( return errors; } - // Expect one vertex with name "__model__" and FrameType MODEL - // or with name "world" and FrameType WORLD + // Expect the scope vertex (which is the source vertex for this graph) to be + // valid and check that the scope name matches the frame type. auto sourceVertex = _in.ScopeVertex(); if (!sourceVertex.Valid()) { @@ -1279,8 +1297,7 @@ Errors validatePoseRelativeToGraph( sdf::FrameType sourceFrameType = sourceVertex.Data(); if (_in.ScopeName() == "__model__" && - sourceFrameType != sdf::FrameType::MODEL && - sourceFrameType != sdf::FrameType::STATIC_MODEL) + sourceFrameType != sdf::FrameType::MODEL) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error, " @@ -1323,7 +1340,7 @@ Errors validatePoseRelativeToGraph( if (inDegree != 0) { std::string graphType = - sourceFrameType == sdf::FrameType::MODEL ? "MODEL" : "WORLD"; + sourceFrameType == sdf::FrameType::WORLD ? "WORLD" : "MODEL"; errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error," " __root__ vertex should have no incoming edges in " + diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index 05423bdeb..d23128b26 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -70,10 +70,8 @@ namespace sdf /// \brief An explicit frame. FRAME = 4, - /// \brief An implicit model frame. + /// \brief An implicit static model frame. STATIC_MODEL = 5, - - ROOT = 6, }; /// \brief Data structure for frame attached_to graphs for Model or World. From a628e1e026119f337396b6fefdbe84240e1952f0 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 14 Oct 2020 01:07:59 -0500 Subject: [PATCH 39/70] Remove obsolete test Signed-off-by: Addisu Z. Taddese --- src/ign_TEST.cc | 13 ------------- test/sdf/model_invalid_nested_joint_child.sdf | 16 ---------------- 2 files changed, 29 deletions(-) delete mode 100644 test/sdf/model_invalid_nested_joint_child.sdf diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 02bd543e6..2d0b7c757 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -584,19 +584,6 @@ TEST(check, SDF) custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_EQ("Valid.\n", output) << output; } - // Check an invalid SDF file with a joint that specifies a child link - // within a sibling nested model using the unsupported :: syntax. - // { - // std::string path = pathBase +"/model_invalid_nested_joint_child.sdf"; - - // // Check model_invalid_nested_joint_child.sdf - // std::string output = - // custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); - // EXPECT_NE(output.find("Error: Child frame with name[M::C] specified by " - // "joint with name[J] not found in model with " - // "name[model_invalid_nested_joint_child]."), - // std::string::npos) << output; - // } // Check an SDF file with joints using the relative_to attribute. // This is a valid file. diff --git a/test/sdf/model_invalid_nested_joint_child.sdf b/test/sdf/model_invalid_nested_joint_child.sdf deleted file mode 100644 index efde66366..000000000 --- a/test/sdf/model_invalid_nested_joint_child.sdf +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - 1 0 0 0 1.5707963267948966 0 - - - - P - - - M::C - - - From 6a7482cac600c44ec5a6f28f15c6232aad653ab9 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 14 Oct 2020 01:39:57 -0500 Subject: [PATCH 40/70] Prevent users from referencing `__root__` in SDFormat XML. Signed-off-by: Addisu Z. Taddese --- src/ign_TEST.cc | 37 +++++++++++++++++++++++ src/parser.cc | 6 ++++ test/sdf/model_invalid_root_reference.sdf | 9 ++++++ test/sdf/world_invalid_root_reference.sdf | 13 ++++++++ 4 files changed, 65 insertions(+) create mode 100644 test/sdf/model_invalid_root_reference.sdf create mode 100644 test/sdf/world_invalid_root_reference.sdf diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 2d0b7c757..2dbaccc4d 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -733,6 +733,43 @@ TEST(check, SDF) custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_EQ("Valid.\n", output) << output; } + + // Check an SDF model file with an invalid usage of __root__ + { + std::string path = pathBase + "/model_invalid_root_reference.sdf"; + + // Check model_invalid_placement_frame.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [relative_to]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a" + " value of attribute [attached_to]"), + std::string::npos) + << output; + } + // Check an SDF world file with an invalid usage of __root__ + { + std::string path = pathBase + "/world_invalid_root_reference.sdf"; + + // Check model_invalid_placement_frame.sdf + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [relative_to]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [attached_to]"), + std::string::npos) + << output; + } } ///////////////////////////////////////////////// diff --git a/src/parser.cc b/src/parser.cc index 85394b465..541be1b65 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -861,6 +861,12 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) ParamPtr p = _sdf->GetAttribute(i); if (p->GetKey() == attribute->Name()) { + if (attribute->Value() == std::string("__root__")) + { + _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, + "'__root__' is reserved; it cannot be used as a value of " + "attribute [" + p->GetKey() + "]"}); + } // Set the value of the SDF attribute if (!p->SetFromString(attribute->Value())) { diff --git a/test/sdf/model_invalid_root_reference.sdf b/test/sdf/model_invalid_root_reference.sdf new file mode 100644 index 000000000..be34acdd9 --- /dev/null +++ b/test/sdf/model_invalid_root_reference.sdf @@ -0,0 +1,9 @@ + + + + + 1 0 0 0 0 0 + + + + diff --git a/test/sdf/world_invalid_root_reference.sdf b/test/sdf/world_invalid_root_reference.sdf new file mode 100644 index 000000000..b8735a4ba --- /dev/null +++ b/test/sdf/world_invalid_root_reference.sdf @@ -0,0 +1,13 @@ + + + + + 1 0 0 0 0 0 + + + + + 1 0 0 0 0 0 + + + From 3956ee698562d35ec8fd13ad234e76028a377c7c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 18 Oct 2020 22:02:59 -0700 Subject: [PATCH 41/70] More expectations in LoadJointNestedParentChild Test `Joint::Resolve*Link` methods in LoadJointNestedParentChild test case. Signed-off-by: Steve Peters --- test/integration/joint_dom.cc | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index b763acc40..da51264ea 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -790,6 +790,13 @@ TEST(DOMJoint, LoadJointNestedParentChild) ASSERT_NE(nullptr, j1); EXPECT_EQ("M1::L1", j1->ParentLinkName()); EXPECT_EQ("L1", j1->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j1->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L1", resolvedLinkName); + EXPECT_TRUE(j1->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + Pose pose; EXPECT_TRUE(j1->SemanticPose().Resolve(pose, "__model__").empty()); EXPECT_EQ(Pose(0, 0, 9, 0, IGN_PI_2, 0), pose); @@ -799,6 +806,13 @@ TEST(DOMJoint, LoadJointNestedParentChild) ASSERT_NE(nullptr, j2); EXPECT_EQ("F1", j2->ParentLinkName()); EXPECT_EQ("L1", j2->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j2->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L1", resolvedLinkName); + EXPECT_TRUE(j2->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + Pose pose; EXPECT_TRUE(j2->SemanticPose().Resolve(pose, "__model__").empty()); EXPECT_EQ(Pose(0, 1, 10, 0, IGN_PI_2, 0), pose); @@ -808,6 +822,13 @@ TEST(DOMJoint, LoadJointNestedParentChild) ASSERT_NE(nullptr, j3); EXPECT_EQ("L1", j3->ParentLinkName()); EXPECT_EQ("M1::L2", j3->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j3->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + EXPECT_TRUE(j3->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L2", resolvedLinkName); + Pose pose; EXPECT_TRUE(j3->SemanticPose().Resolve(pose, "__model__").empty()); EXPECT_EQ(Pose(1, 1, 1, 0, 0, 0), pose); @@ -817,6 +838,13 @@ TEST(DOMJoint, LoadJointNestedParentChild) ASSERT_NE(nullptr, j4); EXPECT_EQ("L1", j4->ParentLinkName()); EXPECT_EQ("M1::F1", j4->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j4->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + EXPECT_TRUE(j4->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::L1", resolvedLinkName); + Pose pose; EXPECT_TRUE(j4->SemanticPose().Resolve(pose, "__model__").empty()); EXPECT_EQ(Pose(1, 0, 1, 0, 0, 0), pose); @@ -826,6 +854,13 @@ TEST(DOMJoint, LoadJointNestedParentChild) ASSERT_NE(nullptr, j5); EXPECT_EQ("L1", j5->ParentLinkName()); EXPECT_EQ("M1::M2", j5->ChildLinkName()); + + std::string resolvedLinkName; + EXPECT_TRUE(j5->ResolveParentLink(resolvedLinkName).empty()); + EXPECT_EQ("L1", resolvedLinkName); + EXPECT_TRUE(j5->ResolveChildLink(resolvedLinkName).empty()); + EXPECT_EQ("M1::M2::L1", resolvedLinkName); + Pose pose; EXPECT_TRUE(j5->SemanticPose().Resolve(pose, "__model__").empty()); EXPECT_EQ(Pose(0, -1, 1, IGN_PI_2, 0, 0), pose); From f4b40983237f2b326a11c4bef5b3d1807073e8c5 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 22 Oct 2020 10:57:07 -0500 Subject: [PATCH 42/70] Address Reviewer feedback Signed-off-by: Addisu Z. Taddese --- Migration.md | 7 ++++--- include/sdf/Frame.hh | 5 ++--- test/sdf/joint_nested_parent_child.sdf | 6 ------ 3 files changed, 6 insertions(+), 12 deletions(-) diff --git a/Migration.md b/Migration.md index 18ec08034..313586594 100644 --- a/Migration.md +++ b/Migration.md @@ -22,9 +22,10 @@ but with improved human-readability.. ### Modifications -1. **sdf/Model.hh**: the following methods now accept nested names - that can begin with a sequence of nested model names separated - by `::` and may end with the name of an object of the specified type. +1. **sdf/Model.hh**: the following methods now accept nested names relative to + the model's scope that can begin with a sequence of nested model names + separated by `::` and may end with the name of an object of the specified + type. + const Frame \*FrameByName(const std::string &) const + const Joint \*JointByName(const std::string &) const + const Link \*LinkByName(const std::string &) const diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index 0435d3a05..7bc919e5c 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -134,9 +134,8 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Resolve the attached-to body of this frame from the - /// FrameAttachedToGraph. If this is in a __model__ scope, it returns - /// the name of a link. In the world scope, it returns the name of a - /// model or the world. + /// FrameAttachedToGraph. Generally, it resolves to the name of a link, but + /// if it is in the world scope, it can resolve to "world". /// \param[out] _body Name of body to which this frame is attached. /// \return Errors. public: Errors ResolveAttachedToBody(std::string &_body) const; diff --git a/test/sdf/joint_nested_parent_child.sdf b/test/sdf/joint_nested_parent_child.sdf index de16f001d..4a495a788 100644 --- a/test/sdf/joint_nested_parent_child.sdf +++ b/test/sdf/joint_nested_parent_child.sdf @@ -1,12 +1,6 @@ - 0 0 1 0 0 0 From 4e368f19966d54530b9197231993f711452d0bea Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 22 Oct 2020 11:10:28 -0500 Subject: [PATCH 43/70] Add and update doxygen for SetFrameAttachedToGraph and SetPoseRelativeToGraph Signed-off-by: Addisu Z. Taddese --- include/sdf/Collision.hh | 6 +++--- include/sdf/Frame.hh | 16 ++++++++-------- include/sdf/Joint.hh | 15 +++++++-------- include/sdf/JointAxis.hh | 8 ++++---- include/sdf/Light.hh | 6 +++--- include/sdf/Link.hh | 7 +++---- include/sdf/Model.hh | 15 ++++++++++----- include/sdf/Sensor.hh | 6 +++--- include/sdf/Visual.hh | 6 +++--- src/Model.cc | 2 +- 10 files changed, 45 insertions(+), 42 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 523561ac4..fc2df8d66 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -142,10 +142,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph Scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index 7bc919e5c..767d4b9d4 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -145,17 +145,17 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the FrameAttachedToGraph to be used - /// for resolving attached bodies. This is private and is intended to - /// be called by Model::Load or World::Load. - /// \param[in] _graph Weak pointer to FrameAttachedToGraph. + /// \brief Give a Scoped FrameAttachedToGraph to be used for resolving + /// attached bodies. This is private and is intended to be called by + /// Model::Load or World::Load. + /// \param[in] _graph Scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( sdf::ScopedGraph _graph); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// Model::Load or World::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Model::Load or + /// World::Load. + /// \param[in] _graph Scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 2c2091bc3..f24aba59d 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -222,17 +222,16 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the FrameAttachedToGraph to be used - /// for resolving parent and child link names. This is private and is - /// intended to be called by Model::Load. - /// \param[in] _graph Weak pointer to FrameAttachedToGraph. + /// \brief Give the Scoped FrameAttachedToGraph to be used for resolving + /// parent and child link names. This is private and is intended to be + /// called by Model::Load. + /// \param[in] _graph Scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( sdf::ScopedGraph _graph); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// Model::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Model::Load. + /// \param[in] _graph Scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index 1690da9f6..22d2d8ab4 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -270,10 +270,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called - /// by Joint::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by + /// Joint::SetPoseRelativeToGraph. + /// \param[in] _graph Scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index 98214bc21..570e464ff 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -274,10 +274,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph or World::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph Scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index f342b0440..da96b3ce5 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -226,10 +226,9 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// Model::Load. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by Model::Load. + /// \param[in] _graph Scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 14989f5ea..da534e28d 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -300,16 +300,21 @@ namespace sdf /// \param[in] _name Name of the placement frame. public: void SetPlacementFrameName(const std::string &_name); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by - /// World::Load and Model::Load if this is a nested model. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. - /// \return Error if graph pointer is invalid. + /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by World::Load and + /// Model::Load if this is a nested model. + /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \return Error if the scoped graph is invalid. private: sdf::Errors SetPoseRelativeToGraph( sdf::ScopedGraph _graph); + /// \brief Give the Scoped FrameAttachedToGraph to be used for resolving + /// attached bodies. This is private and is intended to be called by + /// World::Load and Model::Load if this is a nested model. + /// \param[in] _graph Scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( sdf::ScopedGraph _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 diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index ded5c94cd..3f57c46c4 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -317,10 +317,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph Scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index c65491e3c..a4e283b99 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -169,10 +169,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give a weak pointer to the PoseRelativeToGraph to be used - /// for resolving poses. This is private and is intended to be called by + /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph Scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/src/Model.cc b/src/Model.cc index aee1e564d..77200287a 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -755,7 +755,7 @@ Errors Model::SetPoseRelativeToGraph( if (!_graph) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "Tried to set PoseRelativeToGraph with invalid pointer."}); + "Tried to set an invalid Scoped PoseRelativeToGraph object."}); return errors; } From 9810c10f06344104f557b5035a36279553f8522d Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 22 Oct 2020 11:57:38 -0500 Subject: [PATCH 44/70] Add doxygen to new SemanticPose constructor Signed-off-by: Addisu Z. Taddese --- include/sdf/SemanticPose.hh | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 7062f5f2d..fe8f37912 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -90,6 +90,17 @@ namespace sdf const std::string &_defaultResolveTo, const sdf::ScopedGraph &_graph); + /// \brief Private constructor that is used by object that represent a frame + /// in the PoseRelativeTo graph. Examples are Model, Frame, Link and not + /// Collision or Visual. + /// \param[in] _name Name of object. This should also be the name of the + /// frame represented by this object in the PoseRelativeTo graph. + /// \param[in] _pose Raw pose of object. + /// \param[in] _relativeTo Name of frame in graph relative-to which the + /// raw pose is applied. + /// \param[in] _defaultResolveTo Default frame to resolve-to in Resolve() + /// if no frame is specified. + /// \param[in] _graph Weak pointer to PoseRelativeToGraph. private: SemanticPose( const std::string &_name, const ignition::math::Pose3d &_pose, From ea08d4fbc3855c68ca01023083c8ea05c449704a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 22 Oct 2020 11:59:39 -0500 Subject: [PATCH 45/70] Remove unnecessary code Signed-off-by: Addisu Z. Taddese --- include/sdf/SemanticPose.hh | 4 ---- include/sdf/Types.hh | 3 --- src/Types.cc | 7 ------- 3 files changed, 14 deletions(-) diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index fe8f37912..794422a9c 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -40,10 +40,6 @@ namespace sdf // // Forward declare private data class. - class Model; - class Link; - class Frame; - class Joint; class SemanticPosePrivate; struct PoseRelativeToGraph; template class ScopedGraph; diff --git a/include/sdf/Types.hh b/include/sdf/Types.hh index bfc8cde65..493fcb9f9 100644 --- a/include/sdf/Types.hh +++ b/include/sdf/Types.hh @@ -68,9 +68,6 @@ namespace sdf std::vector split(const std::string &_str, const std::string &_splitter); - SDFORMAT_VISIBLE - bool endswith(const std::string &_a, const std::string &_b); - /// \brief Trim leading and trailing whitespace from a string. /// \param[in] _in The string to trim. /// \return A string containing the trimmed value. diff --git a/src/Types.cc b/src/Types.cc index fe22d3a17..ff724394b 100644 --- a/src/Types.cc +++ b/src/Types.cc @@ -70,13 +70,6 @@ std::string trim(const std::string &_in) return _in.substr(strBegin, strRange); } -////////////////////////////////////////////////// -bool endswith(const std::string& _a, const std::string& _b) -{ - return (_a.size() >= _b.size()) && - (_a.compare(_a.size() - _b.size(), _b.size(), _b) == 0); -} - ///////////////////////////////////////////////// std::string lowercase(const std::string &_in) { From cd08f38b79fd9f46aeffaa4f41e246ce44cd671b Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 23 Oct 2020 14:26:00 -0500 Subject: [PATCH 46/70] Refactor code that handles PlacementFrame into a function Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 171 +++++++++++++++++++++--------------------- src/ScopedGraph.hh | 4 +- 2 files changed, 88 insertions(+), 87 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index dc58fb607..96f8380ae 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -203,6 +203,76 @@ std::pair return _model->CanonicalLinkAndRelativeName(); } +///////////////////////////////////////////////// +/// \brief Resolve the pose of a model taking into account the placement frame +/// attribute. This function is used to calculate the pose of the edge between a +/// model and its parent. +/// +/// Since this function calls sdf::resolvePoseRelativeToRoot to initially +/// resolve the pose of the placement frame, the passed in graph must already +/// have an edge between the input model and its parent frame. This edge will +/// later be updated with the pose calculated by this function. +/// +/// Notation used in this function: +/// Pf - The placement frame inside the model +/// R - Relative to frame of the placement frame's pose. This frame is in the +/// parent scope of the input model. +/// M - The input model's implicit frame (__model__) +/// X_RPf - The pose of frame Pf relative to frame R +/// X_RM - The pose of the frame M relative to frame R +/// X_MPf - The pose of the frame Pf relative to frame M +/// +/// Example: +/// +/// +/// +/// +/// {X_RPf} +/// +/// +/// +/// +/// +/// When the placement_frame is specified in an SDFormat file, //model/pose +/// becomes the pose of the placement frame (X_RPf) instead of the pose of the +/// model frame (X_RM). However, when the model inserted into a pose graph of +/// the parent scope, only the pose (X_RM) of the __model__ frame can be used. +/// i.e, X_RPf cannot be represented in the PoseRelativeTo graph. +/// Thus, the X_RM has to be calculated such that X_RPf = X_RM * X_MPf. +// +/// \param[in] _model Model whose pose to resolve relative to its parent +/// \param[in] _graph The scoped PoseRelativeTo graph used to resolve the model. +/// The scope must contain the frame referenced by the placement frame attribute +/// of the input model. +/// \param[out] _resolvedPose The resolved pose (X_RM). If an error was +/// encountered during `sdf::resolvePoseRelativeToRoot`, _resolvedPose will not +/// be modified. +static Errors resolveModelPoseWithPlacementFrame(const sdf::Model &_model, + const sdf::ScopedGraph &_graph, + ignition::math::Pose3d &_resolvedPose) +{ + sdf::Errors errors; + + // Alias for better pose notation + ignition::math::Pose3d &X_RM = _resolvedPose; + const ignition::math::Pose3d &X_RPf = _model.RawPose(); + + // If the model has a placement frame, calculate the necessary pose to use + if (!_model.PlacementFrameName().empty()) + { + ignition::math::Pose3d X_MPf; + + errors = sdf::resolvePoseRelativeToRoot( + X_MPf, _graph, _model.PlacementFrameName()); + if (errors.empty()) + { + X_RM = X_RPf * X_MPf.Inverse(); + } + } + + return errors; +} + ///////////////////////////////////////////////// Errors buildFrameAttachedToGraph( ScopedGraph &_out, const Model *_model, bool _root) @@ -791,73 +861,27 @@ Errors buildPoseRelativeToGraph( } } - ignition::math::Pose3d X_RM = nestedModel->RawPose(); - // If the model has a placement frame, calculate the necessary pose to use - if (!nestedModel->PlacementFrameName().empty()) - { - ignition::math::Pose3d X_MPf; + ignition::math::Pose3d resolvedModelPose = nestedModel->RawPose(); + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame(*nestedModel, + outModel.ChildModelScope(nestedModel->Name()), resolvedModelPose); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - sdf::Errors resolveErrors = sdf::resolvePoseRelativeToRoot(X_MPf, - outModel.ChildModelScope(nestedModel->Name()), - nestedModel->PlacementFrameName()); - if (resolveErrors.empty()) - { - // Before this update, i.e, as specified in the SDFormat, the model pose - // (X_RPf) is the pose of the placement frame (Pf) relative to a frame - // (R) in the parent scope of the model. However, when this model (M) is - // inserted into a pose graph of the parent scope, only the pose (X_RM) - // of the __model__ frame can be used. Thus, the model pose has to be - // updated to X_RM. - // - // Note that X_RPf is the raw pose specified in //model/pose before this - // update. - const auto &X_RPf = nestedModel->RawPose(); - X_RM = X_RPf * X_MPf.Inverse(); - } - else - { - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - } - } - - outModel.AddEdge({relativeToId, nestedModelId}, X_RM); + outModel.AddEdge({relativeToId, nestedModelId}, resolvedModelPose); } if (_root) { // We have to add this edge now with an identity pose to be able to call + // resolveModelPoseWithPlacementFrame, which in turn calls // sdf::resolvePoseRelativeToRoot. We will later update the edge after the // pose is calculated. auto rootToModel = outModel.AddEdge({rootId, modelId}, {}); - ignition::math::Pose3d X_RM = _model->RawPose(); - // If the model has a placement frame, calculate the necessary pose to use - if (!_model->PlacementFrameName().empty()) - { - ignition::math::Pose3d X_MPf; - - sdf::Errors resolveErrors = sdf::resolvePoseRelativeToRoot( - X_MPf, outModel, _model->PlacementFrameName()); - if (resolveErrors.empty()) - { - // Before this update, i.e, as specified in the SDFormat, the model pose - // (X_RPf) is the pose of the placement frame (Pf) relative to a frame - // (R) in the parent scope of the model. However, when this model (M) is - // inserted into a pose graph of the parent scope, only the pose (X_RM) - // of the __model__ frame can be used. Thus, the model pose has to be - // updated to X_RM. - // - // Note that X_RPf is the raw pose specified in //model/pose before this - // update. - const auto &X_RPf = _model->RawPose(); - X_RM = X_RPf * X_MPf.Inverse(); - } - else - { - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - } - } + ignition::math::Pose3d resolvedModelPose = _model->RawPose(); + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame( + *_model, outModel, resolvedModelPose); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - outModel.UpdateEdge(rootToModel, X_RM); + outModel.UpdateEdge(rootToModel, resolvedModelPose); } return errors; } @@ -968,35 +992,12 @@ Errors buildPoseRelativeToGraph( } } - ignition::math::Pose3d X_RM = model->RawPose(); - // If the model has a placement frame, calculate the necessary pose to use - if (!model->PlacementFrameName().empty()) - { - ignition::math::Pose3d X_MPf; + ignition::math::Pose3d resolvedModelPose = model->RawPose(); + sdf::Errors resolveErrors = resolveModelPoseWithPlacementFrame(*model, + _out.ChildModelScope(model->Name()), resolvedModelPose); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - sdf::Errors resolveErrors = sdf::resolvePoseRelativeToRoot(X_MPf, - _out.ChildModelScope(model->Name()), - model->PlacementFrameName()); - if (resolveErrors.empty()) - { - // Before this update, i.e, as specified in the SDFormat, the model pose - // (X_RPf) is the pose of the placement frame (Pf) relative to a frame - // (R) in the parent scope of the model. However, when this model (M) is - // inserted into a pose graph of the parent scope, only the pose (X_RM) - // of the __model__ frame can be used. Thus, the model pose has to be - // updated to X_RM. - // - // Note that X_RPf is the raw pose specified in //model/pose before this - // update. - const auto &X_RPf = model->RawPose(); - X_RM = X_RPf * X_MPf.Inverse(); - } - else - { - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - } - } - _out.AddEdge({relativeToId, modelId}, X_RM); + _out.AddEdge({relativeToId, modelId}, resolvedModelPose); } for (uint64_t f = 0; f < _world->FrameCount(); ++f) diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index de9b58aba..58ad87304 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -131,7 +131,7 @@ class ScopedGraph /// matches the name. The new scope will have a new prefix formed by appending /// _name to the existing prefix of the current scope. /// \return A new child scope. - public: ScopedGraph ChildModelScope(const std::string &_name); + public: ScopedGraph ChildModelScope(const std::string &_name) const; /// \brief Checks if the scope points to a valid graph. /// \return True if the scope points to a valid graph. @@ -259,7 +259,7 @@ ScopedGraph::ScopedGraph(const std::shared_ptr &_graph) ///////////////////////////////////////////////// template -ScopedGraph ScopedGraph::ChildModelScope(const std::string &_name) +ScopedGraph ScopedGraph::ChildModelScope(const std::string &_name) const { auto newScopedGraph = *this; newScopedGraph.dataPtr = std::make_shared(); From b158998039870e6d2be7cd069e41a0cd95939db8 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 23 Oct 2020 16:06:58 -0500 Subject: [PATCH 47/70] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- include/sdf/Model.hh | 2 +- src/FrameSemantics.cc | 7 +++---- src/Model.cc | 15 ++++++--------- src/ScopedGraph.hh | 4 ++-- src/World.cc | 2 -- src/cmd/cmdsdformat.rb.in | 8 ++++---- src/ign.cc | 5 +++++ src/parser.cc | 2 -- 8 files changed, 21 insertions(+), 24 deletions(-) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index da534e28d..dbd262e3f 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -172,7 +172,7 @@ namespace sdf /// \brief Get whether a joint name exists. /// \param[in] _name Name of the joint to check. - /// To check for a joint in a nested model, prefix the link name with + /// To check for a joint in a nested model, prefix the joint name with /// the sequence of nested models containing this joint, delimited by "::". /// \return True if there exists a joint with the given name. public: bool JointNameExists(const std::string &_name) const; diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 96f8380ae..fb689c1fb 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -440,9 +440,8 @@ Errors buildFrameAttachedToGraph( } // identify canonical link, which may be nested - auto canonicalLinkAndName = modelCanonicalLinkAndRelativeName(_model); - const sdf::Link *canonicalLink = canonicalLinkAndName.first; - const std::string canonicalLinkName = canonicalLinkAndName.second; + const auto[canonicalLink, canonicalLinkName] = + modelCanonicalLinkAndRelativeName(_model); if (!_model->Static()) { if (nullptr == canonicalLink) @@ -457,7 +456,7 @@ Errors buildFrameAttachedToGraph( else { // The canonical link was not found, but the model could have a - // decendant that has a static model, so simply create an edge to the + // descendant that has a static model, so simply create an edge to the // first model and let the attached_to frame resolution take care of // finding the canonical link auto firstChildModelId = diff --git a/src/Model.cc b/src/Model.cc index 77200287a..9cefb330c 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -239,20 +239,20 @@ Errors Model::Load(ElementPtr _sdf) << this->Name() << "].\n"; } - bool modelFile = false; + // Whether this model is defined at the root level of an SDFormat file (i.e + // in XPath "/sdf/model") + bool isModelAtRootOfXmlFile = false; - // if (!this->dataPtr->poseGraph) auto parentElem = this->dataPtr->sdf->GetParent(); if (parentElem && parentElem->GetName() == "sdf") { - // std::cout << "Creating owned graphs for " << this->Name() << std::endl; this->dataPtr->ownedPoseGraph = std::make_shared(); this->dataPtr->poseGraph = ScopedGraph(this->dataPtr->ownedPoseGraph); this->dataPtr->ownedFrameAttachedToGraph = std::make_shared(); this->dataPtr->frameAttachedToGraph = ScopedGraph(this->dataPtr->ownedFrameAttachedToGraph); - modelFile = true; + isModelAtRootOfXmlFile = true; } // Set of implicit and explicit frame names in this model for tracking @@ -402,7 +402,7 @@ Errors Model::Load(ElementPtr _sdf) // static models. if (!this->Static()) { - if (modelFile) + if (isModelAtRootOfXmlFile) { Errors frameAttachedToGraphErrors = buildFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph, this); @@ -418,7 +418,7 @@ Errors Model::Load(ElementPtr _sdf) } // Build the PoseRelativeToGraph - if (modelFile) + if (isModelAtRootOfXmlFile) { Errors poseGraphErrors = buildPoseRelativeToGraph(this->dataPtr->poseGraph, this); @@ -826,9 +826,6 @@ void Model::SetFrameAttachedToGraph( ///////////////////////////////////////////////// sdf::SemanticPose Model::SemanticPose() const { - // Pass an empty string for the defaultResolveTo argument so that it can - // resolve to the scope vertex regardless of whether this model is a - // standalone model or part of a world. return sdf::SemanticPose( this->dataPtr->name, this->dataPtr->pose, diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index 58ad87304..521cf6baa 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -49,8 +49,8 @@ struct ScopedGraphData }; // Forward declarations for static_assert -class PoseRelativeToGraph; -class FrameAttachedToGraph; +struct PoseRelativeToGraph; +struct FrameAttachedToGraph; /// \brief The ScopedGraph allows manipulating FrameAttachedTo and /// PoseRelativeTo graphs within a smaller scope such as the scope of a model diff --git a/src/World.cc b/src/World.cc index 0475961e6..320850ce2 100644 --- a/src/World.cc +++ b/src/World.cc @@ -394,8 +394,6 @@ Errors World::Load(sdf::ElementPtr _sdf) model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), setPoseRelativeToGraphErrors.end()); - errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), - setPoseRelativeToGraphErrors.end()); model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); } for (auto &light : this->dataPtr->lights) diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 9743bfd5b..0b3bb2dfb 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -38,10 +38,10 @@ COMMANDS = { 'sdf' => "Utilities for SDF files.\n\n"\ " ign sdf [options]\n\n"\ "Options:\n\n"\ - " -k [ --check ] arg Check if an SDFormat file is valid.\n" + - " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + - " -p [ --print ] arg Print converted arg.\n" + - " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph.\n" + + " -k [ --check ] arg Check if an SDFormat file is valid.\n" + + " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + + " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph. (This is for advanced use only and the output may change without any promise of stability)\n" + + " -p [ --print ] arg Print converted arg.\n" + COMMON_OPTIONS } diff --git a/src/ign.cc b/src/ign.cc index 2eb2e9664..dfef44a81 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -217,6 +217,11 @@ extern "C" SDFORMAT_VISIBLE int cmdGraph( } std::cout << graph.Graph() << std::endl; } + else + { + std::cerr << R"(Only "pose" and "frame" graph types are supported)" + << std::endl; + } return 0; } diff --git a/src/parser.cc b/src/parser.cc index 541be1b65..4863b0f9d 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1664,7 +1664,6 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) << error.Message() << std::endl; } - std::cout << graph.Graph() << std::endl; modelResult = false; } return modelResult; @@ -1694,7 +1693,6 @@ bool checkPoseRelativeToGraph(const sdf::Root *_root) << error.Message() << std::endl; } - std::cout << graph.Graph() << std::endl; worldResult = false; } return worldResult; From 635b96e6816742ee92bdb1e3b462f58686169822 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 23 Oct 2020 17:33:01 -0500 Subject: [PATCH 48/70] Add failing test showing bug in checkFrameAttachedToNames Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics_TEST.cc | 20 +++++++++++++++++++ .../world_frame_invalid_attached_to_scope.sdf | 18 +++++++++++++++++ 2 files changed, 38 insertions(+) create mode 100644 test/sdf/world_frame_invalid_attached_to_scope.sdf diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index fb3704312..000752003 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -572,3 +572,23 @@ TEST(NestedFrameSemantics, ModelWithoutLinksWithNestedStaticModel) resolvedBody); } +///////////////////////////////////////////////// +TEST(NestedFrameSemantics, InvalidAttachedToScope) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_frame_invalid_attached_to_scope.sdf"); + + // Load the SDF file + sdf::Root root; + sdf::Errors errors = root.Load(testFile); + ASSERT_FALSE(errors.empty()); + const std::string expectedMsg = + "attached_to name[M2::M1::L1] specified by frame with name[F] " + "does not match a model or frame name in world with " + "name[world_frame_invalid_attached_to_scope]."; + EXPECT_EQ(expectedMsg, errors[0].Message()); + + EXPECT_FALSE(sdf::checkFrameAttachedToGraph(&root)); + EXPECT_FALSE(sdf::checkFrameAttachedToNames(&root)); +} diff --git a/test/sdf/world_frame_invalid_attached_to_scope.sdf b/test/sdf/world_frame_invalid_attached_to_scope.sdf new file mode 100644 index 000000000..9496ad782 --- /dev/null +++ b/test/sdf/world_frame_invalid_attached_to_scope.sdf @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + From 76515da08dfe13b40e8e9929013782ccb2e5fecf Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 23 Oct 2020 17:33:40 -0500 Subject: [PATCH 49/70] Fix how frame attached_to names are checked for existence Signed-off-by: Addisu Z. Taddese --- src/parser.cc | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/parser.cc b/src/parser.cc index 4863b0f9d..a2eb1c150 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1420,15 +1420,17 @@ bool checkFrameAttachedToNames(const sdf::Root *_root) return true; } - for (uint64_t m = 0; m < _inWorld->ModelCount(); ++m) + const auto delimIndex = _name.find("::"); + if (delimIndex != std::string::npos && delimIndex + 2 < _name.size()) { - const auto *model = _inWorld->ModelByIndex(m); - auto index = _name.find(model->Name() + "::"); - std::string nameToCheck = _name; - if (index != std::string::npos) + std::string modelName = _name.substr(0, delimIndex); + std::string nameToCheck = _name.substr(delimIndex + 2); + const auto *model = _inWorld->ModelByName(modelName); + if (nullptr == model) { - nameToCheck = _name.substr(index + model->Name().size() + 2); + return false; } + if (model->LinkNameExists(nameToCheck) || model->ModelNameExists(nameToCheck) || model->JointNameExists(nameToCheck) || From d552382ce97ac8f396aa5e4d49f08f0e731a4ce3 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 23 Oct 2020 18:13:57 -0500 Subject: [PATCH 50/70] Add unit test for the output stream of sdf::Errors Signed-off-by: Addisu Z. Taddese --- src/Types_TEST.cc | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/Types_TEST.cc b/src/Types_TEST.cc index 1a406970e..bf1f80930 100644 --- a/src/Types_TEST.cc +++ b/src/Types_TEST.cc @@ -18,8 +18,10 @@ #include #include +#include #include +#include "sdf/Error.hh" #include "sdf/Types.hh" ///////////////////////////////////////////////// @@ -99,6 +101,25 @@ TEST(Types, trim_nothing) EXPECT_EQ(out, "xyz"); } +///////////////////////////////////////////////// +TEST(Types, ErrorsOutputStream) +{ + sdf::Errors errors; + errors.emplace_back(sdf::ErrorCode::FILE_READ, "Error reading file"); + errors.emplace_back(sdf::ErrorCode::DUPLICATE_NAME, "Found duplicate name"); + std::string expected = "Error Code "; + expected += + std::to_string(static_cast(sdf::ErrorCode::FILE_READ)); + expected += " Msg: Error reading file\nError Code "; + expected += + std::to_string(static_cast(sdf::ErrorCode::DUPLICATE_NAME)); + expected += " Msg: Found duplicate name\n"; + + std::stringstream output; + output << errors; + EXPECT_EQ(expected, output.str()); +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) From 60a903dff3e846e2f00288f561a3c85d47770b4d Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 27 Oct 2020 12:06:40 -0500 Subject: [PATCH 51/70] Return void from Model::SetPoseRelativeToGraph Signed-off-by: Addisu Z. Taddese --- include/sdf/Model.hh | 3 +-- src/Model.cc | 19 +++---------------- src/World.cc | 5 +---- 3 files changed, 5 insertions(+), 22 deletions(-) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index dbd262e3f..027a016c1 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -304,8 +304,7 @@ namespace sdf /// poses. This is private and is intended to be called by World::Load and /// Model::Load if this is a nested model. /// \param[in] _graph Scoped PoseRelativeToGraph object. - /// \return Error if the scoped graph is invalid. - private: sdf::Errors SetPoseRelativeToGraph( + private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); /// \brief Give the Scoped FrameAttachedToGraph to be used for resolving diff --git a/src/Model.cc b/src/Model.cc index 9cefb330c..3a7158357 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -747,18 +747,8 @@ void Model::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -Errors Model::SetPoseRelativeToGraph( - sdf::ScopedGraph _graph) +void Model::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { - Errors errors; - - if (!_graph) - { - errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "Tried to set an invalid Scoped PoseRelativeToGraph object."}); - return errors; - } - // If the scoped graph doesn't point to the graph owned by this model, we // clear the owned graph to maintain the invariant that if // ownedPoseGraph is valid poseGraph points to it. @@ -778,10 +768,7 @@ Errors Model::SetPoseRelativeToGraph( } for (auto &model : this->dataPtr->models) { - Errors setPoseRelativeToGraphErrors = - model.SetPoseRelativeToGraph(childPoseGraph); - errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), - setPoseRelativeToGraphErrors.end()); + model.SetPoseRelativeToGraph(childPoseGraph); } for (auto &joint : this->dataPtr->joints) { @@ -791,8 +778,8 @@ Errors Model::SetPoseRelativeToGraph( { frame.SetPoseRelativeToGraph(childPoseGraph); } - return errors; } + ///////////////////////////////////////////////// void Model::SetFrameAttachedToGraph( sdf::ScopedGraph _graph) diff --git a/src/World.cc b/src/World.cc index 320850ce2..ff4ae55b5 100644 --- a/src/World.cc +++ b/src/World.cc @@ -390,10 +390,7 @@ Errors World::Load(sdf::ElementPtr _sdf) } for (auto &model : this->dataPtr->models) { - Errors setPoseRelativeToGraphErrors = - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(), - setPoseRelativeToGraphErrors.end()); + model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); } for (auto &light : this->dataPtr->lights) From 9c297d077fdd80f2223f29281612c461301e892f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 27 Oct 2020 15:29:07 -0500 Subject: [PATCH 52/70] Add test for world level nested references Signed-off-by: Addisu Z. Taddese --- test/integration/nested_model.cc | 61 +++++++++++++++++++ .../world_relative_to_nested_reference.sdf | 45 ++++++++++++++ 2 files changed, 106 insertions(+) create mode 100644 test/sdf/world_relative_to_nested_reference.sdf diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 18e19acf1..10ba1c546 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -1076,6 +1076,67 @@ TEST(NestedReference, PoseRelativeTo) } } +////////////////////////////////////////////////// +TEST(NestedReference, PoseRelativeToInWorld) +{ + const std::string testFile = + sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf", + "world_relative_to_nested_reference.sdf"); + + // Load the SDF file + sdf::Root root; + EXPECT_TRUE(root.Load(testFile).empty()); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + using Pose = ignition::math::Pose3d; + + // The //world/frame elements reference an entity inside M1. Check + // that resolving poses works when referencing such entities. + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F2"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 0, -2, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F3"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 1, -3, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F4"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(2, 0, -4, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F5"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(2, 0, -6, 0, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F6"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, 1, -6, IGN_PI_2, IGN_PI_2, 0), pose); + } + { + Pose pose; + const sdf::Frame *testFrame = world->FrameByName("F7"); + ASSERT_NE(nullptr, testFrame); + EXPECT_TRUE(testFrame->SemanticPose().Resolve(pose).empty()); + EXPECT_EQ(Pose(1, -6, -1, 0, 0, -IGN_PI_2), pose); + } +} + ///////////////////////////////////////////////// TEST(NestedReference, PlacementFrameAttribute) { diff --git a/test/sdf/world_relative_to_nested_reference.sdf b/test/sdf/world_relative_to_nested_reference.sdf new file mode 100644 index 000000000..e19f37233 --- /dev/null +++ b/test/sdf/world_relative_to_nested_reference.sdf @@ -0,0 +1,45 @@ + + + + + 1 0 0 0 1.5707963267948966 0 + + 0 1 0 0 0 0 + + + L1 + L2 + + + 0 0 1 0 0 0 + + + 1 0 0 0 0 0 + + + 0 1 0 1.5707963267948966 0 0 + + 1 0 0 0 -1.5707963267948966 0 + + + + + 2 0 0 0 0 0 + + + 3 0 0 0 0 0 + + + 4 0 0 0 0 0 + + + 5 0 0 0 0 0 + + + 6 0 0 0 0 0 + + + 7 0 0 0 0 0 + + + From de319b9484a3297beb6b83158786c5c81525e3fb Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 27 Oct 2020 15:29:30 -0500 Subject: [PATCH 53/70] Add test for ign graph command Signed-off-by: Addisu Z. Taddese --- src/ign_TEST.cc | 222 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 222 insertions(+) diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 2dbaccc4d..56774149b 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -840,6 +840,228 @@ TEST(print, SDF) } } +///////////////////////////////////////////////// +TEST(GraphCmd, WorldPoseRelativeTo) +{ + const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; + + // world pose relative_to graph + const std::string path = + pathBase + "/world_relative_to_nested_reference.sdf"; + + const std::string output = + custom_exec_str(g_ignCommand + " sdf -g pose " + path + g_sdfVersion); + + std::stringstream expected; + expected << "digraph {\n" + << " 0 [label=\"__root__ (0)\"];\n" + << " 1 [label=\"world (1)\"];\n" + << " 2 [label=\"M1 (2)\"];\n" + << " 3 [label=\"M1::__model__ (3)\"];\n" + << " 4 [label=\"M1::L1 (4)\"];\n" + << " 5 [label=\"M1::L2 (5)\"];\n" + << " 6 [label=\"M1::J1 (6)\"];\n" + << " 7 [label=\"M1::F1 (7)\"];\n" + << " 8 [label=\"M1::CM1 (8)\"];\n" + << " 9 [label=\"M1::CM1::__model__ (9)\"];\n" + << " 10 [label=\"M1::CM1::L (10)\"];\n" + << " 11 [label=\"F2 (11)\"];\n" + << " 12 [label=\"F3 (12)\"];\n" + << " 13 [label=\"F4 (13)\"];\n" + << " 14 [label=\"F5 (14)\"];\n" + << " 15 [label=\"F6 (15)\"];\n" + << " 16 [label=\"F7 (16)\"];\n" + << " 0 -> 1 [label=1];\n" + << " 2 -> 3 [label=0];\n" + << " 3 -> 4 [label=1];\n" + << " 3 -> 5 [label=1];\n" + << " 8 -> 9 [label=0];\n" + << " 9 -> 10 [label=1];\n" + << " 5 -> 6 [label=1];\n" + << " 5 -> 7 [label=1];\n" + << " 3 -> 8 [label=1];\n" + << " 1 -> 2 [label=1];\n" + << " 2 -> 11 [label=1];\n" + << " 4 -> 12 [label=1];\n" + << " 6 -> 13 [label=1];\n" + << " 7 -> 14 [label=1];\n" + << " 8 -> 15 [label=1];\n" + << " 10 -> 16 [label=1];\n" + << "}"; + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + +///////////////////////////////////////////////// +TEST(GraphCmd, ModelPoseRelativeTo) +{ + const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; + const std::string path = pathBase + "/model_relative_to_nested_reference.sdf"; + const std::string output = + custom_exec_str(g_ignCommand + " sdf -g pose " + path + g_sdfVersion); + + std::stringstream expected; + expected << "digraph {\n" + << " 0 [label=\"__root__ (0)\"];\n" + << " 1 [label=\"parent_model (1)\"];\n" + << " 2 [label=\"parent_model::__model__ (2)\"];\n" + << " 3 [label=\"parent_model::L (3)\"];\n" + << " 4 [label=\"parent_model::M1 (4)\"];\n" + << " 5 [label=\"parent_model::M1::__model__ (5)\"];\n" + << " 6 [label=\"parent_model::M1::L1 (6)\"];\n" + << " 7 [label=\"parent_model::M1::L2 (7)\"];\n" + << " 8 [label=\"parent_model::M1::J1 (8)\"];\n" + << " 9 [label=\"parent_model::M1::F1 (9)\"];\n" + << " 10 [label=\"parent_model::M1::CM1 (10)\"];\n" + << " 11 [label=\"parent_model::M1::CM1::__model__ (11)\"];\n" + << " 12 [label=\"parent_model::M1::CM1::L (12)\"];\n" + << " 13 [label=\"parent_model::M2 (13)\"];\n" + << " 14 [label=\"parent_model::M2::__model__ (14)\"];\n" + << " 15 [label=\"parent_model::M2::L (15)\"];\n" + << " 16 [label=\"parent_model::M3 (16)\"];\n" + << " 17 [label=\"parent_model::M3::__model__ (17)\"];\n" + << " 18 [label=\"parent_model::M3::L (18)\"];\n" + << " 19 [label=\"parent_model::M4 (19)\"];\n" + << " 20 [label=\"parent_model::M4::__model__ (20)\"];\n" + << " 21 [label=\"parent_model::M4::L (21)\"];\n" + << " 22 [label=\"parent_model::M5 (22)\"];\n" + << " 23 [label=\"parent_model::M5::__model__ (23)\"];\n" + << " 24 [label=\"parent_model::M5::L (24)\"];\n" + << " 25 [label=\"parent_model::M6 (25)\"];\n" + << " 26 [label=\"parent_model::M6::__model__ (26)\"];\n" + << " 27 [label=\"parent_model::M6::L (27)\"];\n" + << " 28 [label=\"parent_model::M7 (28)\"];\n" + << " 29 [label=\"parent_model::M7::__model__ (29)\"];\n" + << " 30 [label=\"parent_model::M7::L (30)\"];\n" + << " 1 -> 2 [label=0];\n" + << " 2 -> 3 [label=1];\n" + << " 4 -> 5 [label=0];\n" + << " 5 -> 6 [label=1];\n" + << " 5 -> 7 [label=1];\n" + << " 10 -> 11 [label=0];\n" + << " 11 -> 12 [label=1];\n" + << " 7 -> 8 [label=1];\n" + << " 7 -> 9 [label=1];\n" + << " 5 -> 10 [label=1];\n" + << " 13 -> 14 [label=0];\n" + << " 14 -> 15 [label=1];\n" + << " 16 -> 17 [label=0];\n" + << " 17 -> 18 [label=1];\n" + << " 19 -> 20 [label=0];\n" + << " 20 -> 21 [label=1];\n" + << " 22 -> 23 [label=0];\n" + << " 23 -> 24 [label=1];\n" + << " 25 -> 26 [label=0];\n" + << " 26 -> 27 [label=1];\n" + << " 28 -> 29 [label=0];\n" + << " 29 -> 30 [label=1];\n" + << " 2 -> 4 [label=1];\n" + << " 4 -> 13 [label=1];\n" + << " 6 -> 16 [label=1];\n" + << " 8 -> 19 [label=1];\n" + << " 9 -> 22 [label=1];\n" + << " 10 -> 25 [label=1];\n" + << " 12 -> 28 [label=1];\n" + << " 0 -> 1 [label=1];\n" + << "}"; + + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + +///////////////////////////////////////////////// +TEST(GraphCmd, WorldFrameAttachedTo) +{ + const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; + const std::string path = pathBase + "/world_nested_frame_attached_to.sdf"; + const std::string output = + custom_exec_str(g_ignCommand + " sdf -g frame " + path + g_sdfVersion); + + std::stringstream expected; + + expected << "digraph {\n" + << " 0 [label=\"world (0)\"];\n" + << " 1 [label=\"M1 (1)\"];\n" + << " 2 [label=\"M1::__model__ (2)\"];\n" + << " 3 [label=\"M1::L (3)\"];\n" + << " 4 [label=\"M1::J1 (4)\"];\n" + << " 5 [label=\"M1::F0 (5)\"];\n" + << " 6 [label=\"M1::M2 (6)\"];\n" + << " 7 [label=\"M1::M2::__model__ (7)\"];\n" + << " 8 [label=\"M1::M2::L (8)\"];\n" + << " 9 [label=\"world_frame (9)\"];\n" + << " 10 [label=\"F0 (10)\"];\n" + << " 11 [label=\"F1 (11)\"];\n" + << " 12 [label=\"F2 (12)\"];\n" + << " 13 [label=\"F3 (13)\"];\n" + << " 14 [label=\"F4 (14)\"];\n" + << " 15 [label=\"F5 (15)\"];\n" + << " 16 [label=\"F6 (16)\"];\n" + << " 1 -> 2 [label=0];\n" + << " 6 -> 7 [label=0];\n" + << " 7 -> 8 [label=1];\n" + << " 4 -> 5 [label=1];\n" + << " 5 -> 8 [label=1];\n" + << " 2 -> 3 [label=1];\n" + << " 9 -> 0 [label=1];\n" + << " 10 -> 0 [label=1];\n" + << " 11 -> 1 [label=1];\n" + << " 12 -> 3 [label=1];\n" + << " 13 -> 6 [label=1];\n" + << " 14 -> 8 [label=1];\n" + << " 15 -> 5 [label=1];\n" + << " 16 -> 4 [label=1];\n" + << "}"; + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + +///////////////////////////////////////////////// +TEST(GraphCmd, ModelFrameAttachedTo) +{ + const std::string pathBase = std::string(PROJECT_SOURCE_PATH) + "/test/sdf"; + const std::string path = pathBase + "/model_nested_frame_attached_to.sdf"; + const std::string output = + custom_exec_str(g_ignCommand + " sdf -g frame " + path + g_sdfVersion); + + std::stringstream expected; + + expected + << "digraph {\n" + << " 0 [label=\"__root__ (0)\"];\n" + << " 1 [label=\"nested_model_frame_attached_to (1)\"];\n" + << " 2 [label=\"nested_model_frame_attached_to::__model__ (2)\"];\n" + << " 3 [label=\"nested_model_frame_attached_to::L (3)\"];\n" + << " 4 [label=\"nested_model_frame_attached_to::F0 (4)\"];\n" + << " 5 [label=\"nested_model_frame_attached_to::F1 (5)\"];\n" + << " 6 [label=\"nested_model_frame_attached_to::F2 (6)\"];\n" + << " 7 [label=\"nested_model_frame_attached_to::F3 (7)\"];\n" + << " 8 [label=\"nested_model_frame_attached_to::F4 (8)\"];\n" + << " 9 [label=\"nested_model_frame_attached_to::M1 (9)\"];\n" + << " 10 [label=\"nested_model_frame_attached_to::M1::__model__ " + "(10)\"];\n" + << " 11 [label=\"nested_model_frame_attached_to::M1::L (11)\"];\n" + << " 12 [label=\"nested_model_frame_attached_to::M1::J1 (12)\"];\n" + << " 13 [label=\"nested_model_frame_attached_to::M1::F (13)\"];\n" + << " 14 [label=\"nested_model_frame_attached_to::M1::M2 (14)\"];\n" + << " 15 [label=\"nested_model_frame_attached_to::M1::M2::__model__ " + "(15)\"];\n" + << " 16 [label=\"nested_model_frame_attached_to::M1::M2::L (16)\"];\n" + << " 1 -> 2 [label=0];\n" + << " 9 -> 10 [label=0];\n" + << " 14 -> 15 [label=0];\n" + << " 15 -> 16 [label=1];\n" + << " 12 -> 13 [label=1];\n" + << " 13 -> 14 [label=1];\n" + << " 10 -> 11 [label=1];\n" + << " 4 -> 9 [label=1];\n" + << " 5 -> 11 [label=1];\n" + << " 6 -> 5 [label=1];\n" + << " 7 -> 13 [label=1];\n" + << " 8 -> 12 [label=1];\n" + << " 2 -> 3 [label=1];\n" + << "}"; + + EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) From 2a21634532270e9d70007694dacac43d9dd484e4 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 27 Oct 2020 17:18:57 -0500 Subject: [PATCH 54/70] Be more selective about checking for usage of __root__ Instead of checking if any attribute has a value of "__root__", only the following attributes are checked: * //frame/[@attached_to] * //pose/[@relative_to] * //model/[@placement_frame] * //model/[@canonical_link] * //sensor/imu/orientation_reference_frame/custom_rpy/[@parent_frame] In addition, the elements //joint/parent, //joint/child and //include/placement_frame are checked for validity Signed-off-by: Addisu Z. Taddese --- src/Joint.cc | 12 ++++++ src/Utils.cc | 6 +++ src/Utils.hh | 6 +++ src/ign_TEST.cc | 43 ++++++++++++++++++++-- src/parser.cc | 45 ++++++++++++++++++++--- test/sdf/world_invalid_root_reference.sdf | 30 ++++++++++++++- test/sdf/world_valid_root_reference.sdf | 15 ++++++++ 7 files changed, 147 insertions(+), 10 deletions(-) create mode 100644 test/sdf/world_valid_root_reference.sdf diff --git a/src/Joint.cc b/src/Joint.cc index e1b29bac5..711dffb9d 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -183,6 +183,12 @@ Errors Joint::Load(ElementPtr _sdf) if (parentPair.second) { this->dataPtr->parentLinkName = parentPair.first; + if (!isValidFrameReference(this->dataPtr->parentLinkName)) + { + errors.push_back({ErrorCode::RESERVED_NAME, + "The supplied joint parent name [" + this->dataPtr->parentLinkName + + "] is not valid."}); + } } else { @@ -195,6 +201,12 @@ Errors Joint::Load(ElementPtr _sdf) if (childPair.second) { this->dataPtr->childLinkName = childPair.first; + if (!isValidFrameReference(this->dataPtr->childLinkName)) + { + errors.push_back({ErrorCode::RESERVED_NAME, + "The supplied joint child name [" + this->dataPtr->childLinkName + + "] is not valid."}); + } } else { diff --git a/src/Utils.cc b/src/Utils.cc index 9ed658039..b28ac26ee 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -74,5 +74,11 @@ bool loadPose(sdf::ElementPtr _sdf, ignition::math::Pose3d &_pose, // on the pose element value. return posePair.second; } + +///////////////////////////////////////////////// +bool isValidFrameReference(const std::string &_name) +{ + return "__root__" != _name; +} } } diff --git a/src/Utils.hh b/src/Utils.hh index be6ca1432..f46be539e 100644 --- a/src/Utils.hh +++ b/src/Utils.hh @@ -37,6 +37,12 @@ namespace sdf /// \returns True if the name is a reserved name and thus invalid. bool isReservedName(const std::string &_name); + /// \brief Check if the passed string is a valid frame reference. + /// Currently it's only invalid if the name is __root__ + /// \param[in] _name Name to check. + /// \returns True if the name is a valid frame reference. + bool isValidFrameReference(const std::string &_name); + /// \brief Read the "name" attribute from an element. /// \param[in] _sdf SDF element pointer which contains the name. /// \param[out] _name String to hold the name value. diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 56774149b..ecf31ae9c 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -728,7 +728,6 @@ TEST(check, SDF) { std::string path = pathBase + "/nested_model_cross_references.sdf"; - // Check model_invalid_placement_frame.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_EQ("Valid.\n", output) << output; @@ -738,7 +737,6 @@ TEST(check, SDF) { std::string path = pathBase + "/model_invalid_root_reference.sdf"; - // Check model_invalid_placement_frame.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE( @@ -754,9 +752,10 @@ TEST(check, SDF) } // Check an SDF world file with an invalid usage of __root__ { + // Set SDF_PATH so that included models can be found + setenv("SDF_PATH", PROJECT_SOURCE_PATH "/test/integration/model", 1); std::string path = pathBase + "/world_invalid_root_reference.sdf"; - // Check model_invalid_placement_frame.sdf std::string output = custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); EXPECT_NE( @@ -769,6 +768,44 @@ TEST(check, SDF) "value of attribute [attached_to]"), std::string::npos) << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [placement_frame]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [canonical_link]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of attribute [parent_frame]"), + std::string::npos) + << output; + EXPECT_NE( + output.find("Error: '__root__' is reserved; it cannot be used as a " + "value of element [placement_frame]"), + std::string::npos) + << output; + EXPECT_NE( + output.find( + "Error: The supplied joint child name [__root__] is not valid"), + std::string::npos) + << output; + EXPECT_NE( + output.find( + "Error: The supplied joint parent name [__root__] is not valid"), + std::string::npos) + << output; + } + // Check an SDF world file with an valid usage of __root__ + { + std::string path = pathBase + "/world_valid_root_reference.sdf"; + + std::string output = + custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion); + EXPECT_EQ("Valid.\n", output) << output; } } diff --git a/src/parser.cc b/src/parser.cc index a2eb1c150..672ba051f 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include @@ -38,6 +39,7 @@ #include "Converter.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" +#include "Utils.hh" #include "parser_private.hh" #include "parser_urdf.hh" @@ -838,6 +840,21 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) _sdf->Copy(refSDF); } + // A list of attributes where a frame name is referenced. This is used to + // check if the reference is is invalid. + std::unordered_set frameReferenceAttributes { + // //frame/[@attached_to] + "attached_to", + // //pose/[@relative_to] + "relative_to", + // //model/[@placement_frame] + "placement_frame", + // //model/[@canonical_link] + "canonical_link", + // //sensor/imu/orientation_reference_frame/custom_rpy/[@parent_frame] + "parent_frame", + }; + const tinyxml2::XMLAttribute *attribute = _xml->FirstAttribute(); unsigned int i = 0; @@ -861,11 +878,16 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) ParamPtr p = _sdf->GetAttribute(i); if (p->GetKey() == attribute->Name()) { - if (attribute->Value() == std::string("__root__")) + if (frameReferenceAttributes.count(attribute->Name()) != 0) { - _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, - "'__root__' is reserved; it cannot be used as a value of " - "attribute [" + p->GetKey() + "]"}); + if (!isValidFrameReference(attribute->Value())) + { + _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, + "'" + std::string(attribute->Value()) + + "' is reserved; it cannot be used as a value of " + "attribute [" + + p->GetKey() + "]"}); + } } // Set the value of the SDF attribute if (!p->SetFromString(attribute->Value())) @@ -1049,8 +1071,19 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) "element"}); return false; } - topLevelElem->GetAttribute("placement_frame")->SetFromString( - elemXml->FirstChildElement("placement_frame")->GetText()); + + const std::string placementFrameVal = + elemXml->FirstChildElement("placement_frame")->GetText(); + + if (!isValidFrameReference(placementFrameVal)) + { + _errors.push_back({ErrorCode::RESERVED_NAME, + "'" + placementFrameVal + + "' is reserved; it cannot be used as a value of " + "element [placement_frame]"}); + } + topLevelElem->GetAttribute("placement_frame") + ->SetFromString(placementFrameVal); } if (isModel || isActor) diff --git a/test/sdf/world_invalid_root_reference.sdf b/test/sdf/world_invalid_root_reference.sdf index b8735a4ba..8b4e6eba5 100644 --- a/test/sdf/world_invalid_root_reference.sdf +++ b/test/sdf/world_invalid_root_reference.sdf @@ -1,13 +1,41 @@ - + 1 0 0 0 0 0 + + + + __root__ + link2 + + + + link1 + __root__ + + + + + + + + + + + + 1 0 0 0 0 0 + + + test_model_with_frames + __root__ + 1 0 0 0 0 0 + diff --git a/test/sdf/world_valid_root_reference.sdf b/test/sdf/world_valid_root_reference.sdf new file mode 100644 index 000000000..07e5634dd --- /dev/null +++ b/test/sdf/world_valid_root_reference.sdf @@ -0,0 +1,15 @@ + + + + + 1 0 0 0 0 0 + + + __root__ + + + + + + + From f814bb122a347096c8eae900960f891bd973db5f Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 28 Oct 2020 10:33:47 -0500 Subject: [PATCH 55/70] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- include/sdf/SemanticPose.hh | 4 ++-- src/FrameSemantics.cc | 2 +- src/parser.cc | 22 +++++++++++----------- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/include/sdf/SemanticPose.hh b/include/sdf/SemanticPose.hh index 794422a9c..0a263180e 100644 --- a/include/sdf/SemanticPose.hh +++ b/include/sdf/SemanticPose.hh @@ -79,7 +79,7 @@ namespace sdf /// raw pose is applied. /// \param[in] _defaultResolveTo Default frame to resolve-to in Resolve() /// if no frame is specified. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph A scoped PoseRelativeToGraph object. private: SemanticPose( const ignition::math::Pose3d &_pose, const std::string &_relativeTo, @@ -96,7 +96,7 @@ namespace sdf /// raw pose is applied. /// \param[in] _defaultResolveTo Default frame to resolve-to in Resolve() /// if no frame is specified. - /// \param[in] _graph Weak pointer to PoseRelativeToGraph. + /// \param[in] _graph A scoped PoseRelativeToGraph object. private: SemanticPose( const std::string &_name, const ignition::math::Pose3d &_pose, diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index fb689c1fb..eec91aff3 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -291,7 +291,7 @@ Errors buildFrameAttachedToGraph( "Invalid model element in sdf::Model."}); return errors; } - else if (_model->LinkCount() < 1 && _model->ModelCount() < 1 && + else if (_model->LinkCount() == 0 && _model->ModelCount() == 0 && !_model->Static()) { errors.push_back({ErrorCode::MODEL_WITHOUT_LINK, diff --git a/src/parser.cc b/src/parser.cc index 672ba051f..35a9bb468 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -18,8 +18,8 @@ #include #include #include +#include #include -#include #include @@ -840,20 +840,19 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) _sdf->Copy(refSDF); } - // A list of attributes where a frame name is referenced. This is used to - // check if the reference is is invalid. - std::unordered_set frameReferenceAttributes { + // A list of parent element-attributes pairs where a frame name is referenced + // in the attribute. This is used to check if the reference is invalid. + std::set> frameReferenceAttributes { // //frame/[@attached_to] - "attached_to", + {"frame", "attached_to"}, // //pose/[@relative_to] - "relative_to", + {"pose", "relative_to"}, // //model/[@placement_frame] - "placement_frame", + {"model", "placement_frame"}, // //model/[@canonical_link] - "canonical_link", + {"model", "canonical_link"}, // //sensor/imu/orientation_reference_frame/custom_rpy/[@parent_frame] - "parent_frame", - }; + {"custom_rpy", "parent_frame"}}; const tinyxml2::XMLAttribute *attribute = _xml->FirstAttribute(); @@ -878,7 +877,8 @@ bool readXml(tinyxml2::XMLElement *_xml, ElementPtr _sdf, Errors &_errors) ParamPtr p = _sdf->GetAttribute(i); if (p->GetKey() == attribute->Name()) { - if (frameReferenceAttributes.count(attribute->Name()) != 0) + if (frameReferenceAttributes.count( + std::make_pair(_sdf->GetName(), attribute->Name())) != 0) { if (!isValidFrameReference(attribute->Value())) { From eef24a0aec049ce4af6670b7cccc08405b81287a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 29 Oct 2020 22:21:45 -0500 Subject: [PATCH 56/70] Scoped -> scoped Signed-off-by: Addisu Z. Taddese --- include/sdf/Collision.hh | 4 ++-- include/sdf/Frame.hh | 8 ++++---- include/sdf/Joint.hh | 8 ++++---- include/sdf/JointAxis.hh | 4 ++-- include/sdf/Light.hh | 4 ++-- include/sdf/Link.hh | 4 ++-- include/sdf/Model.hh | 8 ++++---- include/sdf/Sensor.hh | 4 ++-- include/sdf/Visual.hh | 4 ++-- 9 files changed, 24 insertions(+), 24 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index fc2df8d66..bf66b9fc6 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -142,10 +142,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Frame.hh b/include/sdf/Frame.hh index 767d4b9d4..d9d9437d9 100644 --- a/include/sdf/Frame.hh +++ b/include/sdf/Frame.hh @@ -145,17 +145,17 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give a Scoped FrameAttachedToGraph to be used for resolving + /// \brief Give a scoped FrameAttachedToGraph to be used for resolving /// attached bodies. This is private and is intended to be called by /// Model::Load or World::Load. - /// \param[in] _graph Scoped FrameAttachedToGraph object. + /// \param[in] _graph scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( sdf::ScopedGraph _graph); - /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Model::Load or /// World::Load. - /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index f24aba59d..3be388a57 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -222,16 +222,16 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give the Scoped FrameAttachedToGraph to be used for resolving + /// \brief Give the scoped FrameAttachedToGraph to be used for resolving /// parent and child link names. This is private and is intended to be /// called by Model::Load. - /// \param[in] _graph Scoped FrameAttachedToGraph object. + /// \param[in] _graph scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( sdf::ScopedGraph _graph); - /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Model::Load. - /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/JointAxis.hh b/include/sdf/JointAxis.hh index 22d2d8ab4..71ae8278c 100644 --- a/include/sdf/JointAxis.hh +++ b/include/sdf/JointAxis.hh @@ -270,10 +270,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by /// Joint::SetPoseRelativeToGraph. - /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Light.hh b/include/sdf/Light.hh index 570e464ff..41b875287 100644 --- a/include/sdf/Light.hh +++ b/include/sdf/Light.hh @@ -274,10 +274,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph or World::Load. - /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index da96b3ce5..87feaf48f 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -226,9 +226,9 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; - /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Model::Load. - /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 027a016c1..020ae674e 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -300,17 +300,17 @@ namespace sdf /// \param[in] _name Name of the placement frame. public: void SetPlacementFrameName(const std::string &_name); - /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by World::Load and /// Model::Load if this is a nested model. - /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); - /// \brief Give the Scoped FrameAttachedToGraph to be used for resolving + /// \brief Give the scoped FrameAttachedToGraph to be used for resolving /// attached bodies. This is private and is intended to be called by /// World::Load and Model::Load if this is a nested model. - /// \param[in] _graph Scoped FrameAttachedToGraph object. + /// \param[in] _graph scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 3f57c46c4..b5b92bd7d 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -317,10 +317,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index a4e283b99..316562dd0 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -169,10 +169,10 @@ namespace sdf /// \param[in] _xmlParentName Name of xml parent object. private: void SetXmlParentName(const std::string &_xmlParentName); - /// \brief Give the Scoped PoseRelativeToGraph to be used for resolving + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by /// Link::SetPoseRelativeToGraph. - /// \param[in] _graph Scoped PoseRelativeToGraph object. + /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); From 3c61c82c241e7b07cc78f4c2375fac8987697388 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 29 Oct 2020 22:51:05 -0500 Subject: [PATCH 57/70] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 10 +++++----- src/FrameSemantics_TEST.cc | 38 ++++++++++++++++++++++++++++++++++++++ src/Model_TEST.cc | 1 + 3 files changed, 44 insertions(+), 5 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index eec91aff3..2acc4466a 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -235,15 +235,15 @@ std::pair /// /// When the placement_frame is specified in an SDFormat file, //model/pose /// becomes the pose of the placement frame (X_RPf) instead of the pose of the -/// model frame (X_RM). However, when the model inserted into a pose graph of +/// model frame (X_RM). However, when the model is inserted into a pose graph of /// the parent scope, only the pose (X_RM) of the __model__ frame can be used. /// i.e, X_RPf cannot be represented in the PoseRelativeTo graph. /// Thus, the X_RM has to be calculated such that X_RPf = X_RM * X_MPf. // -/// \param[in] _model Model whose pose to resolve relative to its parent -/// \param[in] _graph The scoped PoseRelativeTo graph used to resolve the model. -/// The scope must contain the frame referenced by the placement frame attribute -/// of the input model. +/// \param[in] _model Input model whose pose to resolve relative to its parent +/// \param[in] _graph The PoseRelativeTo graph with the same scope as the +/// frame referenced by the placement frame attribute of the input model. i.e, +/// the input model's graph. /// \param[out] _resolvedPose The resolved pose (X_RM). If an error was /// encountered during `sdf::resolvePoseRelativeToRoot`, _resolvedPose will not /// be modified. diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index 000752003..752033610 100644 --- a/src/FrameSemantics_TEST.cc +++ b/src/FrameSemantics_TEST.cc @@ -519,6 +519,15 @@ TEST(NestedFrameSemantics, buildFrameAttachedToGraph_World) EXPECT_EQ(1u, modelGraph.Count("M2::L")); EXPECT_EQ(1u, modelGraph.Count("F0")); EXPECT_EQ(0u, modelGraph.Count("invalid")); + // The following Count expectations are 0 because the frames are out of scope. + EXPECT_EQ(0u, modelGraph.Count("world")); + EXPECT_EQ(0u, modelGraph.Count("world_frame")); + EXPECT_EQ(0u, modelGraph.Count("F1")); + EXPECT_EQ(0u, modelGraph.Count("F2")); + EXPECT_EQ(0u, modelGraph.Count("F3")); + EXPECT_EQ(0u, modelGraph.Count("F4")); + EXPECT_EQ(0u, modelGraph.Count("F5")); + EXPECT_EQ(0u, modelGraph.Count("F6")); EXPECT_TRUE( sdf::resolveFrameAttachedToBody( @@ -540,6 +549,35 @@ TEST(NestedFrameSemantics, buildFrameAttachedToGraph_World) EXPECT_TRUE( sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F0").empty()); EXPECT_EQ("M2::L", resolvedBody); + + errors = sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "world"); + ASSERT_FALSE(errors.empty()); + + EXPECT_EQ( + "FrameAttachedToGraph unable to find unique frame with name " + "[world] in graph.", + errors[0].Message()); + + errors = + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "world_frame"); + ASSERT_FALSE(errors.empty()); + EXPECT_EQ( + "FrameAttachedToGraph unable to find unique frame with name " + "[world_frame] in graph.", + errors[0].Message()); + + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F1").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F2").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F3").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F4").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F5").empty()); + EXPECT_FALSE( + sdf::resolveFrameAttachedToBody(resolvedBody, modelGraph, "F6").empty()); } ///////////////////////////////////////////////// diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index cafa44788..fed8a3109 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -22,6 +22,7 @@ #include "sdf/Model.hh" ///////////////////////////////////////////////// +/// Test default construction of sdf::Model. TEST(DOMModel, Construction) { sdf::Model model; From f5390eecdef8f20a4442b5b13cbac71b6f7a5434 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 29 Oct 2020 23:32:18 -0500 Subject: [PATCH 58/70] Change scopeName to scopeContextName Signed-off-by: Addisu Z. Taddese --- src/Frame.cc | 8 ++-- src/FrameSemantics.cc | 92 +++++++++++++++++++++++-------------------- src/ScopedGraph.hh | 42 ++++++++++---------- 3 files changed, 75 insertions(+), 67 deletions(-) diff --git a/src/Frame.cc b/src/Frame.cc index 59347dd24..5431a9132 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -42,8 +42,8 @@ class sdf::FramePrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Name of graph source. - std::string graphSourceName = ""; + /// \brief The context name of the scoped Pose Relative-To graph + std::string graphScopeContextName = ""; /// \brief Scoped Frame Attached-To graph at the parent model or world scope. public: sdf::ScopedGraph frameAttachedToGraph; @@ -210,7 +210,7 @@ void Frame::SetPoseRelativeToGraph( auto graph = this->dataPtr->poseRelativeToGraph; if (graph) { - this->dataPtr->graphSourceName = graph.ScopeName(); + this->dataPtr->graphScopeContextName = graph.ScopeContextName(); } } @@ -248,7 +248,7 @@ sdf::SemanticPose Frame::SemanticPose() const this->dataPtr->name, this->dataPtr->pose, relativeTo, - this->dataPtr->graphSourceName, + this->dataPtr->graphScopeContextName, this->dataPtr->poseRelativeToGraph); } diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 2acc4466a..0b83bdc36 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -302,20 +302,20 @@ Errors buildFrameAttachedToGraph( auto frameType = _model->Static() ? sdf::FrameType::STATIC_MODEL : sdf::FrameType::MODEL; - const std::string scopeName = "__model__"; + const std::string scopeContextName = "__model__"; auto rootId = ignition::math::graph::kNullId; if (_root) { _out = _out.AddScopeVertex( - "", "__root__", scopeName, sdf::FrameType::STATIC_MODEL); + "", "__root__", scopeContextName, sdf::FrameType::STATIC_MODEL); rootId = _out.ScopeVertexId(); } const auto modelId = _out.AddVertex(_model->Name(), frameType).Id(); - auto outModel = - _out.AddScopeVertex(_model->Name(), scopeName, scopeName, frameType); + auto outModel = _out.AddScopeVertex( + _model->Name(), scopeContextName, scopeContextName, frameType); const auto modelFrameId = outModel.ScopeVertexId(); // Add an aliasing edge from the model vertex to the @@ -412,8 +412,8 @@ Errors buildFrameAttachedToGraph( std::string attachedTo = frame->AttachedTo(); if (attachedTo.empty()) { - // if the attached-to name is empty, use the scope name - attachedTo = scopeName; + // if the attached-to name is empty, use the scope context name + attachedTo = scopeContextName; } if (outModel.Count(attachedTo) != 1) { @@ -507,8 +507,9 @@ Errors buildFrameAttachedToGraph( } // add implicit world frame vertex first - const std::string scopeName = "world"; - _out = _out.AddScopeVertex("", scopeName, scopeName, sdf::FrameType::WORLD); + const std::string scopeContextName = "world"; + _out = _out.AddScopeVertex( + "", scopeContextName, scopeContextName, sdf::FrameType::WORLD); // add model vertices for (uint64_t m = 0; m < _world->ModelCount(); ++m) @@ -550,13 +551,13 @@ Errors buildFrameAttachedToGraph( std::string attachedTo = frame->AttachedTo(); if (attachedTo.empty()) { - // if the attached-to name is empty, use the scope name - attachedTo = scopeName; - if (_out.Count(scopeName) != 1) + // if the attached-to name is empty, use the scope context name + attachedTo = scopeContextName; + if (_out.Count(scopeContextName) != 1) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + - scopeName + "] not found in map."}); + scopeContextName + "] not found in map."}); continue; } } @@ -606,18 +607,18 @@ Errors buildPoseRelativeToGraph( return errors; } - const std::string scopeName = "__model__"; + const std::string scopeContextName = "__model__"; auto rootId = ignition::math::graph::kNullId; // add the model frame vertex first if (_root) { _out = _out.AddScopeVertex( - "", "__root__", scopeName, sdf::FrameType::MODEL); + "", "__root__", scopeContextName, sdf::FrameType::MODEL); rootId = _out.ScopeVertexId(); } auto modelId = _out.AddVertex(_model->Name(), sdf::FrameType::MODEL).Id(); - auto outModel = _out.AddScopeVertex( - _model->Name(), scopeName, scopeName, sdf::FrameType::MODEL); + auto outModel = _out.AddScopeVertex(_model->Name(), scopeContextName, + scopeContextName, sdf::FrameType::MODEL); auto modelFrameId = outModel.ScopeVertexId(); // Add an aliasing edge from the model vertex to the @@ -904,12 +905,14 @@ Errors buildPoseRelativeToGraph( return errors; } // add implicit world frame vertex first - const std::string sourceName = "world"; + const std::string scopeContextName = "world"; - _out = _out.AddScopeVertex("", "__root__", sourceName, sdf::FrameType::WORLD); + _out = _out.AddScopeVertex( + "", "__root__", scopeContextName, sdf::FrameType::WORLD); auto rootId = _out.ScopeVertexId(); - _out = _out.AddScopeVertex("", sourceName, sourceName, sdf::FrameType::WORLD); + _out = _out.AddScopeVertex( + "", scopeContextName, scopeContextName, sdf::FrameType::WORLD); auto worldFrameId = _out.ScopeVertexId(); _out.AddEdge({rootId, worldFrameId}, {}); @@ -1057,28 +1060,29 @@ Errors validateFrameAttachedToGraph( { Errors errors; - // Expect scopeName to be either "__model__" or "world" - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") + // Expect ScopeContextName to be either "__model__" or "world" + if (_in.ScopeContextName() != "__model__" && + _in.ScopeContextName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] " - " does not match __model__ or world."}); + "FrameAttachedToGraph error: scope context name[" + + _in.ScopeContextName() + "] does not match __model__ or world."}); return errors; } - // Expect the scope vertex to be valid and check that the scope name matches - // the frame type. + // Expect the scope vertex to be valid and check that the scope context + // matches the frame type. auto scopeVertex = _in.ScopeVertex(); if (!scopeVertex.Valid()) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: scope frame[" + - _in.ScopeName() + "] not found in graph."}); + _in.ScopeContextName() + "] not found in graph."}); return errors; } sdf::FrameType scopeFrameType = scopeVertex.Data(); - if (_in.ScopeName() == "__model__" && + if (_in.ScopeContextName() == "__model__" && scopeFrameType != sdf::FrameType::MODEL && scopeFrameType != sdf::FrameType::STATIC_MODEL) { @@ -1088,7 +1092,7 @@ Errors validateFrameAttachedToGraph( "STATIC_MODEL."}); return errors; } - else if (_in.ScopeName() == "world" && + else if (_in.ScopeContextName() == "world" && scopeFrameType != sdf::FrameType::WORLD) { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, @@ -1275,28 +1279,29 @@ Errors validatePoseRelativeToGraph( { Errors errors; - // Expect scopeName to be either "__model__" or "world" - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") + // Expect scopeContextName to be either "__model__" or "world" + if (_in.ScopeContextName() != "__model__" && + _in.ScopeContextName() != "world") { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, - "PoseRelativeToGraph error: source vertex name " + _in.ScopeName() + - " does not match __model__ or world."}); + "PoseRelativeToGraph error: scope context name " + + _in.ScopeContextName() + " does not match __model__ or world."}); return errors; } // Expect the scope vertex (which is the source vertex for this graph) to be - // valid and check that the scope name matches the frame type. + // valid and check that the scope context matches the frame type. auto sourceVertex = _in.ScopeVertex(); if (!sourceVertex.Valid()) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: source frame[" + - _in.ScopeName() + "] not found in graph."}); + _in.ScopeContextName() + "] not found in graph."}); return errors; } sdf::FrameType sourceFrameType = sourceVertex.Data(); - if (_in.ScopeName() == "__model__" && + if (_in.ScopeContextName() == "__model__" && sourceFrameType != sdf::FrameType::MODEL) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, @@ -1304,7 +1309,7 @@ Errors validatePoseRelativeToGraph( "source vertex with name __model__ should have FrameType MODEL."}); return errors; } - else if (_in.ScopeName() == "world" && + else if (_in.ScopeContextName() == "world" && sourceFrameType != sdf::FrameType::WORLD) { errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, @@ -1455,11 +1460,12 @@ Errors resolveFrameAttachedToBody( { Errors errors; - if (_in.ScopeName() != "__model__" && _in.ScopeName() != "world") + if (_in.ScopeContextName() != "__model__" && + _in.ScopeContextName() != "world") { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, - "FrameAttachedToGraph error: scope frame[" + _in.ScopeName() + "] " - " does not match __model__ or world."}); + "FrameAttachedToGraph error: scope context name[" + + _in.ScopeContextName() + "] does not match __model__ or world."}); return errors; } @@ -1488,7 +1494,7 @@ Errors resolveFrameAttachedToBody( return errors; } - if (_in.ScopeName() == "world" && + if (_in.ScopeContextName() == "world" && !(sinkVertex.Data() == FrameType::WORLD || sinkVertex.Data() == FrameType::STATIC_MODEL || sinkVertex.Data() == FrameType::LINK)) @@ -1501,7 +1507,7 @@ Errors resolveFrameAttachedToBody( return errors; } - if (_in.ScopeName() == "__model__") + if (_in.ScopeContextName() == "__model__") { if (sinkVertex.Data() == FrameType::MODEL && sinkVertex.Name() == "__model__") @@ -1574,8 +1580,8 @@ Errors resolvePoseRelativeToRoot( errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph frame with name [" + std::to_string(_vertexId) + "] is disconnected; its source vertex has name [" + - incomingVertexEdges.first.Name() + - "], but its source name should be " + _graph.ScopeName() + "."}); + incomingVertexEdges.first.Name() + "], but its source name should " + "be " + _graph.ScopeContextName() + "."}); return errors; } diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index 521cf6baa..6f8d9542d 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -44,8 +44,8 @@ struct ScopedGraphData /// \brief The prefix for all names under this scope std::string prefix {}; - /// \brief The name of this scope. Either world or __model__ - std::string scopeName {}; + /// \brief The context name of this scope. Either world or __model__ + std::string scopeContextName {}; }; // Forward declarations for static_assert @@ -61,7 +61,7 @@ struct FrameAttachedToGraph; /// contains the following: /// - The prefix of the scope, which is empty for a world scope and the /// name of a model for a model scope. -/// - The scope name of the scope, which can be either "world" or "__model__". +/// - The context name of the scope, which can be either "world" or "__model__". /// - ID of the vertex on which the scope is anchored. For /// PoseRelativeToGraph's, this vertex is the root of the sub tree represented /// by the ScopedGraph instance. For FrameAttachedToGraph, this vertex only @@ -146,14 +146,16 @@ class ScopedGraph public: const MapType &Map() const; /// \brief Adds a scope vertex to the graph. This creates a new - /// scope by updating the prefix of the current scope, adding a new scope - /// vertex to the graph, and setting a new scope name. + /// scope by making a copy of the current scope with a new prefix and scope + /// type name. A new scope vertex then added to the graph. /// \param[in] _prefix The new prefix of the scope. /// \param[in] _name The Name of the scope vertex to be added. The full name /// of the vertex will be newPrefix::_name, where newPrefix is the prefix - /// obtained by adding _prefix to the existing prefix of the scope. + /// obtained by adding _prefix to the prefix of the current scope. + /// \param[in] _scopeTypeName Name of scope type (either __model__ or world) + /// \param[in] _data Vertex data public: ScopedGraph AddScopeVertex(const std::string &_prefix, - const std::string &_name, const std::string &_scopeName, + const std::string &_name, const std::string &_scopeTypeName, const VertexType &_data); /// \brief Adds a vertex to the graph. @@ -219,13 +221,13 @@ class ScopedGraph /// \return True if this scope points to the same graph as the input. public: bool PointsTo(const std::shared_ptr &_graph) const; - /// \brief Set the scope name. - /// \param[in] _name New scope name. - public: void SetScopeName(const std::string &_name); + /// \brief Set the context name of the scope. + /// \param[in] _name New context name. + public: void SetScopeContextName(const std::string &_name); - /// \brief Get the current scope name. - /// \return The current scope name. - public: const std::string &ScopeName() const; + /// \brief Get the current context name. + /// \return The current context name. + public: const std::string &ScopeContextName() const; /// \brief Add the current prefix to the given name. /// \param[in] _name Input name. @@ -266,7 +268,7 @@ ScopedGraph ScopedGraph::ChildModelScope(const std::string &_name) const newScopedGraph.dataPtr->prefix = this->AddPrefix(_name); newScopedGraph.dataPtr->scopeVertexId = newScopedGraph.VertexIdByName("__model__"); - newScopedGraph.dataPtr->scopeName = "__model__"; + newScopedGraph.dataPtr->scopeContextName = "__model__"; return newScopedGraph; } @@ -294,7 +296,7 @@ auto ScopedGraph::Map() const -> const MapType & ///////////////////////////////////////////////// template ScopedGraph ScopedGraph::AddScopeVertex(const std::string &_prefix, - const std::string &_name, const std::string &_scopeName, + const std::string &_name, const std::string &_scopeTypeName, const VertexType &_data) { auto newScopedGraph = *this; @@ -302,7 +304,7 @@ ScopedGraph ScopedGraph::AddScopeVertex(const std::string &_prefix, newScopedGraph.dataPtr->prefix = this->AddPrefix(_prefix); Vertex &vert = newScopedGraph.AddVertex(_name, _data); newScopedGraph.dataPtr->scopeVertexId = vert.Id(); - newScopedGraph.SetScopeName(_scopeName); + newScopedGraph.SetScopeContextName(_scopeTypeName); return newScopedGraph; } @@ -374,16 +376,16 @@ void ScopedGraph::UpdateEdge(Edge &_edge, const EdgeType &_data) ///////////////////////////////////////////////// template -const std::string &ScopedGraph::ScopeName() const +const std::string &ScopedGraph::ScopeContextName() const { - return this->dataPtr->scopeName; + return this->dataPtr->scopeContextName; } ///////////////////////////////////////////////// template -void ScopedGraph::SetScopeName(const std::string &_name) +void ScopedGraph::SetScopeContextName(const std::string &_name) { - this->dataPtr->scopeName = _name; + this->dataPtr->scopeContextName = _name; } ///////////////////////////////////////////////// From f3a234ef547657cd353105eaeca994eaae48b770 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 29 Oct 2020 23:41:08 -0500 Subject: [PATCH 59/70] Add warning to the help message of ign sdf -g command Signed-off-by: Addisu Z. Taddese --- src/cmd/cmdsdformat.rb.in | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 0b3bb2dfb..52130611d 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -40,7 +40,8 @@ COMMANDS = { 'sdf' => "Options:\n\n"\ " -k [ --check ] arg Check if an SDFormat file is valid.\n" + " -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" + - " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph. (This is for advanced use only and the output may change without any promise of stability)\n" + + " -g [ --graph ] arg Print the PoseRelativeTo or FrameAttachedTo graph. (WARNING: This is for advanced\n" + + " use only and the output may change without any promise of stability)\n" + " -p [ --print ] arg Print converted arg.\n" + COMMON_OPTIONS } From b5a3a480edcc6e19767009810015dcfebb88d84d Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 29 Oct 2020 23:42:44 -0500 Subject: [PATCH 60/70] Remove extraneous error comments Signed-off-by: Addisu Z. Taddese --- test/integration/model_dom.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index 0e9ffac3b..df92b148d 100644 --- a/test/integration/model_dom.cc +++ b/test/integration/model_dom.cc @@ -570,7 +570,4 @@ TEST(DOMRoot, LoadInvalidNestedModelWithoutLinks) // errors[4] // errors[5] // errors[6] - // errors[7] - // errors[8] - // errors[9] } From 38bee6e780937eff9510573a4b59a8f95175d996 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 30 Oct 2020 09:46:25 -0500 Subject: [PATCH 61/70] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 3 +-- src/Model.cc | 8 ++++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 0b83bdc36..4b4a16d1c 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -215,8 +215,7 @@ std::pair /// /// Notation used in this function: /// Pf - The placement frame inside the model -/// R - Relative to frame of the placement frame's pose. This frame is in the -/// parent scope of the input model. +/// R - The `relative_to` frame of the placement frame's //pose element. /// M - The input model's implicit frame (__model__) /// X_RPf - The pose of frame Pf relative to frame R /// X_RM - The pose of the frame M relative to frame R diff --git a/src/Model.cc b/src/Model.cc index 3a7158357..e9e7e14bb 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -762,14 +762,14 @@ void Model::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) auto childPoseGraph = this->dataPtr->poseGraph.ChildModelScope(this->Name()); - for (auto &link : this->dataPtr->links) - { - link.SetPoseRelativeToGraph(childPoseGraph); - } for (auto &model : this->dataPtr->models) { model.SetPoseRelativeToGraph(childPoseGraph); } + for (auto &link : this->dataPtr->links) + { + link.SetPoseRelativeToGraph(childPoseGraph); + } for (auto &joint : this->dataPtr->joints) { joint.SetPoseRelativeToGraph(childPoseGraph); From da10cb4cc405ca5363b3df010860b0664dcfdd8a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 30 Oct 2020 11:36:14 -0500 Subject: [PATCH 62/70] Resolve additional merge conflicts Signed-off-by: Addisu Z. Taddese --- test/integration/nested_model.cc | 2 +- .../nested_model_with_frames_expected.sdf | 11 +-- test/integration/partially_flattened.sdf | 74 +++++++++++++++++++ ...evel_nested_model_with_frames_expected.sdf | 4 +- 4 files changed, 83 insertions(+), 8 deletions(-) create mode 100644 test/integration/partially_flattened.sdf diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 353a5f536..64e3e9a53 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -622,7 +622,7 @@ TEST(NestedModel, PartiallyFlattened) { const std::string testFile = sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", - "nested_model_with_frames_expected.sdf"); + "partially_flattened.sdf"); // Load the SDF file sdf::Root root; diff --git a/test/integration/nested_model_with_frames_expected.sdf b/test/integration/nested_model_with_frames_expected.sdf index f9ed7ec96..feb1b349f 100644 --- a/test/integration/nested_model_with_frames_expected.sdf +++ b/test/integration/nested_model_with_frames_expected.sdf @@ -61,12 +61,13 @@ L3 L4 + + 1 0 0 0 -0 0 + + 1 0 0 0 -0 0 + + 10 0 0 0 -0 1.5708 - - 1 0 0 0 -0 0 - - 1 0 0 0 -0 0 - diff --git a/test/integration/partially_flattened.sdf b/test/integration/partially_flattened.sdf new file mode 100644 index 000000000..2385f000f --- /dev/null +++ b/test/integration/partially_flattened.sdf @@ -0,0 +1,74 @@ + + + + + + 10 0 0 0 -0 1.5708 + + + 0 0 0 1.5708 -0 0 + + + 0 0 0 0 0.785398 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + + + 1 + + + + + 0 0 0 0 -0 0 + + + 1 + + + + + + 1 0 0 0 -0 0 + + + 0 1 0 0 -0 0 + + + 0 0 1 0 -0 0 + + + 0 0 0 0 -0 0 + M1::L1 + M1::L2 + + 0 0 1 + + + 1 0 0 + + + + 0 0 1 0 -0 0 + M1::L2 + M1::L3 + + 0 0 1 + + + + 1 0 1 0 -0 0 + M1::L3 + M1::L4 + + + 1 0 0 0 -0 0 + + 1 0 0 0 -0 0 + + + + + diff --git a/test/integration/two_level_nested_model_with_frames_expected.sdf b/test/integration/two_level_nested_model_with_frames_expected.sdf index 9e9d206b1..2bd80e1eb 100644 --- a/test/integration/two_level_nested_model_with_frames_expected.sdf +++ b/test/integration/two_level_nested_model_with_frames_expected.sdf @@ -62,8 +62,8 @@ L3 L4 - - 1 0 0 0 -0 0 + + 1 0 0 0 -0 0 1 0 0 0 -0 0 From b0d3185c5ab139bed15604150d1ed77b884a4128 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 30 Oct 2020 11:53:57 -0500 Subject: [PATCH 63/70] Cleanup Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 1 + src/FrameSemantics.hh | 2 -- src/Model.cc | 1 - 3 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 4b4a16d1c..c6802fb9a 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -35,6 +35,7 @@ namespace sdf { inline namespace SDF_VERSION_NAMESPACE { +// Helpful functions when debugging in gdb void printGraph(const ScopedGraph &_graph) { std::stringstream ss; diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh index d23128b26..416580a10 100644 --- a/src/FrameSemantics.hh +++ b/src/FrameSemantics.hh @@ -43,12 +43,10 @@ namespace sdf // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // -#ifndef DOXYGEN_SHOULD_SKIP_THIS // Forward declaration. class Model; class World; template class ScopedGraph; -#endif /// \enum FrameType /// \brief The set of frame types. INVALID indicates that frame type has diff --git a/src/Model.cc b/src/Model.cc index e9e7e14bb..ed0511d37 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -14,7 +14,6 @@ * limitations under the License. * */ -#include #include #include #include From 5c42e39b30b4336c47b67ad133523d71f15a2bf9 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 3 Nov 2020 17:20:00 -0600 Subject: [PATCH 64/70] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 26 +++++++++++++++----------- src/ScopedGraph.hh | 2 +- test/integration/nested_model.cc | 12 ++++++++---- 3 files changed, 24 insertions(+), 16 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index c6802fb9a..ec5185a97 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -475,10 +475,7 @@ Errors buildFrameAttachedToGraph( } else { - // 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 "::". + // Add an edge from the implicit model frame to the canonical link found. auto linkId = outModel.VertexIdByName(canonicalLinkName); outModel.AddEdge({modelFrameId, linkId}, true); } @@ -832,8 +829,8 @@ Errors buildPoseRelativeToGraph( auto nestedModel = _model->ModelByIndex(m); auto nestedModelId = outModel.VertexIdByName(nestedModel->Name()); - // relative_to is empty, so add edge from implicit model frame - // to nestedModel + // if relative_to is empty, add edge from implicit model frame to + // nestedModel auto relativeToId = modelFrameId; const std::string &relativeTo = nestedModel->PoseRelativeTo(); @@ -964,8 +961,8 @@ Errors buildPoseRelativeToGraph( auto model = _world->ModelByIndex(m); auto modelId = _out.VertexIdByName(model->Name()); - // relative_to is empty, so add edge from implicit model frame - // to world + // if relative_to is empty, add edge from implicit model frame to + // world auto relativeToId = worldFrameId; // check if we've already added a default edge @@ -1124,14 +1121,21 @@ Errors validateFrameAttachedToGraph( } else if (vertexName == "__root__" ) { - if (outDegree != 0) + if (sdf::FrameType::STATIC_MODEL != scopeFrameType && + sdf::FrameType::WORLD != scopeFrameType) + { + errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, + "FrameAttachedToGraph error," + " __root__ should have FrameType STATIC_MODEL or WORLD."}); + } + else if (outDegree != 0) { std::string graphType = scopeFrameType == sdf::FrameType::WORLD ? "WORLD" : "MODEL"; errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error," " __root__ should have no outgoing edges in " + - graphType + " relative_to graph."}); + graphType + " attached_to graph."}); } } else if (sdf::FrameType::MODEL == scopeFrameType || @@ -1219,7 +1223,7 @@ Errors validateFrameAttachedToGraph( { errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error, " - "LINK/STATIC_MODEL vertex with name [" + + "LINK vertex with name [" + vertexName + "] should have no outgoing edges " "in WORLD attached_to graph."}); diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index 6f8d9542d..e289aea71 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -147,7 +147,7 @@ class ScopedGraph /// \brief Adds a scope vertex to the graph. This creates a new /// scope by making a copy of the current scope with a new prefix and scope - /// type name. A new scope vertex then added to the graph. + /// type name. A new scope vertex is then added to the graph. /// \param[in] _prefix The new prefix of the scope. /// \param[in] _name The Name of the scope vertex to be added. The full name /// of the vertex will be newPrefix::_name, where newPrefix is the prefix diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 64e3e9a53..16e62d396 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -324,8 +324,10 @@ TEST(NestedModel, NestedInclude) // each model has 3 links, and the link names of the nested models have // been transformed EXPECT_EQ(3u, model1->LinkCount()); - EXPECT_EQ(0u, model2->LinkCount()); - EXPECT_EQ(0u, model3->LinkCount()); + // TODO (addisu): Update the following two xpectations to account for the fact + // that included models are no longer flattened. + // EXPECT_EQ(3u, model2->LinkCount()); + // EXPECT_EQ(3u, model3->LinkCount()); const sdf::Link *baseLink1 = model1->LinkByName("base"); const sdf::Link *baseLink2 = model2->LinkByName(name + "::base"); const sdf::Link *baseLink3 = model3->LinkByName(name + "_14::base"); @@ -357,8 +359,10 @@ TEST(NestedModel, NestedInclude) // each model has 2 joints, and the joint names of the nested models have // been transformed EXPECT_EQ(2u, model1->JointCount()); - EXPECT_EQ(0u, model2->JointCount()); - EXPECT_EQ(0u, model3->JointCount()); + // TODO (addisu): Update the following two xpectations to account for the fact + // that included models are no longer flattened. + // EXPECT_EQ(2u, model2->JointCount()); + // EXPECT_EQ(2u, model3->JointCount()); auto *lowerJoint1 = model1->JointByName("lower_joint"); auto *lowerJoint2 = model2->JointByName(name + "::lower_joint"); auto *lowerJoint3 = model3->JointByName(name + "_14::lower_joint"); From f544805d57ea8246a1ffdc8aec69b9b0897f4081 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 5 Nov 2020 08:21:02 -0600 Subject: [PATCH 65/70] State that *Count functions only count immediate child elements Signed-off-by: Addisu Z. Taddese --- include/sdf/Model.hh | 30 ++++++++++++++++++++++-------- include/sdf/Root.hh | 5 ++++- include/sdf/World.hh | 19 +++++++++++++++---- 3 files changed, 41 insertions(+), 13 deletions(-) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 020ae674e..6a1fa7149 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -134,11 +134,14 @@ namespace sdf /// should be subject to wind. public: void SetEnableWind(bool _enableWind); - /// \brief Get the number of links. + /// \brief Get the number of links that are immediate (not nested) children + /// of this Model object. + /// \remark LinkByName() can find links that are not immediate children of + /// this Model object. /// \return Number of links contained in this Model object. public: uint64_t LinkCount() const; - /// \brief Get a link based on an index. + /// \brief Get an immediate (not nested) child link based on an index. /// \param[in] _index Index of the link. The index should be in the /// range [0..LinkCount()). /// \return Pointer to the link. Nullptr if the index does not exist. @@ -159,11 +162,14 @@ namespace sdf /// \return True if there exists a link with the given name. public: bool LinkNameExists(const std::string &_name) const; - /// \brief Get the number of joints. + /// \brief Get the number of joints that are immediate (not nested) children + /// of this Model object. + /// \remark JointByName() can find joints that are not immediate children of + /// this Model object. /// \return Number of joints contained in this Model object. public: uint64_t JointCount() const; - /// \brief Get a joint based on an index. + /// \brief Get an immediate (not nested) child joint based on an index. /// \param[in] _index Index of the joint. The index should be in the /// range [0..JointCount()). /// \return Pointer to the joint. Nullptr if the index does not exist. @@ -186,11 +192,15 @@ namespace sdf /// \sa bool JointNameExists(const std::string &_name) const public: const Joint *JointByName(const std::string &_name) const; - /// \brief Get the number of explicit frames. + /// \brief Get the number of explicit frames that are immediate (not nested) + /// children of this Model object. + /// \remark FrameByName() can find explicit frames that are not immediate + /// children of this Model object. /// \return Number of explicit frames contained in this Model object. public: uint64_t FrameCount() const; - /// \brief Get an explicit frame based on an index. + /// \brief Get an immediate (not nested) child explicit frame based on an + /// index. /// \param[in] _index Index of the explicit frame. The index should be in /// the range [0..FrameCount()). /// \return Pointer to the explicit frame. Nullptr if the index does not @@ -213,11 +223,15 @@ namespace sdf /// \return True if there exists an explicit frame with the given name. public: bool FrameNameExists(const std::string &_name) const; - /// \brief Get the number of nested models. + /// \brief Get the number of nested models that are immediate (not + /// recursively nested) children of this Model object. + /// \remark ModelByName() can find nested models that are not immediate + /// children of this Model object. /// \return Number of nested models contained in this Model object. public: uint64_t ModelCount() const; - /// \brief Get a nested model based on an index. + /// \brief Get an immediate (not recursively nested) child model based on an + /// index. /// \param[in] _index Index of the nested model. The index should be in the /// range [0..ModelCount()). /// \return Pointer to the model. Nullptr if the index does not exist. diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index ae88cd037..3b9c1965b 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -107,7 +107,10 @@ namespace sdf /// \return True if there exists a world with the given name. public: bool WorldNameExists(const std::string &_name) const; - /// \brief Get the number of models. + /// \brief Get the number of models that are immediate (not nested) children + /// of this Root object. + /// \remark ModelNameExists() can find nested models that are not immediate + /// children of this Root object. /// \return Number of models contained in this Root object. public: uint64_t ModelCount() const; diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 2f513eaaa..0e280d8f2 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -136,11 +136,14 @@ namespace sdf /// \sa SphericalCoordinates public: void SetMagneticField(const ignition::math::Vector3d &_mag); - /// \brief Get the number of models. + /// \brief Get the number of models that are immediate (not nested) children + /// of this World object. + /// \remark ModelByName() and ModelNameExists() can find nested models that + /// are not immediate children of this World object. /// \return Number of models contained in this World object. public: uint64_t ModelCount() const; - /// \brief Get a model based on an index. + /// \brief Get an immediate (not nested) child model based on an index. /// \param[in] _index Index of the model. The index should be in the /// range [0..ModelCount()). /// \return Pointer to the model. Nullptr if the index does not exist. @@ -149,6 +152,8 @@ namespace sdf /// \brief Get a model based on a name. /// \param[in] _name Name of the model. + /// To get a model nested in other models, prefix the model name + /// with the sequence of nested model names, delimited by "::". /// \return Pointer to the model. Nullptr if the name does not exist. public: const Model *ModelByName(const std::string &_name) const; @@ -173,11 +178,15 @@ namespace sdf /// \return True if there exists an actor with the given name. public: bool ActorNameExists(const std::string &_name) const; - /// \brief Get the number of explicit frames. + /// \brief Get the number of explicit frames that are immediate (not nested) + /// children of this World object. + /// \remark FrameByName() can find explicit frames that are not immediate + /// children of this World object. /// \return Number of explicit frames contained in this World object. public: uint64_t FrameCount() const; - /// \brief Get an explicit frame based on an index. + /// \brief Get an immediate (not nested) child explicit frame based on an + /// index. /// \param[in] _index Index of the explicit frame. The index should be in /// the range [0..FrameCount()). /// \return Pointer to the explicit frame. Nullptr if the index does not @@ -187,6 +196,8 @@ namespace sdf /// \brief Get an explicit frame based on a name. /// \param[in] _name Name of the explicit frame. + /// To get a frame in a nested model, prefix the frame name with the + /// sequence of nested models containing this frame, delimited by "::". /// \return Pointer to the explicit frame. Nullptr if the name does not /// exist. public: const Frame *FrameByName(const std::string &_name) const; From 661a502687980b6692d7ad568e9706b27544fa4e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 5 Nov 2020 08:30:24 -0600 Subject: [PATCH 66/70] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- src/FrameSemantics.cc | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index ec5185a97..136d36854 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -307,6 +307,14 @@ Errors buildFrameAttachedToGraph( auto rootId = ignition::math::graph::kNullId; if (_root) { + // The __root__ vertex identifies the scope that contains a root level + // model. In the PoseRelativeTo graph, this vertex is connected to the model + // with an edge that holds the value of //model/pose. However, in the + // FrameAttachedTo graph, the vertex is disconnected because nothing is + // attached to it. Since only links and static models are allowed to be + // disconnected in this graph, the STATIC_MODEL was chosen as its frame + // type. A different frame type could potentially be used, but that adds + // more complexity to the validateFrameAttachedToGraph code. _out = _out.AddScopeVertex( "", "__root__", scopeContextName, sdf::FrameType::STATIC_MODEL); rootId = _out.ScopeVertexId(); @@ -1346,7 +1354,15 @@ Errors validatePoseRelativeToGraph( } else if (vertexName == "__root__" ) { - if (inDegree != 0) + if (sdf::FrameType::MODEL != sourceFrameType && + sdf::FrameType::STATIC_MODEL != sourceFrameType && + sdf::FrameType::WORLD != sourceFrameType) + { + errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, + "PoseRelativeToGraph error," + " __root__ should have FrameType MODEL, STATIC_MODEL or WORLD."}); + } + else if (inDegree != 0) { std::string graphType = sourceFrameType == sdf::FrameType::WORLD ? "WORLD" : "MODEL"; From d06275b49051d4c56ad20dd5fe7da713b49e0294 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 5 Nov 2020 08:35:06 -0600 Subject: [PATCH 67/70] Typo Signed-off-by: Addisu Z. Taddese --- test/integration/nested_model.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 16e62d396..cb61aa45a 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -324,8 +324,8 @@ TEST(NestedModel, NestedInclude) // each model has 3 links, and the link names of the nested models have // been transformed EXPECT_EQ(3u, model1->LinkCount()); - // TODO (addisu): Update the following two xpectations to account for the fact - // that included models are no longer flattened. + // TODO (addisu): Update the following two expectations to account for the + // fact that included models are no longer flattened. // EXPECT_EQ(3u, model2->LinkCount()); // EXPECT_EQ(3u, model3->LinkCount()); const sdf::Link *baseLink1 = model1->LinkByName("base"); @@ -359,8 +359,8 @@ TEST(NestedModel, NestedInclude) // each model has 2 joints, and the joint names of the nested models have // been transformed EXPECT_EQ(2u, model1->JointCount()); - // TODO (addisu): Update the following two xpectations to account for the fact - // that included models are no longer flattened. + // TODO (addisu): Update the following two expectations to account for the + // fact that included models are no longer flattened. // EXPECT_EQ(2u, model2->JointCount()); // EXPECT_EQ(2u, model3->JointCount()); auto *lowerJoint1 = model1->JointByName("lower_joint"); From 8b0665a7a97f48d322978cad2b37ac360e9e1221 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 6 Nov 2020 15:04:47 -0600 Subject: [PATCH 68/70] Revert documentation updates to Root::*ByName and World::*ByName functions Signed-off-by: Addisu Z. Taddese --- include/sdf/Root.hh | 5 +---- include/sdf/World.hh | 19 ++++--------------- 2 files changed, 5 insertions(+), 19 deletions(-) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 3b9c1965b..ae88cd037 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -107,10 +107,7 @@ namespace sdf /// \return True if there exists a world with the given name. public: bool WorldNameExists(const std::string &_name) const; - /// \brief Get the number of models that are immediate (not nested) children - /// of this Root object. - /// \remark ModelNameExists() can find nested models that are not immediate - /// children of this Root object. + /// \brief Get the number of models. /// \return Number of models contained in this Root object. public: uint64_t ModelCount() const; diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 0e280d8f2..2f513eaaa 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -136,14 +136,11 @@ namespace sdf /// \sa SphericalCoordinates public: void SetMagneticField(const ignition::math::Vector3d &_mag); - /// \brief Get the number of models that are immediate (not nested) children - /// of this World object. - /// \remark ModelByName() and ModelNameExists() can find nested models that - /// are not immediate children of this World object. + /// \brief Get the number of models. /// \return Number of models contained in this World object. public: uint64_t ModelCount() const; - /// \brief Get an immediate (not nested) child model based on an index. + /// \brief Get a model based on an index. /// \param[in] _index Index of the model. The index should be in the /// range [0..ModelCount()). /// \return Pointer to the model. Nullptr if the index does not exist. @@ -152,8 +149,6 @@ namespace sdf /// \brief Get a model based on a name. /// \param[in] _name Name of the model. - /// To get a model nested in other models, prefix the model name - /// with the sequence of nested model names, delimited by "::". /// \return Pointer to the model. Nullptr if the name does not exist. public: const Model *ModelByName(const std::string &_name) const; @@ -178,15 +173,11 @@ namespace sdf /// \return True if there exists an actor with the given name. public: bool ActorNameExists(const std::string &_name) const; - /// \brief Get the number of explicit frames that are immediate (not nested) - /// children of this World object. - /// \remark FrameByName() can find explicit frames that are not immediate - /// children of this World object. + /// \brief Get the number of explicit frames. /// \return Number of explicit frames contained in this World object. public: uint64_t FrameCount() const; - /// \brief Get an immediate (not nested) child explicit frame based on an - /// index. + /// \brief Get an explicit frame based on an index. /// \param[in] _index Index of the explicit frame. The index should be in /// the range [0..FrameCount()). /// \return Pointer to the explicit frame. Nullptr if the index does not @@ -196,8 +187,6 @@ namespace sdf /// \brief Get an explicit frame based on a name. /// \param[in] _name Name of the explicit frame. - /// To get a frame in a nested model, prefix the frame name with the - /// sequence of nested models containing this frame, delimited by "::". /// \return Pointer to the explicit frame. Nullptr if the name does not /// exist. public: const Frame *FrameByName(const std::string &_name) const; From 639063300d776ad1dfed7d4130cd7255a378d4de Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 10 Nov 2020 18:49:26 -0600 Subject: [PATCH 69/70] Keep some of the changes from the previous commit Signed-off-by: Addisu Z. Taddese --- include/sdf/Root.hh | 3 ++- include/sdf/World.hh | 11 +++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index ae88cd037..968cf94e2 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -107,7 +107,8 @@ namespace sdf /// \return True if there exists a world with the given name. public: bool WorldNameExists(const std::string &_name) const; - /// \brief Get the number of models. + /// \brief Get the number of models that are immediate (not nested) children + /// of this Root object. /// \return Number of models contained in this Root object. public: uint64_t ModelCount() const; diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 2f513eaaa..cea8afcc7 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -136,11 +136,12 @@ namespace sdf /// \sa SphericalCoordinates public: void SetMagneticField(const ignition::math::Vector3d &_mag); - /// \brief Get the number of models. + /// \brief Get the number of models that are immediate (not nested) children + /// of this World object. /// \return Number of models contained in this World object. public: uint64_t ModelCount() const; - /// \brief Get a model based on an index. + /// \brief Get an immediate (not nested) child model based on an index. /// \param[in] _index Index of the model. The index should be in the /// range [0..ModelCount()). /// \return Pointer to the model. Nullptr if the index does not exist. @@ -173,11 +174,13 @@ namespace sdf /// \return True if there exists an actor with the given name. public: bool ActorNameExists(const std::string &_name) const; - /// \brief Get the number of explicit frames. + /// \brief Get the number of explicit frames that are immediate (not nested) + /// children of this World object. /// \return Number of explicit frames contained in this World object. public: uint64_t FrameCount() const; - /// \brief Get an explicit frame based on an index. + /// \brief Get an immediate (not nested) child explicit frame based on an + /// index. /// \param[in] _index Index of the explicit frame. The index should be in /// the range [0..FrameCount()). /// \return Pointer to the explicit frame. Nullptr if the index does not From 0a8a7e060449d94f463a0f3a88fe13f135bcb406 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 18 Nov 2020 15:25:05 -0600 Subject: [PATCH 70/70] Move construction of PoseRelativeToGraph and FrameAttachedToGraph to sdf::Root (#1) * Move graph creation to sdf::Root from sdf::World and sdf::Model. Signed-off-by: Addisu Z. Taddese * Convert ScopedGraph to hold a strong reference to the underlying graph Signed-off-by: Addisu Z. Taddese * Add copy/move constructors to sdf::Root Signed-off-by: Addisu Z. Taddese * Additional test of sdf::Root objects before copy and move Signed-off-by: Addisu Z. Taddese * Fix typo Signed-off-by: Addisu Z. Taddese * Explicitly delete copy constructor and assignment. Signed-off-by: Addisu Z. Taddese --- include/sdf/Model.hh | 12 +++-- include/sdf/Root.hh | 17 ++++++ include/sdf/World.hh | 21 ++++++++ src/Model.cc | 120 +----------------------------------------- src/Root.cc | 96 +++++++++++++++++++++++++++++++++- src/Root_TEST.cc | 111 +++++++++++++++++++++++++++++++++++++++ src/ScopedGraph.hh | 33 ++++++------ src/World.cc | 121 ++++++++++++++----------------------------- 8 files changed, 311 insertions(+), 220 deletions(-) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 6a1fa7149..b581aa9f2 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -315,15 +315,17 @@ namespace sdf public: void SetPlacementFrameName(const std::string &_name); /// \brief Give the scoped PoseRelativeToGraph to be used for resolving - /// poses. This is private and is intended to be called by World::Load and - /// Model::Load if this is a nested model. + /// poses. This is private and is intended to be called by Root::Load or + /// World::SetPoseRelativeToGraph if this is a standalone model and + /// Model::SetPoseRelativeToGraph if this is a nested model. /// \param[in] _graph scoped PoseRelativeToGraph object. private: void SetPoseRelativeToGraph( sdf::ScopedGraph _graph); /// \brief Give the scoped FrameAttachedToGraph to be used for resolving /// attached bodies. This is private and is intended to be called by - /// World::Load and Model::Load if this is a nested model. + /// Root::Load or World::SetFrameAttachedToGraph if this is a standalone + /// model and Model::SetFrameAttachedToGraph if this is a nested model. /// \param[in] _graph scoped FrameAttachedToGraph object. private: void SetFrameAttachedToGraph( sdf::ScopedGraph _graph); @@ -335,8 +337,10 @@ namespace sdf private: std::pair CanonicalLinkAndRelativeName() const; - /// \brief Allow World::Load to call SetPoseRelativeToGraph and + /// \brief Allow Root::Load, World::SetPoseRelativeToGraph, or + /// World::SetFrameAttachedToGraph to call SetPoseRelativeToGraph and /// SetFrameAttachedToGraph + friend class Root; friend class World; /// \brief Allow helper function in FrameSemantics.cc to call diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 968cf94e2..a37f44ea1 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -56,6 +56,23 @@ namespace sdf /// \brief Default constructor public: Root(); + /// \brief Copy constructor is explicitly deleted to avoid copying the + /// FrameAttachedToGraph and PoseRelativeToGraphs contained in Root. + public: Root(const Root &_root) = delete; + + /// \brief Move constructor + /// \param[in] _root Root to move. + public: Root(Root &&_root) noexcept; + + /// \brief Move assignment operator. + /// \param[in] _root Root to move. + /// \return Reference to this. + public: Root &operator=(Root &&_root) noexcept; + + /// \brief Copy assignment operator is explicitly deleted to avoid copying + /// the FrameAttachedToGraph and PoseRelativeToGraphs contained in Root. + public: Root &operator=(const Root &_root) = delete; + /// \brief Destructor public: ~Root(); diff --git a/include/sdf/World.hh b/include/sdf/World.hh index cea8afcc7..e6eca06f5 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -41,6 +41,9 @@ namespace sdf class Model; class Physics; class WorldPrivate; + struct PoseRelativeToGraph; + struct FrameAttachedToGraph; + template class ScopedGraph; class SDFORMAT_VISIBLE World { @@ -272,6 +275,24 @@ namespace sdf /// \return True if there exists a physics profile with the given name. public: bool PhysicsNameExists(const std::string &_name) const; + /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child + /// entities for resolving poses. This is private and is intended to be + /// called by Root::Load. + /// \param[in] _graph Scoped PoseRelativeToGraph object. + private: void SetPoseRelativeToGraph( + sdf::ScopedGraph _graph); + + /// \brief Give the Scoped FrameAttachedToGraph to be passed on to child + /// entities for attached bodes. This is private and is intended to be + /// called by Root::Load. + /// \param[in] _graph Scoped FrameAttachedToGraph object. + private: void SetFrameAttachedToGraph( + sdf::ScopedGraph _graph); + + /// \brief Allow Root::Load to call SetPoseRelativeToGraph and + /// SetFrameAttachedToGraph + friend class Root; + /// \brief Private data pointer. private: WorldPrivate *dataPtr = nullptr; }; diff --git a/src/Model.cc b/src/Model.cc index ed0511d37..d1e68f197 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -77,22 +77,10 @@ class sdf::ModelPrivate /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; - /// \brief Frame Attached-To Graph constructed during Load. - /// This would only be allocated if this model is a top level model, i.e, a - /// model file. - public: std::shared_ptr ownedFrameAttachedToGraph; - - /// \brief Scoped Frame Attached-To graph that can either point to a graph - /// owned by this model or a subgraph of a parent entity. + /// \brief Scoped Frame Attached-To graph at the parent model or world scope. public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Pose Relative-To Graph constructed during Load. - /// This would only be allocated if this model is a top level model, i.e, a - /// model file. - public: std::shared_ptr ownedPoseGraph; - - /// \brief Scoped Pose Relative-To graph that can either point to a graph - /// owned by this model or a subgraph of a parent entity. + /// \brief Scoped Pose Relative-To graph at the parent model or world scope. public: sdf::ScopedGraph poseGraph; /// \brief Scope name of parent Pose Relative-To Graph (world or __model__). @@ -116,43 +104,6 @@ Model::~Model() Model::Model(const Model &_model) : dataPtr(new ModelPrivate(*_model.dataPtr)) { - if (_model.dataPtr->ownedFrameAttachedToGraph) - { - // If the model owns the frameAttachedToGraph, we need to allocate a new - // sdf::FrameAttachedToGraph object and copy it. We also need to assign the - // ScopedGraph to this object - this->dataPtr->ownedFrameAttachedToGraph = - std::make_shared( - *_model.dataPtr->ownedFrameAttachedToGraph); - this->dataPtr->frameAttachedToGraph = - ScopedGraph( - this->dataPtr->ownedFrameAttachedToGraph); - } - if (_model.dataPtr->ownedPoseGraph) - { - this->dataPtr->ownedPoseGraph = std::make_shared( - *_model.dataPtr->ownedPoseGraph); - this->dataPtr->poseGraph = - ScopedGraph(this->dataPtr->ownedPoseGraph); - } - for (auto &link : this->dataPtr->links) - { - link.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &model : this->dataPtr->models) - { - model.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &joint : this->dataPtr->joints) - { - joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - frame.SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } } ///////////////////////////////////////////////// @@ -238,22 +189,6 @@ Errors Model::Load(ElementPtr _sdf) << this->Name() << "].\n"; } - // Whether this model is defined at the root level of an SDFormat file (i.e - // in XPath "/sdf/model") - bool isModelAtRootOfXmlFile = false; - - auto parentElem = this->dataPtr->sdf->GetParent(); - if (parentElem && parentElem->GetName() == "sdf") - { - this->dataPtr->ownedPoseGraph = std::make_shared(); - this->dataPtr->poseGraph = ScopedGraph(this->dataPtr->ownedPoseGraph); - this->dataPtr->ownedFrameAttachedToGraph = - std::make_shared(); - this->dataPtr->frameAttachedToGraph = - ScopedGraph(this->dataPtr->ownedFrameAttachedToGraph); - isModelAtRootOfXmlFile = true; - } - // Set of implicit and explicit frame names in this model for tracking // name collisions std::unordered_set frameNames; @@ -394,42 +329,6 @@ Errors Model::Load(ElementPtr _sdf) frameNames.insert(frameName); } - // Build the graphs. - - // Build the FrameAttachedToGraph if the model is not static. - // Re-enable this when the buildFrameAttachedToGraph implementation handles - // static models. - if (!this->Static()) - { - if (isModelAtRootOfXmlFile) - { - Errors frameAttachedToGraphErrors = - buildFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph, this); - errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), - frameAttachedToGraphErrors.end()); - Errors validateFrameAttachedGraphErrors = - validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), - validateFrameAttachedGraphErrors.end()); - - this->SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - } - - // Build the PoseRelativeToGraph - if (isModelAtRootOfXmlFile) - { - Errors poseGraphErrors = - buildPoseRelativeToGraph(this->dataPtr->poseGraph, this); - errors.insert(errors.end(), poseGraphErrors.begin(), poseGraphErrors.end()); - Errors validatePoseGraphErrors = - validatePoseRelativeToGraph(this->dataPtr->poseGraph); - errors.insert(errors.end(), validatePoseGraphErrors.begin(), - validatePoseGraphErrors.end()); - - this->SetPoseRelativeToGraph(this->dataPtr->poseGraph); - } - return errors; } @@ -748,13 +647,6 @@ void Model::SetPoseRelativeTo(const std::string &_frame) ///////////////////////////////////////////////// void Model::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { - // If the scoped graph doesn't point to the graph owned by this model, we - // clear the owned graph to maintain the invariant that if - // ownedPoseGraph is valid poseGraph points to it. - if (!_graph.PointsTo(this->dataPtr->ownedPoseGraph)) - { - this->dataPtr->ownedPoseGraph.reset(); - } this->dataPtr->poseGraph = _graph; this->dataPtr->poseGraphScopeVertexName = _graph.VertexLocalName(_graph.ScopeVertexId()); @@ -783,14 +675,6 @@ void Model::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) void Model::SetFrameAttachedToGraph( sdf::ScopedGraph _graph) { - // If the scoped graph doesn't point to the graph owned by this model, we - // clear the owned graph to maintain the invariant that if - // ownedFrameAttachedToGraph is valid frameAttachedToGraph points to it. - if (!_graph.PointsTo(this->dataPtr->ownedFrameAttachedToGraph)) - { - this->dataPtr->ownedFrameAttachedToGraph.reset(); - } - this->dataPtr->frameAttachedToGraph = _graph; auto childFrameAttachedToGraph = diff --git a/src/Root.cc b/src/Root.cc index 7898182c2..30a08203c 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -26,6 +26,8 @@ #include "sdf/World.hh" #include "sdf/parser.hh" #include "sdf/sdf_config.h" +#include "FrameSemantics.hh" +#include "ScopedGraph.hh" #include "Utils.hh" using namespace sdf; @@ -48,16 +50,82 @@ class sdf::RootPrivate /// \brief The actors specified under the root SDF element public: std::vector actors; + /// \brief Frame Attached-To Graphs constructed when loading Worlds. + public: std::vector> + worldFrameAttachedToGraphs; + + /// \brief Frame Attached-To Graphs constructed when loading Models. + public: std::vector> + modelFrameAttachedToGraphs; + + /// \brief Pose Relative-To Graphs constructed when loading Worlds. + public: std::vector> + worldPoseRelativeToGraphs; + + /// \brief Pose Relative-To Graphs constructed when loading Models. + public: std::vector> + modelPoseRelativeToGraphs; + /// \brief The SDF element pointer generated during load. public: sdf::ElementPtr sdf; }; +///////////////////////////////////////////////// +template +sdf::ScopedGraph addFrameAttachedToGraph( + std::vector> &_graphList, + const T &_domObj, sdf::Errors &_errors) +{ + auto &frameGraph = + _graphList.emplace_back(std::make_shared()); + + sdf::Errors buildErrors = + sdf::buildFrameAttachedToGraph(frameGraph, &_domObj); + _errors.insert(_errors.end(), buildErrors.begin(), buildErrors.end()); + + sdf::Errors validateErrors = sdf::validateFrameAttachedToGraph(frameGraph); + _errors.insert(_errors.end(), validateErrors.begin(), validateErrors.end()); + + return frameGraph; +} + +///////////////////////////////////////////////// +template +ScopedGraph addPoseRelativeToGraph( + std::vector> &_graphList, + const T &_domObj, Errors &_errors) +{ + auto &poseGraph = + _graphList.emplace_back(std::make_shared()); + + Errors buildErrors = buildPoseRelativeToGraph(poseGraph, &_domObj); + _errors.insert(_errors.end(), buildErrors.begin(), buildErrors.end()); + + Errors validateErrors = validatePoseRelativeToGraph(poseGraph); + _errors.insert(_errors.end(), validateErrors.begin(), validateErrors.end()); + + return poseGraph; +} + ///////////////////////////////////////////////// Root::Root() : dataPtr(new RootPrivate) { } +///////////////////////////////////////////////// +Root::Root(Root &&_root) noexcept + : dataPtr(std::exchange(_root.dataPtr, nullptr)) +{ +} + +///////////////////////////////////////////////// +Root &Root::operator=(Root &&_root) noexcept +{ + std::swap(this->dataPtr, _root.dataPtr); + return *this; +} + ///////////////////////////////////////////////// Root::~Root() { @@ -151,6 +219,16 @@ Errors Root::Load(SDFPtr _sdf) World world; Errors worldErrors = world.Load(elem); + + // Build the graphs. + auto frameAttachedToGraph = addFrameAttachedToGraph( + this->dataPtr->worldFrameAttachedToGraphs, world, worldErrors); + world.SetFrameAttachedToGraph(frameAttachedToGraph); + + auto poseRelativeToGraph = addPoseRelativeToGraph( + this->dataPtr->worldPoseRelativeToGraphs, world, worldErrors); + world.SetPoseRelativeToGraph(poseRelativeToGraph); + // Attempt to load the world if (worldErrors.empty()) { @@ -169,16 +247,30 @@ Errors Root::Load(SDFPtr _sdf) errors.push_back({ErrorCode::ELEMENT_INVALID, "Failed to load a world."}); } + this->dataPtr->worlds.push_back(std::move(world)); elem = elem->GetNextElement("world"); } } // Load all the models. - Errors modelLoadErrors = loadUniqueRepeated(this->dataPtr->sdf, - "model", this->dataPtr->models); + Errors modelLoadErrors = loadUniqueRepeated( + this->dataPtr->sdf, "model", this->dataPtr->models); errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end()); + // Build the graphs. + for (sdf::Model &model : this->dataPtr->models) + { + auto frameAttachedToGraph = addFrameAttachedToGraph( + this->dataPtr->modelFrameAttachedToGraphs, model, errors); + + model.SetFrameAttachedToGraph(frameAttachedToGraph); + + auto poseRelativeToGraph = addPoseRelativeToGraph( + this->dataPtr->modelPoseRelativeToGraphs, model, errors); + model.SetPoseRelativeToGraph(poseRelativeToGraph); + } + // Load all the lights. Errors lightLoadErrors = loadUniqueRepeated(this->dataPtr->sdf, "light", this->dataPtr->lights); diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index 55fb747db..60e2e3746 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -23,6 +23,8 @@ #include "sdf/Link.hh" #include "sdf/Light.hh" #include "sdf/Model.hh" +#include "sdf/World.hh" +#include "sdf/Frame.hh" #include "sdf/Root.hh" ///////////////////////////////////////////////// @@ -56,6 +58,27 @@ TEST(DOMRoot, Construction) EXPECT_TRUE(root.ActorByIndex(1) == nullptr); } +///////////////////////////////////////////////// +TEST(DOMRoot, MoveConstructor) +{ + sdf::Root root; + root.SetVersion("test_root"); + + sdf::Root root2(std::move(root)); + EXPECT_EQ("test_root", root2.Version()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, MoveAssignmentOperator) +{ + sdf::Root root; + root.SetVersion("test_root"); + + sdf::Root root2; + root2 = std::move(root); + EXPECT_EQ("test_root", root2.Version()); +} + ///////////////////////////////////////////////// TEST(DOMRoot, StringParse) { @@ -140,3 +163,91 @@ TEST(DOMRoot, Set) root.SetVersion(SDF_PROTOCOL_VERSION); EXPECT_STREQ(SDF_PROTOCOL_VERSION, root.Version().c_str()); } + +///////////////////////////////////////////////// +TEST(DOMRoot, FrameSemanticsOnMove) +{ + const std::string sdfString1 = R"( + + + + 0 1 0 0 0 0 + + + )"; + + const std::string sdfString2 = R"( + + + + 1 1 0 0 0 0 + + + )"; + + auto testFrame1 = [](const sdf::Root &_root) + { + auto *world = _root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *frame = world->FrameByIndex(0); + ASSERT_NE(nullptr, frame); + EXPECT_EQ("frame1", frame->Name()); + ignition::math::Pose3d pose; + sdf::Errors errors = frame->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(0, 1, 0, 0, 0, 0), pose); + }; + + auto testFrame2 = [](const sdf::Root &_root) + { + auto *world = _root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + auto *frame = world->FrameByIndex(0); + ASSERT_NE(nullptr, frame); + EXPECT_EQ("frame2", frame->Name()); + ignition::math::Pose3d pose; + sdf::Errors errors = frame->SemanticPose().Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(ignition::math::Pose3d(1, 1, 0, 0, 0, 0), pose); + }; + + { + sdf::Root root1; + sdf::Errors errors = root1.LoadSdfString(sdfString1); + EXPECT_TRUE(errors.empty()) << errors; + testFrame1(root1); + } + + { + sdf::Root root2; + sdf::Errors errors = root2.LoadSdfString(sdfString2); + EXPECT_TRUE(errors.empty()) << errors; + testFrame2(root2); + } + + { + sdf::Root root1; + sdf::Errors errors = root1.LoadSdfString(sdfString1); + EXPECT_TRUE(errors.empty()) << errors; + + // then root1 is moved into root2 via the move constructor + sdf::Root root2(std::move(root1)); + testFrame1(root2); + } + + { + sdf::Root root1; + sdf::Errors errors = root1.LoadSdfString(sdfString1); + EXPECT_TRUE(errors.empty()) << errors; + sdf::Root root2; + errors = root2.LoadSdfString(sdfString2); + EXPECT_TRUE(errors.empty()) << errors; + + testFrame1(root1); + testFrame2(root2); + + // root1 is moved into root2 via the move assignment operator. + root2 = std::move(root1); + testFrame1(root2); + } +} diff --git a/src/ScopedGraph.hh b/src/ScopedGraph.hh index e289aea71..0c30ae588 100644 --- a/src/ScopedGraph.hh +++ b/src/ScopedGraph.hh @@ -123,6 +123,11 @@ class ScopedGraph /// passed in graph. /// \param[in] _graph A shared pointer to PoseRelativeTo or FrameAttachedTo /// graph. + /// \remark The state of the ScopedGraph after construction is invalid because + /// member variables such as "ScopedGraphData::prefix" and + /// "ScopedGraphData::scopeName" will be empty. Therefore, it is imperative + /// that this constructor should only be used right before calling either + /// buildPoseRelativeToGraph or buildFrameAttachedToGraph. public: explicit ScopedGraph(const std::shared_ptr &_graph); /// \brief Creates a scope anchored at an existing child model vertex. @@ -242,9 +247,9 @@ class ScopedGraph public: std::pair FindAndRemovePrefix( const std::string &_name) const; - /// \brief Weak pointer to either a FrameAttachedToGraph or + /// \brief Shared pointer to either a FrameAttachedToGraph or /// PoseRelativeToGraph. - private: std::weak_ptr graphWeak; + private: std::shared_ptr graphPtr; /// \brief Shared pointer to the scope's data. A shared_ptr is used because /// this data has to be shared among many DOM objects. @@ -254,7 +259,7 @@ class ScopedGraph ///////////////////////////////////////////////// template ScopedGraph::ScopedGraph(const std::shared_ptr &_graph) - : graphWeak(_graph) + : graphPtr(_graph) , dataPtr(std::make_shared()) { } @@ -276,21 +281,21 @@ ScopedGraph ScopedGraph::ChildModelScope(const std::string &_name) const template ScopedGraph::operator bool() const { - return !this->graphWeak.expired() && (this->dataPtr != nullptr); + return (this->graphPtr != nullptr) && (this->dataPtr != nullptr); } ///////////////////////////////////////////////// template auto ScopedGraph::Graph() const -> const MathGraphType & { - return this->graphWeak.lock()->graph; + return this->graphPtr->graph; } ///////////////////////////////////////////////// template auto ScopedGraph::Map() const -> const MapType & { - return this->graphWeak.lock()->map; + return this->graphPtr->map; } ///////////////////////////////////////////////// @@ -313,10 +318,9 @@ template auto ScopedGraph::AddVertex( const std::string &_name, const VertexType &_data) -> Vertex & { - auto graph = this->graphWeak.lock(); const std::string newName = this->AddPrefix(_name); - Vertex &vert = graph->graph.AddVertex(newName, _data); - graph->map[newName] = vert.Id(); + Vertex &vert = this->graphPtr->graph.AddVertex(newName, _data); + this->graphPtr->map[newName] = vert.Id(); return vert; } @@ -326,8 +330,7 @@ auto ScopedGraph::AddEdge( const ignition::math::graph::VertexId_P &_vertexPair, const EdgeType &_data) -> Edge & { - auto graph = graphWeak.lock(); - Edge &edge = graph->graph.AddEdge(_vertexPair, _data); + Edge &edge = this->graphPtr->graph.AddEdge(_vertexPair, _data); return edge; } @@ -369,7 +372,7 @@ void ScopedGraph::UpdateEdge(Edge &_edge, const EdgeType &_data) // insert a new one with the new pose. auto tailVertexId = _edge.Tail(); auto headVertexId = _edge.Head(); - auto &graph = this->graphWeak.lock()->graph; + auto &graph = this->graphPtr->graph; graph.RemoveEdge(_edge.Id()); _edge = graph.AddEdge({tailVertexId, headVertexId}, _data); } @@ -392,7 +395,7 @@ void ScopedGraph::SetScopeContextName(const std::string &_name) template std::size_t ScopedGraph::Count(const std::string &_name) const { - return this->graphWeak.lock()->map.count(this->AddPrefix(_name)); + return this->graphPtr->map.count(this->AddPrefix(_name)); } ///////////////////////////////////////////////// @@ -411,7 +414,7 @@ auto ScopedGraph::VertexIdByName(const std::string &_name) const -> VertexId template auto ScopedGraph::ScopeVertex() const -> const Vertex & { - return this->graphWeak.lock()->graph.VertexFromId( + return this->graphPtr->graph.VertexFromId( this->dataPtr->scopeVertexId); } @@ -426,7 +429,7 @@ auto ScopedGraph::ScopeVertexId() const -> VertexId template bool ScopedGraph::PointsTo(const std::shared_ptr &_graph) const { - return this->graphWeak.lock() == _graph; + return this->graphPtr == _graph; } ///////////////////////////////////////////////// diff --git a/src/World.cc b/src/World.cc index ff4ae55b5..704408e13 100644 --- a/src/World.cc +++ b/src/World.cc @@ -35,7 +35,7 @@ using namespace sdf; class sdf::WorldPrivate { /// \brief Default constructor - public: WorldPrivate(); + public: WorldPrivate() = default; /// \brief Copy constructor /// \param[in] _worldPrivate Joint axis to move. @@ -89,30 +89,15 @@ class sdf::WorldPrivate public: ignition::math::Vector3d windLinearVelocity = ignition::math::Vector3d::Zero; - /// \brief Frame Attached-To Graph constructed during Load. - public: std::shared_ptr ownedFrameAttachedToGraph; - /// \brief Scoped Frame Attached-To graph that points to a graph owned /// by this world. public: sdf::ScopedGraph frameAttachedToGraph; - /// \brief Pose Relative-To Graph constructed during Load. - public: std::shared_ptr ownedPoseRelativeToGraph; - /// \brief Scoped Pose Relative-To graph that points to a graph owned by this /// world. public: sdf::ScopedGraph poseRelativeToGraph; }; -///////////////////////////////////////////////// -WorldPrivate::WorldPrivate() - : ownedFrameAttachedToGraph(std::make_shared()) - , frameAttachedToGraph(ownedFrameAttachedToGraph) - , ownedPoseRelativeToGraph(std::make_shared()) - , poseRelativeToGraph(ownedPoseRelativeToGraph) -{ -} - ///////////////////////////////////////////////// WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) : audioDevice(_worldPrivate.audioDevice), @@ -125,7 +110,9 @@ WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) name(_worldPrivate.name), physics(_worldPrivate.physics), sdf(_worldPrivate.sdf), - windLinearVelocity(_worldPrivate.windLinearVelocity) + windLinearVelocity(_worldPrivate.windLinearVelocity), + frameAttachedToGraph(_worldPrivate.frameAttachedToGraph), + poseRelativeToGraph(_worldPrivate.poseRelativeToGraph) { if (_worldPrivate.atmosphere) { @@ -136,19 +123,6 @@ WorldPrivate::WorldPrivate(const WorldPrivate &_worldPrivate) { this->gui = std::make_unique(*(_worldPrivate.gui)); } - if (_worldPrivate.ownedFrameAttachedToGraph) - { - this->ownedFrameAttachedToGraph = - std::make_shared( - *(_worldPrivate.ownedFrameAttachedToGraph)); - this->frameAttachedToGraph = ScopedGraph(this->ownedFrameAttachedToGraph); - } - if (_worldPrivate.ownedPoseRelativeToGraph) - { - this->ownedPoseRelativeToGraph = std::make_shared( - *(_worldPrivate.ownedPoseRelativeToGraph)); - this->poseRelativeToGraph = ScopedGraph(this->ownedPoseRelativeToGraph); - } if (_worldPrivate.scene) { this->scene = std::make_unique(*(_worldPrivate.scene)); @@ -173,20 +147,6 @@ World::~World() World::World(const World &_world) : dataPtr(new WorldPrivate(*_world.dataPtr)) { - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &model : this->dataPtr->models) - { - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &light : this->dataPtr->lights) - { - light.SetXmlParentName("world"); - light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } } ///////////////////////////////////////////////// @@ -362,43 +322,6 @@ Errors World::Load(sdf::ElementPtr _sdf) errors.insert(errors.end(), sceneLoadErrors.begin(), sceneLoadErrors.end()); } - // Build the graphs. - Errors frameAttachedToGraphErrors = - buildFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph, this); - errors.insert(errors.end(), frameAttachedToGraphErrors.begin(), - frameAttachedToGraphErrors.end()); - Errors validateFrameAttachedGraphErrors = - validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - errors.insert(errors.end(), validateFrameAttachedGraphErrors.begin(), - validateFrameAttachedGraphErrors.end()); - for (auto &frame : this->dataPtr->frames) - { - frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - - Errors poseRelativeToGraphErrors = - buildPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph, this); - errors.insert(errors.end(), poseRelativeToGraphErrors.begin(), - poseRelativeToGraphErrors.end()); - Errors validatePoseGraphErrors = - validatePoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - errors.insert(errors.end(), validatePoseGraphErrors.begin(), - validatePoseGraphErrors.end()); - for (auto &frame : this->dataPtr->frames) - { - frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - for (auto &model : this->dataPtr->models) - { - model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); - } - for (auto &light : this->dataPtr->lights) - { - light.SetXmlParentName("world"); - light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); - } - return errors; } @@ -682,3 +605,39 @@ bool World::PhysicsNameExists(const std::string &_name) const return false; } + +///////////////////////////////////////////////// +void World::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) +{ + this->dataPtr->poseRelativeToGraph = _graph; + + for (auto &model : this->dataPtr->models) + { + model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } + for (auto &frame : this->dataPtr->frames) + { + frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } + for (auto &light : this->dataPtr->lights) + { + light.SetXmlParentName("world"); + light.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph); + } +} + +///////////////////////////////////////////////// +void World::SetFrameAttachedToGraph( + sdf::ScopedGraph _graph) +{ + this->dataPtr->frameAttachedToGraph = _graph; + + for (auto &frame : this->dataPtr->frames) + { + frame.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } + for (auto &model : this->dataPtr->models) + { + model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph); + } +}