Pass ChildScope with new scope name

Signed-off-by: Addisu Z. Taddese <[email protected]>

include/sdf/Types.hh

@@ -68,6 +68,9 @@ namespace sdf
   std::vector<std::string> split(const std::string &_str,
                                  const std::string &_splitter);
 
+  SDFORMAT_VISIBLE
+  bool endswith(const std::string &_a, const std::string &_b);
+
   /// \brief Trim leading and trailing whitespace from a string.
   /// \param[in] _in The string to trim.
   /// \return A string containing the trimmed value.

src/FrameSemantics.cc

@@ -33,6 +33,15 @@ namespace sdf
 {
 inline namespace SDF_VERSION_NAMESPACE {
 
+void printGraph(const ScopedGraph<PoseRelativeToGraph> &_graph)
+{
+  std::cout << _graph.Graph() << std::endl;
+}
+void printGraph(const ScopedGraph<FrameAttachedToGraph> &_graph)
+{
+  std::cout << _graph.Graph() << std::endl;
+}
+
 // The following two functions were originally submitted to ign-math,
 // but were not accepted as they were not generic enough.
 // For now, they will be kept here.
@@ -181,6 +190,9 @@ Errors buildFrameAttachedToGraph(
 {
   Errors errors;
 
+  if (_model->Static())
+    return errors;
+
   // add implicit model frame vertex first
   const std::string scopeName = "__model__";
   _out.SetScopeName(scopeName);
@@ -207,6 +219,8 @@ Errors buildFrameAttachedToGraph(
     return errors;
   }
 
+  // TODO (addisu) Handle finding the canonical link in a nested model.
+  // TODO (addisu) Handle models with no links
   // identify canonical link
   const sdf::Link *canonicalLink = nullptr;
   if (_model->CanonicalLinkName().empty())
@@ -361,7 +375,7 @@ Errors buildFrameAttachedToGraph(
     _out.AddEdge({frameId, attachedToId}, edgeData);
   }
 
-  std::cout << "Model FrameAttachedToGraph\n" << _out.Graph() << std::endl;
+  // std::cout << "Model FrameAttachedToGraph\n" << _out.Graph() << std::endl;
   return errors;
 }
 
@@ -476,7 +490,7 @@ Errors buildFrameAttachedToGraph(
     _out.AddEdge({frameId, attachedToId}, edgeData);
   }
 
-  std::cout << "World FrameAttachedToGraph\n" << _out.Graph() << std::endl;
+  // std::cout << "World FrameAttachedToGraph\n" << _out.Graph() << std::endl;
   return errors;
 }
 
@@ -761,7 +775,7 @@ Errors buildPoseRelativeToGraph(
     _out.AddEdge({relativeToId, nestedModelId}, nestedModel->RawPose());
   }
 
-  std::cout << "Model PoseRelativeToGraph\n" << _out.Graph() << std::endl;
+  // std::cout << "Model PoseRelativeToGraph\n" << _out.Graph() << std::endl;
   return errors;
 }
 
@@ -938,7 +952,7 @@ Errors buildPoseRelativeToGraph(
     _out.AddEdge({relativeToId, frameId}, frame->RawPose());
   }
 
-  std::cout << "World PoseRelativeToGraph\n" << _out.Graph() << std::endl;
+  // std::cout << "World PoseRelativeToGraph\n" << _out.Graph() << std::endl;
   return errors;
 }
 
@@ -1026,22 +1040,6 @@ Errors validateFrameAttachedToGraph(const ScopedGraph<FrameAttachedToGraph> &_in
                 "in MODEL attached_to graph."});
           }
           break;
-        case sdf::FrameType::MODEL:
-          if (_in.ScopeVertexId() != vertexPair.second.get().Id())
-          {
-            if (outDegree != 0)
-            {
-              errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
-                  "FrameAttachedToGraph error, "
-                  "nested MODEL vertex with name [" +
-                  vertexName +
-                  "] should have no outgoing edges "
-                  "in MODEL attached_to graph."});
-            }
-            break;
-          }
-          // fall through to default case for __model__
-          [[fallthrough]];
         default:
           if (outDegree == 0)
           {
@@ -1075,7 +1073,7 @@ Errors validateFrameAttachedToGraph(const ScopedGraph<FrameAttachedToGraph> &_in
           {
             errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
                 "FrameAttachedToGraph error, "
-                "MODEL and WORLD vertices should have no outgoing edges "
+                "WORLD vertices should have no outgoing edges "
                 "in WORLD attached_to graph."});
           }
           break;
