Merge 96aec38 into e9e9546

gazebosim · Jul 19, 2022 · 1f336f6 · 1f336f6
2 parents e9e9546 + 96aec38
commit 1f336f6
Show file tree

Hide file tree

Showing 9 changed files with 78 additions and 19 deletions.
diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc
@@ -54,9 +54,9 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model)
  EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root));
  EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root));
 
- EXPECT_EQ(6u, graph.map.size());
- EXPECT_EQ(6u, graph.graph.Vertices().size());
- EXPECT_EQ(5u, graph.graph.Edges().size());
+ EXPECT_EQ(7u, graph.map.size());
+ EXPECT_EQ(7u, graph.graph.Vertices().size());
+ EXPECT_EQ(6u, graph.graph.Edges().size());
 
  EXPECT_EQ(1u, graph.map.count("__model__"));
  EXPECT_EQ(1u, graph.map.count("L"));
@@ -118,9 +118,9 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World)
  EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root));
  EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root));
 
- EXPECT_EQ(6u, graph.map.size());
- EXPECT_EQ(6u, graph.graph.Vertices().size());
- EXPECT_EQ(4u, graph.graph.Edges().size());
+ EXPECT_EQ(7u, graph.map.size());
+ EXPECT_EQ(7u, graph.graph.Vertices().size());
+ EXPECT_EQ(5u, graph.graph.Edges().size());
 
  EXPECT_EQ(1u, graph.map.count("world"));
  EXPECT_EQ(1u, graph.map.count("world_frame"));

diff --git a/src/Model.cc b/src/Model.cc
@@ -333,7 +333,7 @@ Errors Model::Load(ElementPtr _sdf)
  std::string frameName = frame.Name();
  if (frameNames.count(frameName) > 0)
  {
- // This joint has a name collision
+ // This frame has a name collision
  if (sdfVersion < ignition::math::SemanticVersion(1, 7))
  {
  // This came from an old file, so try to workaround by renaming frame

diff --git a/src/parser.cc b/src/parser.cc
@@ -1554,8 +1554,8 @@ bool checkFrameAttachedToNames(const sdf::Root *_root)
 
  const std::string &attachedTo = frame->AttachedTo();
 
- // the attached_to attribute is always permitted to be empty
- if (attachedTo.empty())
+ // the attached_to attribute is always permitted to be empty or __model__
+ if (attachedTo.empty() || "__model__" == attachedTo)
  {
  continue;
  }
@@ -1597,8 +1597,8 @@ bool checkFrameAttachedToNames(const sdf::Root *_root)
 
  const std::string &attachedTo = frame->AttachedTo();
 
- // the attached_to attribute is always permitted to be empty
- if (attachedTo.empty())
+ // the attached_to attribute is always permitted to be empty or world
+ if (attachedTo.empty() || "world" == attachedTo)
  {
  continue;
  }

diff --git a/test/integration/frame.cc b/test/integration/frame.cc
@@ -436,28 +436,34 @@ TEST(DOMFrame, LoadModelFramesAttachedTo)
  EXPECT_EQ(0u, model->JointCount());
  EXPECT_EQ(nullptr, model->JointByIndex(0));
 
- EXPECT_EQ(4u, model->FrameCount());
+ EXPECT_EQ(5u, model->FrameCount());
  EXPECT_NE(nullptr, model->FrameByIndex(0));
  EXPECT_NE(nullptr, model->FrameByIndex(1));
  EXPECT_NE(nullptr, model->FrameByIndex(2));
  EXPECT_NE(nullptr, model->FrameByIndex(3));
- EXPECT_EQ(nullptr, model->FrameByIndex(4));
+ EXPECT_NE(nullptr, model->FrameByIndex(4));
+ EXPECT_EQ(nullptr, model->FrameByIndex(5));
+ ASSERT_TRUE(model->FrameNameExists("F000"));
  ASSERT_TRUE(model->FrameNameExists("F00"));
  ASSERT_TRUE(model->FrameNameExists("F0"));
  ASSERT_TRUE(model->FrameNameExists("F1"));
  ASSERT_TRUE(model->FrameNameExists("F2"));
 
+ EXPECT_EQ("__model__", model->FrameByName("F000")->AttachedTo());
  EXPECT_TRUE(model->FrameByName("F00")->AttachedTo().empty());
  EXPECT_TRUE(model->FrameByName("F0")->AttachedTo().empty());
  EXPECT_EQ("L", model->FrameByName("F1")->AttachedTo());
  EXPECT_EQ("F1", model->FrameByName("F2")->AttachedTo());
 
+ EXPECT_TRUE(model->FrameByName("F000")->PoseRelativeTo().empty());
  EXPECT_TRUE(model->FrameByName("F00")->PoseRelativeTo().empty());
  EXPECT_TRUE(model->FrameByName("F0")->PoseRelativeTo().empty());
  EXPECT_TRUE(model->FrameByName("F1")->PoseRelativeTo().empty());
  EXPECT_TRUE(model->FrameByName("F2")->PoseRelativeTo().empty());
 
  std::string body;
+ EXPECT_TRUE(model->FrameByName("F000")->ResolveAttachedToBody(body).empty());
+ EXPECT_EQ("L", body);
  EXPECT_TRUE(model->FrameByName("F00")->ResolveAttachedToBody(body).empty());
  EXPECT_EQ("L", body);
  EXPECT_TRUE(model->FrameByName("F0")->ResolveAttachedToBody(body).empty());
@@ -718,23 +724,27 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo)
  ASSERT_TRUE(model->FrameNameExists("F0"));
  EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo());
 
