From f0001e81483bfe1c4172942d6ef74161fa6561fc Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 25 Jun 2021 00:24:31 -0700 Subject: [PATCH 1/4] Add World::ValidateGraphs method The graph data is private, so expose a method for checking that the graphs are valid. Signed-off-by: Steve Peters --- include/sdf/World.hh | 6 ++++++ src/FrameSemantics.cc | 16 ++++++++++++++++ src/World.cc | 11 +++++++++++ src/World_TEST.cc | 11 +++++++++++ test/integration/world_dom.cc | 2 ++ 5 files changed, 46 insertions(+) diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 82fa00c97..183e83e2e 100644 --- a/include/sdf/World.hh +++ b/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; diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 5b290e7af..1659257e4 100644 --- a/src/FrameSemantics.cc +++ b/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") diff --git a/src/World.cc b/src/World.cc index bd0822076..d3041cb49 100644 --- a/src/World.cc +++ b/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 { diff --git a/src/World_TEST.cc b/src/World_TEST.cc index 987eed93e..f36e513e3 100644 --- a/src/World_TEST.cc +++ b/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")); } ///////////////////////////////////////////////// diff --git a/test/integration/world_dom.cc b/test/integration/world_dom.cc index c119aa246..1e1aa1ab7 100644 --- a/test/integration/world_dom.cc +++ b/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); From acff4da51cb51c1e381efd93792bbabe093c21bd Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 25 Jun 2021 01:13:27 -0700 Subject: [PATCH 2/4] fix typo Signed-off-by: Steve Peters --- src/World_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/World_TEST.cc b/src/World_TEST.cc index f36e513e3..f48e0eca2 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -61,7 +61,7 @@ TEST(DOMWorld, Construction) EXPECT_EQ(1u, world.PhysicsCount()); auto errors = world.ValidateGraphs(); - EXPECT_EQ(2u, errors.size()) >> errors; + 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( From ffc7086571f84c8c2e019f9db7eb13f46dc3b8bc Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 28 Jun 2021 10:41:30 -0700 Subject: [PATCH 3/4] Add to test showing graph errors Load an sdf::World instead of an sdf::Root to see the graph errors. Signed-off-by: Steve Peters --- test/integration/frame.cc | 39 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 316f27ff3..94b42f646 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -718,6 +718,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 +729,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")); } ///////////////////////////////////////////////// From b32275c0501eef089fa7051ebcbf2a7fc1cc6485 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Wed, 30 Jun 2021 06:01:37 -0700 Subject: [PATCH 4/4] Add Model::ValidateGraphs as well Signed-off-by: Steven Peters --- include/sdf/Model.hh | 6 ++++++ src/FrameSemantics.cc | 2 +- src/Model.cc | 11 +++++++++++ src/Model_TEST.cc | 13 +++++++++++++ test/integration/frame.cc | 34 +++++++++++++++++++++++++++++++++- test/integration/model_dom.cc | 3 +++ 6 files changed, 67 insertions(+), 2 deletions(-) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index f9f3f83af..aa85a7b3e 100644 --- a/include/sdf/Model.hh +++ b/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. diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 1659257e4..496111f18 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -2105,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; } diff --git a/src/Model.cc b/src/Model.cc index 8d0bbe1e2..1aa981069 100644 --- a/src/Model.cc +++ b/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 { diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 837f100ba..06cc2e881 100644 --- a/src/Model_TEST.cc +++ b/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]; } ///////////////////////////////////////////////// diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 94b42f646..c5ae76113 100644 --- a/test/integration/frame.cc +++ b/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")); } ///////////////////////////////////////////////// diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc index e8931eb0c..ff46d7ec6 100644 --- a/test/integration/model_dom.cc +++ b/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()); } /////////////////////////////////////////////////