Merge branch 'ahcorde/sdf_to_usd_links' into adlarkin/sdf_to_usd_joints

Author: adlarkin

include/sdf/Error.hh

@@ -145,7 +145,7 @@ namespace sdf
     VERSION_DEPRECATED,
 
     /// \brief Merge include is unspported for the type of entity being
-    /// included.
+    /// included, or the custom parser does not support merge includes.
     MERGE_INCLUDE_UNSUPPORTED,
   };
 

include/sdf/InterfaceElements.hh

@@ -154,6 +154,17 @@ class SDFORMAT_VISIBLE NestedInclude
   /// \param[in] _includeElement The include element
   public: void SetIncludeElement(sdf::ElementPtr _includeElement);
 
+
+  /// \brief Set whether the interface model is to be merge-included (i.e
+  /// set the value of `//include/[@merge]`)
+  /// \param[in] _isMerge True if the interface model is to be merge included
+  public: void SetIsMerge(bool _isMerge);
+
+  /// \brief Whether the interface model is to be merge-included
+  /// \return If `//include/[@merge]` is set, this returns the value of the
+  /// attribute, otherwise, nullopt.
+  public: const std::optional<bool> &IsMerge() const;
+
   /// \brief Provides the URI as specified in `//include/uri`. This may or may
   /// not end with a file extension (it will not end with an extension if it
   /// refers to a model package).

include/sdf/InterfaceModel.hh

@@ -124,10 +124,20 @@ class SDFORMAT_VISIBLE InterfaceModel
   /// \brief Gets registered links.
   public: const std::vector<sdf::InterfaceLink> &Links() const;
 
+  /// \brief Whether the custom parser supports merge-include.
+  /// \return True if the custom parser supports merge-include
+  public: bool ParserSupportsMergeInclude() const;
+
+  /// \brief Set whether the custom parser supports merge-include.
+  /// \brief[in] _val True if the custom parser supports merge-include.
+  public: void SetParserSupportsMergeInclude(bool _val);
+
   /// \brief Recursively invoke the reposture callback if a the callback is set.
   /// \param[in] _poseGraph Object used for resolving poses.
-  private: void InvokeRespostureFunction(
-      sdf::ScopedGraph<PoseRelativeToGraph> _graph) const;
+  /// \param[in] _name Override name of graph scope.
+  private: void InvokeRepostureFunction(
+               sdf::ScopedGraph<PoseRelativeToGraph> _graph,
+               const std::optional<std::string> &_name) const;
 
   friend World;
   friend Model;

include/sdf/Link.hh

@@ -76,6 +76,13 @@ namespace sdf
     /// \sa uint64_t VisualCount() const
     public: const Visual *VisualByIndex(const uint64_t _index) const;
 
+    /// \brief Get a mutable visual based on an index.
+    /// \param[in] _index Index of the visual. The index should be in the
+    /// range [0..VisualCount()).
+    /// \return Pointer to the visual. Nullptr if the index does not exist.
+    /// \sa uint64_t VisualCount() const
+    public: Visual *VisualByIndex(uint64_t _index);
+
     /// \brief Get whether a visual name exists.
     /// \param[in] _name Name of the visual to check.
     /// \return True if there exists a visual with the given name.
@@ -86,6 +93,11 @@ namespace sdf
     /// \return Pointer to the visual. Nullptr if the name does not exist.
     public: const Visual *VisualByName(const std::string &_name) const;
 
+    /// \brief Get a mutable visual based on a name.
+    /// \param[in] _name Name of the visual.
+    /// \return Pointer to the visual. Nullptr if the name does not exist.
+    public: Visual *VisualByName(const std::string &_name);
+
     /// \brief Get the number of collisions.
     /// \return Number of collisions contained in this Link object.
     public: uint64_t CollisionCount() const;
@@ -97,6 +109,13 @@ namespace sdf
     /// \sa uint64_t CollisionCount() const
     public: const Collision *CollisionByIndex(const uint64_t _index) const;
 
+    /// \brief Get a mutable collision based on an index.
+    /// \param[in] _index Index of the collision. The index should be in the
+    /// range [0..CollisionCount()).
+    /// \return Pointer to the collision. Nullptr if the index does not exist.
+    /// \sa uint64_t CollisionCount() const
+    public: Collision *CollisionByIndex(uint64_t _index);
+
     /// \brief Get whether a collision name exists.
     /// \param[in] _name Name of the collision to check.
     /// \return True if there exists a collision with the given name.
