diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 995ecd5423..e96b9b6e8d 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -521,19 +521,26 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, Entity originalParentLink = kNullEntity; Entity originalChildLink = kNullEntity; + auto origParentComp = + this->Component(_entity); + const auto &parentName = this->Component(_entity); if (parentName) { - originalParentLink = this->EntityByComponents( - components::Name(parentName->Data())); + originalParentLink = + this->EntityByComponents( + components::Name(parentName->Data()), + components::ParentEntity(origParentComp->Data())); } const auto &childName = this->Component(_entity); if (childName) { - originalChildLink = this->EntityByComponents( - components::Name(childName->Data())); + originalChildLink = + this->EntityByComponents( + components::Name(childName->Data()), + components::ParentEntity(origParentComp->Data())); } if (!originalParentLink || !originalChildLink) @@ -1968,6 +1975,7 @@ template bool EntityComponentManagerPrivate::ClonedJointLinkName(Entity _joint, Entity _originalLink, EntityComponentManager *_ecm) { + if (ComponentTypeT::typeId != components::ParentLinkName::typeId && ComponentTypeT::typeId != components::ChildLinkName::typeId) { diff --git a/test/integration/recreate_entities.cc b/test/integration/recreate_entities.cc index 0a6b6908c2..a9c3e21229 100644 --- a/test/integration/recreate_entities.cc +++ b/test/integration/recreate_entities.cc @@ -25,6 +25,7 @@ #include #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" @@ -316,3 +317,161 @@ TEST_F(RecreateEntitiesFixture, RecreateEntities) components::Recreate()); EXPECT_TRUE(entities.empty()); } + +///////////////////////////////////////////////// +TEST_F(RecreateEntitiesFixture, RecreateEntities_Joints) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/double_pendulum.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + EntityComponentManager *ecm = nullptr; + bool recreateEntities = false; + + // Create a system that adds a recreate component to models + test::Relay testSystem; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (!ecm) + { + ecm = &_ecm; + recreateEntities = true; + return; + } + + if (recreateEntities) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Model *) -> bool + { + // add a components::Recreate to indicate that this entity + // needs to be recreated + _ecm.CreateComponent(_entity, components::Recreate()); + return true; + }); + + // recreate entities only once so set it back to false + recreateEntities = false; + return; + } + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + auto validateEntities = [&]() + { + // Check entities + // 1 x world + 1 x (default) level + 1 x wind + 5 x model + 5 x link + 5 x + // collision + 5 x visual + 1 x light + EXPECT_EQ(48u, ecm->EntityCount()); + + Entity worldEntity = + ecm->EntityByComponents(components::World()); + EXPECT_NE(kNullEntity, worldEntity); + + // Check models + unsigned int modelCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Model *_model, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _model); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + modelCount++; + + EXPECT_EQ(worldEntity, _parent->Data()); + return true; + }); + + EXPECT_EQ(3u, modelCount); + + // Check links + unsigned int linkCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Link *_link, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _link); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + linkCount++; + + return true; + }); + + EXPECT_EQ(7u, linkCount); + + // Check links + unsigned int jointCount{0}; + ecm->Each( + [&](const Entity &_entity, + const components::Joint *_joint, + const components::Pose *_pose, + const components::ParentEntity *_parent, + const components::Name *_name)->bool + { + EXPECT_NE(nullptr, _joint); + EXPECT_NE(nullptr, _pose); + EXPECT_NE(nullptr, _parent); + EXPECT_NE(nullptr, _name); + + jointCount++; + + return true; + }); + + EXPECT_EQ(4u, jointCount); + }; + + // validate initial state + validateEntities(); + + // Run once to let the test relay system creates the components::Recreate + server.Run(true, 1, false); + + // Run again so that entities get recreated in the ECM + server.Run(true, 1, false); + + // validate that the entities are recreated + validateEntities(); + + // Run again to make sure the recreate components are removed and no + // entities to to be recreated + server.Run(true, 1, false); + + auto entities = ecm->EntitiesByComponents(components::Model(), + components::Recreate()); + EXPECT_TRUE(entities.empty()); +} + diff --git a/test/worlds/double_pendulum.sdf b/test/worlds/double_pendulum.sdf new file mode 100644 index 0000000000..10940e785f --- /dev/null +++ b/test/worlds/double_pendulum.sdf @@ -0,0 +1,451 @@ + + + + + 0.001 + 1.0 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + + 5 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + +