diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 6a7869f528..1200ffa436 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -74,6 +75,38 @@ namespace ignition /// \return An id for the Entity, or kNullEntity on failure. public: Entity CreateEntity(); + /// \brief Clone an entity and its components. If the entity has any child + /// entities, they will also be cloned. + /// When cloning entities, the following rules apply: + /// 1. The name component of a cloned entity will consist of a unique + /// name, since all entities should have a unique name. + /// 2. Cloned entities that have a canonical link will have their + /// canonical link set to the cloned canonical link, not the original + /// 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 + /// remain unchanged. + /// \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. + /// \param[in] _name The name that should be given to the cloned entity. + /// Set this to an empty string if the cloned entity name should be + /// auto-generated to something unique. + /// \param[in] _allowRename True if _name can be modified to be a unique + /// name if it isn't already a unique name. False if _name cannot be + /// modified to be a unique name. If _allowRename is set to False, and + /// _name is not unique, _entity will not be cloned. If _name is an + /// empty string, _allowRename is ignored since the cloned entity will + /// have an auto-generated unique name. + /// \return The cloned entity, which will have a unique name. kNullEntity + /// is returned if cloning failed. Failure could occur if _entity does not + /// exist, or if a unique name could not be generated for the entity to be + /// cloned. + /// \sa Clone + public: Entity Clone(Entity _entity, Entity _parent, + const std::string &_name, bool _allowRename); + /// \brief Get the number of entities on the server. /// \return Entity count. public: size_t EntityCount() const; @@ -333,6 +366,21 @@ namespace ignition private: template struct identity; // NOLINT + /// \brief Helper function for cloning an entity and its children (this + /// includes cloning components attached to these entities). This method + /// should never be called directly - it is called internally from the + /// public Clone method. + /// \param[in] _entity The entity to clone. + /// \param[in] _parent The parent of the cloned entity. + /// \param[in] _name The name that should be given to the cloned entity. + /// \param[in] _allowRename True if _name can be modified to be a unique + /// name if it isn't already a unique name. False if _name cannot be + /// modified to be a unique name. + /// \return The cloned entity. kNullEntity is returned if cloning failed. + /// \sa Clone + private: Entity CloneImpl(Entity _entity, Entity _parent, + const std::string &_name, bool _allowRename); + /// \brief A version of Each() that doesn't use a cache. The cached /// version, Each(), is preferred. /// Get all entities which contain given component types, as well diff --git a/include/ignition/gazebo/components/Component.hh b/include/ignition/gazebo/components/Component.hh index bbee6d34c5..59eee24fb6 100644 --- a/include/ignition/gazebo/components/Component.hh +++ b/include/ignition/gazebo/components/Component.hh @@ -279,6 +279,10 @@ namespace components /// Factory registration and is guaranteed to be the same across compilers /// and runs. public: virtual ComponentTypeId TypeId() const = 0; + + /// \brief Clone the component. + /// \return A pointer to the component. + public: virtual std::unique_ptr Clone() = 0; }; /// \brief A component type that wraps any data type. The intention is for @@ -342,6 +346,9 @@ namespace components /// \return True if different. public: bool operator!=(const Component &_component) const; + // Documentation inherited + public: std::unique_ptr Clone() override; + // Documentation inherited public: ComponentTypeId TypeId() const override; @@ -408,6 +415,9 @@ namespace components public: bool operator!=(const Component &_component) const; + // Documentation inherited + public: std::unique_ptr Clone() override; + // Documentation inherited public: ComponentTypeId TypeId() const override; @@ -490,6 +500,16 @@ namespace components Serializer::Deserialize(_in, this->Data()); } + ////////////////////////////////////////////////// + template + std::unique_ptr + Component::Clone() + { + Component clonedComp(this->Data()); + return std::make_unique>( + clonedComp); + } + ////////////////////////////////////////////////// template ComponentTypeId Component::TypeId() const @@ -513,6 +533,14 @@ namespace components return false; } + ////////////////////////////////////////////////// + template + std::unique_ptr + Component::Clone() + { + return std::make_unique>(); + } + ////////////////////////////////////////////////// template ComponentTypeId Component::TypeId() const diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 73d91ee725..f951e3e9ff 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -177,26 +177,30 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Create an actor /// \param[in] _id Unique actor id /// \param[in] _actor Actor sdf dom + /// \param[in] _name Actor's name /// \param[in] _parentId Parent id /// \return Actor object created from the sdf dom public: rendering::VisualPtr CreateActor(Entity _id, - const sdf::Actor &_actor, Entity _parentId = 0); + const sdf::Actor &_actor, const std::string &_name, + Entity _parentId = 0); /// \brief Create a light /// \param[in] _id Unique light id /// \param[in] _light Light sdf dom + /// \param[in] _name Light's name /// \param[in] _parentId Parent id /// \return Light object created from the sdf dom public: rendering::LightPtr CreateLight(Entity _id, - const sdf::Light &_light, Entity _parentId); + const sdf::Light &_light, const std::string &_name, Entity _parentId); /// \brief Create a light /// \param[in] _id Unique light id /// \param[in] _light Light sdf dom + /// \param[in] _name Light's name /// \param[in] _parentId Parent id /// \return Light object created from the sdf dom public: rendering::VisualPtr CreateLightVisual(Entity _id, - const sdf::Light &_light, Entity _parentId); + const sdf::Light &_light, const std::string &_name, Entity _parentId); /// \brief Create a particle emitter. /// \param[in] _id Unique particle emitter id diff --git a/src/Component_TEST.cc b/src/Component_TEST.cc index 9451cee170..2202b05753 100644 --- a/src/Component_TEST.cc +++ b/src/Component_TEST.cc @@ -177,6 +177,11 @@ class NoSerialize : public components::BaseComponent { return 0; } + + public: std::unique_ptr Clone() override + { + return nullptr; + } }; ////////////////////////////////////////////////// @@ -550,3 +555,40 @@ TEST_F(ComponentTest, TypeName) EXPECT_EQ("123456", comp.typeName); } } + +////////////////////////////////////////////////// +TEST_F(ComponentTest, Clone) +{ + // Component with data + { + using Custom = components::Component; + + // create a component and a clone of it. The clone should initially have the + // same data as the original component + Custom comp(5); + auto clonedComp = comp.Clone(); + auto derivedClone = static_cast(clonedComp.get()); + EXPECT_EQ(comp, *derivedClone); + + // modify the data of the cloned component, and make sure that only the + // cloned component is modified, not the original component + derivedClone->Data() = 10; + EXPECT_NE(comp, *derivedClone); + EXPECT_EQ(5, comp.Data()); + EXPECT_EQ(10, derivedClone->Data()); + } + + // Component without data + { + using Custom = components::Component; + + Custom comp; + auto clonedComp = comp.Clone(); + auto derivedClone = static_cast(clonedComp.get()); + + // since this component has no data, we cannot do the same check as we did + // above for a component with data. However, we can make sure that the + // pointers for the components are different + EXPECT_NE(&comp, derivedClone); + } +} diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 659e6132f7..2e895bc323 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -28,8 +29,11 @@ #include #include +#include "ignition/gazebo/components/CanonicalLink.hh" #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" using namespace ignition; using namespace gazebo; @@ -202,6 +206,25 @@ class ignition::gazebo::EntityComponentManagerPrivate /// by the multithreading functionality in `State()` to allocate work to /// each thread. public: bool componentTypeIndexDirty{true}; + + /// \brief During cloning, we populate two maps: + /// - map of cloned model entities to the non-cloned model's canonical link + /// - map of non-cloned canonical links to the cloned canonical link + /// After cloning is done, these maps can be used to update the cloned model's + /// canonical link to be the cloned canonical link instead of the original + /// model's canonical link. We populate maps during cloning and then update + /// canonical links after cloning since cloning is done top-down, and + /// canonical links are children of models (when a model is cloned, its + /// canonical link has not been cloned yet, so we have no way of knowing what + /// to set the cloned model's canonical link to until the canonical link has + /// been cloned). + /// \TODO(anyone) We shouldn't be giving canonical links special treatment. + /// This may happen to any component that holds an Entity, so we should figure + /// out a way to generalize this for any such component. + std::unordered_map oldModelCanonicalLink; + + /// \brief See above + std::unordered_map oldToClonedCanonicalLink; }; ////////////////////////////////////////////////// @@ -270,6 +293,139 @@ Entity EntityComponentManagerPrivate::CreateEntityImplementation(Entity _entity) return _entity; } +///////////////////////////////////////////////// +Entity EntityComponentManager::Clone(Entity _entity, Entity _parent, + const std::string &_name, bool _allowRename) +{ + // Clear maps so they're populated for the entity being cloned + this->dataPtr->oldToClonedCanonicalLink.clear(); + this->dataPtr->oldModelCanonicalLink.clear(); + + auto clonedEntity = this->CloneImpl(_entity, _parent, _name, _allowRename); + + if (kNullEntity != clonedEntity) + { + for (const auto &[clonedModel, oldCanonicalLink] : + this->dataPtr->oldModelCanonicalLink) + { + auto iter = this->dataPtr->oldToClonedCanonicalLink.find( + oldCanonicalLink); + if (iter == this->dataPtr->oldToClonedCanonicalLink.end()) + { + ignerr << "Error: attempted to clone model(s) with canonical link(s), " + << "but entity [" << oldCanonicalLink << "] was not cloned as a " + << "canonical link." << std::endl; + continue; + } + const auto clonedCanonicalLink = iter->second; + this->SetComponentData(clonedModel, + clonedCanonicalLink); + } + } + + return clonedEntity; +} + +///////////////////////////////////////////////// +Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, + const std::string &_name, bool _allowRename) +{ + auto uniqueNameGenerated = false; + + // Before cloning, we should make sure that: + // 1. The entity to be cloned exists + // 2. We can generate a unique name for the cloned entity + if (!this->HasEntity(_entity)) + { + ignerr << "Requested to clone entity [" << _entity + << "], but this entity does not exist." << std::endl; + return kNullEntity; + } + else if (!_name.empty() && !_allowRename) + { + if (kNullEntity != this->EntityByComponents(components::Name(_name))) + { + ignerr << "Requested to clone entity [" << _entity + << "] with a name of [" << _name << "], but another entity already " + << "has this name." << std::endl; + return kNullEntity; + } + uniqueNameGenerated = true; + } + + auto clonedEntity = this->CreateEntity(); + + if (_parent != kNullEntity) + { + this->SetParentEntity(clonedEntity, _parent); + this->CreateComponent(clonedEntity, components::ParentEntity(_parent)); + } + + // make sure that the cloned entity has a unique name + auto clonedName = _name; + if (!uniqueNameGenerated) + { + if (clonedName.empty()) + { + auto originalNameComp = this->Component(_entity); + clonedName = + originalNameComp ? originalNameComp->Data() : "cloned_entity"; + } + uint64_t suffix = 1; + while (kNullEntity != this->EntityByComponents( + components::Name(clonedName + "_" + std::to_string(suffix)))) + suffix++; + clonedName += "_" + std::to_string(suffix); + } + this->CreateComponent(clonedEntity, components::Name(clonedName)); + + // copy all components from _entity to clonedEntity + for (const auto &type : this->ComponentTypes(_entity)) + { + // skip the Name and ParentEntity components since those were already + // handled above + if ((type == components::Name::typeId) || + (type == components::ParentEntity::typeId)) + continue; + + auto originalComp = this->ComponentImplementation(_entity, type); + auto clonedComp = originalComp->Clone(); + + this->CreateComponentImplementation(clonedEntity, type, clonedComp.get()); + } + + // keep track of canonical link information (for clones of models, the cloned + // model should not share the same canonical link as the original model) + if (auto modelCanonLinkComp = + this->Component(clonedEntity)) + { + // we're cloning a model, so we map the cloned model to the original + // model's canonical link + this->dataPtr->oldModelCanonicalLink[clonedEntity] = + modelCanonLinkComp->Data(); + } + else if (this->Component(clonedEntity)) + { + // we're cloning a canonical link, so we map the original canonical link + // to the cloned canonical link + this->dataPtr->oldToClonedCanonicalLink[_entity] = clonedEntity; + } + + for (const auto &childEntity : + this->EntitiesByComponents(components::ParentEntity(_entity))) + { + auto clonedChild = this->CloneImpl(childEntity, clonedEntity, "", true); + if (kNullEntity == clonedChild) + { + ignerr << "Cloning child entity [" << childEntity << "] failed.\n"; + this->RequestRemoveEntity(clonedEntity); + return kNullEntity; + } + } + + return clonedEntity; +} + ///////////////////////////////////////////////// void EntityComponentManager::ClearNewlyCreatedEntities() { diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 9d7b8b5176..2d5ee89632 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -23,7 +23,10 @@ #include #include +#include "ignition/gazebo/components/CanonicalLink.hh" #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/config.hh" @@ -2585,6 +2588,223 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) } } +/// \brief Helper function for comparing the same type of component across two +/// different entities +/// \param[in] _ecm The entity component manager +/// \param[in] _entity1 The first entity +/// \param[in] _entity2 The second entity +/// \param[in] _equal Whether the component's data between _entity1 and +/// _entity2 should be equal (true) or not (false) +/// \tparam ComponentTypeT Component type +template +static void CompareEntityComponents(const EntityComponentManager &_ecm, + const Entity _entity1, const Entity _entity2, bool _equal) +{ + auto comp1 = _ecm.Component(_entity1); + ASSERT_NE(nullptr, comp1); + + auto comp2 = _ecm.Component(_entity2); + ASSERT_NE(nullptr, comp2); + + if (_equal) + EXPECT_EQ(*comp1, *comp2); + else + EXPECT_NE(*comp1, *comp2); +} + +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, CloneEntities) +{ + // testing entity cloning with the following entity structure: + // - topLevelEntity + // - childEntity1 + // - grandChildEntity1 (canonical link for childEntity1) + // - childEntity2 (canonical link for topLevelEntity) + + const auto allowRename = true; + const auto noAllowRename = false; + + Entity topLevelEntity = manager.CreateEntity(); + manager.CreateComponent(topLevelEntity, components::Name("topLevelEntity")); + manager.CreateComponent(topLevelEntity, IntComponent(123)); + manager.CreateComponent(topLevelEntity, StringComponent("string0")); + + Entity childEntity1 = manager.CreateEntity(); + manager.CreateComponent(childEntity1, components::Name("childEntity1")); + manager.CreateComponent(childEntity1, + components::ParentEntity(topLevelEntity)); + manager.CreateComponent(childEntity1, IntComponent(456)); + manager.CreateComponent(childEntity1, StringComponent("string1")); + + Entity grandChildEntity1 = manager.CreateEntity(); + manager.CreateComponent(grandChildEntity1, + components::Name("grandChildEntity1")); + manager.CreateComponent(grandChildEntity1, + components::ParentEntity(childEntity1)); + + Entity childEntity2 = manager.CreateEntity(); + manager.CreateComponent(childEntity2, components::Name("childEntity2")); + manager.CreateComponent(childEntity2, + components::ParentEntity(topLevelEntity)); + manager.CreateComponent(childEntity2, IntComponent(789)); + manager.CreateComponent(childEntity2, StringComponent("string2")); + + manager.CreateComponent(topLevelEntity, + components::ModelCanonicalLink(childEntity2)); + manager.CreateComponent(childEntity1, + components::ModelCanonicalLink(grandChildEntity1)); + manager.CreateComponent(childEntity2, components::CanonicalLink()); + manager.CreateComponent(grandChildEntity1, components::CanonicalLink()); + + EXPECT_EQ(4u, manager.EntityCount()); + + std::unordered_set clonedEntities; + + auto validateTopLevelClone = + [&](const Entity _clonedEntity) + { + EXPECT_NE(kNullEntity, _clonedEntity); + EXPECT_NE(_clonedEntity, topLevelEntity); + EXPECT_EQ(manager.ComponentTypes(_clonedEntity), + manager.ComponentTypes(topLevelEntity)); + EXPECT_FALSE(manager.EntityHasComponentType(_clonedEntity, + components::ParentEntity::typeId)); + CompareEntityComponents(manager, topLevelEntity, + _clonedEntity, false); + CompareEntityComponents(manager, topLevelEntity, + _clonedEntity, true); + CompareEntityComponents(manager, topLevelEntity, + _clonedEntity, true); + CompareEntityComponents(manager, + topLevelEntity, _clonedEntity, false); + }; + + // clone the topLevelEntity + auto clonedTopLevelEntity = + manager.Clone(topLevelEntity, kNullEntity, "", allowRename); + EXPECT_EQ(8u, manager.EntityCount()); + clonedEntities.insert(clonedTopLevelEntity); + validateTopLevelClone(clonedTopLevelEntity); + + auto validateChildClone = + [&](const Entity _clonedChild, const Entity _originalChild) + { + EXPECT_NE(kNullEntity, _clonedChild); + EXPECT_NE(_clonedChild, _originalChild); + EXPECT_EQ(manager.ComponentTypes(_clonedChild), + manager.ComponentTypes(_originalChild)); + auto parentComp = + manager.Component(_clonedChild); + ASSERT_NE(nullptr, parentComp); + EXPECT_EQ(clonedTopLevelEntity, parentComp->Data()); + CompareEntityComponents(manager, _clonedChild, + _originalChild, false); + CompareEntityComponents(manager, _clonedChild, + _originalChild, true); + CompareEntityComponents(manager, _clonedChild, + _originalChild, true); + }; + + auto validateGrandChildClone = + [&](const Entity _clonedEntity, bool _sameParent) + { + EXPECT_NE(kNullEntity, _clonedEntity); + EXPECT_EQ(manager.ComponentTypes(_clonedEntity), + manager.ComponentTypes(grandChildEntity1)); + CompareEntityComponents(manager, _clonedEntity, + grandChildEntity1, false); + CompareEntityComponents(manager, + _clonedEntity, grandChildEntity1, _sameParent); + EXPECT_TRUE(manager.EntitiesByComponents( + components::ParentEntity(_clonedEntity)).empty()); + }; + + // Verify that all child entities were properly cloned + auto clonedChildEntities = manager.EntitiesByComponents( + components::ParentEntity(clonedTopLevelEntity)); + EXPECT_EQ(2u, clonedChildEntities.size()); + for (const auto &child : clonedChildEntities) + { + clonedEntities.insert(child); + + auto clonedGrandChildren = manager.EntitiesByComponents( + components::ParentEntity(child)); + + auto comparedToOriginalChild = false; + auto intComp = manager.Component(child); + ASSERT_NE(nullptr, intComp); + if (intComp->Data() == 456) + { + validateChildClone(child, childEntity1); + CompareEntityComponents(manager, child, + childEntity1, false); + comparedToOriginalChild = true; + + ASSERT_EQ(1u, clonedGrandChildren.size()); + clonedEntities.insert(clonedGrandChildren[0]); + validateGrandChildClone(clonedGrandChildren[0], false); + auto parentComp = + manager.Component(clonedGrandChildren[0]); + ASSERT_NE(nullptr, parentComp); + EXPECT_EQ(child, parentComp->Data()); + EXPECT_NE(nullptr, manager.Component( + clonedGrandChildren[0])); + } + else if (intComp->Data() == 789) + { + validateChildClone(child, childEntity2); + EXPECT_NE(nullptr, manager.Component(child)); + comparedToOriginalChild = true; + + EXPECT_TRUE(clonedGrandChildren.empty()); + } + + EXPECT_TRUE(comparedToOriginalChild); + } + + // clone a child entity + auto grandChildParentComp = + manager.Component(grandChildEntity1); + ASSERT_NE(nullptr, grandChildParentComp); + auto clonedGrandChildEntity = manager.Clone(grandChildEntity1, + grandChildParentComp->Data(), "", allowRename); + EXPECT_EQ(9u, manager.EntityCount()); + clonedEntities.insert(clonedGrandChildEntity); + validateGrandChildClone(clonedGrandChildEntity, true); + + // Try cloning an entity with a name that already exists, but allow renaming. + // This should succeed and generate a cloned entity with a unique name. + const auto existingName = "grandChildEntity1"; + EXPECT_NE(kNullEntity, + manager.EntityByComponents(components::Name(existingName))); + auto renamedClonedEntity = manager.Clone(grandChildEntity1, + grandChildParentComp->Data(), existingName, allowRename); + EXPECT_EQ(10u, manager.EntityCount()); + clonedEntities.insert(clonedGrandChildEntity); + validateGrandChildClone(renamedClonedEntity, true); + + // Try cloning an entity with a name that already exists, without allowing + // renaming. This should fail since entities should have unique names. + auto failedClonedEntity = manager.Clone(grandChildEntity1, + grandChildParentComp->Data(), existingName, noAllowRename); + EXPECT_EQ(10u, manager.EntityCount()); + EXPECT_EQ(kNullEntity, failedClonedEntity); + + // make sure that the name given to each cloned entity is unique + EXPECT_EQ(5u, clonedEntities.size()); + for (const auto &entity : clonedEntities) + { + auto nameComp = manager.Component(entity); + ASSERT_NE(nullptr, nameComp); + EXPECT_EQ(1u, manager.EntitiesByComponents(*nameComp).size()); + } + + // try to clone an entity that does not exist + EXPECT_EQ(kNullEntity, manager.Clone(kNullEntity, topLevelEntity, "", + allowRename)); + EXPECT_EQ(10u, manager.EntityCount()); +} + ///////////////////////////////////////////////// // Check that some widely used deprecated APIs still work TEST_P(EntityComponentManagerFixture, Deprecated) diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 32bd92ac91..95ac9b187d 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -1225,12 +1225,10 @@ bool IgnRenderer::GeneratePreview(const sdf::Root &_sdf) } this->dataPtr->spawnPreview = this->dataPtr->renderUtil.SceneManager().CreateLight( - lightId, light, + lightId, light, light.Name(), this->dataPtr->renderUtil.SceneManager().WorldId()); this->dataPtr->renderUtil.SceneManager().CreateLightVisual( - lightVisualId, light, lightId); - - + lightVisualId, light, light.Name(), lightId); this->dataPtr->previewIds.push_back(lightId); this->dataPtr->previewIds.push_back(lightVisualId); diff --git a/src/gui/plugins/spawn/Spawn.cc b/src/gui/plugins/spawn/Spawn.cc index eaac31e183..76e22bd4ee 100644 --- a/src/gui/plugins/spawn/Spawn.cc +++ b/src/gui/plugins/spawn/Spawn.cc @@ -445,9 +445,9 @@ bool SpawnPrivate::GeneratePreview(const sdf::Root &_sdf) return false; } this->spawnPreview = this->sceneManager.CreateLight( - lightId, light, this->sceneManager.WorldId()); + lightId, light, light.Name(), this->sceneManager.WorldId()); this->sceneManager.CreateLightVisual( - lightVisualId, light, lightId); + lightVisualId, light, light.Name(), lightId); this->previewIds.push_back(lightId); this->previewIds.push_back(lightVisualId); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 02d67c53c2..2a2224255a 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -162,31 +162,33 @@ class ignition::gazebo::RenderUtilPrivate newModels; /// \brief New links to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id + /// [0] entity id, [1] SDF DOM, [2] parent entity id public: std::vector> newLinks; /// \brief New visuals to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id + /// [0] entity id, [1] SDF DOM, [2] parent entity id public: std::vector> newVisuals; /// \brief New actors to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id - public: std::vector> newActors; + /// [0] entity id, [1] SDF DOM, [2] actor name, [3] parent entity id + public: std::vector> + newActors; /// \brief New lights to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id - public: std::vector> newLights; + /// [0] entity id, [1] SDF DOM, [2] light name, [3] parent entity id + public: std::vector> + newLights; /// \brief A map of entity light ids and light visuals public: std::map matchLightWithVisuals; /// \brief New sensors to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id + /// [0] entity id, [1] SDF DOM, [2] parent entity id public: std::vector> newSensors; /// \brief New particle emitter to be created. The elements in the tuple are: - /// [0] entity id, [1], particle emitter, [2] parent entity id + /// [0] entity id, [1] particle emitter, [2] parent entity id public: std::vector> newParticleEmitters; @@ -1012,13 +1014,14 @@ void RenderUtil::Update() for (const auto &actor : newActors) { this->dataPtr->sceneManager.CreateActor( - std::get<0>(actor), std::get<1>(actor), std::get<2>(actor)); + std::get<0>(actor), std::get<1>(actor), std::get<2>(actor), + std::get<3>(actor)); } for (const auto &light : newLights) { - this->dataPtr->sceneManager.CreateLight( - std::get<0>(light), std::get<1>(light), std::get<2>(light)); + this->dataPtr->sceneManager.CreateLight(std::get<0>(light), + std::get<1>(light), std::get<2>(light), std::get<3>(light)); // TODO(anyone) This needs to be updated for when sensors and GUI use // the same scene @@ -1033,7 +1036,7 @@ void RenderUtil::Update() { rendering::VisualPtr lightVisual = this->dataPtr->sceneManager.CreateLightVisual( - id, std::get<1>(light), std::get<0>(light)); + id, std::get<1>(light), std::get<2>(light), std::get<0>(light)); this->dataPtr->matchLightWithVisuals[std::get<0>(light)] = id; break; } @@ -1421,9 +1424,8 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - this->newModels.push_back( - std::make_tuple(_entity, model, _parent->Data(), - _info.iterations)); + this->newModels.push_back(std::make_tuple(_entity, model, + _parent->Data(), _info.iterations)); this->modelToModelEntities[_parent->Data()].push_back(_entity); return true; }); @@ -1490,9 +1492,8 @@ void RenderUtilPrivate::CreateRenderingEntities( if (auto temp = _ecm.Component(_entity)) { // get the uniform temperature for the entity - this->entityTemp[_entity] = - std::make_tuple( - temp->Data().Kelvin(), 0.0, ""); + this->entityTemp[_entity] = std::make_tuple + (temp->Data().Kelvin(), 0.0, ""); } else { @@ -1520,24 +1521,26 @@ void RenderUtilPrivate::CreateRenderingEntities( }); // actors - _ecm.Each( + _ecm.Each( [&](const Entity &_entity, const components::Actor *_actor, + const components::Name *_name, const components::ParentEntity *_parent) -> bool { - this->newActors.push_back( - std::make_tuple(_entity, _actor->Data(), _parent->Data())); + this->newActors.push_back(std::make_tuple(_entity, _actor->Data(), + _name->Data(), _parent->Data())); return true; }); // lights - _ecm.Each( + _ecm.Each( [&](const Entity &_entity, const components::Light *_light, + const components::Name *_name, const components::ParentEntity *_parent) -> bool { - this->newLights.push_back( - std::make_tuple(_entity, _light->Data(), _parent->Data())); + this->newLights.push_back(std::make_tuple(_entity, _light->Data(), + _name->Data(), _parent->Data())); return true; }); @@ -1703,9 +1706,8 @@ void RenderUtilPrivate::CreateRenderingEntities( sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - this->newModels.push_back( - std::make_tuple(_entity, model, _parent->Data(), - _info.iterations)); + this->newModels.push_back(std::make_tuple(_entity, model, + _parent->Data(), _info.iterations)); this->modelToModelEntities[_parent->Data()].push_back(_entity); return true; }); @@ -1772,9 +1774,8 @@ void RenderUtilPrivate::CreateRenderingEntities( if (auto temp = _ecm.Component(_entity)) { // get the uniform temperature for the entity - this->entityTemp[_entity] = - std::make_tuple( - temp->Data().Kelvin(), 0.0, ""); + this->entityTemp[_entity] = std::make_tuple + (temp->Data().Kelvin(), 0.0, ""); } else { @@ -1802,24 +1803,27 @@ void RenderUtilPrivate::CreateRenderingEntities( }); // actors - _ecm.EachNew( + _ecm.EachNew( [&](const Entity &_entity, const components::Actor *_actor, + const components::Name *_name, const components::ParentEntity *_parent) -> bool { this->newActors.push_back( - std::make_tuple(_entity, _actor->Data(), _parent->Data())); + std::make_tuple(_entity, _actor->Data(), _name->Data(), + _parent->Data())); return true; }); // lights - _ecm.EachNew( + _ecm.EachNew( [&](const Entity &_entity, const components::Light *_light, + const components::Name *_name, const components::ParentEntity *_parent) -> bool { - this->newLights.push_back( - std::make_tuple(_entity, _light->Data(), _parent->Data())); + this->newLights.push_back(std::make_tuple(_entity, _light->Data(), + _name->Data(), _parent->Data())); return true; }); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index ee19fae158..db33506eec 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -701,7 +701,7 @@ rendering::MaterialPtr SceneManager::LoadMaterial( ///////////////////////////////////////////////// rendering::VisualPtr SceneManager::CreateActor(Entity _id, - const sdf::Actor &_actor, Entity _parentId) + const sdf::Actor &_actor, const std::string &_name, Entity _parentId) { if (!this->dataPtr->scene) return rendering::VisualPtr(); @@ -717,9 +717,6 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, return rendering::VisualPtr(); } - std::string name = _actor.Name().empty() ? std::to_string(_id) : - _actor.Name(); - rendering::VisualPtr parent; if (_parentId != this->dataPtr->worldId) { @@ -727,14 +724,15 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, if (it == this->dataPtr->visuals.end()) { ignerr << "Parent entity with Id: [" << _parentId << "] not found. " - << "Not adding actor with ID[" << _id - << "] and name [" << name << "] to the rendering scene." + << "Not adding actor with ID [" << _id + << "] and name [" << _name << "] to the rendering scene." << std::endl; return rendering::VisualPtr(); } parent = it->second; } + std::string name = _name; if (parent) name = parent->Name() + "::" + name; @@ -1007,7 +1005,7 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, ///////////////////////////////////////////////// rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, - const sdf::Light &_light, Entity _parentId) + const sdf::Light &_light, const std::string &_name, Entity _parentId) { if (!this->dataPtr->scene) return rendering::VisualPtr(); @@ -1019,9 +1017,6 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, return rendering::VisualPtr(); } - std::string name = _light.Name().empty() ? std::to_string(_id) : - _light.Name(); - rendering::LightPtr lightParent; auto it = this->dataPtr->lights.find(_parentId); if (it != this->dataPtr->lights.end()) @@ -1031,13 +1026,13 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, else { ignerr << "Parent entity with Id: [" << _parentId << "] not found. " - << "Not adding light visual with ID[" << _id - << "] and name [" << name << "] to the rendering scene." + << "Not adding light visual with ID [" << _id + << "] and name [" << _name << "] to the rendering scene." << std::endl; return rendering::VisualPtr(); } - name = lightParent->Name() + "::" + name + "Visual"; + std::string name = lightParent->Name() + "::" + _name + "Visual"; if (this->dataPtr->scene->HasVisualName(name)) { @@ -1077,7 +1072,7 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, ///////////////////////////////////////////////// rendering::LightPtr SceneManager::CreateLight(Entity _id, - const sdf::Light &_light, Entity _parentId) + const sdf::Light &_light, const std::string &_name, Entity _parentId) { if (!this->dataPtr->scene) return rendering::LightPtr(); @@ -1102,8 +1097,7 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, parent = it->second; } - std::string name = _light.Name().empty() ? std::to_string(_id) : - _light.Name(); + std::string name = _name; if (parent) name = parent->Name() + "::" + name; diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index c395c4c6ff..66c0a80ec9 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -725,9 +725,41 @@ bool CreateCommand::Execute() } case msgs::EntityFactory::kCloneName: { - // TODO(louise) Implement clone - ignerr << "Cloning an entity is not yet supported." << std::endl; - return false; + auto validClone = false; + auto clonedEntity = kNullEntity; + auto entityToClone = this->iface->ecm->EntityByComponents( + components::Name(createMsg->clone_name())); + if (kNullEntity != entityToClone) + { + auto parentComp = + this->iface->ecm->Component(entityToClone); + + // TODO(anyone) add better support for creating non-top level entities. + // For now, we will only clone top level entities + if (parentComp && parentComp->Data() == this->iface->worldEntity) + { + auto parentEntity = parentComp->Data(); + clonedEntity = this->iface->ecm->Clone(entityToClone, + parentEntity, createMsg->name(), createMsg->allow_renaming()); + validClone = kNullEntity != clonedEntity; + } + } + + if (!validClone) + { + ignerr << "Request to clone an entity named [" + << createMsg->clone_name() << "] failed." << std::endl; + return false; + } + + if (createMsg->has_pose()) + { + // TODO(anyone) handle if relative_to is filled + auto pose = gazebo::convert(createMsg->pose()); + this->iface->ecm->SetComponentData(clonedEntity, + pose); + } + return true; } default: {