Add World::ValidateGraphs method

include/sdf/Model.hh

@@ -67,6 +67,12 @@ namespace sdf
     /// an error code and message. An empty vector indicates no error.
     public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config);
 
+    /// \brief Check that the FrameAttachedToGraph and PoseRelativeToGraph
+    /// are valid.
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error.
+    public: Errors ValidateGraphs() const;
+
     /// \brief Get the name of the model.
     /// The name of the model should be unique within the scope of a World.
     /// \return Name of the model.

include/sdf/World.hh

@@ -70,6 +70,12 @@ namespace sdf
     /// an error code and message. An empty vector indicates no error.
     public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config);
 
+    /// \brief Check that the FrameAttachedToGraph and PoseRelativeToGraph
+    /// are valid.
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error.
+    public: Errors ValidateGraphs() const;
+
     /// \brief Get the name of the world.
     /// \return Name of the world.
     public: std::string Name() const;

src/FrameSemantics.cc

@@ -1611,6 +1611,14 @@ Errors validateFrameAttachedToGraph(
 {
   Errors errors;
 
+  // Check if scope points to a valid graph
+  if (!_in)
+  {
+    errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
+        "FrameAttachedToGraph error: scope does not point to a valid graph."});
+    return errors;
+  }
+
   // Expect ScopeContextName to be either "__model__" or "world"
   if (_in.ScopeContextName() != "__model__" &&
       _in.ScopeContextName() != "world")
@@ -1837,6 +1845,14 @@ Errors validatePoseRelativeToGraph(
 {
   Errors errors;
 
+  // Check if scope points to a valid graph
+  if (!_in)
+  {
+    errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
+        "PoseRelativeToGraph error: scope does not point to a valid graph."});
+    return errors;
+  }
+
   // Expect scopeContextName to be either "__model__" or "world"
   if (_in.ScopeContextName() != "__model__" &&
       _in.ScopeContextName() != "world")
@@ -2089,7 +2105,7 @@ Errors resolveFrameAttachedToBody(
     {
       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 STATIC_MODEL "
+          sinkVertex.Name() + "] does not have FrameType LINK or STATIC_MODEL "
           "when starting from vertex with name [" + _vertexName + "]."});
       return errors;
     }

src/Model.cc

@@ -326,6 +326,17 @@ Errors Model::Load(sdf::ElementPtr _sdf, const ParserConfig &_config)
   return errors;
 }
 
+/////////////////////////////////////////////////
+Errors Model::ValidateGraphs() const
+{
+  Errors errors =
+      validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph);
+  Errors poseErrors =
+      validatePoseRelativeToGraph(this->dataPtr->poseGraph);
+  errors.insert(errors.end(), poseErrors.begin(), poseErrors.end());
+  return errors;
+}
+
 /////////////////////////////////////////////////
 std::string Model::Name() const
 {

src/Model_TEST.cc

@@ -138,6 +138,19 @@ TEST(DOMModel, Construction)
     // expect errors when trying to resolve pose
     EXPECT_FALSE(semanticPose.Resolve(pose).empty());
   }
+
+  auto errors = model.ValidateGraphs();
+  EXPECT_EQ(2u, errors.size());
+  EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR);
+  EXPECT_NE(std::string::npos,
+    errors[0].Message().find(
+      "FrameAttachedToGraph error: scope does not point to a valid graph"))
+      << errors[0];
+  EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR);
+  EXPECT_NE(std::string::npos,
+    errors[1].Message().find(
+      "PoseRelativeToGraph error: scope does not point to a valid graph"))
+      << errors[1];
 }
 
 /////////////////////////////////////////////////

src/World.cc

@@ -279,6 +279,17 @@ Errors World::Load(sdf::ElementPtr _sdf, const ParserConfig &_config)
   return errors;
 }
 
+/////////////////////////////////////////////////
+Errors World::ValidateGraphs() const
+{
+  Errors errors =
+      validateFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph);
+  Errors poseErrors =
+      validatePoseRelativeToGraph(this->dataPtr->poseRelativeToGraph);
+  errors.insert(errors.end(), poseErrors.begin(), poseErrors.end());
+  return errors;
+}
+
 /////////////////////////////////////////////////
 std::string World::Name() const
 {

src/World_TEST.cc

@@ -59,6 +59,17 @@ TEST(DOMWorld, Construction)
   EXPECT_FALSE(world.FrameNameExists("default"));
 
   EXPECT_EQ(1u, world.PhysicsCount());
+
+  auto errors = world.ValidateGraphs();
+  EXPECT_EQ(2u, errors.size()) << errors;
+  EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR);
+  EXPECT_NE(std::string::npos,
+    errors[0].Message().find(
+      "FrameAttachedToGraph error: scope does not point to a valid graph"));
+  EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR);
+  EXPECT_NE(std::string::npos,
+    errors[1].Message().find(
+      "PoseRelativeToGraph error: scope does not point to a valid graph"));
 }
 
 /////////////////////////////////////////////////

test/integration/frame.cc

@@ -507,7 +507,39 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo)
       "causing a graph cycle"));
   // errors[7]
   // errors[8]
