diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc index 27fad211e..5f38c4df0 100644 --- 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 index e08ec8e24..7c88c2094 100644 --- 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 index afb781704..4b108fc04 100644 --- 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 index fd1b89080..1cb6c0772 100644 --- 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 index 506c868a6..aeb507449 100644 --- 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 index 51230db48..b8434e565 100644 --- a/test/sdf/model_frame_attached_to.sdf +++ b/test/sdf/model_frame_attached_to.sdf @@ -4,6 +4,7 @@ + diff --git a/test/sdf/model_link_relative_to.sdf b/test/sdf/model_link_relative_to.sdf index a3a48eac7..c02770982 100644 --- a/test/sdf/model_link_relative_to.sdf +++ b/test/sdf/model_link_relative_to.sdf @@ -7,6 +7,9 @@ 2 0 0 0 0 0 + + 12 0 0 0 0 0 + 3 0 0 0 0 0 diff --git a/test/sdf/world_frame_attached_to.sdf b/test/sdf/world_frame_attached_to.sdf index a9a027a4b..d5db45a28 100644 --- a/test/sdf/world_frame_attached_to.sdf +++ b/test/sdf/world_frame_attached_to.sdf @@ -3,6 +3,7 @@ + diff --git a/test/sdf/world_frame_relative_to.sdf b/test/sdf/world_frame_relative_to.sdf index 713e00f4f..bcc224829 100644 --- a/test/sdf/world_frame_relative_to.sdf +++ b/test/sdf/world_frame_relative_to.sdf @@ -9,6 +9,9 @@ 2 0 0 0 0 0 + + 4 0 0 0 0 0 +