diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 415016041..9d6a2b324 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,31 @@ 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 Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors AddWorld(const World &_world); + + /// \brief Remove all worlds. + public: void ClearWorlds(); + + /// \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: sdf::Root Clone() const; + + /// \brief Recreate the frame and pose graphs for the worlds and model + /// 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(); + /// \brief Private data pointer IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index f9353df3c..47270bf3d 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -960,11 +960,6 @@ Errors buildFrameAttachedToGraph( { return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::Model pointer."}}; } - else if (!_model->Element()) - { - return Errors{ - {ErrorCode::ELEMENT_INVALID, "Invalid model element in sdf::Model."}}; - } return wrapperBuildFrameAttachedToGraph(_out, ModelWrapper(*_model), _isRoot); } @@ -1012,11 +1007,6 @@ Errors buildFrameAttachedToGraph( { return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::World pointer."}}; } - else if (!_world->Element()) - { - return Errors{ - {ErrorCode::ELEMENT_INVALID, "Invalid world element in sdf::World."}}; - } return buildFrameAttachedToGraph(_out, WorldWrapper(*_world)); } @@ -1133,11 +1123,6 @@ Errors buildPoseRelativeToGraph( { return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::Model pointer."}}; } - else if (!_model->Element()) - { - return Errors{ - {ErrorCode::ELEMENT_INVALID, "Invalid model element in sdf::Model."}}; - } return wrapperBuildPoseRelativeToGraph(_out, ModelWrapper(*_model), _isRoot); } @@ -1197,11 +1182,7 @@ Errors buildPoseRelativeToGraph( { return Errors{{ErrorCode::ELEMENT_INVALID, "Invalid sdf::World pointer."}}; } - else if (!_world->Element()) - { - return Errors{ - {ErrorCode::ELEMENT_INVALID, "Invalid world element in sdf::World."}}; - } + return wrapperBuildPoseRelativeToGraph(_out, WorldWrapper(*_world)); } diff --git a/src/Root.cc b/src/Root.cc index 3fe61496b..d3ab5b2fc 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. @@ -381,6 +376,13 @@ const World *Root::WorldByIndex(const uint64_t _index) const return nullptr; } +///////////////////////////////////////////////// +World *Root::WorldByIndex(const uint64_t _index) +{ + return const_cast( + static_cast(this)->WorldByIndex(_index)); +} + ///////////////////////////////////////////////// bool Root::WorldNameExists(const std::string &_name) const { @@ -417,3 +419,88 @@ sdf::ElementPtr Root::Element() const { return this->dataPtr->sdf; } + +///////////////////////////////////////////////// +sdf::Errors Root::AddWorld(const World &_world) +{ + if (!this->WorldNameExists(_world.Name())) + { + this->dataPtr->worlds.push_back(_world); + return this->UpdateGraphs(); + } + + sdf::Errors errors; + errors.push_back({ErrorCode::DUPLICATE_NAME, + "World with name[" + _world.Name() + "] already exists."}); + + return errors; +} + +///////////////////////////////////////////////// +void Root::ClearWorlds() +{ + this->dataPtr->worlds.clear(); + this->dataPtr->worldFrameAttachedToGraphs.clear(); + this->dataPtr->worldPoseRelativeToGraphs.clear(); +} + +///////////////////////////////////////////////// +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; +} + +///////////////////////////////////////////////// +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); + } + + // 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); + } + + return errors; +} + +////////////////////////////////////////////////// +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->modelPoseRelativeToGraph = createPoseRelativeToGraph(_model, _errors); + _model.SetPoseRelativeToGraph(this->modelPoseRelativeToGraph); +} diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index d687f6e2b..4be950e90 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -109,6 +109,31 @@ TEST(DOMRoot, StringModelSdfParse) EXPECT_EQ(nullptr, root.Light()); EXPECT_EQ(nullptr, root.Actor()); EXPECT_EQ(0u, root.WorldCount()); + + // Test cloning + sdf::Root root2 = root.Clone(); + + 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()); } ///////////////////////////////////////////////// @@ -270,3 +295,47 @@ TEST(DOMRoot, FrameSemanticsOnMove) testFrame1(root2); } } + +///////////////////////////////////////////////// +TEST(DOMRoot, AddWorld) +{ + sdf::Root root; + EXPECT_EQ(0u, root.WorldCount()); + + sdf::World world; + world.SetName("world1"); + sdf::Errors errors = root.AddWorld(world); + EXPECT_TRUE(errors.empty()); + EXPECT_EQ(1u, root.WorldCount()); + ASSERT_FALSE(root.AddWorld(world).empty()); + EXPECT_EQ(sdf::ErrorCode::DUPLICATE_NAME, root.AddWorld(world)[0].Code()); + EXPECT_EQ(1u, root.WorldCount()); + + root.ClearWorlds(); + EXPECT_EQ(0u, root.WorldCount()); + + EXPECT_TRUE(root.AddWorld(world).empty()); + 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).empty()); + 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()); +} 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); +}