diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 5711cb8b38..a98c2b854b 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -85,8 +85,11 @@ namespace ignition /// canonical link. /// 3. Child entities that are cloned will have their parent set to the /// cloned parent entity. - /// 4. Aside from the changes listed above, all other cloned components + /// 4. Cloned joints with parent/child links will have their parent and + /// child links set to the cloned parent/child links. + /// 5. Aside from the changes listed above, all other cloned components /// remain unchanged. + /// Currently, cloning detachable joints is not supported. /// \param[in] _entity The entity to clone. /// \param[in] _parent The parent of the cloned entity. Set this to /// kNullEntity if the cloned entity should not have a parent. diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 9d4ac0c994..814be6a0ce 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -30,10 +30,14 @@ #include <ignition/math/graph/GraphAlgorithms.hh> #include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" using namespace ignition; using namespace gazebo; @@ -97,6 +101,22 @@ class ignition::gazebo::EntityComponentManagerPrivate public: bool ComponentMarkedAsRemoved(const Entity _entity, const ComponentTypeId _typeId) const; + /// \brief Set a cloned joint's parent or child link name. + /// \param[in] _joint The cloned joint. + /// \param[in] _originalLink The original joint's parent or child link. + /// \param[in] _ecm Entity component manager. + /// \tparam The component type, which must be either + /// components::ParentLinkName or components::ChildLinkName + /// \return True if _joint's parent or child link name was set. + /// False otherwise + /// \note This method should only be called in EntityComponentManager::Clone. + /// This is a temporary workaround until we find a way to clone entites and + /// components that don't require special treatment for particular component + /// types. + public: template<typename ComponentTypeT> + bool ClonedJointLinkName(Entity _joint, Entity _originalLink, + EntityComponentManager *_ecm); + /// \brief All component types that have ever been created. public: std::unordered_set<ComponentTypeId> createdCompTypes; @@ -233,6 +253,24 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief See above public: std::unordered_map<Entity, Entity> oldToClonedCanonicalLink; + /// \brief During cloning, we populate two maps: + /// - map of link entities to their cloned link + /// - map of cloned joint entities to the original joint entity's parent and + /// child links + /// After cloning is done, these maps can be used to update the cloned joint + /// entity's parent and child links to the cloned parent and child links. + /// \TODO(anyone) We shouldn't be giving joints special treatment. + /// We should figure out a way to update a joint's parent/child links without + /// having to explicitly search/track for the cloned links. + public: std::unordered_map<Entity, Entity> originalToClonedLink; + + /// \brief See above + /// The key is the cloned joint entity, and the value is a pair where the + /// first element is the original joint's parent link, and the second element + /// is the original joint's child link + public: std::unordered_map<Entity, std::pair<Entity, Entity>> + clonedToOriginalJointLinks; + /// \brief Set of entities that are prevented from removal. public: std::unordered_set<Entity> pinnedEntities; }; @@ -310,11 +348,15 @@ Entity EntityComponentManager::Clone(Entity _entity, Entity _parent, // Clear maps so they're populated for the entity being cloned this->dataPtr->oldToClonedCanonicalLink.clear(); this->dataPtr->oldModelCanonicalLink.clear(); + this->dataPtr->originalToClonedLink.clear(); + this->dataPtr->clonedToOriginalJointLinks.clear(); auto clonedEntity = this->CloneImpl(_entity, _parent, _name, _allowRename); if (kNullEntity != clonedEntity) { + // make sure that cloned models have their canonical link updated to the + // cloned canonical link for (const auto &[clonedModel, oldCanonicalLink] : this->dataPtr->oldModelCanonicalLink) { @@ -331,6 +373,30 @@ Entity EntityComponentManager::Clone(Entity _entity, Entity _parent, this->SetComponentData<components::ModelCanonicalLink>(clonedModel, clonedCanonicalLink); } + + // make sure that cloned joints have their parent/child links + // updated to the cloned parent/child links + for (const auto &[clonedJoint, originalJointLinks] : + this->dataPtr->clonedToOriginalJointLinks) + { + auto originalParentLink = originalJointLinks.first; + if (!this->dataPtr->ClonedJointLinkName<components::ParentLinkName>( + clonedJoint, originalParentLink, this)) + { + ignerr << "Error updating the cloned parent link name for cloned " + << "joint [" << clonedJoint << "]\n"; + continue; + } + + auto originalChildLink = originalJointLinks.second; + if (!this->dataPtr->ClonedJointLinkName<components::ChildLinkName>( + clonedJoint, originalChildLink, this)) + { + ignerr << "Error updating the cloned child link name for cloned " + << "joint [" << clonedJoint << "]\n"; + continue; + } + } } return clonedEntity; @@ -421,6 +487,48 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, this->dataPtr->oldToClonedCanonicalLink[_entity] = clonedEntity; } + // keep track of all joints and links that have been cloned so that cloned + // joints can be updated to their cloned parent/child links + if (this->Component<components::Joint>(clonedEntity)) + { + // this is a joint, so we need to find the original joint's parent and child + // link entities + Entity originalParentLink = kNullEntity; + Entity originalChildLink = kNullEntity; + + const auto &parentName = + this->Component<components::ParentLinkName>(_entity); + if (parentName) + { + originalParentLink = this->EntityByComponents<components::Name>( + components::Name(parentName->Data())); + } + + const auto &childName = this->Component<components::ChildLinkName>(_entity); + if (childName) + { + originalChildLink = this->EntityByComponents<components::Name>( + components::Name(childName->Data())); + } + + if (!originalParentLink || !originalChildLink) + { + ignerr << "The cloned joint entity [" << clonedEntity << "] was unable " + << "to find the original joint entity's parent and/or child link.\n"; + this->RequestRemoveEntity(clonedEntity); + return kNullEntity; + } + + this->dataPtr->clonedToOriginalJointLinks[clonedEntity] = + {originalParentLink, originalChildLink}; + } + else if (this->Component<components::Link>(clonedEntity) || + this->Component<components::CanonicalLink>(clonedEntity)) + { + // save a mapping between the original link and the cloned link + this->dataPtr->originalToClonedLink[_entity] = clonedEntity; + } + for (const auto &childEntity : this->EntitiesByComponents(components::ParentEntity(_entity))) { @@ -1832,6 +1940,40 @@ bool EntityComponentManagerPrivate::ComponentMarkedAsRemoved( return false; } +///////////////////////////////////////////////// +template<typename ComponentTypeT> +bool EntityComponentManagerPrivate::ClonedJointLinkName(Entity _joint, + Entity _originalLink, EntityComponentManager *_ecm) +{ + if (ComponentTypeT::typeId != components::ParentLinkName::typeId && + ComponentTypeT::typeId != components::ChildLinkName::typeId) + { + ignerr << "Template type is invalid. Must be either " + << "components::ParentLinkName or components::ChildLinkName\n"; + return false; + } + + auto iter = this->originalToClonedLink.find(_originalLink); + if (iter == this->originalToClonedLink.end()) + { + ignerr << "Error: attempted to clone links, but link [" + << _originalLink << "] was never cloned.\n"; + return false; + } + auto clonedLink = iter->second; + + auto name = _ecm->Component<components::Name>(clonedLink); + if (!name) + { + ignerr << "Link [" << _originalLink << "] was cloned, but its clone has no " + << "name.\n"; + return false; + } + + _ecm->SetComponentData<ComponentTypeT>(_joint, name->Data()); + return true; +} + ///////////////////////////////////////////////// void EntityComponentManager::PinEntity(const Entity _entity, bool _recursive) { diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 21d0a2d056..1aaefc0fc6 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -24,9 +24,13 @@ #include <ignition/utils/SuppressWarning.hh> #include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/config.hh" @@ -2790,8 +2794,66 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) EXPECT_EQ(10u, manager.EntityCount()); EXPECT_EQ(kNullEntity, failedClonedEntity); + // create a joint with a parent and child link + const std::string parentLinkEntityName = "parentLinkEntity"; + const std::string childLinkEntityName = "childLinkEntity"; + Entity parentLinkEntity = manager.CreateEntity(); + manager.CreateComponent(parentLinkEntity, + components::Name(parentLinkEntityName)); + manager.CreateComponent(parentLinkEntity, components::CanonicalLink()); + Entity jointEntity = manager.CreateEntity(); + manager.CreateComponent(jointEntity, + components::ParentEntity(parentLinkEntity)); + manager.CreateComponent(jointEntity, components::Name("jointEntity")); + manager.CreateComponent(jointEntity, components::Joint()); + manager.CreateComponent(jointEntity, + components::ParentLinkName(parentLinkEntityName)); + manager.CreateComponent(jointEntity, + components::ChildLinkName(childLinkEntityName)); + Entity childLinkEntity = manager.CreateEntity(); + manager.CreateComponent(childLinkEntity, + components::ParentEntity(jointEntity)); + manager.CreateComponent(childLinkEntity, + components::Name(childLinkEntityName)); + manager.CreateComponent(childLinkEntity, components::Link()); + EXPECT_EQ(13u, manager.EntityCount()); + + // clone a joint that has a parent and child link. + auto clonedParentLinkEntity = manager.Clone(parentLinkEntity, kNullEntity, + "", true); + ASSERT_NE(kNullEntity, clonedParentLinkEntity); + EXPECT_EQ(16u, manager.EntityCount()); + clonedEntities.insert(clonedParentLinkEntity); + auto clonedJoints = manager.EntitiesByComponents( + components::ParentEntity(clonedParentLinkEntity)); + ASSERT_EQ(1u, clonedJoints.size()); + clonedEntities.insert(clonedJoints[0]); + auto clonedChildLinks = manager.EntitiesByComponents( + components::ParentEntity(clonedJoints[0])); + ASSERT_EQ(1u, clonedChildLinks.size()); + clonedEntities.insert(clonedChildLinks[0]); + + // The cloned joint should have the cloned parent/child link names attached to + // it, not the original parent/child link names + auto clonedJointParentLinkName = + manager.Component<components::ParentLinkName>(clonedJoints[0]); + ASSERT_NE(nullptr, clonedJointParentLinkName); + EXPECT_NE(clonedJointParentLinkName->Data(), parentLinkEntityName); + auto clonedJointChildLinkName = + manager.Component<components::ChildLinkName>(clonedJoints[0]); + ASSERT_NE(nullptr, clonedJointChildLinkName); + EXPECT_NE(clonedJointChildLinkName->Data(), childLinkEntityName); + auto clonedParentLinkName = + manager.Component<components::Name>(clonedParentLinkEntity); + ASSERT_NE(nullptr, clonedParentLinkName); + EXPECT_EQ(clonedParentLinkName->Data(), clonedJointParentLinkName->Data()); + auto clonedChildLinkName = + manager.Component<components::Name>(clonedChildLinks[0]); + ASSERT_NE(nullptr, clonedChildLinkName); + EXPECT_EQ(clonedJointChildLinkName->Data(), clonedChildLinkName->Data()); + // make sure that the name given to each cloned entity is unique - EXPECT_EQ(5u, clonedEntities.size()); + EXPECT_EQ(8u, clonedEntities.size()); for (const auto &entity : clonedEntities) { auto nameComp = manager.Component<components::Name>(entity); @@ -2802,7 +2864,7 @@ TEST_P(EntityComponentManagerFixture, CloneEntities) // try to clone an entity that does not exist EXPECT_EQ(kNullEntity, manager.Clone(kNullEntity, topLevelEntity, "", allowRename)); - EXPECT_EQ(10u, manager.EntityCount()); + EXPECT_EQ(16u, manager.EntityCount()); } /////////////////////////////////////////////////