Move construction of PoseRelativeToGraph and FrameAttachedToGraph to sdf::Root

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<PoseRelativeToGraph> _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<FrameAttachedToGraph> _graph);
@@ -335,8 +337,10 @@ namespace sdf
     private: std::pair<const Link *, std::string> 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

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

include/sdf/World.hh

@@ -41,6 +41,9 @@ namespace sdf
   class Model;
   class Physics;
   class WorldPrivate;
+  struct PoseRelativeToGraph;
+  struct FrameAttachedToGraph;
+  template <typename T> 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<PoseRelativeToGraph> _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<FrameAttachedToGraph> _graph);
+
+    /// \brief Allow Root::Load to call SetPoseRelativeToGraph and
+    /// SetFrameAttachedToGraph
+    friend class Root;
+
     /// \brief Private data pointer.
     private: WorldPrivate *dataPtr = nullptr;
   };

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<sdf::FrameAttachedToGraph> 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<sdf::FrameAttachedToGraph> 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<sdf::PoseRelativeToGraph> 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<sdf::PoseRelativeToGraph> 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<sdf::FrameAttachedToGraph>(
-            *_model.dataPtr->ownedFrameAttachedToGraph);
-    this->dataPtr->frameAttachedToGraph =
-        ScopedGraph<sdf::FrameAttachedToGraph>(
-            this->dataPtr->ownedFrameAttachedToGraph);
-  }
-  if (_model.dataPtr->ownedPoseGraph)
-  {
-    this->dataPtr->ownedPoseGraph = std::make_shared<sdf::PoseRelativeToGraph>(
-        *_model.dataPtr->ownedPoseGraph);
-    this->dataPtr->poseGraph =
-        ScopedGraph<sdf::PoseRelativeToGraph>(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<PoseRelativeToGraph>();
-    this->dataPtr->poseGraph = ScopedGraph(this->dataPtr->ownedPoseGraph);
-    this->dataPtr->ownedFrameAttachedToGraph =
-        std::make_shared<FrameAttachedToGraph>();
-    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<std::string> 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<PoseRelativeToGraph> _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<PoseRelativeToGraph> _graph)
 void Model::SetFrameAttachedToGraph(
     sdf::ScopedGraph<FrameAttachedToGraph> _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 =

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<Actor> actors;
 
+  /// \brief Frame Attached-To Graphs constructed when loading Worlds.
+  public: std::vector<sdf::ScopedGraph<FrameAttachedToGraph>>
+              worldFrameAttachedToGraphs;
+
+  /// \brief Frame Attached-To Graphs constructed when loading Models.
+  public: std::vector<sdf::ScopedGraph<FrameAttachedToGraph>>
+              modelFrameAttachedToGraphs;
+
+  /// \brief Pose Relative-To Graphs constructed when loading Worlds.
+  public: std::vector<sdf::ScopedGraph<PoseRelativeToGraph>>
+              worldPoseRelativeToGraphs;
+
+  /// \brief Pose Relative-To Graphs constructed when loading Models.
+  public: std::vector<sdf::ScopedGraph<PoseRelativeToGraph>>
+              modelPoseRelativeToGraphs;
+
   /// \brief The SDF element pointer generated during load.
   public: sdf::ElementPtr sdf;
 };
 
+/////////////////////////////////////////////////
+template <typename T>
+sdf::ScopedGraph<FrameAttachedToGraph> addFrameAttachedToGraph(
+    std::vector<sdf::ScopedGraph<sdf::FrameAttachedToGraph>> &_graphList,
+    const T &_domObj, sdf::Errors &_errors)
+{
+  auto &frameGraph =
+      _graphList.emplace_back(std::make_shared<FrameAttachedToGraph>());
+
+  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 <typename T>
+ScopedGraph<PoseRelativeToGraph> addPoseRelativeToGraph(
+    std::vector<sdf::ScopedGraph<sdf::PoseRelativeToGraph>> &_graphList,
+    const T &_domObj, Errors &_errors)
+{
+  auto &poseGraph =
+      _graphList.emplace_back(std::make_shared<sdf::PoseRelativeToGraph>());
+
+  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<Model>(this->dataPtr->sdf,
-      "model", this->dataPtr->models);
+  Errors modelLoadErrors = loadUniqueRepeated<Model>(
+      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<Light>(this->dataPtr->sdf,
       "light", this->dataPtr->lights);