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 <[email protected]>

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.

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,

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,

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)
 {

src/parser.cc

@@ -2502,25 +2502,25 @@ bool checkJointParentChildNames(const sdf::Root *_root)
 }
 
 //////////////////////////////////////////////////
-void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors)
-{
-  auto checkModelJointParentChildNames = [](
-      const sdf::Model *_model, Errors &errors) -> void
+template <typename TPtr>
+void checkScopedJointParentChildNames(
+    const TPtr _scope, const std::string &_scopeType, Errors &errors)
 {
-    for (uint64_t j = 0; j < _model->JointCount(); ++j)
+  for (uint64_t j = 0; j < _scope->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;
 
     if (parentName != "world" && parentLocalName != "__model__" &&
-          !_model->NameExistsInFrameAttachedToGraph(parentName))
+        !_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 model with name[" + _model->Name() + "]."});
+        "] not found in " + _scopeType + " with name[" +
+        _scope->Name() + "]."});
     }
 
     const std::string &childName = joint->ChildName();
@@ -2529,31 +2529,33 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors)
     {
       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() + "]."});
+        joint->Name() + "] in " + _scopeType + " with name[" +
+        _scope->Name() + "]."});
     }
 
     if (childLocalName != "__model__" &&
-          !_model->NameExistsInFrameAttachedToGraph(childName))
+        !_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 model with name[" + _model->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() +
+        "] 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() +
+        "] in " + _scopeType + " with name[" + _scope->Name() +
         "] must not specify its own name as the parent frame."});
     }
 
@@ -2571,18 +2573,21 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors)
     {
       errors.push_back({ErrorCode::JOINT_PARENT_SAME_AS_CHILD,
         "joint with name[" + joint->Name() +
-          "] in model with name[" + _model->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);
   }
 }
 

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(