@@ -1206,7 +1204,7 @@ Errors validatePoseRelativeToGraph(
         case sdf::FrameType::MODEL:
           // TODO: What we have to check here is that if this is the scope
           // vertex any incoming edge is from outside this scope.
-          if (_in.ScopeVertexId() == vertexPair.second.get().Id())
+          if (sdf::endswith(vertexName, "__model__"))
           {
             if (inDegree != 0)
             {
@@ -1283,12 +1281,12 @@ Errors validatePoseRelativeToGraph(
   }
 
   // check graph for cycles by resolving pose of each vertex relative to root
-  // for (auto const &name : _in.VertexNames())
-  // {
-  //   ignition::math::Pose3d pose;
-  //   Errors e = resolvePoseRelativeToRoot(pose, _in, name);
-  //   errors.insert(errors.end(), e.begin(), e.end());
-  // }
+  for (auto const &name : _in.VertexNames())
+  {
+    ignition::math::Pose3d pose;
+    Errors e = resolvePoseRelativeToRoot(pose, _in, name);
+    errors.insert(errors.end(), e.begin(), e.end());
+  }
 
   return errors;
 }
@@ -1340,7 +1338,7 @@ Errors resolveFrameAttachedToBody(
   {
     errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
         "Graph has world scope but sink vertex named [" +
-        sinkVertex.Name() + "] does not have FrameType WORLD or MODEL "
+        sinkVertex.Name() + "] does not have FrameType WORLD or LINK "
         "when starting from vertex with name [" + _vertexName + "]."});
     return errors;
   }

src/ScopedGraph.hh

@@ -62,6 +62,8 @@ class ScopedGraph
 
   public: ScopedGraph() = default;
   public: ScopedGraph(const std::shared_ptr<T> _graph);
+  public: ScopedGraph<T> ChildScope(const std::string &_name, const
+    std::string &_scopeName);
   public: explicit operator bool() const;
 
   public: const MathGraphType &Graph() const;
@@ -106,6 +108,18 @@ ScopedGraph<T>::ScopedGraph(const std::shared_ptr<T> _graph): graphWeak(_graph)
 {
 }
 
+/////////////////////////////////////////////////
+template <typename T>
+ScopedGraph<T> ScopedGraph<T>::ChildScope(
+    const std::string &_name, const std::string &_scopeName)
+{
+  auto newScopedGraph = *this;
+  newScopedGraph.scopeVertexId = newScopedGraph.VertexIdByName(_name);
+  newScopedGraph.prefix = this->AddPrefix(_name);
+  newScopedGraph.scopeName = _scopeName;
+  return newScopedGraph;
+}
+
 /////////////////////////////////////////////////
 template <typename T>
 ScopedGraph<T>::operator bool() const

src/Types.cc

@@ -70,6 +70,13 @@ std::string trim(const std::string &_in)
   return _in.substr(strBegin, strRange);
 }
 
+//////////////////////////////////////////////////
+bool endswith(const std::string& _a, const std::string& _b)
+{
+  return (_a.size() >= _b.size()) &&
+      (_a.compare(_a.size() - _b.size(), _b.size(), _b) == 0);
+}
+
 /////////////////////////////////////////////////
 std::string lowercase(const std::string &_in)
 {

src/World.cc

@@ -281,14 +281,9 @@ Errors World::Load(sdf::ElementPtr _sdf)
   // name collisions
   std::unordered_set<std::string> frameNames;
 
-  std::function <void(Model &)> beforeLoad = [this](Model &_model)
-  {
-    _model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph);
-    _model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph);
-  };
   // Load all the models.
-  Errors modelLoadErrors = loadUniqueRepeated<Model>(_sdf, "model",
-      this->dataPtr->models, beforeLoad);
+  Errors modelLoadErrors =
+      loadUniqueRepeated<Model>(_sdf, "model", this->dataPtr->models);
   errors.insert(errors.end(), modelLoadErrors.begin(), modelLoadErrors.end());
 
   // Models are loaded first, and loadUniqueRepeated ensures there are no
