From 35d3502ed18f9932463c3e031c63433ef83eabaa Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 2 Feb 2022 08:14:48 -0800 Subject: [PATCH 01/10] Added non-const mutable accessors for root child objects Signed-off-by: Nate Koenig --- include/sdf/Root.hh | 17 +++++++++++++++++ src/Root.cc | 26 ++++++++++++++++++++++++++ src/Root_TEST.cc | 42 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 85 insertions(+) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 415016041..974f7c5ff 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -125,6 +125,13 @@ namespace sdf /// \sa uint64_t WorldCount() const public: const World *WorldByIndex(const uint64_t _index) const; + /// \brief Get a mutable world based on an index. + /// \param[in] _index Index of the world. The index should be in the + /// range [0..WorldCount()). + /// \return Pointer to the world. Nullptr if the index does not exist. + /// \sa uint64_t WorldCount() const + public: World *WorldByIndex(const uint64_t _index); + /// \brief Get whether a world name exists. /// \param[in] _name Name of the world to check. /// \return True if there exists a world with the given name. @@ -151,6 +158,16 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Add a world to the root. + /// \param[in] _word World to add. + /// \return True if successful, false if a world with the name already + /// exists. + /// \return True on success, false if the world name already exists. + public: bool AddWorld(const World &_world); + + /// \brief Remove all worlds. + public: void ClearWorlds(); + /// \brief Private data pointer IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/src/Root.cc b/src/Root.cc index 3fe61496b..f01379e5c 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -381,6 +381,14 @@ const World *Root::WorldByIndex(const uint64_t _index) const return nullptr; } +///////////////////////////////////////////////// +World *Root::WorldByIndex(const uint64_t _index) +{ + if (_index < this->dataPtr->worlds.size()) + return &this->dataPtr->worlds[_index]; + return nullptr; +} + ///////////////////////////////////////////////// bool Root::WorldNameExists(const std::string &_name) const { @@ -417,3 +425,21 @@ sdf::ElementPtr Root::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +bool Root::AddWorld(const World &_world) +{ + if (!this->WorldNameExists(_world.Name())) + { + this->dataPtr->worlds.push_back(_world); + // this->UpdateGraphs(); + return true; + } + return false; +} + +///////////////////////////////////////////////// +void Root::ClearWorlds() +{ + this->dataPtr->worlds.clear(); +} diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index d687f6e2b..84a4fa132 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -270,3 +270,45 @@ TEST(DOMRoot, FrameSemanticsOnMove) testFrame1(root2); } } + +///////////////////////////////////////////////// +TEST(DOMRoot, AddWorld) +{ + sdf::Root root; + EXPECT_EQ(0u, root.WorldCount()); + + sdf::World world; + world.SetName("world1"); + EXPECT_TRUE(root.AddWorld(world)); + EXPECT_EQ(1u, root.WorldCount()); + EXPECT_FALSE(root.AddWorld(world)); + EXPECT_EQ(1u, root.WorldCount()); + + root.ClearWorlds(); + EXPECT_EQ(0u, root.WorldCount()); + + EXPECT_TRUE(root.AddWorld(world)); + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *worldFromRoot = root.WorldByIndex(0); + ASSERT_NE(nullptr, worldFromRoot); + EXPECT_EQ(worldFromRoot->Name(), world.Name()); +} + +///////////////////////////////////////////////// +TEST(DOMRoot, MutableByIndex) +{ + sdf::Root root; + EXPECT_EQ(0u, root.WorldCount()); + + sdf::World world; + world.SetName("world1"); + EXPECT_TRUE(root.AddWorld(world)); + EXPECT_EQ(1u, root.WorldCount()); + + // Modify the world + sdf::World *w = root.WorldByIndex(0); + ASSERT_NE(nullptr, w); + EXPECT_EQ("world1", w->Name()); + w->SetName("world2"); + EXPECT_EQ("world2", root.WorldByIndex(0)->Name()); +} From 10fcc38af62c2b2c337eeaceae1817b4bf57a178 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 2 Feb 2022 08:15:08 -0800 Subject: [PATCH 02/10] Remove comment Signed-off-by: Nate Koenig --- src/Root.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Root.cc b/src/Root.cc index f01379e5c..c1293d52f 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -432,7 +432,6 @@ bool Root::AddWorld(const World &_world) if (!this->WorldNameExists(_world.Name())) { this->dataPtr->worlds.push_back(_world); - // this->UpdateGraphs(); return true; } return false; From f0771db3f6ccf77836fb7172790a42f81424e5e5 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 2 Feb 2022 13:07:27 -0800 Subject: [PATCH 03/10] Use casts Signed-off-by: Nate Koenig --- src/Root.cc | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/Root.cc b/src/Root.cc index c1293d52f..d274ce47b 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -384,9 +384,8 @@ const World *Root::WorldByIndex(const uint64_t _index) const ///////////////////////////////////////////////// World *Root::WorldByIndex(const uint64_t _index) { - if (_index < this->dataPtr->worlds.size()) - return &this->dataPtr->worlds[_index]; - return nullptr; + return const_cast( + static_cast(this)->WorldByIndex(_index)); } ///////////////////////////////////////////////// From 7f33dfd52631951899860d8d60c32d0f25b146de Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 7 Feb 2022 11:36:23 -0800 Subject: [PATCH 04/10] Added Root::Clone function Signed-off-by: Nate Koenig --- include/sdf/Root.hh | 4 ++++ src/Root.cc | 24 ++++++++++++++++++++++++ src/Root_TEST.cc | 26 ++++++++++++++++++++++++++ 3 files changed, 54 insertions(+) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 415016041..3d0ea3af6 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -151,6 +151,10 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Deep copy the provided Root object into this Root object. + /// \param[in] _root The Root object to copy. + public: void Clone(const sdf::Root &_root); + /// \brief Private data pointer IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/src/Root.cc b/src/Root.cc index 3fe61496b..012ff1f24 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -417,3 +417,27 @@ sdf::ElementPtr Root::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +void Root::Clone(const sdf::Root &_root) +{ + this->dataPtr->version = _root.dataPtr->version; + + this->dataPtr->worlds = _root.dataPtr->worlds; + + this->dataPtr->modelLightOrActor = _root.dataPtr->modelLightOrActor; + + this->dataPtr->worldFrameAttachedToGraphs = + _root.dataPtr->worldFrameAttachedToGraphs ; + + this->dataPtr->modelFrameAttachedToGraph = + _root.dataPtr->modelFrameAttachedToGraph ; + + this->dataPtr->worldPoseRelativeToGraphs = + _root.dataPtr->worldPoseRelativeToGraphs; + + this->dataPtr->modelPoseRelativeToGraph = + _root.dataPtr->modelPoseRelativeToGraph; + + this->dataPtr->sdf = _root.dataPtr->sdf; +} diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index d687f6e2b..692fabb70 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -109,6 +109,32 @@ TEST(DOMRoot, StringModelSdfParse) EXPECT_EQ(nullptr, root.Light()); EXPECT_EQ(nullptr, root.Actor()); EXPECT_EQ(0u, root.WorldCount()); + + // Test cloning + sdf::Root root2; + root2.Clone(root); + + const sdf::Model *model2 = root2.Model(); + ASSERT_NE(nullptr, model2); + EXPECT_NE(nullptr, model2->Element()); + + EXPECT_EQ("shapes", model2->Name()); + EXPECT_EQ(1u, model2->LinkCount()); + + const sdf::Link *link2 = model2->LinkByIndex(0); + ASSERT_NE(nullptr, link2); + EXPECT_NE(nullptr, link2->Element()); + EXPECT_EQ("link", link2->Name()); + EXPECT_EQ(1u, link2->CollisionCount()); + + const sdf::Collision *collision2 = link2->CollisionByIndex(0); + ASSERT_NE(nullptr, collision2); + EXPECT_NE(nullptr, collision2->Element()); + EXPECT_EQ("box_col", collision2->Name()); + + EXPECT_EQ(nullptr, root2.Light()); + EXPECT_EQ(nullptr, root2.Actor()); + EXPECT_EQ(0u, root2.WorldCount()); } ///////////////////////////////////////////////// From 5d29b5e4eb13f0f605e2f8a82e504cc5f27c70fe Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 7 Feb 2022 11:50:27 -0800 Subject: [PATCH 05/10] Added deprecation note Signed-off-by: Nate Koenig --- include/sdf/Root.hh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 3d0ea3af6..febe17668 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -153,6 +153,8 @@ namespace sdf /// \brief Deep copy the provided Root object into this Root object. /// \param[in] _root The Root object to copy. + /// Deprecate this function in SDF version 13, and use + /// IGN_UTILS_IMPL_PTR instead. public: void Clone(const sdf::Root &_root); /// \brief Private data pointer From 9c16a72792e9f3d096fdb68c04e38a347d461f97 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 10 Feb 2022 14:20:44 -0800 Subject: [PATCH 06/10] Added Root::UpdateGraphs functions Signed-off-by: Nate Koenig --- include/sdf/Root.hh | 6 ++ src/Root.cc | 85 ++++++++++++++++++--------- test/integration/frame.cc | 120 +++++++++++++++++++++----------------- 3 files changed, 133 insertions(+), 78 deletions(-) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index febe17668..f993dfeb1 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -157,6 +157,12 @@ namespace sdf /// IGN_UTILS_IMPL_PTR instead. public: void Clone(const sdf::Root &_root); + /// \brief Recreate the frame and pose graphs for the worlds and model + /// that are children of this Root object. + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors UpdateGraphs(); + /// \brief Private data pointer IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/src/Root.cc b/src/Root.cc index 012ff1f24..768288a18 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -37,6 +37,16 @@ using namespace sdf; /// \brief Private data for sdf::Root class sdf::Root::Implementation { + /// \brief Build frame and pose graphs for the provided world. + /// \param[in, out] _world World object to build graphs for. + /// \param[out] _errors The list of errors generated by this function. + public: void UpdateGraphs(sdf::World &_world, sdf::Errors &_errors); + + /// \brief Build frame and pose graphs for the provided model. + /// \param[in, out] _model Model object to build graphs for. + /// \param[out] _errors The list of errors generated by this function. + public: void UpdateGraphs(sdf::Model &_model, sdf::Errors &_errors); + /// \brief Version string public: std::string version = ""; @@ -235,14 +245,7 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) Errors worldErrors = world.Load(elem, _config); - // 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); + this->dataPtr->UpdateGraphs(world, worldErrors); // Attempt to load the world if (worldErrors.empty()) @@ -283,15 +286,7 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) } this->dataPtr->modelLightOrActor = std::move(models.front()); sdf::Model &model = std::get(this->dataPtr->modelLightOrActor); - // Build the graphs. - this->dataPtr->modelFrameAttachedToGraph = - createFrameAttachedToGraph(model, errors); - - model.SetFrameAttachedToGraph(this->dataPtr->modelFrameAttachedToGraph); - - this->dataPtr->modelPoseRelativeToGraph = - createPoseRelativeToGraph(model, errors); - model.SetPoseRelativeToGraph(this->dataPtr->modelPoseRelativeToGraph); + this->dataPtr->UpdateGraphs(model, errors); } // Load all the lights. @@ -427,17 +422,55 @@ void Root::Clone(const sdf::Root &_root) this->dataPtr->modelLightOrActor = _root.dataPtr->modelLightOrActor; - this->dataPtr->worldFrameAttachedToGraphs = - _root.dataPtr->worldFrameAttachedToGraphs ; + this->UpdateGraphs(); +} + +///////////////////////////////////////////////// +Errors Root::UpdateGraphs() +{ + sdf::Errors errors; + + this->dataPtr->worldFrameAttachedToGraphs.clear(); + this->dataPtr->worldPoseRelativeToGraphs.clear(); + + // Build graphs for each world. + for (World &world : this->dataPtr->worlds) + { + this->dataPtr->UpdateGraphs(world, errors); + } - this->dataPtr->modelFrameAttachedToGraph = - _root.dataPtr->modelFrameAttachedToGraph ; + // Build graphs for the model, if one is present. + if (std::holds_alternative(this->dataPtr->modelLightOrActor)) + { + sdf::Model &model = std::get(this->dataPtr->modelLightOrActor); + this->dataPtr->UpdateGraphs(model, errors); + } - this->dataPtr->worldPoseRelativeToGraphs = - _root.dataPtr->worldPoseRelativeToGraphs; + return errors; +} - this->dataPtr->modelPoseRelativeToGraph = - _root.dataPtr->modelPoseRelativeToGraph; +////////////////////////////////////////////////// +void Root::Implementation::UpdateGraphs(sdf::World &_world, + sdf::Errors &_errors) +{ + // Build the frame graph. + auto frameAttachedToGraph = addFrameAttachedToGraph( + this->worldFrameAttachedToGraphs, _world, _errors); + _world.SetFrameAttachedToGraph(frameAttachedToGraph); + + // Build the pose graph. + auto poseRelativeToGraph = addPoseRelativeToGraph( + this->worldPoseRelativeToGraphs, _world, _errors); + _world.SetPoseRelativeToGraph(poseRelativeToGraph); +} + +////////////////////////////////////////////////// +void Root::Implementation::UpdateGraphs(sdf::Model &_model, + sdf::Errors &_errors) +{ + this->modelFrameAttachedToGraph = createFrameAttachedToGraph(_model, _errors); + _model.SetFrameAttachedToGraph(this->modelFrameAttachedToGraph); - this->dataPtr->sdf = _root.dataPtr->sdf; + this->modelPoseRelativeToGraph = createPoseRelativeToGraph(_model, _errors); + _model.SetPoseRelativeToGraph(this->modelPoseRelativeToGraph); } diff --git a/test/integration/frame.cc b/test/integration/frame.cc index ec2d112fc..0b7287bfa 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -676,60 +676,76 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - // Get the first world - const sdf::World *world = root.WorldByIndex(0); - ASSERT_NE(nullptr, world); - EXPECT_EQ("world_frame_attached_to", world->Name()); - EXPECT_EQ(1u, world->ModelCount()); - EXPECT_NE(nullptr, world->ModelByIndex(0)); - EXPECT_EQ(nullptr, world->ModelByIndex(1)); - - EXPECT_TRUE(world->ModelNameExists("M1")); - - const sdf::Model *model = world->ModelByIndex(0); - ASSERT_NE(nullptr, model); - EXPECT_EQ("M1", model->Name()); - EXPECT_EQ(1u, model->LinkCount()); - EXPECT_NE(nullptr, model->LinkByIndex(0)); - EXPECT_EQ(nullptr, model->LinkByIndex(1)); - EXPECT_EQ(1u, model->FrameCount()); - EXPECT_NE(nullptr, model->FrameByIndex(0)); - EXPECT_EQ(nullptr, model->FrameByIndex(1)); - ASSERT_TRUE(model->LinkNameExists("L")); - ASSERT_TRUE(model->FrameNameExists("F0")); - EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo()); - - EXPECT_EQ(4u, world->FrameCount()); - EXPECT_NE(nullptr, world->FrameByIndex(0)); - EXPECT_NE(nullptr, world->FrameByIndex(1)); - EXPECT_NE(nullptr, world->FrameByIndex(2)); - EXPECT_NE(nullptr, world->FrameByIndex(3)); - EXPECT_EQ(nullptr, world->FrameByIndex(4)); - ASSERT_TRUE(world->FrameNameExists("world_frame")); - ASSERT_TRUE(world->FrameNameExists("F0")); - ASSERT_TRUE(world->FrameNameExists("F1")); - ASSERT_TRUE(world->FrameNameExists("F2")); + auto testFunc = std::function( + [](sdf::Root &_root) + { + // Get the first world + const sdf::World *world = _root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ("world_frame_attached_to", world->Name()); + EXPECT_EQ(1u, world->ModelCount()); + EXPECT_NE(nullptr, world->ModelByIndex(0)); + EXPECT_EQ(nullptr, world->ModelByIndex(1)); - EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty()); - EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty()); - EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo()); - EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo()); + EXPECT_TRUE(world->ModelNameExists("M1")); - EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty()); - EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty()); - EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty()); - EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty()); - - std::string body; - EXPECT_TRUE( - world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("world", body); - EXPECT_TRUE(world->FrameByName("F0")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("world", body); - EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("world", body); - EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("M1::L", body); + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("M1", model->Name()); + EXPECT_EQ(1u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_EQ(nullptr, model->LinkByIndex(1)); + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + ASSERT_TRUE(model->LinkNameExists("L")); + ASSERT_TRUE(model->FrameNameExists("F0")); + EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo()); + + EXPECT_EQ(4u, world->FrameCount()); + EXPECT_NE(nullptr, world->FrameByIndex(0)); + EXPECT_NE(nullptr, world->FrameByIndex(1)); + EXPECT_NE(nullptr, world->FrameByIndex(2)); + EXPECT_NE(nullptr, world->FrameByIndex(3)); + EXPECT_EQ(nullptr, world->FrameByIndex(4)); + ASSERT_TRUE(world->FrameNameExists("world_frame")); + ASSERT_TRUE(world->FrameNameExists("F0")); + ASSERT_TRUE(world->FrameNameExists("F1")); + ASSERT_TRUE(world->FrameNameExists("F2")); + + EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty()); + EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty()); + EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo()); + EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo()); + + EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty()); + + std::string body; + EXPECT_TRUE( + world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("world", body); + EXPECT_TRUE(world->FrameByName("F0")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("world", body); + EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("world", body); + EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("M1::L", body); + }); + + testFunc(root); + + // Test UpdateGraphs + EXPECT_TRUE(root.UpdateGraphs().empty()); + testFunc(root); + + // Test cloning + sdf::Root root2; + root2.Clone(root); + testFunc(root); + testFunc(root2); } ///////////////////////////////////////////////// From 1fab523003b9fb1f50248d67bdd5b968e062c3ac Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 10 Feb 2022 15:23:48 -0800 Subject: [PATCH 07/10] Clear graphs on ClearWorlds Signed-off-by: Nate Koenig --- src/Root.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/Root.cc b/src/Root.cc index 9d3af825d..d2e454cb9 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -440,6 +440,8 @@ sdf::Errors Root::AddWorld(const World &_world) void Root::ClearWorlds() { this->dataPtr->worlds.clear(); + this->dataPtr->worldFrameAttachedToGraphs.clear(); + this->dataPtr->worldPoseRelativeToGraphs.clear(); } ///////////////////////////////////////////////// From ab3d64d0ec05357f65dcba19150ba8869af43517 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 08:24:04 -0800 Subject: [PATCH 08/10] Review updates Signed-off-by: Nate Koenig --- include/sdf/Root.hh | 10 +- src/Root.cc | 13 +- src/Root_TEST.cc | 8 +- test/integration/frame.cc | 225 +++++++++-------------------------- test/integration/root_dom.cc | 100 ++++++++++++++++ 5 files changed, 174 insertions(+), 182 deletions(-) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 670b69843..9d6a2b324 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -169,14 +169,16 @@ namespace sdf /// \brief Remove all worlds. public: void ClearWorlds(); - /// \brief Deep copy the provided Root object into this Root object. - /// \param[in] _root The Root object to copy. + /// \brief Deep copy this Root object and return the new Root object. + /// \return A clone of this Root object. /// Deprecate this function in SDF version 13, and use /// IGN_UTILS_IMPL_PTR instead. - public: void Clone(const sdf::Root &_root); + public: sdf::Root Clone() const; /// \brief Recreate the frame and pose graphs for the worlds and model - /// that are children of this Root object. + /// that are children of this Root object. You can call this function + /// to build new graphs when the DOM was created programmatically, or + /// if you want to regenerate the graphs after editing the DOM. /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error. public: Errors UpdateGraphs(); diff --git a/src/Root.cc b/src/Root.cc index d2e454cb9..17ae7ea75 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -445,7 +445,7 @@ void Root::ClearWorlds() } ///////////////////////////////////////////////// -void Root::Clone(const sdf::Root &_root) +/*void Root::Clone(const sdf::Root &_root) { this->dataPtr->version = _root.dataPtr->version; @@ -454,6 +454,17 @@ void Root::Clone(const sdf::Root &_root) this->dataPtr->modelLightOrActor = _root.dataPtr->modelLightOrActor; this->UpdateGraphs(); +}*/ + +///////////////////////////////////////////////// +sdf::Root Root::Clone() const +{ + sdf::Root r; + r.dataPtr->version = this->dataPtr->version; + r.dataPtr->worlds = this->dataPtr->worlds; + r.dataPtr->modelLightOrActor = this->dataPtr->modelLightOrActor; + r.UpdateGraphs(); + return r; } ///////////////////////////////////////////////// diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index c5d7c9146..1f6e28830 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -111,8 +111,7 @@ TEST(DOMRoot, StringModelSdfParse) EXPECT_EQ(0u, root.WorldCount()); // Test cloning - sdf::Root root2; - root2.Clone(root); + sdf::Root root2 = root.Clone(); const sdf::Model *model2 = root2.Model(); ASSERT_NE(nullptr, model2); @@ -307,12 +306,9 @@ TEST(DOMRoot, AddWorld) world.SetName("world1"); sdf::Errors errors = root.AddWorld(world); EXPECT_TRUE(errors.empty()); - std::cout << "+++\n"; - for (const auto &e : errors) - std::cout << e.Message() << std::endl; - std::cout << "---\n"; EXPECT_EQ(1u, root.WorldCount()); EXPECT_FALSE(root.AddWorld(world).empty()); + ASSERT_FALSE(root.AddWorld(world).empty()); EXPECT_EQ(sdf::ErrorCode::DUPLICATE_NAME, root.AddWorld(world)[0].Code()); EXPECT_EQ(1u, root.WorldCount()); diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 6c8eb6f8f..90b15be93 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -489,7 +489,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - ASSERT_EQ(10u, errors.size()); + EXPECT_EQ(10u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -676,76 +676,60 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo) sdf::Root root; EXPECT_TRUE(root.Load(testFile).empty()); - auto testFunc = std::function( - [](sdf::Root &_root) - { - // Get the first world - const sdf::World *world = _root.WorldByIndex(0); - ASSERT_NE(nullptr, world); - EXPECT_EQ("world_frame_attached_to", world->Name()); - EXPECT_EQ(1u, world->ModelCount()); - EXPECT_NE(nullptr, world->ModelByIndex(0)); - EXPECT_EQ(nullptr, world->ModelByIndex(1)); + // Get the first world + const sdf::World *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ("world_frame_attached_to", world->Name()); + EXPECT_EQ(1u, world->ModelCount()); + EXPECT_NE(nullptr, world->ModelByIndex(0)); + EXPECT_EQ(nullptr, world->ModelByIndex(1)); - EXPECT_TRUE(world->ModelNameExists("M1")); + EXPECT_TRUE(world->ModelNameExists("M1")); - const sdf::Model *model = world->ModelByIndex(0); - ASSERT_NE(nullptr, model); - EXPECT_EQ("M1", model->Name()); - EXPECT_EQ(1u, model->LinkCount()); - EXPECT_NE(nullptr, model->LinkByIndex(0)); - EXPECT_EQ(nullptr, model->LinkByIndex(1)); - EXPECT_EQ(1u, model->FrameCount()); - EXPECT_NE(nullptr, model->FrameByIndex(0)); - EXPECT_EQ(nullptr, model->FrameByIndex(1)); - ASSERT_TRUE(model->LinkNameExists("L")); - ASSERT_TRUE(model->FrameNameExists("F0")); - EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo()); - - EXPECT_EQ(4u, world->FrameCount()); - EXPECT_NE(nullptr, world->FrameByIndex(0)); - EXPECT_NE(nullptr, world->FrameByIndex(1)); - EXPECT_NE(nullptr, world->FrameByIndex(2)); - EXPECT_NE(nullptr, world->FrameByIndex(3)); - EXPECT_EQ(nullptr, world->FrameByIndex(4)); - ASSERT_TRUE(world->FrameNameExists("world_frame")); - ASSERT_TRUE(world->FrameNameExists("F0")); - ASSERT_TRUE(world->FrameNameExists("F1")); - ASSERT_TRUE(world->FrameNameExists("F2")); - - EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty()); - EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty()); - EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo()); - EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo()); - - EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty()); - EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty()); - EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty()); - EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty()); - - std::string body; - EXPECT_TRUE( - world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("world", body); - EXPECT_TRUE(world->FrameByName("F0")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("world", body); - EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("world", body); - EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("M1::L", body); - }); - - testFunc(root); - - // Test UpdateGraphs - EXPECT_TRUE(root.UpdateGraphs().empty()); - testFunc(root); - - // Test cloning - sdf::Root root2; - root2.Clone(root); - testFunc(root); - testFunc(root2); + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("M1", model->Name()); + EXPECT_EQ(1u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_EQ(nullptr, model->LinkByIndex(1)); + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + ASSERT_TRUE(model->LinkNameExists("L")); + ASSERT_TRUE(model->FrameNameExists("F0")); + EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo()); + + EXPECT_EQ(4u, world->FrameCount()); + EXPECT_NE(nullptr, world->FrameByIndex(0)); + EXPECT_NE(nullptr, world->FrameByIndex(1)); + EXPECT_NE(nullptr, world->FrameByIndex(2)); + EXPECT_NE(nullptr, world->FrameByIndex(3)); + EXPECT_EQ(nullptr, world->FrameByIndex(4)); + ASSERT_TRUE(world->FrameNameExists("world_frame")); + ASSERT_TRUE(world->FrameNameExists("F0")); + ASSERT_TRUE(world->FrameNameExists("F1")); + ASSERT_TRUE(world->FrameNameExists("F2")); + + EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty()); + EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty()); + EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo()); + EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo()); + + EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty()); + + std::string body; + EXPECT_TRUE( + world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("world", body); + EXPECT_TRUE(world->FrameByName("F0")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("world", body); + EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("world", body); + EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("M1::L", body); } ///////////////////////////////////////////////// @@ -761,7 +745,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidAttachedTo) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - ASSERT_EQ(11u, errors.size()); + EXPECT_EQ(11u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -1348,104 +1332,3 @@ TEST(Frame, IncludeFrameWithSubmodel) EXPECT_EQ(submodelPose.Inverse(), ignition::math::Pose3d(5, 5, 0, 0, 0, 0)); } - -///////////////////////////////////////////////// -TEST(DOMFrame, CreateMulipleWorlds) -{ - const std::string testFile = - sdf::testing::TestFile("sdf", - "world_frame_attached_to.sdf"); - - // Load the SDF file - sdf::Root loadedRoot; - EXPECT_TRUE(loadedRoot.Load(testFile).empty()); - sdf::World *loadedWorld = loadedRoot.WorldByIndex(0); - - sdf::Root root; - loadedWorld->SetName("world0"); - EXPECT_TRUE(root.AddWorld(*loadedWorld).empty()); - - loadedWorld->SetName("world1"); - EXPECT_TRUE(root.AddWorld(*loadedWorld).empty()); - EXPECT_FALSE(root.AddWorld(*loadedWorld).empty()); - - auto testFunc = std::function( - [](sdf::Root &_root) - { - EXPECT_EQ(2, _root.WorldCount()); - for (int i = 0; i < 1; ++i) - { - // Get the first world - const sdf::World *world = _root.WorldByIndex(i); - ASSERT_NE(nullptr, world); - - EXPECT_EQ(std::string("world") + std::to_string(i), world->Name()); - EXPECT_EQ(1u, world->ModelCount()); - EXPECT_NE(nullptr, world->ModelByIndex(0)); - EXPECT_EQ(nullptr, world->ModelByIndex(1)); - - EXPECT_TRUE(world->ModelNameExists("M1")); - - const sdf::Model *model = world->ModelByIndex(0); - ASSERT_NE(nullptr, model); - EXPECT_EQ("M1", model->Name()); - EXPECT_EQ(1u, model->LinkCount()); - EXPECT_NE(nullptr, model->LinkByIndex(0)); - EXPECT_EQ(nullptr, model->LinkByIndex(1)); - EXPECT_EQ(1u, model->FrameCount()); - EXPECT_NE(nullptr, model->FrameByIndex(0)); - EXPECT_EQ(nullptr, model->FrameByIndex(1)); - ASSERT_TRUE(model->LinkNameExists("L")); - ASSERT_TRUE(model->FrameNameExists("F0")); - EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo()); - - EXPECT_EQ(4u, world->FrameCount()); - EXPECT_NE(nullptr, world->FrameByIndex(0)); - EXPECT_NE(nullptr, world->FrameByIndex(1)); - EXPECT_NE(nullptr, world->FrameByIndex(2)); - EXPECT_NE(nullptr, world->FrameByIndex(3)); - EXPECT_EQ(nullptr, world->FrameByIndex(4)); - ASSERT_TRUE(world->FrameNameExists("world_frame")); - ASSERT_TRUE(world->FrameNameExists("F0")); - ASSERT_TRUE(world->FrameNameExists("F1")); - ASSERT_TRUE(world->FrameNameExists("F2")); - - EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty()); - EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty()); - EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo()); - EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo()); - - EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty()); - EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty()); - EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty()); - EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty()); - - std::string body; - EXPECT_TRUE( - world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("world", body); - EXPECT_TRUE( - world->FrameByName("F0")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("world", body); - EXPECT_TRUE( - world->FrameByName("F1")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("world", body); - EXPECT_TRUE( - world->FrameByName("F2")->ResolveAttachedToBody(body).empty()); - EXPECT_EQ("M1::L", body); - } - }); - - testFunc(root); - - // Test UpdateGraphs - EXPECT_TRUE(root.UpdateGraphs().empty()); - testFunc(root); - - // Test cloning - sdf::Root root2; - root2.Clone(root); - testFunc(root); - testFunc(root2); -} - diff --git a/test/integration/root_dom.cc b/test/integration/root_dom.cc index aee222c92..816975dcc 100644 --- a/test/integration/root_dom.cc +++ b/test/integration/root_dom.cc @@ -20,6 +20,7 @@ #include "sdf/Error.hh" #include "sdf/Filesystem.hh" +#include "sdf/Frame.hh" #include "sdf/Model.hh" #include "sdf/Root.hh" #include "sdf/Types.hh" @@ -108,3 +109,102 @@ TEST(DOMRoot, LoadDuplicateModels) EXPECT_NE(nullptr, root.Model()); EXPECT_EQ("robot1", root.Model()->Name()); } + +///////////////////////////////////////////////// +TEST(DOMRoot, CreateMulipleWorlds) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", + "world_frame_attached_to.sdf"); + + // Load the SDF file + sdf::Root loadedRoot; + EXPECT_TRUE(loadedRoot.Load(testFile).empty()); + sdf::World *loadedWorld = loadedRoot.WorldByIndex(0); + + sdf::Root root; + loadedWorld->SetName("world0"); + EXPECT_TRUE(root.AddWorld(*loadedWorld).empty()); + + loadedWorld->SetName("world1"); + EXPECT_TRUE(root.AddWorld(*loadedWorld).empty()); + EXPECT_FALSE(root.AddWorld(*loadedWorld).empty()); + + auto testFunc = std::function( + [](sdf::Root &_root) + { + EXPECT_EQ(2u, _root.WorldCount()); + for (int i = 0; i < 1; ++i) + { + // Get the first world + const sdf::World *world = _root.WorldByIndex(i); + ASSERT_NE(nullptr, world); + + EXPECT_EQ(std::string("world") + std::to_string(i), world->Name()); + EXPECT_EQ(1u, world->ModelCount()); + EXPECT_NE(nullptr, world->ModelByIndex(0)); + EXPECT_EQ(nullptr, world->ModelByIndex(1)); + + EXPECT_TRUE(world->ModelNameExists("M1")); + + const sdf::Model *model = world->ModelByIndex(0); + ASSERT_NE(nullptr, model); + EXPECT_EQ("M1", model->Name()); + EXPECT_EQ(1u, model->LinkCount()); + EXPECT_NE(nullptr, model->LinkByIndex(0)); + EXPECT_EQ(nullptr, model->LinkByIndex(1)); + EXPECT_EQ(1u, model->FrameCount()); + EXPECT_NE(nullptr, model->FrameByIndex(0)); + EXPECT_EQ(nullptr, model->FrameByIndex(1)); + ASSERT_TRUE(model->LinkNameExists("L")); + ASSERT_TRUE(model->FrameNameExists("F0")); + EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo()); + + EXPECT_EQ(4u, world->FrameCount()); + EXPECT_NE(nullptr, world->FrameByIndex(0)); + EXPECT_NE(nullptr, world->FrameByIndex(1)); + EXPECT_NE(nullptr, world->FrameByIndex(2)); + EXPECT_NE(nullptr, world->FrameByIndex(3)); + EXPECT_EQ(nullptr, world->FrameByIndex(4)); + ASSERT_TRUE(world->FrameNameExists("world_frame")); + ASSERT_TRUE(world->FrameNameExists("F0")); + ASSERT_TRUE(world->FrameNameExists("F1")); + ASSERT_TRUE(world->FrameNameExists("F2")); + + EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty()); + EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty()); + EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo()); + EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo()); + + EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty()); + EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty()); + + std::string body; + EXPECT_TRUE( + world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("world", body); + EXPECT_TRUE( + world->FrameByName("F0")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("world", body); + EXPECT_TRUE( + world->FrameByName("F1")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("world", body); + EXPECT_TRUE( + world->FrameByName("F2")->ResolveAttachedToBody(body).empty()); + EXPECT_EQ("M1::L", body); + } + }); + + testFunc(root); + + // Test UpdateGraphs + EXPECT_TRUE(root.UpdateGraphs().empty()); + testFunc(root); + + // Test cloning + sdf::Root root2 = root.Clone(); + testFunc(root); + testFunc(root2); +} From fc1c2110bdeabe76fbe547b7d95980e245a4228f Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 08:41:18 -0800 Subject: [PATCH 09/10] Cleanup Signed-off-by: Nate Koenig --- src/Root.cc | 12 ------------ test/integration/frame.cc | 4 ++-- 2 files changed, 2 insertions(+), 14 deletions(-) diff --git a/src/Root.cc b/src/Root.cc index 17ae7ea75..d3ab5b2fc 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -444,18 +444,6 @@ void Root::ClearWorlds() this->dataPtr->worldPoseRelativeToGraphs.clear(); } -///////////////////////////////////////////////// -/*void Root::Clone(const sdf::Root &_root) -{ - this->dataPtr->version = _root.dataPtr->version; - - this->dataPtr->worlds = _root.dataPtr->worlds; - - this->dataPtr->modelLightOrActor = _root.dataPtr->modelLightOrActor; - - this->UpdateGraphs(); -}*/ - ///////////////////////////////////////////////// sdf::Root Root::Clone() const { diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 90b15be93..ec2d112fc 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -489,7 +489,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(10u, errors.size()); + ASSERT_EQ(10u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( @@ -745,7 +745,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidAttachedTo) for (auto e : errors) std::cout << e << std::endl; EXPECT_FALSE(errors.empty()); - EXPECT_EQ(11u, errors.size()); + ASSERT_EQ(11u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( From 2776439e61a33993d104441c518a63857e3de1b9 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 16 Feb 2022 09:54:05 -0800 Subject: [PATCH 10/10] Remove extra check Signed-off-by: Nate Koenig --- src/Root_TEST.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index 1f6e28830..4be950e90 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -307,7 +307,6 @@ TEST(DOMRoot, AddWorld) sdf::Errors errors = root.AddWorld(world); EXPECT_TRUE(errors.empty()); EXPECT_EQ(1u, root.WorldCount()); - EXPECT_FALSE(root.AddWorld(world).empty()); ASSERT_FALSE(root.AddWorld(world).empty()); EXPECT_EQ(sdf::ErrorCode::DUPLICATE_NAME, root.AddWorld(world)[0].Code()); EXPECT_EQ(1u, root.WorldCount());