From c9e9558044aab70f8b5fdb68f25cc361ea8ca834 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 15 Oct 2022 02:22:39 -0700 Subject: [PATCH] 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(