From 4e86c3edccd28dde7fdaf9613a31f703239ba46e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 15 Oct 2022 01:57:59 -0700 Subject: [PATCH 1/4] Add test for //world/joint parent / child Test that parent and child frames do not resolve to the same link for //world/joint. Signed-off-by: Steve Peters --- test/integration/joint_dom.cc | 21 +++++++++++++++ ..._invalid_resolved_parent_same_as_child.sdf | 27 +++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index 0587457b6..e8b2bb36a 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -609,6 +609,27 @@ TEST(DOMJoint, WorldJointInvalidChildWorld) "Joint with name[J2] specified invalid child link [world]")); } +///////////////////////////////////////////////// +TEST(DOMJoint, WorldJointInvalidResolvedParentSameAsChild) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", + "world_joint_invalid_resolved_parent_same_as_child.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + std::cerr << errors << std::endl; + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_PARENT_SAME_AS_CHILD); + EXPECT_NE(std::string::npos, + errors[0].Message().find( + "joint with name[J] in world with name[" + "joint_invalid_resolved_parent_same_as_child.sdf] specified parent " + "frame [child_model] and child frame [child_frame] that both resolve " + "to [child_model::L]")); +} + ///////////////////////////////////////////////// TEST(DOMJoint, LoadJointPoseRelativeTo) { diff --git a/test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf b/test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf new file mode 100644 index 000000000..aabdd0ad4 --- /dev/null +++ b/test/sdf/world_joint_invalid_resolved_parent_same_as_child.sdf @@ -0,0 +1,27 @@ + + + + + + 0 0 1 0 0 0 + + + + 0 0 10 0 0 0 + + + + 0 1 0 0 0 0 + + + 10 0 0 0 0 0 + child_model + child_frame + + + From c9e9558044aab70f8b5fdb68f25cc361ea8ca834 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 15 Oct 2022 02:22:39 -0700 Subject: [PATCH 2/4] Check World joints in checkJointParentChildNames This refactors the checkModelJointParentChildNames lambda function into a template function that works for both sdf::Model and sdf::World objects, after adding the World::NameExistsInFrameAttachedToGraph API. Signed-off-by: Steve Peters --- include/sdf/World.hh | 12 +++ python/src/sdf/pyModel.cc | 2 +- python/src/sdf/pyWorld.cc | 4 + src/World.cc | 10 +++ src/parser.cc | 138 ++++++++++++++++++---------------- test/integration/joint_dom.cc | 5 +- 6 files changed, 102 insertions(+), 69 deletions(-) diff --git a/include/sdf/World.hh b/include/sdf/World.hh index d99dc4992..0afc85472 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -487,6 +487,18 @@ namespace sdf public: sdf::ElementPtr ToElement( const OutputConfig &_config = OutputConfig::GlobalConfig()) const; + /// \brief Check if a given name exists in the FrameAttachedTo graph at the + /// scope of the world. + /// \param[in] _name Name of the implicit or explicit frame to check. + /// To check for a frame in a nested model, prefix the frame name with + /// the sequence of nested models containing this frame, delimited by "::". + /// \return True if the frame name is found in the FrameAttachedTo graph. + /// False otherwise, or if the frame graph is invalid. + /// \note This function assumes the world has a valid FrameAttachedTo graph. + /// It will return false if the graph is invalid. + public: bool NameExistsInFrameAttachedToGraph( + const std::string &_name) const; + /// \brief Get the plugins attached to this object. /// \return A vector of Plugin, which will be empty if there are no /// plugins. diff --git a/python/src/sdf/pyModel.cc b/python/src/sdf/pyModel.cc index e38d3b17c..fce94d0b9 100644 --- a/python/src/sdf/pyModel.cc +++ b/python/src/sdf/pyModel.cc @@ -175,7 +175,7 @@ void defineModel(pybind11::object module) .def("name_exists_in_frame_attached_to_graph", &sdf::Model::NameExistsInFrameAttachedToGraph, "Check if a given name exists in the FrameAttachedTo graph at the " - "scope of the model..") + "scope of the model.") .def("add_link", &sdf::Model::AddLink, "Add a link to the model.") .def("add_joint", &sdf::Model::AddJoint, diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index bc2619877..41b930fc4 100644 --- a/python/src/sdf/pyWorld.cc +++ b/python/src/sdf/pyWorld.cc @@ -107,6 +107,10 @@ void defineWorld(pybind11::object module) "index.") .def("model_name_exists", &sdf::World::ModelNameExists, "Get whether a model name exists.") + .def("name_exists_in_frame_attached_to_graph", + &sdf::World::NameExistsInFrameAttachedToGraph, + "Check if a given name exists in the FrameAttachedTo graph at the " + "scope of the world.") .def("add_model", &sdf::World::AddModel, "Add a model to the world.") // .def("add_actor", &sdf::World::AddActor, diff --git a/src/World.cc b/src/World.cc index 05dccde19..aacb571c9 100644 --- a/src/World.cc +++ b/src/World.cc @@ -1091,6 +1091,16 @@ void World::ClearFrames() this->dataPtr->frames.clear(); } +///////////////////////////////////////////////// +bool World::NameExistsInFrameAttachedToGraph(const std::string &_name) const +{ + if (!this->dataPtr->frameAttachedToGraph) + return false; + + return this->dataPtr->frameAttachedToGraph.VertexIdByName(_name) + != gz::math::graph::kNullId; +} + ///////////////////////////////////////////////// bool World::AddModel(const Model &_model) { diff --git a/src/parser.cc b/src/parser.cc index f5cd07786..218e9239e 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -2502,87 +2502,92 @@ bool checkJointParentChildNames(const sdf::Root *_root) } ////////////////////////////////////////////////// -void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) +template +void checkScopedJointParentChildNames( + const TPtr _scope, const std::string &_scopeType, Errors &errors) { - auto checkModelJointParentChildNames = []( - const sdf::Model *_model, Errors &errors) -> void + for (uint64_t j = 0; j < _scope->JointCount(); ++j) { - for (uint64_t j = 0; j < _model->JointCount(); ++j) - { - auto joint = _model->JointByIndex(j); + auto joint = _scope->JointByIndex(j); - const std::string &parentName = joint->ParentName(); - const std::string parentLocalName = sdf::SplitName(parentName).second; + const std::string &parentName = joint->ParentName(); + const std::string parentLocalName = sdf::SplitName(parentName).second; - if (parentName != "world" && parentLocalName != "__model__" && - !_model->NameExistsInFrameAttachedToGraph(parentName)) - { - errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, - "parent frame with name[" + parentName + - "] specified by joint with name[" + joint->Name() + - "] not found in model with name[" + _model->Name() + "]."}); - } + if (parentName != "world" && parentLocalName != "__model__" && + !_scope->NameExistsInFrameAttachedToGraph(parentName)) + { + errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, + "parent frame with name[" + parentName + + "] specified by joint with name[" + joint->Name() + + "] not found in " + _scopeType + " with name[" + + _scope->Name() + "]."}); + } - const std::string &childName = joint->ChildName(); - const std::string childLocalName = sdf::SplitName(childName).second; - if (childName == "world") - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "invalid child name[world] specified by joint with name[" + - joint->Name() + "] in model with name[" + _model->Name() + "]."}); - } + const std::string &childName = joint->ChildName(); + const std::string childLocalName = sdf::SplitName(childName).second; + if (childName == "world") + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "invalid child name[world] specified by joint with name[" + + joint->Name() + "] in " + _scopeType + " with name[" + + _scope->Name() + "]."}); + } - if (childLocalName != "__model__" && - !_model->NameExistsInFrameAttachedToGraph(childName)) - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "child frame with name[" + childName + - "] specified by joint with name[" + joint->Name() + - "] not found in model with name[" + _model->Name() + "]."}); - } + if (childLocalName != "__model__" && + !_scope->NameExistsInFrameAttachedToGraph(childName)) + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "child frame with name[" + childName + + "] specified by joint with name[" + joint->Name() + + "] not found in " + _scopeType + " with name[" + + _scope->Name() + "]."}); + } - if (childName == joint->Name()) - { - errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, - "joint with name[" + joint->Name() + - "] in model with name[" + _model->Name() + - "] must not specify its own name as the child frame."}); - } + if (childName == joint->Name()) + { + errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " with name[" + _scope->Name() + + "] must not specify its own name as the child frame."}); + } - if (parentName == joint->Name()) - { - errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, - "joint with name[" + joint->Name() + - "] in model with name[" + _model->Name() + - "] must not specify its own name as the parent frame."}); - } + if (parentName == joint->Name()) + { + errors.push_back({ErrorCode::JOINT_PARENT_LINK_INVALID, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " with name[" + _scope->Name() + + "] must not specify its own name as the parent frame."}); + } - // Check that parent and child frames resolve to different links - std::string resolvedChildName; - std::string resolvedParentName; + // Check that parent and child frames resolve to different links + std::string resolvedChildName; + std::string resolvedParentName; - auto resolveErrors = joint->ResolveChildLink(resolvedChildName); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + auto resolveErrors = joint->ResolveChildLink(resolvedChildName); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - resolveErrors = joint->ResolveParentLink(resolvedParentName); - errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); + resolveErrors = joint->ResolveParentLink(resolvedParentName); + errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end()); - if (resolvedChildName == resolvedParentName) - { - errors.push_back({ErrorCode::JOINT_PARENT_SAME_AS_CHILD, - "joint with name[" + joint->Name() + - "] in model with name[" + _model->Name() + - "] specified parent frame [" + parentName + - "] and child frame [" + childName + - "] that both resolve to [" + resolvedChildName + - "], but they should resolve to different values."}); - } + if (resolvedChildName == resolvedParentName) + { + errors.push_back({ErrorCode::JOINT_PARENT_SAME_AS_CHILD, + "joint with name[" + joint->Name() + + "] in " + _scopeType + " with name[" + _scope->Name() + + "] specified parent frame [" + parentName + + "] and child frame [" + childName + + "] that both resolve to [" + resolvedChildName + + "], but they should resolve to different values."}); } - }; + } +} +////////////////////////////////////////////////// +void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) +{ if (_root->Model()) { - checkModelJointParentChildNames(_root->Model(), _errors); + checkScopedJointParentChildNames(_root->Model(), "model", _errors); } for (uint64_t w = 0; w < _root->WorldCount(); ++w) @@ -2591,8 +2596,9 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors) for (uint64_t m = 0; m < world->ModelCount(); ++m) { auto model = world->ModelByIndex(m); - checkModelJointParentChildNames(model, _errors); + checkScopedJointParentChildNames(model, "model", _errors); } + checkScopedJointParentChildNames(world, "world", _errors); } } diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc index e8b2bb36a..b13b20bf4 100644 --- a/test/integration/joint_dom.cc +++ b/test/integration/joint_dom.cc @@ -487,7 +487,8 @@ TEST(DOMJoint, LoadWorldJointChildFrame) // Load the SDF file sdf::Root root; - EXPECT_TRUE(root.Load(testFile).empty()); + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()) << errors; using Pose = gz::math::Pose3d; @@ -602,7 +603,7 @@ TEST(DOMJoint, WorldJointInvalidChildWorld) auto errors = root.Load(testFile); for (auto e : errors) std::cout << e << std::endl; - ASSERT_EQ(2u, errors.size()); + ASSERT_EQ(3u, errors.size()); EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::JOINT_CHILD_LINK_INVALID); EXPECT_NE(std::string::npos, errors[0].Message().find( From 4de3d603f4d9a3f61b364aa0986bc4075c1991c0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 15 Oct 2022 16:02:08 -0700 Subject: [PATCH 3/4] Improve coverage Signed-off-by: Steve Peters --- python/test/pyModel_TEST.py | 4 ++++ python/test/pyWorld_TEST.py | 4 ++++ src/Model_TEST.cc | 7 +++++++ src/World_TEST.cc | 7 +++++++ 4 files changed, 22 insertions(+) diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py index ce8d7e5b4..2fcb021df 100644 --- a/python/test/pyModel_TEST.py +++ b/python/test/pyModel_TEST.py @@ -145,6 +145,10 @@ def test_default_construction(self): self.assertEqual(errors[1].message(), "PoseRelativeToGraph error: scope does not point to a valid graph.") + # model dosn't have graphs, so no names should exist in graphs + self.assertFalse(model.name_exists_in_frame_attached_to_graph("")); + self.assertFalse(model.name_exists_in_frame_attached_to_graph("link")); + def test_copy_construction(self): model = Model() diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 63fe26a01..73947b0a9 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -86,6 +86,10 @@ def test_default_construction(self): self.assertEqual(errors[1].message(), "PoseRelativeToGraph error: scope does not point to a valid graph.") + # world dosn't have graphs, so no names should exist in graphs + self.assertFalse(world.name_exists_in_frame_attached_to_graph("")); + self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")); + def test_copy_construction(self): world = World() diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 0da219e48..4f1e710cc 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -154,6 +154,10 @@ TEST(DOMModel, Construction) errors[1].Message().find( "PoseRelativeToGraph error: scope does not point to a valid graph")) << errors[1]; + + // model dosn't have graphs, so no names should exist in graphs + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("link")); } ///////////////////////////////////////////////// @@ -229,6 +233,9 @@ TEST(DOMModel, AddLink) EXPECT_EQ(1u, model.LinkCount()); EXPECT_FALSE(model.AddLink(link)); EXPECT_EQ(1u, model.LinkCount()); + // model dosn't have graphs, so no names should exist in graphs + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("link1")); model.ClearLinks(); EXPECT_EQ(0u, model.LinkCount()); diff --git a/src/World_TEST.cc b/src/World_TEST.cc index 88679e4e5..5f912f3c7 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -93,6 +93,10 @@ TEST(DOMWorld, Construction) EXPECT_NE(std::string::npos, errors[1].Message().find( "PoseRelativeToGraph error: scope does not point to a valid graph")); + + // world dosn't have graphs, so no names should exist in graphs + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("model1")); } ///////////////////////////////////////////////// @@ -384,6 +388,9 @@ TEST(DOMWorld, AddModel) EXPECT_EQ(1u, world.ModelCount()); EXPECT_FALSE(world.AddModel(model)); EXPECT_EQ(1u, world.ModelCount()); + // world dosn't have graphs, so no names should exist in graphs + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("")); + EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("model1")); world.ClearModels(); EXPECT_EQ(0u, world.ModelCount()); From d8d8cfd7ccb99554a15d7d4d30ec98d68e5998af Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 18 Oct 2022 12:31:01 -0700 Subject: [PATCH 4/4] Fix spelling: dosn't -> doesn't Signed-off-by: Steve Peters --- python/test/pyModel_TEST.py | 2 +- python/test/pyWorld_TEST.py | 2 +- src/Model_TEST.cc | 4 ++-- src/World_TEST.cc | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/python/test/pyModel_TEST.py b/python/test/pyModel_TEST.py index 2fcb021df..126c1b579 100644 --- a/python/test/pyModel_TEST.py +++ b/python/test/pyModel_TEST.py @@ -145,7 +145,7 @@ def test_default_construction(self): self.assertEqual(errors[1].message(), "PoseRelativeToGraph error: scope does not point to a valid graph.") - # model dosn't have graphs, so no names should exist in graphs + # model doesn't have graphs, so no names should exist in graphs self.assertFalse(model.name_exists_in_frame_attached_to_graph("")); self.assertFalse(model.name_exists_in_frame_attached_to_graph("link")); diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index 73947b0a9..08c62ab12 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -86,7 +86,7 @@ def test_default_construction(self): self.assertEqual(errors[1].message(), "PoseRelativeToGraph error: scope does not point to a valid graph.") - # world dosn't have graphs, so no names should exist in graphs + # world doesn't have graphs, so no names should exist in graphs self.assertFalse(world.name_exists_in_frame_attached_to_graph("")); self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")); diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc index 4f1e710cc..14e26bf5c 100644 --- a/src/Model_TEST.cc +++ b/src/Model_TEST.cc @@ -155,7 +155,7 @@ TEST(DOMModel, Construction) "PoseRelativeToGraph error: scope does not point to a valid graph")) << errors[1]; - // model dosn't have graphs, so no names should exist in graphs + // model doesn't have graphs, so no names should exist in graphs EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("")); EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("link")); } @@ -233,7 +233,7 @@ TEST(DOMModel, AddLink) EXPECT_EQ(1u, model.LinkCount()); EXPECT_FALSE(model.AddLink(link)); EXPECT_EQ(1u, model.LinkCount()); - // model dosn't have graphs, so no names should exist in graphs + // model doesn't have graphs, so no names should exist in graphs EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("")); EXPECT_FALSE(model.NameExistsInFrameAttachedToGraph("link1")); diff --git a/src/World_TEST.cc b/src/World_TEST.cc index 5f912f3c7..9e9174a4f 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -94,7 +94,7 @@ TEST(DOMWorld, Construction) errors[1].Message().find( "PoseRelativeToGraph error: scope does not point to a valid graph")); - // world dosn't have graphs, so no names should exist in graphs + // world doesn't have graphs, so no names should exist in graphs EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("")); EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("model1")); } @@ -388,7 +388,7 @@ TEST(DOMWorld, AddModel) EXPECT_EQ(1u, world.ModelCount()); EXPECT_FALSE(world.AddModel(model)); EXPECT_EQ(1u, world.ModelCount()); - // world dosn't have graphs, so no names should exist in graphs + // world doesn't have graphs, so no names should exist in graphs EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("")); EXPECT_FALSE(world.NameExistsInFrameAttachedToGraph("model1"));