- EXPECT_EQ(4u, world->FrameCount());
+ EXPECT_EQ(5u, world->FrameCount());
  EXPECT_NE(nullptr, world->FrameByIndex(0));
  EXPECT_NE(nullptr, world->FrameByIndex(1));
  EXPECT_NE(nullptr, world->FrameByIndex(2));
  EXPECT_NE(nullptr, world->FrameByIndex(3));
- EXPECT_EQ(nullptr, world->FrameByIndex(4));
+ EXPECT_NE(nullptr, world->FrameByIndex(4));
+ EXPECT_EQ(nullptr, world->FrameByIndex(5));
  ASSERT_TRUE(world->FrameNameExists("world_frame"));
+ ASSERT_TRUE(world->FrameNameExists("F00"));
  ASSERT_TRUE(world->FrameNameExists("F0"));
  ASSERT_TRUE(world->FrameNameExists("F1"));
  ASSERT_TRUE(world->FrameNameExists("F2"));
 
  EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty());
  EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty());
+ EXPECT_EQ("world", world->FrameByName("F00")->AttachedTo());
  EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo());
  EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo());
 
  EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty());
+ EXPECT_TRUE(world->FrameByName("F00")->PoseRelativeTo().empty());
  EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty());
  EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty());
  EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty());
@@ -743,6 +753,8 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo)
  EXPECT_TRUE(
  world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty());
  EXPECT_EQ("world", body);
+ EXPECT_TRUE(world->FrameByName("F00")->ResolveAttachedToBody(body).empty());
+ EXPECT_EQ("world", body);
  EXPECT_TRUE(world->FrameByName("F0")->ResolveAttachedToBody(body).empty());
  EXPECT_EQ("world", body);
  EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty());
@@ -1194,26 +1206,53 @@ TEST(DOMFrame, LoadWorldFramesRelativeTo)
  EXPECT_EQ("M2", world->ModelByName("M3")->PoseRelativeTo());
  EXPECT_EQ("F1", world->ModelByName("M4")->PoseRelativeTo());
 
