diff --git a/src/parser.cc b/src/parser.cc index 775fe4dcb..6631567c4 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -324,6 +324,18 @@ static void insertIncludedElement(sdf::SDFPtr _includeSDF, { setAttributeToProxyFrame("relative_to", elem->GetElementImpl("pose"), false); + + auto parent = elem->FindElement("parent"); + if (nullptr != parent && parent->Get() == "__model__") + { + parent->Set(proxyModelFrameName); + } + auto child = elem->FindElement("child"); + if (nullptr != child && child->Get() == "__model__") + { + child->Set(proxyModelFrameName); + } + // cppcheck-suppress syntaxError // cppcheck-suppress unmatchedSuppression if (auto axis = elem->GetElementImpl("axis"); axis) diff --git a/test/integration/includes.cc b/test/integration/includes.cc index 1c678d13a..1243ac907 100644 --- a/test/integration/includes.cc +++ b/test/integration/includes.cc @@ -411,8 +411,8 @@ TEST(IncludesTest, MergeInclude) ASSERT_NE(nullptr, world); auto model = world->ModelByIndex(0); EXPECT_EQ("robot1", model->Name()); - EXPECT_EQ(5u, model->LinkCount()); - EXPECT_EQ(4u, model->JointCount()); + EXPECT_EQ(7u, model->LinkCount()); + EXPECT_EQ(6u, model->JointCount()); EXPECT_EQ(1u, model->ModelCount()); ASSERT_NE(nullptr, model->CanonicalLink()); EXPECT_EQ(model->LinkByIndex(0), model->CanonicalLink()); @@ -521,6 +521,20 @@ TEST(IncludesTest, MergeInclude) EXPECT_EQ(expectedXyz, xyz); } + // Check joint parent set as __model__ + { + // left_wheel_joint's axis is expressed in __model__. + auto joint = model->JointByName("test_model_parent"); + ASSERT_NE(nullptr, joint); + EXPECT_EQ(prefixedFrameName, joint->ParentLinkName()); + } + // Check joint child set as __model__ + { + // left_wheel_joint's axis is expressed in __model__. + auto joint = model->JointByName("test_model_child"); + ASSERT_NE(nullptr, joint); + EXPECT_EQ(prefixedFrameName, joint->ChildLinkName()); + } // Verify that plugins get merged auto modelElem = model->Element(); @@ -557,8 +571,8 @@ TEST(IncludesTest, MergeIncludePlacementFrame) ASSERT_NE(nullptr, world); auto model = world->ModelByIndex(1); EXPECT_EQ("robot2", model->Name()); - EXPECT_EQ(5u, model->LinkCount()); - EXPECT_EQ(4u, model->JointCount()); + EXPECT_EQ(7u, model->LinkCount()); + EXPECT_EQ(6u, model->JointCount()); auto topLink = model->LinkByName("top"); ASSERT_NE(nullptr, topLink); Pose3d topLinkPose; diff --git a/test/integration/model/merge_robot/model.sdf b/test/integration/model/merge_robot/model.sdf index 4215df4a7..37d03440f 100644 --- a/test/integration/model/merge_robot/model.sdf +++ b/test/integration/model/merge_robot/model.sdf @@ -289,6 +289,20 @@ caster + + + + + + __model__ + test_child_link + + + + test_parent_link + __model__ + + 0 1 0 0 45 0