FrameSemantics: support nested models

Add nested models to FrameAttachedToGraph and PoseRelativeToGraph
and support frame semantics.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

include/sdf/Model.hh

@@ -306,7 +306,8 @@ namespace sdf
     /// for resolving poses. This is private and is intended to be called by
     /// World::Load.
     /// \param[in] _graph Weak pointer to PoseRelativeToGraph.
-    private: void SetPoseRelativeToGraph(
+    /// \return Error if graph pointer is invalid.
+    private: sdf::Errors SetPoseRelativeToGraph(
         std::weak_ptr<const PoseRelativeToGraph> _graph);
 
     /// \brief Allow World::Load to call SetPoseRelativeToGraph.

src/FrameSemantics.cc

@@ -285,6 +285,23 @@ Errors buildFrameAttachedToGraph(
     _out.map[frame->Name()] = frameId;
   }
 
+  // add nested model vertices
+  for (uint64_t m = 0; m < _model->ModelCount(); ++m)
+  {
+    auto nestedModel = _model->ModelByIndex(m);
+    if (_out.map.count(nestedModel->Name()) > 0)
+    {
+      errors.push_back({ErrorCode::DUPLICATE_NAME,
+          "Nested model with non-unique name [" + nestedModel->Name() +
+          "] detected in model with name [" + _model->Name() +
+          "]."});
+      continue;
+    }
+    auto nestedModelId =
+        _out.graph.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id();
+    _out.map[nestedModel->Name()] = nestedModelId;
+  }
+
   // add frame edges
   for (uint64_t f = 0; f < _model->FrameCount(); ++f)
   {
@@ -352,9 +369,9 @@ Errors buildFrameAttachedToGraph(
   }
 
   // add model vertices
-  for (uint64_t l = 0; l < _world->ModelCount(); ++l)
+  for (uint64_t m = 0; m < _world->ModelCount(); ++m)
   {
-    auto model = _world->ModelByIndex(l);
+    auto model = _world->ModelByIndex(m);
     if (_out.map.count(model->Name()) > 0)
     {
       errors.push_back({ErrorCode::DUPLICATE_NAME,
@@ -537,6 +554,30 @@ Errors buildPoseRelativeToGraph(
     }
   }
 
+  // add nested model vertices and default edge if relative_to is empty
+  for (uint64_t m = 0; m < _model->ModelCount(); ++m)
+  {
+    auto nestedModel = _model->ModelByIndex(m);
+    if (_out.map.count(nestedModel->Name()) > 0)
+    {
+      errors.push_back({ErrorCode::DUPLICATE_NAME,
+          "Nested model with non-unique name [" + nestedModel->Name() +
+          "] detected in model with name [" + _model->Name() +
+          "]."});
+      continue;
+    }
+    auto nestedModelId =
+        _out.graph.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id();
+    _out.map[nestedModel->Name()] = nestedModelId;
+
+    if (nestedModel->PoseRelativeTo().empty())
+    {
+      // relative_to is empty, so add edge from implicit model frame
+      // to nestedModel
+      _out.graph.AddEdge({modelFrameId, nestedModelId}, nestedModel->RawPose());
+    }
+  }
+
   // now that all vertices have been added to the graph,
   // add the edges that reference other vertices
 
@@ -659,6 +700,41 @@ Errors buildPoseRelativeToGraph(
     _out.graph.AddEdge({relativeToId, frameId}, frame->RawPose());
   }
 
+  for (uint64_t m = 0; m < _model->ModelCount(); ++m)
+  {
+    auto nestedModel = _model->ModelByIndex(m);
+
+    // check if we've already added a default edge
+    const std::string relativeTo = nestedModel->PoseRelativeTo();
+    if (relativeTo.empty())
+    {
+      continue;
+    }
+
+    auto nestedModelId = _out.map.at(nestedModel->Name());
+
+    // look for vertex in graph that matches relative_to value
+    if (_out.map.count(relativeTo) != 1)
+    {
+      errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID,
+          "relative_to name[" + relativeTo +
+          "] specified by nested model with name[" + nestedModel->Name() +
+          "] does not match a nested model, link, joint, or frame name "
+          "in model with name[" + _model->Name() + "]."});
+      continue;
+    }
+    auto relativeToId = _out.map[relativeTo];
+    if (nestedModel->Name() == relativeTo)
+    {
+      errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE,
+          "relative_to name[" + relativeTo +
+          "] is identical to nested model name[" + nestedModel->Name() +
+          "], causing a graph cycle "
+          "in model with name[" + _model->Name() + "]."});
+    }
+    _out.graph.AddEdge({relativeToId, nestedModelId}, nestedModel->RawPose());
+  }
+
   return errors;
 }
 