- EXPECT_EQ(4u, world->FrameCount());
+ EXPECT_EQ(5u, world->FrameCount());
  EXPECT_NE(nullptr, world->FrameByIndex(0));
  EXPECT_NE(nullptr, world->FrameByIndex(1));
  EXPECT_NE(nullptr, world->FrameByIndex(2));
  EXPECT_NE(nullptr, world->FrameByIndex(3));
- EXPECT_EQ(nullptr, world->FrameByIndex(4));
+ EXPECT_NE(nullptr, world->FrameByIndex(4));
+ EXPECT_EQ(nullptr, world->FrameByIndex(5));
  ASSERT_TRUE(world->FrameNameExists("world_frame"));
  ASSERT_TRUE(world->FrameNameExists("F0"));
  ASSERT_TRUE(world->FrameNameExists("F1"));
+ ASSERT_TRUE(world->FrameNameExists("F1a"));
  ASSERT_TRUE(world->FrameNameExists("F2"));
 
  EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty());
  EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty());
  EXPECT_EQ("F0", world->FrameByName("F1")->PoseRelativeTo());
+ EXPECT_EQ("world", world->FrameByName("F1a")->PoseRelativeTo());
  EXPECT_EQ("M1", world->FrameByName("F2")->PoseRelativeTo());
 
  EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty());
  EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty());
  EXPECT_TRUE(world->FrameByName("F1")->AttachedTo().empty());
+ EXPECT_EQ("F1", world->FrameByName("F1a")->AttachedTo());
  EXPECT_TRUE(world->FrameByName("F2")->AttachedTo().empty());
+
+ using Pose = ignition::math::Pose3d;
+ Pose pose;
+ EXPECT_TRUE(
+ world->FrameByName("world_frame")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ world->FrameByName("F0")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ world->FrameByName("F1")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ world->FrameByName("F1a")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(4, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ world->FrameByName("F2")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), pose);
 }
 
 /////////////////////////////////////////////////

diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc
@@ -622,29 +622,35 @@ TEST(DOMLink, LoadLinkPoseRelativeTo)
  const sdf::Model *model = root.ModelByIndex(0);
  ASSERT_NE(nullptr, model);
  EXPECT_EQ("model_link_relative_to", model->Name());
- EXPECT_EQ(3u, model->LinkCount());
+ EXPECT_EQ(4u, model->LinkCount());
  EXPECT_NE(nullptr, model->LinkByIndex(0));
  EXPECT_NE(nullptr, model->LinkByIndex(1));
  EXPECT_NE(nullptr, model->LinkByIndex(2));
- EXPECT_EQ(nullptr, model->LinkByIndex(3));
+ EXPECT_NE(nullptr, model->LinkByIndex(3));
+ EXPECT_EQ(nullptr, model->LinkByIndex(4));
  EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose());
  EXPECT_EQ("", model->PoseRelativeTo());
 
  ASSERT_TRUE(model->LinkNameExists("L1"));
  ASSERT_TRUE(model->LinkNameExists("L2"));
+ ASSERT_TRUE(model->LinkNameExists("L2a"));
  ASSERT_TRUE(model->LinkNameExists("L3"));
  EXPECT_TRUE(model->LinkByName("L1")->PoseRelativeTo().empty());
  EXPECT_TRUE(model->LinkByName("L2")->PoseRelativeTo().empty());
+ EXPECT_EQ("__model__", model->LinkByName("L2a")->PoseRelativeTo());
  EXPECT_EQ("L1", model->LinkByName("L3")->PoseRelativeTo());
 
  EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), model->LinkByName("L1")->RawPose());
  EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), model->LinkByName("L2")->RawPose());
+ EXPECT_EQ(Pose(12, 0, 0, 0, 0, 0), model->LinkByName("L2a")->RawPose());
  EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), model->LinkByName("L3")->RawPose());
 
  EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI / 2, 0),
  model->LinkByName("L1")->SemanticPose().RawPose());
  EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0),
  model->LinkByName("L2")->SemanticPose().RawPose());