@@ -388,6 +383,16 @@ Errors World::Load(sdf::ElementPtr _sdf)
   {
     frame.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph);
   }
+  for (auto &model : this->dataPtr->models)
+  {
+    Errors setPoseRelativeToGraphErrors =
+      model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph);
+    errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(),
+                                setPoseRelativeToGraphErrors.end());
+    errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(),
+                                setPoseRelativeToGraphErrors.end());
+    model.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph);
+  }
   for (auto &light : this->dataPtr->lights)
   {
     light.SetXmlParentName("world");

src/ign_TEST.cc

@@ -223,8 +223,8 @@ TEST(check, SDF)
     // Check joint_invalid_self_child.sdf
     std::string output =
       custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
-    EXPECT_NE(output.find("Error: FrameAttachedToGraph cycle detected, "
-                          "already visited vertex [self]."),
+    EXPECT_NE(output.find("Error: FrameAttachedToGraph cycle detected, already "
+                          "visited vertex [joint_invalid_self_child::self]."),
               std::string::npos) << output;
   }
 
@@ -460,11 +460,13 @@ TEST(check, SDF)
     // Check model_frame_invalid_attached_to_cycle.sdf
     std::string output =
       custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
-    EXPECT_NE(std::string::npos, output.find(
-      "FrameAttachedToGraph cycle detected, already visited vertex [F1]."))
+    EXPECT_NE(std::string::npos,
+        output.find("FrameAttachedToGraph cycle detected, already visited "
+                    "vertex [model_frame_invalid_attached_to_cycle::F1]."))
         << output;
-    EXPECT_NE(std::string::npos, output.find(
-      "FrameAttachedToGraph cycle detected, already visited vertex [F2]."))
+    EXPECT_NE(std::string::npos,
+        output.find("FrameAttachedToGraph cycle detected, already visited "
+                    "vertex [model_frame_invalid_attached_to_cycle::F2]."))
         << output;
   }
 
@@ -540,17 +542,17 @@ TEST(check, SDF)
 
   // Check an invalid SDF file with a joint that specifies a child link
   // within a sibling nested model using the unsupported :: syntax.
-  {
-    std::string path = pathBase +"/model_invalid_nested_joint_child.sdf";
-
-    // Check model_invalid_nested_joint_child.sdf
-    std::string output =
-      custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
-    EXPECT_NE(output.find("Error: Child frame with name[M::C] specified by "
-                          "joint with name[J] not found in model with "
-                          "name[model_invalid_nested_joint_child]."),
-              std::string::npos) << output;
-  }
+  // {
+  //   std::string path = pathBase +"/model_invalid_nested_joint_child.sdf";
+
+  //   // Check model_invalid_nested_joint_child.sdf
+  //   std::string output =
+  //     custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+  //   EXPECT_NE(output.find("Error: Child frame with name[M::C] specified by "
+  //                         "joint with name[J] not found in model with "
+  //                         "name[model_invalid_nested_joint_child]."),
+  //             std::string::npos) << output;
+  // }
 
   // Check an SDF file with joints using the relative_to attribute.
   // This is a valid file.
@@ -628,11 +630,13 @@ TEST(check, SDF)
     // Check model_invalid_frame_relative_to_cycle.sdf
     std::string output =
       custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
-    EXPECT_NE(std::string::npos, output.find(
-      "PoseRelativeToGraph cycle detected, already visited vertex [F1]."))
+    EXPECT_NE(std::string::npos,
+        output.find("PoseRelativeToGraph cycle detected, already visited "
+                    "vertex [model_invalid_frame_relative_to_cycle::F1]."))
         << output;
-    EXPECT_NE(std::string::npos, output.find(
-      "PoseRelativeToGraph cycle detected, already visited vertex [F2]."))
+    EXPECT_NE(std::string::npos,
+        output.find("PoseRelativeToGraph cycle detected, already visited "
+                    "vertex [model_invalid_frame_relative_to_cycle::F2]."))
         << output;
   }
 