@@ -916,6 +992,22 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in)
                 "in MODEL attached_to graph."});
           }
           break;
+        case sdf::FrameType::MODEL:
+          if ("__model__" != vertexPair.second.get().Name())
+          {
+            if (outDegree != 0)
+            {
+              errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
+                  "FrameAttachedToGraph error, "
+                  "nested MODEL vertex with name [" +
+                  vertexPair.second.get().Name() +
+                  "] should have no outgoing edges "
+                  "in MODEL attached_to graph."});
+            }
+            break;
+          }
+          // fall through to default case for __model__
+          [[fallthrough]];
         default:
           if (outDegree == 0)
           {
@@ -1065,22 +1157,26 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in)
               "should not have type WORLD in MODEL relative_to graph."});
           break;
         case sdf::FrameType::MODEL:
-          if (inDegree != 0)
+          if ("__model__" == vertexPair.second.get().Name())
           {
-            errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
-                "PoseRelativeToGraph error, "
-                "MODEL vertex with name [" +
-                vertexPair.second.get().Name() +
-                "] should have no incoming edges "
-                "in MODEL relative_to graph."});
+            if (inDegree != 0)
+            {
+              errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
+                  "PoseRelativeToGraph error, "
+                  "MODEL vertex with name [__model__"
+                  "] should have no incoming edges "
+                  "in MODEL relative_to graph."});
+            }
+            break;
           }
-          break;
+          // fall through to default case for nested models
+          [[fallthrough]];
         default:
           if (inDegree == 0)
           {
             errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
                 "PoseRelativeToGraph error, "
-                "Non-MODEL vertex with name [" +
+                "Vertex with name [" +
                 vertexPair.second.get().Name() +
                 "] is disconnected; it should have 1 incoming edge " +
                 "in MODEL relative_to graph."});
@@ -1206,13 +1302,26 @@ Errors resolveFrameAttachedToBody(
     return errors;
   }
 
-  if (_in.scopeName == "__model__" && sinkVertex.Data() != FrameType::LINK)
+  if (_in.scopeName == "__model__")
   {
-    errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
-        "Graph has __model__ scope but sink vertex named [" +
-        sinkVertex.Name() + "] does not have FrameType LINK "
-        "when starting from vertex with name [" + _vertexName + "]."});
-    return errors;
+    if (sinkVertex.Data() == FrameType::MODEL &&
+        sinkVertex.Name() == "__model__")
+    {
+      errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
+          "Graph with __model__ scope has sink vertex named [__model__] "
+          "when starting from vertex with name [" + _vertexName + "], "
+          "which is not permitted."});
+      return errors;
+    }
+    else if (sinkVertex.Data() != FrameType::LINK &&
+             sinkVertex.Data() != FrameType::MODEL)
+    {
+      errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
+          "Graph has __model__ scope but sink vertex named [" +
+          sinkVertex.Name() + "] does not have FrameType LINK OR MODEL "
+          "when starting from vertex with name [" + _vertexName + "]."});
+      return errors;
+    }
   }
 
   _attachedToBody = sinkVertex.Name();

src/Model.cc

@@ -79,8 +79,11 @@ class sdf::ModelPrivate
   /// \brief Pose Relative-To Graph constructed during Load.
   public: std::shared_ptr<sdf::PoseRelativeToGraph> poseGraph;
 
-  /// \brief Pose Relative-To Graph in parent (world) scope.
+  /// \brief Pose Relative-To Graph in parent (world or __model__) scope.
   public: std::weak_ptr<const sdf::PoseRelativeToGraph> parentPoseGraph;
+
+  /// \brief Scope name of parent Pose Relative-To Graph (world or __model__).
+  public: std::string parentPoseGraphScopeName;
 };
 
 /////////////////////////////////////////////////
@@ -115,6 +118,10 @@ Model::Model(const Model &_model)
   {
     link.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
   }
