Check World joints in checkJointParentChildNames (#1189)

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. An example world and test is included. Signed-off-by: Steve Peters <[email protected]>
gazebosim · Oct 20, 2022 · c703dee · c703dee
1 parent 1c2c2af
commit c703dee
Show file tree

Hide file tree

Showing 11 changed files with 172 additions and 69 deletions.
diff --git 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
@@ -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
@@ -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/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 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"));
+
 
     def test_copy_construction(self):
         model = Model()

diff --git 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 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"));
+
 
     def test_copy_construction(self):
         world = World()

diff --git 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 doesn'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 doesn'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.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/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 doesn'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 doesn'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());

diff --git 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 <typename TPtr>
+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
@@ -530,7 +530,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;
 
@@ -645,13 +646,34 @@ 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(
       "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
@@ -0,0 +1,27 @@
+<?xml version="1.0" ?>
+<sdf version="1.10">
+  <world name="joint_invalid_resolved_parent_same_as_child.sdf">
+    <!--
+      For ease of unittesting unique values,
+      each model's pose is displaced along the z-axis,
+      frames are displaced along the y-axis,
+      and joints are displaced along the x-axis.
+    -->
+    <model name="parent_model">
+      <pose>0 0 1 0 0 0</pose>
+      <link name="L"/>
+    </model>
+    <model name="child_model">
+      <pose>0 0 10 0 0 0</pose>
+      <link name="L"/>
+    </model>
+    <frame name="child_frame" attached_to="child_model">
+      <pose>0 1 0 0 0 0</pose>
+    </frame>
+    <joint name="J" type="ball">
+      <pose>10 0 0 0 0 0</pose>
+      <parent>child_model</parent>
+      <child>child_frame</child>
+    </joint>
+  </world>
+</sdf>