Added Root mutable accessors, and Root::Clone function

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

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

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<sdf::Model>(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<World*>(
+      static_cast<const Root*>(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<sdf::Model>(this->dataPtr->modelLightOrActor))
+  {
+    sdf::Model &model = std::get<sdf::Model>(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);
+}

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,48 @@ 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());
+  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());
+
+  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());
+}