+  for (auto &model : this->dataPtr->models)
+  {
+    model.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
+  }
   for (auto &joint : this->dataPtr->joints)
   {
     joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
@@ -382,6 +389,13 @@ Errors Model::Load(ElementPtr _sdf)
   {
     link.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
   }
+  for (auto &model : this->dataPtr->models)
+  {
+    Errors setPoseRelativeToGraphErrors =
+      model.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
+    errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(),
+                                setPoseRelativeToGraphErrors.end());
+  }
   for (auto &joint : this->dataPtr->joints)
   {
     joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
@@ -675,10 +689,23 @@ void Model::SetPoseRelativeTo(const std::string &_frame)
 }
 
 /////////////////////////////////////////////////
-void Model::SetPoseRelativeToGraph(
+Errors Model::SetPoseRelativeToGraph(
     std::weak_ptr<const PoseRelativeToGraph> _graph)
 {
+  Errors errors;
+
+  auto graph = _graph.lock();
+  if (!graph)
+  {
+    errors.push_back({ErrorCode::ELEMENT_INVALID,
+        "Tried to set PoseRelativeToGraph with invalid pointer."});
+    return errors;
+  }
+
+  this->dataPtr->parentPoseGraphScopeName = graph->sourceName;
   this->dataPtr->parentPoseGraph = _graph;
+
+  return errors;
 }
 
 /////////////////////////////////////////////////
@@ -687,7 +714,7 @@ sdf::SemanticPose Model::SemanticPose() const
   return sdf::SemanticPose(
       this->dataPtr->pose,
       this->dataPtr->poseRelativeTo,
-      "world",
+      this->dataPtr->parentPoseGraphScopeName,
       this->dataPtr->parentPoseGraph);
 }
 

src/World.cc

@@ -372,7 +372,10 @@ Errors World::Load(sdf::ElementPtr _sdf)
   }
   for (auto &model : this->dataPtr->models)
   {
-    model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph);
+    Errors setPoseRelativeToGraphErrors =
+      model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph);
+    errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(),
+                                setPoseRelativeToGraphErrors.end());
   }
   for (auto &light : this->dataPtr->lights)
   {

src/ign_TEST.cc

@@ -275,6 +275,17 @@ TEST(check, SDF)
               std::string::npos) << output;
   }
 
+  // Check an SDF file with an invalid model without links.
+  {
+    std::string path = pathBase +"/model_without_links.sdf";
+
+    // Check model_without_links.sdf
+    std::string output =
+      custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+    EXPECT_NE(output.find("Error: A model must have at least one link."),
+              std::string::npos) << output;
+  }
+
   // Check an SDF file with a nested model.
   {
     std::string path = pathBase +"/nested_model.sdf";
@@ -351,6 +362,17 @@ TEST(check, SDF)
     EXPECT_EQ("Valid.\n", output) << output;
   }
 
+  // Check an SDF file with model frames attached_to a nested model.
+  // This is a valid file.
+  {
+    std::string path = pathBase +"/model_frame_attached_to_nested_model.sdf";
+
+    // Check model_frame_attached_to_nested_model.sdf
+    std::string output =
+      custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+    EXPECT_EQ("Valid.\n", output) << output;
+  }
+
   // Check an SDF file with model frames with invalid attached_to attributes.
   {
     std::string path = pathBase +"/model_frame_invalid_attached_to.sdf";
@@ -443,6 +465,17 @@ TEST(check, SDF)
               std::string::npos) << output;
   }
 
+  // Check an SDF file with nested_models using the relative_to attribute.
+  // This is a valid file.
+  {
+    std::string path = pathBase +"/model_nested_model_relative_to.sdf";
+
+    // Check model_nested_model_relative_to.sdf
+    std::string output =
+      custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+    EXPECT_EQ("Valid.\n", output) << output;
+  }
+
   // Check an SDF file with joints using the relative_to attribute.
   // This is a valid file.
   {

src/parser.cc

@@ -1523,13 +1523,14 @@ bool checkFrameAttachedToNames(const sdf::Root *_root)
         modelResult = false;
       }
       else if (!_model->LinkNameExists(attachedTo) &&
+               !_model->ModelNameExists(attachedTo) &&
                !_model->JointNameExists(attachedTo) &&
                !_model->FrameNameExists(attachedTo))
       {
         std::cerr << "Error: attached_to name[" << attachedTo
                   << "] specified by frame with name[" << frame->Name()
-                  << "] does not match a link, joint, or frame name "
-                  << "in model with name[" << _model->Name()
+                  << "] does not match a nested model, link, joint, "
+                  << "or frame name in model with name[" << _model->Name()
                   << "]."
                   << std::endl;
         modelResult = false;