@@ -107,6 +126,11 @@ namespace sdf
     /// \return Pointer to the collision. Nullptr if the name does not exist.
     public: const Collision *CollisionByName(const std::string &_name) const;
 
+    /// \brief Get a mutable collision based on a name.
+    /// \param[in] _name Name of the collision.
+    /// \return Pointer to the collision. Nullptr if the name does not exist.
+    public: Collision *CollisionByName(const std::string &_name);
+
     /// \brief Get the number of lights.
     /// \return Number of lights contained in this Link object.
     public: uint64_t LightCount() const;
@@ -118,6 +142,13 @@ namespace sdf
     /// \sa uint64_t LightCount() const
     public: const Light *LightByIndex(const uint64_t _index) const;
 
+    /// \brief Get a mutable light based on an index.
+    /// \param[in] _index Index of the light. The index should be in the
+    /// range [0..LightCount()).
+    /// \return Pointer to the light. Nullptr if the index does not exist.
+    /// \sa uint64_t LightCount() const
+    public: Light *LightByIndex(uint64_t _index);
+
     /// \brief Get whether a light name exists.
     /// \param[in] _name Name of the light to check.
     /// \return True if there exists a light with the given name.
@@ -128,6 +159,11 @@ namespace sdf
     /// \return Pointer to the light. Nullptr if the name does not exist.
     public: const Light *LightByName(const std::string &_name) const;
 
+    /// \brief Get a mutable light based on a name.
+    /// \param[in] _name Name of the light.
+    /// \return Pointer to the light. Nullptr if the name does not exist.
+    public: Light *LightByName(const std::string &_name);
+
     /// \brief Get the number of sensors.
     /// \return Number of sensors contained in this Link object.
     public: uint64_t SensorCount() const;
@@ -139,6 +175,13 @@ namespace sdf
     /// \sa uint64_t SensorCount() const
     public: const Sensor *SensorByIndex(const uint64_t _index) const;
 
+    /// \brief Get a mutable sensor based on an index.
+    /// \param[in] _index Index of the sensor. The index should be in the
+    /// range [0..SensorCount()).
+    /// \return Pointer to the sensor. Nullptr if the index does not exist.
+    /// \sa uint64_t SensorCount() const
+    public: Sensor *SensorByIndex(uint64_t _index);
+
     /// \brief Get whether a sensor name exists.
     /// \param[in] _name Name of the sensor to check.
     /// \return True if there exists a sensor with the given name.
@@ -151,6 +194,13 @@ namespace sdf
     /// \sa bool SensorNameExists(const std::string &_name) const
     public: const Sensor *SensorByName(const std::string &_name) const;
 
+    /// \brief Get a mutable sensor based on a name.
+    /// \param[in] _name Name of the sensor.
+    /// \return Pointer to the sensor. Nullptr if a sensor with the given name
+    ///  does not exist.
+    /// \sa bool SensorNameExists(const std::string &_name) const
+    public: Sensor *SensorByName(const std::string &_name);
+
     /// \brief Get the number of particle emitters.
     /// \return Number of particle emitters contained in this Link object.
     public: uint64_t ParticleEmitterCount() const;
@@ -164,6 +214,14 @@ namespace sdf
     public: const ParticleEmitter *ParticleEmitterByIndex(
                 const uint64_t _index) const;
 
+    /// \brief Get a mutable particle emitter based on an index.
+    /// \param[in] _index Index of the particle emitter.
+    /// The index should be in the range [0..ParticleEmitterCount()).
+    /// \return Pointer to the particle emitter. Nullptr if the index does
+    /// not exist.
+    /// \sa uint64_t ParticleEmitterCount() const
+    public: ParticleEmitter *ParticleEmitterByIndex(uint64_t _index);
+
     /// \brief Get whether a particle emitter name exists.
     /// \param[in] _name Name of the particle emitter to check.
     /// \return True if there exists a particle emitter with the given name.
@@ -177,6 +235,13 @@ namespace sdf
     public: const ParticleEmitter *ParticleEmitterByName(
                 const std::string &_name) const;
 