+ EXPECT_EQ(Pose(12, 0, 0, 0, 0, 0),
+ model->LinkByName("L2a")->SemanticPose().RawPose());
  EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0),
  model->LinkByName("L3")->SemanticPose().RawPose());
 
@@ -656,6 +662,10 @@ TEST(DOMLink, LoadLinkPoseRelativeTo)
  EXPECT_TRUE(
  model->LinkByName("L2")->SemanticPose().Resolve(pose, "__model__").empty());
  EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ model->LinkByName("L2a")->SemanticPose().Resolve(pose,
+ "__model__").empty());
+ EXPECT_EQ(Pose(12, 0, 0, 0, 0, 0), pose);
  EXPECT_TRUE(
  model->LinkByName("L3")->SemanticPose().Resolve(pose, "__model__").empty());
  EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose);
@@ -664,6 +674,8 @@ TEST(DOMLink, LoadLinkPoseRelativeTo)
  EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), pose);
  EXPECT_TRUE(model->LinkByName("L2")->SemanticPose().Resolve(pose).empty());
  EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(model->LinkByName("L2a")->SemanticPose().Resolve(pose).empty());
+ EXPECT_EQ(Pose(12, 0, 0, 0, 0, 0), pose);
  EXPECT_TRUE(model->LinkByName("L3")->SemanticPose().Resolve(pose).empty());
  EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose);
 

diff --git a/test/sdf/model_frame_attached_to.sdf b/test/sdf/model_frame_attached_to.sdf
@@ -4,6 +4,7 @@
  <link name="L"/>
  <frame name="F00"/> <!-- VALID: Indirectly attached_to canonical link L via the model frame. -->
  <frame name="F0" attached_to=""/> <!-- VALID: Indirectly attached_to canonical link L via the model frame. -->
+ <frame name="F000" attached_to="__model__"/> <!-- VALID: Indirectly attached_to canonical link L via the model frame. -->
  <frame name="F1" attached_to="L"/> <!-- VALID: Directly attached_to link L. -->
  <frame name="F2" attached_to="F1"/> <!-- VALID: Indirectly attached_to link L via frame F1. -->
  </model>

diff --git a/test/sdf/model_link_relative_to.sdf b/test/sdf/model_link_relative_to.sdf
@@ -7,6 +7,9 @@
  <link name="L2">
  <pose relative_to="">2 0 0 0 0 0</pose>
  </link>
+ <link name="L2a">
+ <pose relative_to="__model__">12 0 0 0 0 0</pose>
+ </link>
  <link name="L3">
  <pose relative_to="L1">3 0 0 0 0 0</pose>
  </link>

diff --git a/test/sdf/world_frame_attached_to.sdf b/test/sdf/world_frame_attached_to.sdf
@@ -3,6 +3,7 @@
  <world name="world_frame_attached_to">
 
  <frame name="world_frame"/> <!-- VALID: Directly attached_to implicit world frame. -->
+ <frame name="F00" attached_to="world"/> <!-- VALID: Directly attached_to implicit world frame. -->
  <frame name="F0" attached_to=""/> <!-- VALID: Directly attached_to implicit world frame by default. -->
  <frame name="F1" attached_to="F0"/> <!-- VALID: Indirectly attached_to implicit world frame via frame F0. -->
 

diff --git a/test/sdf/world_frame_relative_to.sdf b/test/sdf/world_frame_relative_to.sdf
@@ -9,6 +9,9 @@
  <frame name="F1">
  <pose relative_to="F0">2 0 0 0 0 0</pose> <!-- VALID: Indirectly relative_to implicit world frame via frame F0. -->
  </frame>
+ <frame name="F1a" attached_to="F1">
+ <pose relative_to="world">4 0 0 0 0 0</pose> <!-- VALID: Directly relative_to implicit world frame. -->
+ </frame>
 
  <model name="M1">
  <link name="L"/>