Add support for merge-include in the Interface API

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

src/InterfaceElements.cc

@@ -21,7 +21,10 @@ using namespace sdf;
 
 class sdf::NestedInclude::Implementation
 {
-
+  /// \brief Whether the included model should be merged as specified in
+  /// //include/[@merge]
+  /// This is nullopt if `//include/[@merge]` is is not set.
+  public: std::optional<bool> isMerge;
 };
 
 SDF_SUPPRESS_DEPRECATED_BEGIN
@@ -144,3 +147,15 @@ void NestedInclude::SetIncludeElement(sdf::ElementPtr _includeElement)
   this->includeElement = _includeElement;
 }
 SDF_SUPPRESS_DEPRECATED_END
+
+/////////////////////////////////////////////////
+void NestedInclude::SetIsMerge(bool _isMerge)
+{
+  this->dataPtr->isMerge = _isMerge;
+}
+
+/////////////////////////////////////////////////
+const std::optional<bool> &NestedInclude::IsMerge() const
+{
+  return this->dataPtr->isMerge;
+}

src/InterfaceModel.cc

@@ -52,6 +52,9 @@ class InterfaceModel::Implementation
 
   /// \brief Collection of child interface links
   public: std::vector<sdf::InterfaceLink> links;
+
+  /// \brief Whether the custom parser supports merge-includes
+  public: bool parserSupportsMergeInclude {false};
 };
 
 InterfaceModel::InterfaceModel(const std::string &_name,
@@ -143,19 +146,34 @@ const std::vector<sdf::InterfaceLink> &InterfaceModel::Links() const
 }
 
 /////////////////////////////////////////////////
-void InterfaceModel::InvokeRespostureFunction(
-    sdf::ScopedGraph<PoseRelativeToGraph> _graph) const
+bool InterfaceModel::ParserSupportsMergeInclude() const
+{
+  return this->dataPtr->parserSupportsMergeInclude;
+}
+
+/////////////////////////////////////////////////
+void InterfaceModel::SetParserSupportsMergeInclude(bool _val)
 {
+  this->dataPtr->parserSupportsMergeInclude = _val;
+}
+
+/////////////////////////////////////////////////
+void InterfaceModel::InvokeRepostureFunction(
+    sdf::ScopedGraph<PoseRelativeToGraph> _graph,
+    const std::optional<std::string> &_name) const
+{
+  const auto name = _name.value_or(this->Name());
+
   if (this->dataPtr->repostureFunction)
   {
     this->dataPtr->repostureFunction(
-        sdf::InterfaceModelPoseGraph(this->dataPtr->name, _graph));
+        sdf::InterfaceModelPoseGraph(name, _graph));
   }
 
   for (const auto &nestedIfaceModel : this->dataPtr->nestedModels)
   {
-    nestedIfaceModel->InvokeRespostureFunction(
-        _graph.ChildModelScope(this->Name()));
+    nestedIfaceModel->InvokeRepostureFunction(
+        _graph.ChildModelScope(name), {});
   }
 }
 }