+    /// \brief Get a mutable particle emitter based on a name.
+    /// \param[in] _name Name of the particle emitter.
+    /// \return Pointer to the particle emitter. Nullptr if a particle emitter
+    /// with the given name does not exist.
+    /// \sa bool ParticleEmitterNameExists(const std::string &_name) const
+    public: ParticleEmitter *ParticleEmitterByName(const std::string &_name);
+
     /// \brief Get the inertial value for this link. The inertial object
     /// consists of the link's mass, a 3x3 rotational inertia matrix, and
     /// a pose for the inertial reference frame. The units for mass is

include/sdf/Model.hh

@@ -20,6 +20,7 @@
 #include <memory>
 #include <string>
 #include <utility>
+#include <vector>
 #include <ignition/math/Pose3.hh>
 #include <ignition/utils/ImplPtr.hh>
 #include "sdf/Element.hh"
@@ -44,6 +45,8 @@ namespace sdf
   struct PoseRelativeToGraph;
   struct FrameAttachedToGraph;
   template <typename T> class ScopedGraph;
+  using InterfaceModelConstPtr = std::shared_ptr<const InterfaceModel>;
+
 
   class SDFORMAT_VISIBLE Model
   {
@@ -410,12 +413,20 @@ namespace sdf
     private: void SetFrameAttachedToGraph(
         sdf::ScopedGraph<FrameAttachedToGraph> _graph);
 
+    /// \brief Get the list of merged interface models.
+    /// \return The list of merged interface models.
+    private: const std::vector<std::pair<std::optional<sdf::NestedInclude>,
+             sdf::InterfaceModelConstPtr>> &MergedInterfaceModels() const;
+
     /// \brief Allow Root::Load, World::SetPoseRelativeToGraph, or
     /// World::SetFrameAttachedToGraph to call SetPoseRelativeToGraph and
     /// SetFrameAttachedToGraph
     friend class Root;
     friend class World;
 
+    // Allow ModelWrapper from FrameSemantics.cc to call MergedInterfaceModels
+    friend struct ModelWrapper;
+
     /// \brief Private data pointer.
     IGN_UTILS_IMPL_PTR(dataPtr)
   };

src/CMakeLists.txt

@@ -68,7 +68,7 @@ if (BUILD_TESTING)
   endif()
 
   if (TARGET UNIT_FrameSemantics_TEST)
-    target_sources(UNIT_FrameSemantics_TEST PRIVATE FrameSemantics.cc)
+    target_sources(UNIT_FrameSemantics_TEST PRIVATE FrameSemantics.cc Utils.cc)
   endif()
 
   if (TARGET UNIT_ParamPassing_TEST)

src/FrameSemantics.cc

@@ -36,6 +36,7 @@
 
 #include "FrameSemantics.hh"
 #include "ScopedGraph.hh"
+#include "Utils.hh"
 
 namespace sdf
 {
@@ -306,6 +307,17 @@ struct LinkWrapper : public WrapperBase
   {
   }
 
+  /// \brief Constructor from name and pose data (without sdf::Link or
+  /// sdf::InterfaceLink)
+  LinkWrapper(const std::string &_name, const ignition::math::Pose3d &_rawPose,
+              const std::string &_relativeTo)
+      : WrapperBase{_name, "Link", FrameType::LINK},
+        rawPose(_rawPose),
+        rawRelativeTo(_relativeTo),
+        relativeTo(rawRelativeTo)
+  {
+  }
+
   /// \brief Raw pose of the entity.
   const ignition::math::Pose3d rawPose;
   /// \brief The //pose/@relative_to attribute.
@@ -339,6 +351,18 @@ struct FrameWrapper : public WrapperBase
   {
   }
 
+  /// \brief Constructor from name, pose, and attachement data (without
+  /// sdf::Frame or sdf::InterfaceFrame)
+  FrameWrapper(const std::string &_name, const ignition::math::Pose3d &_rawPose,
+               const std::string &_relativeTo, const std::string &_attachedTo)
+      : WrapperBase{_name, "Frame", FrameType::FRAME},
+        rawPose(_rawPose),
+        rawRelativeTo(_relativeTo),
+        attachedTo(_attachedTo),
+        relativeTo(rawRelativeTo.empty() ? attachedTo : rawRelativeTo)
+  {
+  }
+
   /// \brief Raw pose of the entity.
   const ignition::math::Pose3d rawPose;
   /// \brief The //pose/@relative_to attribute.
@@ -386,6 +410,15 @@ struct JointWrapper : public WrapperBase
   const std::string relativeTo;
 };
 
