diff --git a/include/ignition/gazebo/detail/View.hh b/include/ignition/gazebo/detail/View.hh index 93ea32c1b1..7b6ffd34b8 100644 --- a/include/ignition/gazebo/detail/View.hh +++ b/include/ignition/gazebo/detail/View.hh @@ -280,6 +280,10 @@ template bool View::NotifyComponentRemoval(const Entity _entity, const ComponentTypeId _typeId) { + // if entity is still marked as to add, remove from the view + if (this->RequiresComponent(_typeId)) + this->toAddEntities.erase(_entity); + // make sure that _typeId is a type required by the view and that _entity is // already a part of the view if (!this->RequiresComponent(_typeId) || diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 1a094afb01..995ecd5423 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -421,11 +421,19 @@ Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, } else if (!_name.empty() && !_allowRename) { - // if there is an entity with the same name and user indicated renaming is + // Get the entity's original parent. This is used to make sure we get + // the correct entity. For example, two different models may have a + // child with the name "link". + auto origParentComp = + this->Component(_entity); + + // If there is an entity with the same name and user indicated renaming is // not allowed then return null entity. // If the entity or one of its ancestor has a Recreate component then carry // on since the ECM is supposed to create a new entity with the same name. - Entity ent = this->EntityByComponents(components::Name(_name)); + Entity ent = this->EntityByComponents(components::Name(_name), + components::ParentEntity(origParentComp->Data())); + bool hasRecreateComp = false; Entity recreateEnt = ent; while (recreateEnt != kNullEntity && !hasRecreateComp) @@ -1061,6 +1069,14 @@ bool EntityComponentManager::CreateComponentImplementation( this->dataPtr->createdCompTypes.insert(_componentTypeId); + // If the component is a components::ParentEntity, then make sure to + // update the entities graph. + if (_componentTypeId == components::ParentEntity::typeId) + { + auto parentComp = this->Component(_entity); + this->SetParentEntity(_entity, parentComp->Data()); + } + return updateData; } diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 798044386b..c0a765af27 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -822,6 +822,10 @@ void SimulationRunner::Step(const UpdateInfo &_info) IGN_PROFILE("SimulationRunner::Step"); this->currentInfo = _info; + // Process new ECM state information, typically sent from the GUI after + // a change was made to the GUI's ECM. + this->ProcessNewWorldControlState(); + // Publish info this->PublishStats(); @@ -842,10 +846,6 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Update all the systems. this->UpdateSystems(); - // Recreate any entities that have the Recreate component - // The entities will have different Entity ids but keep the same name - this->ProcessRecreateEntitiesCreate(); - if (!this->Paused() && this->requestedRunToSimTime > std::chrono::steady_clock::duration::zero() && @@ -873,6 +873,12 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Clear all new entities this->entityCompMgr.ClearNewlyCreatedEntities(); + // Recreate any entities that have the Recreate component + // The entities will have different Entity ids but keep the same name + // Make sure this happens after ClearNewlyCreatedEntities, otherwise the + // cloned entities will loose their "New" state. + this->ProcessRecreateEntitiesCreate(); + // Process entity removals. this->entityCompMgr.ProcessRemoveEntityRequests(); @@ -1148,11 +1154,13 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, { std::lock_guard lock(this->msgBufferMutex); - // update the server ECM if the request contains SerializedState information + // Copy the state information if it exists if (_req.has_state()) - this->entityCompMgr.SetState(_req.state()); - // TODO(anyone) notify server systems of changes made to the ECM, if there - // were any? + { + if (this->newWorldControlState == nullptr) + this->newWorldControlState = _req.New(); + this->newWorldControlState->CopyFrom(_req); + } WorldControl control; control.pause = _req.world_control().pause(); @@ -1182,7 +1190,7 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, { control.runToSimTime = std::chrono::seconds( _req.world_control().run_to_sim_time().sec()) + - std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec()); + std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec()); } this->worldControls.push_back(control); @@ -1191,6 +1199,22 @@ bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, return true; } +///////////////////////////////////////////////// +void SimulationRunner::ProcessNewWorldControlState() +{ + std::lock_guard lock(this->msgBufferMutex); + // update the server ECM if the request contains SerializedState information + if (this->newWorldControlState && this->newWorldControlState->has_state()) + { + this->entityCompMgr.SetState(this->newWorldControlState->state()); + + delete this->newWorldControlState; + this->newWorldControlState = nullptr; + } + // TODO(anyone) notify server systems of changes made to the ECM, if there + // were any? +} + ///////////////////////////////////////////////// bool SimulationRunner::OnPlaybackControl(const msgs::LogPlaybackControl &_req, msgs::Boolean &_res) @@ -1288,7 +1312,7 @@ void SimulationRunner::ProcessRecreateEntitiesCreate() IGN_PROFILE("SimulationRunner::ProcessRecreateEntitiesCreate"); // clone the original entities - std::set clonedEntities; + std::set entitiesToRemoveRecreateComp; for (auto & ent : this->entitiesToRecreate) { auto nameComp = this->entityCompMgr.Component(ent); @@ -1297,14 +1321,19 @@ void SimulationRunner::ProcessRecreateEntitiesCreate() // set allowRenaming to false so the entities keep their original name Entity clonedEntity = this->entityCompMgr.Clone(ent, parentComp->Data(), nameComp->Data(), false); - clonedEntities.insert(clonedEntity); - + entitiesToRemoveRecreateComp.insert(clonedEntity); + entitiesToRemoveRecreateComp.insert(ent); } + // remove the Recreate component so they do not get recreated again in the // next iteration - for (auto &ent : clonedEntities) + for (auto &ent : entitiesToRemoveRecreateComp) { - this->entityCompMgr.RemoveComponent(ent); + if (!this->entityCompMgr.RemoveComponent(ent)) + { + ignerr << "Failed to remove Recreate component from entity[" + << ent << "]" << std::endl; + } } this->entitiesToRecreate.clear(); diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 407483b4b3..509e2b957a 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -475,6 +475,10 @@ namespace ignition /// Reccreate the entities by cloning from the original ones. private: void ProcessRecreateEntitiesCreate(); + /// \brief Process the new world state message, if it is present. + /// See the newWorldControlState variable below. + private: void ProcessNewWorldControlState(); + /// \brief This is used to indicate that a stop event has been received. private: std::atomic stopReceived{false}; @@ -639,6 +643,10 @@ namespace ignition /// \brief A set of entities that need to be recreated private: std::set entitiesToRecreate; + /// \brief Holds new world state information so that it can processed + /// at the appropriate time. + private: msgs::WorldControlState *newWorldControlState{nullptr}; + friend class LevelManager; }; } diff --git a/src/gui/plugins/component_inspector/ModelEditor.cc b/src/gui/plugins/component_inspector/ModelEditor.cc index d40cae6e1a..a40e8117da 100644 --- a/src/gui/plugins/component_inspector/ModelEditor.cc +++ b/src/gui/plugins/component_inspector/ModelEditor.cc @@ -32,6 +32,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Recreate.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" @@ -181,6 +182,9 @@ void ModelEditor::Update(const UpdateInfo &, linkSdf.SetName(linkName); auto entity = this->dataPtr->entityCreator->CreateEntities(&linkSdf); this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity); + // Make sure to mark the parent as needing recreation. This will + // tell the server to rebuild the model with the new link. + _ecm.CreateComponent(eta.parentEntity, components::Recreate()); // traverse the tree and add all new entities created by the entity // creator to the set diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 9407cc8d33..f2414936ba 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -122,6 +123,7 @@ #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/Recreate.hh" #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/SlipComplianceCmd.hh" #include "ignition/gazebo/components/Static.hh" @@ -612,6 +614,13 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief A map between collision entity ids in the ECM to FreeGroup Entities /// in ign-physics. public: EntityFreeGroupMap entityFreeGroupMap; + + /// \brief Set of links that were added to an existing model. This set + /// is used to track links that were added to an existing model, such as + /// through the GUI model editor, so that we can avoid premature creation + /// of links and collision elements. This also lets us suppress some + /// invalid error messages. + public: std::set linkAddedToModel; }; ////////////////////////////////////////////////// @@ -778,6 +787,9 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) ////////////////////////////////////////////////// void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) { + // Clear the set of links that were added to a model. + this->linkAddedToModel.clear(); + this->CreateWorldEntities(_ecm); this->CreateModelEntities(_ecm); this->CreateLinkEntities(_ecm); @@ -879,6 +891,9 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + if (_ecm.EntityHasComponentType(_entity, components::Recreate::typeId)) + return true; + // Check if model already exists if (this->entityModelMap.HasEntity(_entity)) { @@ -1011,6 +1026,18 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + // If the parent model is scheduled for recreation, then do not + // try to create a new link. This situation can occur when a link + // is added to a model from the GUI model editor. + if (_ecm.EntityHasComponentType(_parent->Data(), + components::Recreate::typeId)) + { + // Add this entity to the set of newly added links to existing + // models. + this->linkAddedToModel.insert(_entity); + return true; + } + // Check if link already exists if (this->entityLinkMap.HasEntity(_entity)) { @@ -1072,6 +1099,14 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) const components::CollisionElement *_collElement, const components::ParentEntity *_parent) -> bool { + // Check to see if this collision's parent is a link that was + // not created because the parent model is marked for recreation. + if (this->linkAddedToModel.find(_parent->Data()) != + this->linkAddedToModel.end()) + { + return true; + } + if (this->entityCollisionMap.HasEntity(_entity)) { ignwarn << "Collision entity [" << _entity @@ -2182,14 +2217,23 @@ std::map PhysicsPrivate::ChangedLinks( _ecm.Each( [&](const Entity &_entity, components::Link *) -> bool { - if (this->staticEntities.find(_entity) != this->staticEntities.end()) + if (this->staticEntities.find(_entity) != this->staticEntities.end() || + _ecm.EntityHasComponentType(_entity, components::Recreate::typeId)) + { return true; + } + // This `once` variable is here to aid in debugging, make sure to + // remove it. auto linkPhys = this->entityLinkMap.Get(_entity); if (nullptr == linkPhys) { - ignerr << "Internal error: link [" << _entity - << "] not in entity map" << std::endl; + if (this->linkAddedToModel.find(_entity) == + this->linkAddedToModel.end()) + { + ignerr << "Internal error: link [" << _entity + << "] not in entity map" << std::endl; + } return true; } @@ -2339,8 +2383,12 @@ bool PhysicsPrivate::GetFrameDataRelativeToWorld(const Entity _entity, auto entityPhys = this->entityLinkMap.Get(_entity); if (nullptr == entityPhys) { - ignerr << "Internal error: entity [" << _entity - << "] not in entity map" << std::endl; + // Suppress error message if the link has just been added to the model. + if (this->linkAddedToModel.find(_entity) == this->linkAddedToModel.end()) + { + ignerr << "Internal error: entity [" << _entity + << "] not in entity map" << std::endl; + } return false; } @@ -2437,7 +2485,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, if (parentModelPoseIt == this->modelWorldPoses.end()) { ignerr << "Internal error: parent model [" << parentEntity - << "] does not have a world pose available" << std::endl; + << "] does not have a world pose available for child entity[" + << entity << "]" << std::endl; continue; } const math::Pose3d &parentWorldPose = parentModelPoseIt->second; diff --git a/test/integration/recreate_entities.cc b/test/integration/recreate_entities.cc index 1c1e1e1fb1..0a6b6908c2 100644 --- a/test/integration/recreate_entities.cc +++ b/test/integration/recreate_entities.cc @@ -90,6 +90,7 @@ TEST_F(RecreateEntitiesFixture, RecreateEntities) // recreate entities only once so set it back to false recreateEntities = false; + return; } }); server.AddSystem(testSystem.systemPtr); @@ -306,4 +307,12 @@ TEST_F(RecreateEntitiesFixture, RecreateEntities) EXPECT_NE(originalsphLinkEntity, sphLinkEntity); EXPECT_NE(originalcapLinkEntity, capLinkEntity); EXPECT_NE(originalellipLinkEntity, ellipLinkEntity); + + // 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()); }