-  // errors[9]
+  EXPECT_EQ(errors[9].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE);
+  EXPECT_NE(std::string::npos,
+    errors[9].Message().find(
+      "PoseRelativeToGraph cycle detected, "
+      "already visited vertex [model_frame_invalid_attached_to::F4]"));
+
+  // Try loading sdf::Model object
+  sdf::SDFPtr sdfParsed(new sdf::SDF());
+  sdf::init(sdfParsed);
+  ASSERT_TRUE(sdf::readFile(testFile, sdfParsed));
+
+  auto rootElem = sdfParsed->Root();
+  ASSERT_NE(nullptr, rootElem);
+  ASSERT_TRUE(rootElem->HasElement("model"));
+  auto modelElem = rootElem->GetElement("model");
+  ASSERT_NE(nullptr, modelElem);
+
+  // there are no errors when loading an sdf::Model directly since it doesn't
+  // build graphs
+  sdf::Model model;
+  auto modelLoadErrors = model.Load(modelElem);
+  EXPECT_EQ(0u, modelLoadErrors.size()) << modelLoadErrors;
+
+  errors = model.ValidateGraphs();
+  ASSERT_EQ(2u, errors.size()) << errors;
+  EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR);
+  EXPECT_NE(std::string::npos,
+    errors[0].Message().find(
+      "FrameAttachedToGraph error: scope does not point to a valid graph"));
+  EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR);
+  EXPECT_NE(std::string::npos,
+    errors[1].Message().find(
+      "PoseRelativeToGraph error: scope does not point to a valid graph"));
 }
 
 /////////////////////////////////////////////////
@@ -718,6 +750,7 @@ TEST(DOMFrame, LoadWorldFramesInvalidAttachedTo)
       "causing a graph cycle"));
   // errors[2]
   // errors[3]
+  // errors[4]
   EXPECT_EQ(errors[5].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_INVALID);
   EXPECT_NE(std::string::npos,
     errors[5].Message().find(
@@ -728,9 +761,45 @@ TEST(DOMFrame, LoadWorldFramesInvalidAttachedTo)
     errors[6].Message().find(
       "relative_to name[self_cycle] is identical to frame name[self_cycle], "
       "causing a graph cycle"));
-  // errors[6]
   // errors[7]
   // errors[8]
+  EXPECT_EQ(errors[9].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE);
+  EXPECT_NE(std::string::npos,
+    errors[9].Message().find(
+      "PoseRelativeToGraph cycle detected, "
+      "already visited vertex [self_cycle]"));
+  EXPECT_EQ(errors[10].Code(), sdf::ErrorCode::ELEMENT_INVALID);
+  EXPECT_NE(std::string::npos,
+    errors[10].Message().find(
+      "Failed to load a world"));
+
+  // Try loading sdf::World object
+  sdf::SDFPtr sdfParsed(new sdf::SDF());
+  sdf::init(sdfParsed);
+  ASSERT_TRUE(sdf::readFile(testFile, sdfParsed));
+
+  auto rootElem = sdfParsed->Root();
+  ASSERT_NE(nullptr, rootElem);
+  ASSERT_TRUE(rootElem->HasElement("world"));
+  auto worldElem = rootElem->GetElement("world");
+  ASSERT_NE(nullptr, worldElem);
+
+  // there are no errors when loading an sdf::World directly since it doesn't
+  // build graphs
+  sdf::World world;
+  auto worldLoadErrors = world.Load(worldElem);
+  EXPECT_EQ(0u, worldLoadErrors.size()) << worldLoadErrors;
+
+  errors = world.ValidateGraphs();
+  ASSERT_EQ(2u, errors.size()) << errors;
+  EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR);
+  EXPECT_NE(std::string::npos,
+    errors[0].Message().find(
+      "FrameAttachedToGraph error: scope does not point to a valid graph"));
+  EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR);
+  EXPECT_NE(std::string::npos,
+    errors[1].Message().find(
+      "PoseRelativeToGraph error: scope does not point to a valid graph"));
 }
 
 /////////////////////////////////////////////////

test/integration/model_dom.cc

@@ -140,6 +140,9 @@ TEST(DOMRoot, LoadDoublePendulum)
 
   EXPECT_TRUE(model->JointNameExists("upper_joint"));
   EXPECT_TRUE(model->JointNameExists("lower_joint"));
+
+  auto graphErrors = model->ValidateGraphs();
+  EXPECT_EQ(0u, graphErrors.size());
 }
 
 /////////////////////////////////////////////////

test/integration/world_dom.cc

@@ -97,6 +97,8 @@ TEST(DOMWorld, Load)
   EXPECT_EQ(world->Gravity(), ignition::math::Vector3d(1, 2, 3));
   EXPECT_EQ(world->MagneticField(), ignition::math::Vector3d(-1, 0.5, 10));
   EXPECT_EQ(testFile, world->Element()->FilePath());
+  auto graphErrors = world->ValidateGraphs();
+  EXPECT_EQ(0u, graphErrors.size());
 
   const sdf::Atmosphere *atmosphere = world->Atmosphere();
   ASSERT_NE(nullptr, atmosphere);