+/// \brief Placement frame information
+struct PlacementFrameInfo
+{
+  /// \brief Computed name of the proxy model frame
+  std::string proxyName;
+  /// \brief The name of placement frame from //include/placement_frame.
+  std::string placementFrameName;
+};
+
 /// \brief Wrapper for sdf::Model and sdf::InterfaceModel
 struct ModelWrapper : public WrapperBase
 {
@@ -422,6 +455,56 @@ struct ModelWrapper : public WrapperBase
       this->models.emplace_back(*_model.InterfaceModelNestedIncludeByIndex(i),
                                 *_model.InterfaceModelByIndex(i));
     }
+    for (const auto &[nestedInclude, model] : _model.MergedInterfaceModels())
+    {
+      const std::string proxyModelFrameName = computeMergedModelProxyFrameName(
+          nestedInclude->LocalModelName().value_or(model->Name()));
+
+      std::string poseRelativeTo =
+          nestedInclude->IncludePoseRelativeTo().value_or("");
+      if (poseRelativeTo.empty())
+      {
+        poseRelativeTo = "__model__";
+      }
+      this->frames.emplace_back(proxyModelFrameName,
+                                nestedInclude->IncludeRawPose().value_or(
+                                    model->ModelFramePoseInParentFrame()),
+                                poseRelativeTo, model->CanonicalLinkName());
+
+      for (const auto &item : model->Links())
+      {
+        this->links.emplace_back(item.Name(), item.PoseInModelFrame(),
+                                 proxyModelFrameName);
+      }
+      for (const auto &item : model->Frames())
+      {
+        std::string attachedTo = item.AttachedTo();
+        if (item.AttachedTo() == "__model__")
+        {
+          attachedTo = proxyModelFrameName;
+        }
+        this->frames.emplace_back(item.Name(), item.PoseInAttachedToFrame(),
+                                  attachedTo, attachedTo);
+      }
+      for (const auto &item : model->Joints())
+      {
+        std::string childName = item.ChildName();
+        if (item.ChildName() == "__model__")
+        {
+          childName = proxyModelFrameName;
+        }
+        this->joints.emplace_back(sdf::InterfaceJoint(item.Name(), childName,
+                                                      item.PoseInChildFrame()));
+      }
+      if (nestedInclude->PlacementFrame().has_value())
+      {
+        this->mergedModelPlacements.push_back(
+            {proxyModelFrameName, *nestedInclude->PlacementFrame()});
+      }
+
+      // Skip adding nested interface models because they are already included
+      // in the parent model's list of nested models.
+    }
   }
 
   /// \brief Constructor that takes an sdf::NestedInclude and
@@ -479,6 +562,8 @@ struct ModelWrapper : public WrapperBase
   std::vector<JointWrapper> joints;
   /// \brief Children nested models and interface models.
   std::vector<ModelWrapper> models;
+  /// \brief Placement frame information for each merged model.
+  std::vector<PlacementFrameInfo> mergedModelPlacements;
 
   /// \brief Helper function to add children of interface models.
   private: void AddInterfaceChildren(const sdf::InterfaceModel &_ifaceModel)
@@ -1014,6 +1099,30 @@ Errors wrapperBuildPoseRelativeToGraph(ScopedGraph<PoseRelativeToGraph> &_out,
 
     outModel.UpdateEdge(rootToModel, resolvedModelPose);
   }
+
+  // For each merge model, update the edge between the parent model and the
+  // proxy model frame to take into account the placement frame used when
+  // nesting the merged model via //include.
+  for (const auto &[proxyName, placementFrameName] :
+       _model.mergedModelPlacements)
+  {
+    auto proxyId = outModel.VertexIdByName(proxyName);
+    auto modelToProxy =
+        outModel.Graph().EdgeFromVertices(modelFrameId, proxyId);
+    const auto rawPose = modelToProxy.Data();
+
+    // We have to first set the edge data to 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.
+    outModel.UpdateEdge(modelToProxy, ignition::math::Pose3d::Zero);
+    ignition::math::Pose3d resolvedModelPose;
+    sdf::Errors resolveErrors =
+        resolveModelPoseWithPlacementFrame(rawPose,
+            placementFrameName, outModel, resolvedModelPose);
+    errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end());
+    outModel.UpdateEdge(modelToProxy, resolvedModelPose);
+  }
   return errors;
 }
 /////////////////////////////////////////////////