test/integration/frame.cc

@@ -587,7 +587,8 @@ TEST(DOMFrame, LoadModelFramesAttachedToNestedModel)
 
   // Load the SDF file
   sdf::Root root;
-  EXPECT_TRUE(root.Load(testFile).empty());
+  sdf::Errors errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty()) << errors;
 
   // Get the first model
   const sdf::Model *model = root.ModelByIndex(0);
@@ -622,9 +623,9 @@ TEST(DOMFrame, LoadModelFramesAttachedToNestedModel)
 
   std::string body;
   EXPECT_TRUE(model->FrameByName("F1")->ResolveAttachedToBody(body).empty());
-  EXPECT_EQ("nested_model", body);
+  EXPECT_EQ("nested_model::nested_link", body);
   EXPECT_TRUE(model->FrameByName("F2")->ResolveAttachedToBody(body).empty());
-  EXPECT_EQ("nested_model", body);
+  EXPECT_EQ("nested_model::nested_link", body);
 }
 
 /////////////////////////////////////////////////
@@ -691,7 +692,7 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo)
   EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty());
   EXPECT_EQ("world", body);
   EXPECT_TRUE(world->FrameByName("F2")->ResolveAttachedToBody(body).empty());
-  EXPECT_EQ("M1", body);
+  EXPECT_EQ("M1::L", body);
 }
 
 /////////////////////////////////////////////////
@@ -1109,7 +1110,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidRelativeTo)
   for (auto e : errors)
     std::cout << e << std::endl;
   EXPECT_FALSE(errors.empty());
-  EXPECT_EQ(11u, errors.size());
+  EXPECT_EQ(15u, errors.size());
   EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_INVALID);
   EXPECT_NE(std::string::npos,
     errors[0].Message().find(
@@ -1175,7 +1176,7 @@ TEST(DOMFrame, WorldIncludeModel)
     ASSERT_NE(nullptr, model);
     ignition::math::Pose3d modelPose;
     sdf::Errors resolveErrors = model->SemanticPose().Resolve(modelPose);
-    EXPECT_TRUE(resolveErrors.empty());
+    EXPECT_TRUE(resolveErrors.empty()) << resolveErrors;
     EXPECT_EQ(expectedPoses[i], modelPose);
   }
 }

test/integration/model_dom.cc

@@ -69,7 +69,7 @@ TEST(DOMModel, NoLinks)
   sdf::Root root;
   auto errors = root.Load(testFile);
   EXPECT_FALSE(errors.empty());
-  ASSERT_EQ(4u, errors.size());
+  ASSERT_EQ(3u, errors.size());
   EXPECT_EQ(sdf::ErrorCode::MODEL_WITHOUT_LINK, errors[0].Code());
   EXPECT_TRUE(errors[0].Message().find("model must have at least one link") !=
                std::string::npos);

test/integration/nested_model.cc

@@ -859,7 +859,7 @@ class PlacementFrame: public ::testing::Test
     {
       std::cout << this->root.Element()->ToString("") << std::endl;
     }
-    EXPECT_TRUE(errors.empty());
+    EXPECT_TRUE(errors.empty()) << errors;
 
     this->world = this->root.WorldByIndex(0);
     ASSERT_NE(nullptr, world);

test/integration/root_dom.cc

@@ -61,7 +61,8 @@ TEST(DOMRoot, Load)
 
   sdf::Root root;
   EXPECT_EQ(0u, root.WorldCount());
-  EXPECT_TRUE(root.Load(testFile).empty());
+  sdf::Errors errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty()) << errors;
   EXPECT_EQ(SDF_PROTOCOL_VERSION, root.Version());
   EXPECT_EQ(1u, root.WorldCount());
   EXPECT_TRUE(root.WorldByIndex(0) != nullptr);
@@ -83,7 +84,8 @@ TEST(DOMRoot, LoadMultipleModels)
         "root_multiple_models.sdf");
 
   sdf::Root root;
-  EXPECT_TRUE(root.Load(testFile).empty());
+  sdf::Errors errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty()) << errors;
   EXPECT_EQ(3u, root.ModelCount());
 
   EXPECT_EQ("robot1", root.ModelByIndex(0)->Name());