diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index b87ada778c..6f62a23b15 100644 --- a/include/gz/sim/EntityComponentManager.hh +++ b/include/gz/sim/EntityComponentManager.hh @@ -675,6 +675,14 @@ namespace gz /// \return True if there are components marked for removal. public: bool HasRemovedComponents() const; + /// \brief Get an Entity based on a name component that is associated + /// with the entity. + /// \param[in] _name Name associated with the Entity + /// \return The Entity, if an Entity with the given name exists, + /// otherwise return std::nullopt. + public: std::optional EntityByName( + const std::string &_name) const; + /// \brief Clear the list of newly added entities so that a call to /// EachAdded after this will have no entities to iterate. This function /// is protected to facilitate testing. diff --git a/include/gz/sim/SdfEntityCreator.hh b/include/gz/sim/SdfEntityCreator.hh index 8e5292ef10..be96451b8a 100644 --- a/include/gz/sim/SdfEntityCreator.hh +++ b/include/gz/sim/SdfEntityCreator.hh @@ -93,6 +93,13 @@ namespace gz /// \return World entity. public: Entity CreateEntities(const sdf::World *_world); + /// \brief Create all entities that exist in the sdf::World object and + /// load their plugins. + /// \param[in] _world SDF world object. + /// \param[in] _worldEntity The world entity object. + public: void CreateEntities(const sdf::World *_world, + Entity _worldEntity); + /// \brief Create all entities that exist in the sdf::Model object and /// load their plugins. Also loads plugins of child sensors. /// \param[in] _model SDF model object. @@ -186,6 +193,9 @@ namespace gz private: Entity CreateEntities(const sdf::Model *_model, bool _staticParent); + /// \brief Load plugins for all models + private: void LoadModelPlugins(); + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/include/gz/sim/components/Performer.hh b/include/gz/sim/components/Performer.hh index 2323b67275..58c0092047 100644 --- a/include/gz/sim/components/Performer.hh +++ b/include/gz/sim/components/Performer.hh @@ -17,6 +17,7 @@ #ifndef GZ_SIM_COMPONENTS_PERFORMER_HH_ #define GZ_SIM_COMPONENTS_PERFORMER_HH_ +#include #include #include @@ -34,6 +35,11 @@ namespace components /// \brief This component identifies an entity as being a performer. using Performer = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.Performer", Performer) + + /// \brief This component contains the performer reference name. + using PerformerRef = Component; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.PerformerRef", PerformerRef) } } } diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 5ab9b0a635..2fd4e03c6e 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -2314,3 +2314,15 @@ void EntityComponentManager::ResetTo(const EntityComponentManager &_other) tmpCopy.ApplyEntityDiff(*this, ecmDiff); this->CopyFrom(tmpCopy); } + +///////////////////////////////////////////////// +std::optional EntityComponentManager::EntityByName( + const std::string &_name) const +{ + std::optional entity; + Entity entByName = EntityByComponents(components::Name(_name)); + if (entByName != kNullEntity) + entity = entByName; + + return entity; +} diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index bc7f03ff99..f7a6c11e7e 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -3421,6 +3421,23 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(321, comp->Data()); } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, EntityByName) +{ + // Create an entity, and give it a name + Entity entity = manager.CreateEntity(); + manager.CreateComponent(entity, components::Name("entity_name_a")); + + // Try to get an entity that doesn't exist + std::optional entityByName = manager.EntityByName("a_bad_name"); + EXPECT_FALSE(entityByName); + + entityByName = manager.EntityByName("entity_name_a"); + EXPECT_TRUE(entityByName); + CompareEntityComponents(manager, entity, + *entityByName, true); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat, diff --git a/src/LevelManager.cc b/src/LevelManager.cc index e4091bace3..a41a56d335 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -42,7 +42,6 @@ #include "gz/sim/components/LevelEntityNames.hh" #include "gz/sim/components/Light.hh" #include "gz/sim/components/LinearVelocity.hh" -#include "gz/sim/components/LinearVelocitySeed.hh" #include "gz/sim/components/MagneticField.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" @@ -50,14 +49,9 @@ #include "gz/sim/components/Performer.hh" #include "gz/sim/components/PerformerLevels.hh" #include "gz/sim/components/Physics.hh" -#include "gz/sim/components/PhysicsEnginePlugin.hh" #include "gz/sim/components/Pose.hh" -#include "gz/sim/components/RenderEngineGuiPlugin.hh" -#include "gz/sim/components/RenderEngineServerHeadless.hh" -#include "gz/sim/components/RenderEngineServerPlugin.hh" #include "gz/sim/components/Scene.hh" #include "gz/sim/components/SphericalCoordinates.hh" -#include "gz/sim/components/Wind.hh" #include "gz/sim/components/World.hh" #include "SimulationRunner.hh" @@ -79,183 +73,57 @@ LevelManager::LevelManager(SimulationRunner *_runner, const bool _useLevels) this->runner->entityCompMgr, this->runner->eventMgr); - this->ReadLevelPerformerInfo(); - this->CreatePerformers(); - std::string service = transport::TopicUtils::AsValidTopic("/world/" + - this->runner->sdfWorld->Name() + "/level/set_performer"); + this->runner->sdfWorld.Name() + "/level/set_performer"); if (service.empty()) { gzerr << "Failed to generate set_performer topic for world [" - << this->runner->sdfWorld->Name() << "]" << std::endl; + << this->runner->sdfWorld.Name() << "]" << std::endl; return; } this->node.Advertise(service, &LevelManager::OnSetPerformer, this); } ///////////////////////////////////////////////// -void LevelManager::ReadLevelPerformerInfo() +void LevelManager::ReadLevelPerformerInfo(const sdf::World &_world) { - // \todo(anyone) Use SdfEntityCreator to avoid duplication - this->worldEntity = this->runner->entityCompMgr.CreateEntity(); - - // World components - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::World()); - this->runner->entityCompMgr.CreateComponent( - this->worldEntity, components::Name(this->runner->sdfWorld->Name())); - - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::Gravity(this->runner->sdfWorld->Gravity())); - - auto physics = this->runner->sdfWorld->PhysicsByIndex(0); - if (!physics) - { - physics = this->runner->sdfWorld->PhysicsDefault(); - } - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::Physics(*physics)); - - // Populate physics options that aren't accessible outside the Element() - // See https://github.com/osrf/sdformat/issues/508 - if (physics->Element() && physics->Element()->HasElement("dart")) - { - auto dartElem = physics->Element()->GetElement("dart"); - - if (dartElem->HasElement("collision_detector")) - { - auto collisionDetector = - dartElem->Get("collision_detector"); - - this->runner->entityCompMgr.CreateComponent(worldEntity, - components::PhysicsCollisionDetector(collisionDetector)); - } - if (dartElem->HasElement("solver") && - dartElem->GetElement("solver")->HasElement("solver_type")) - { - auto solver = - dartElem->GetElement("solver")->Get("solver_type"); - - this->runner->entityCompMgr.CreateComponent(worldEntity, - components::PhysicsSolver(solver)); - } - } - - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::MagneticField(this->runner->sdfWorld->MagneticField())); - - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::PhysicsEnginePlugin( - this->runner->serverConfig.PhysicsEngine())); - - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::RenderEngineServerPlugin( - this->runner->serverConfig.RenderEngineServer())); - - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::RenderEngineServerHeadless( - this->runner->serverConfig.HeadlessRendering())); - - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::RenderEngineGuiPlugin( - this->runner->serverConfig.RenderEngineGui())); - - auto worldElem = this->runner->sdfWorld->Element(); - - // Create Wind - auto windEntity = this->runner->entityCompMgr.CreateEntity(); - this->runner->entityCompMgr.CreateComponent(windEntity, components::Wind()); - this->runner->entityCompMgr.CreateComponent( - windEntity, components::WorldLinearVelocity( - this->runner->sdfWorld->WindLinearVelocity())); - // Initially the wind linear velocity is used as the seed velocity - this->runner->entityCompMgr.CreateComponent( - windEntity, components::WorldLinearVelocitySeed( - this->runner->sdfWorld->WindLinearVelocity())); - - this->entityCreator->SetParent(windEntity, this->worldEntity); - - // scene - if (this->runner->sdfWorld->Scene()) - { - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::Scene(*this->runner->sdfWorld->Scene())); - } - - // atmosphere - if (this->runner->sdfWorld->Atmosphere()) - { - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::Atmosphere(*this->runner->sdfWorld->Atmosphere())); - } - - // spherical coordinates - if (this->runner->sdfWorld->SphericalCoordinates()) - { - this->runner->entityCompMgr.CreateComponent(this->worldEntity, - components::SphericalCoordinates( - *this->runner->sdfWorld->SphericalCoordinates())); - } - // TODO(anyone) This should probably go somewhere else as it is a global // constant. const std::string kPluginName{"gz::sim"}; - sdf::ElementPtr pluginElem; - // Get the gz::sim plugin element - for (auto plugin = worldElem->FindElement("plugin"); plugin; - plugin = plugin->GetNextElement("plugin")) + bool found = false; + for (const sdf::Plugin &plugin : _world.Plugins()) { - if (plugin->Get("name") == kPluginName) + if (plugin.Name() == kPluginName) { - pluginElem = plugin; + this->ReadPerformers(plugin); + if (this->useLevels) + this->ReadLevels(plugin); + found = true; break; } } - if (pluginElem == nullptr) - { - if (this->useLevels) - { - gzerr << "Could not find a plugin tag with name " << kPluginName - << ". Levels and distributed simulation will not work.\n"; - } - } - else + if (!found && this->useLevels) { - this->ReadPerformers(pluginElem); - if (this->useLevels) - this->ReadLevels(pluginElem); + gzerr << "Could not find a plugin tag with name " << kPluginName + << ". Levels and distributed simulation will not work.\n"; } - - this->ConfigureDefaultLevel(); - - // Load world plugins. - this->runner->EventMgr().Emit(this->worldEntity, - this->runner->sdfWorld->Plugins()); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - this->runner->EventMgr().Emit(this->worldEntity, - this->runner->sdfWorld->Element()); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - - // Store the world's SDF DOM to be used when saving the world to file - this->runner->entityCompMgr.CreateComponent( - worldEntity, components::WorldSdf(*this->runner->sdfWorld)); } ///////////////////////////////////////////////// -void LevelManager::ReadPerformers(const sdf::ElementPtr &_sdf) +void LevelManager::ReadPerformers(const sdf::Plugin &_plugin) { GZ_PROFILE("LevelManager::ReadPerformers"); - if (_sdf == nullptr) + sdf::ElementPtr sdf = _plugin.ToElement(); + if (sdf == nullptr) return; - if (_sdf->HasElement("performer")) + if (sdf->HasElement("performer")) { gzdbg << "Reading performer info\n"; - for (auto performer = _sdf->GetElement("performer"); performer; + for (auto performer = sdf->GetElement("performer"); performer; performer = performer->GetNextElement("performer")) { auto name = performer->Get("name"); @@ -263,6 +131,7 @@ void LevelManager::ReadPerformers(const sdf::ElementPtr &_sdf) Entity performerEntity = this->runner->entityCompMgr.CreateEntity(); // We use the ref to create a parent entity component later on std::string ref = performer->GetElement("ref")->GetValue()->GetAsString(); + if (this->performerMap.find(ref) == this->performerMap.end()) { this->performerMap[ref] = performerEntity; @@ -281,6 +150,8 @@ void LevelManager::ReadPerformers(const sdf::ElementPtr &_sdf) geometry.Load(performer->GetElement("geometry")); this->runner->entityCompMgr.CreateComponent(performerEntity, components::Performer()); + this->runner->entityCompMgr.CreateComponent(performerEntity, + components::PerformerRef(ref)); this->runner->entityCompMgr.CreateComponent(performerEntity, components::PerformerLevels()); this->runner->entityCompMgr.CreateComponent(performerEntity, @@ -288,8 +159,9 @@ void LevelManager::ReadPerformers(const sdf::ElementPtr &_sdf) this->runner->entityCompMgr.CreateComponent(performerEntity, components::Geometry(geometry)); - gzmsg << "Created performer [" << performerEntity << " / " << name << "]" - << std::endl; + gzmsg << "Created performer. EntityId[" << performerEntity + << "] EntityName[" << name << "] Ref[" << ref << "]" + << std::endl; } } @@ -380,16 +252,20 @@ bool LevelManager::OnSetPerformer(const msgs::StringMsg &_req, } ///////////////////////////////////////////////// -void LevelManager::ReadLevels(const sdf::ElementPtr &_sdf) +void LevelManager::ReadLevels(const sdf::Plugin &_plugin) { GZ_PROFILE("LevelManager::ReadLevels"); gzdbg << "Reading levels info\n"; - if (_sdf == nullptr) + sdf::ElementPtr sdf = _plugin.ToElement(); + if (sdf == nullptr) + return; + + if (!sdf->HasElement("level")) return; - for (auto level = _sdf->GetElement("level"); level; + for (auto level = sdf->GetElement("level"); level; level = level->GetNextElement("level")) { auto name = level->Get("name"); @@ -440,7 +316,15 @@ void LevelManager::ReadLevels(const sdf::ElementPtr &_sdf) this->runner->entityCompMgr.CreateComponent( levelEntity, components::LevelBuffer(buffer)); - this->entityCreator->SetParent(levelEntity, this->worldEntity); + auto worldEntity = + this->runner->entityCompMgr.EntityByComponents(components::World()); + + // All levels start inactive and unloaded. + this->UnloadLevel(levelEntity); + this->entityCreator->SetParent(levelEntity, worldEntity); + + gzdbg << "Created level with name[" << name << "] and pose[" + << pose << "]\n"; } } @@ -460,11 +344,17 @@ void LevelManager::ConfigureDefaultLevel() // Models for (uint64_t modelIndex = 0; - modelIndex < this->runner->sdfWorld->ModelCount(); ++modelIndex) + modelIndex < this->runner->sdfWorld.ModelCount(); ++modelIndex) { // There is no sdf::World::ModelByName so we have to iterate by index and // check if the model is in this level - auto model = this->runner->sdfWorld->ModelByIndex(modelIndex); + auto model = this->runner->sdfWorld.ModelByIndex(modelIndex); + if (!this->useLevels) + { + entityNamesInDefault.insert(model->Name()); + continue; + } + // If model is a performer, it will be handled separately if (this->performerMap.find(model->Name()) != this->performerMap.end()) { @@ -480,11 +370,18 @@ void LevelManager::ConfigureDefaultLevel() // Actors for (uint64_t actorIndex = 0; - actorIndex < this->runner->sdfWorld->ActorCount(); ++actorIndex) + actorIndex < this->runner->sdfWorld.ActorCount(); ++actorIndex) { // There is no sdf::World::ActorByName so we have to iterate by index and // check if the actor is in this level - auto actor = this->runner->sdfWorld->ActorByIndex(actorIndex); + auto actor = this->runner->sdfWorld.ActorByIndex(actorIndex); + + if (!this->useLevels) + { + entityNamesInDefault.insert(actor->Name()); + continue; + } + // If actor is a performer, it will be handled separately if (this->performerMap.find(actor->Name()) != this->performerMap.end()) { @@ -501,9 +398,9 @@ void LevelManager::ConfigureDefaultLevel() // Lights // We assume no performers are lights for (uint64_t lightIndex = 0; - lightIndex < this->runner->sdfWorld->LightCount(); ++lightIndex) + lightIndex < this->runner->sdfWorld.LightCount(); ++lightIndex) { - auto light = this->runner->sdfWorld->LightByIndex(lightIndex); + auto light = this->runner->sdfWorld.LightByIndex(lightIndex); if (this->entityNamesInLevels.find(light->Name()) == this->entityNamesInLevels.end()) { @@ -514,11 +411,12 @@ void LevelManager::ConfigureDefaultLevel() // Joints // We assume no performers are joints for (uint64_t jointIndex = 0; - jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex) + jointIndex < this->runner->sdfWorld.JointCount(); ++jointIndex) { - auto joint = this->runner->sdfWorld->JointByIndex(jointIndex); + auto joint = this->runner->sdfWorld.JointByIndex(jointIndex); - if (this->entityNamesInLevels.find(joint->Name()) == + if ( + this->entityNamesInLevels.find(joint->Name()) == this->entityNamesInLevels.end()) { entityNamesInDefault.insert(joint->Name()); @@ -530,57 +428,15 @@ void LevelManager::ConfigureDefaultLevel() defaultLevel, components::Level()); this->runner->entityCompMgr.CreateComponent( defaultLevel, components::DefaultLevel()); + this->runner->entityCompMgr.CreateComponent( + defaultLevel, components::Name("default")); this->runner->entityCompMgr.CreateComponent( defaultLevel, components::LevelEntityNames(entityNamesInDefault)); - this->entityCreator->SetParent(defaultLevel, this->worldEntity); -} - -///////////////////////////////////////////////// -void LevelManager::CreatePerformers() -{ - GZ_PROFILE("LevelManager::CreatePerformers"); - - if (this->worldEntity == kNullEntity) - { - gzerr << "Could not find the world entity while creating performers\n"; - return; - } - // Models - for (uint64_t modelIndex = 0; - modelIndex < this->runner->sdfWorld->ModelCount(); ++modelIndex) - { - auto model = this->runner->sdfWorld->ModelByIndex(modelIndex); - if (this->performerMap.find(model->Name()) != this->performerMap.end()) - { - Entity modelEntity = this->entityCreator->CreateEntities(model); - - // Make the model a parent of this performer - this->entityCreator->SetParent(this->performerMap[model->Name()], - modelEntity); - - // Add parent world to the model - this->entityCreator->SetParent(modelEntity, this->worldEntity); - } - } - - // Actors - for (uint64_t actorIndex = 0; - actorIndex < this->runner->sdfWorld->ActorCount(); ++actorIndex) - { - auto actor = this->runner->sdfWorld->ActorByIndex(actorIndex); - if (this->performerMap.find(actor->Name()) != this->performerMap.end()) - { - Entity actorEntity = this->entityCreator->CreateEntities(actor); - - // Make the actor a parent of this performer - this->entityCreator->SetParent(this->performerMap[actor->Name()], - actorEntity); + auto worldEntity = + this->runner->entityCompMgr.EntityByComponents(components::World()); - // Add parent world to the actor - this->entityCreator->SetParent(actorEntity, this->worldEntity); - } - } + this->entityCreator->SetParent(defaultLevel, worldEntity); } ///////////////////////////////////////////////// @@ -764,50 +620,27 @@ void LevelManager::UpdateLevelsState() // Make a list of entity names to unload making sure to leave out the ones // that have been marked to be loaded above - std::set entityNamesToUnload; - for (const auto &toUnload : levelsToUnload) + for (const Entity &toUnload : levelsToUnload) { - auto entityNames = this->runner->entityCompMgr - .Component(toUnload) - ->Data(); - - for (const auto &name : entityNames) - { - if (entityNamesMarked.find(name) == entityNamesMarked.end()) - { - entityNamesToUnload.insert(name); - } - } + this->UnloadLevel(toUnload, entityNamesMarked); } - // Load and unload the entities + // Load the entities if (entityNamesToLoad.size() > 0) - { this->LoadActiveEntities(entityNamesToLoad); - } - if (entityNamesToUnload.size() > 0) - { - this->UnloadInactiveEntities(entityNamesToUnload); - } - // Finally, upadte the list of active levels + // Finally, update the list of active levels for (const auto &level : levelsToLoad) { if (!this->IsLevelActive(level)) { - gzmsg << "Loaded level [" << level << "]" << std::endl; - this->activeLevels.push_back(level); - } - } + const components::Name *lvlName = + this->runner->entityCompMgr.Component(level); - auto pendingEnd = this->activeLevels.end(); - for (const auto &toUnload : levelsToUnload) - { - gzmsg << "Unloaded level [" << toUnload << "]" << std::endl; - pendingEnd = std::remove(this->activeLevels.begin(), pendingEnd, toUnload); + gzmsg << "Loaded level [" << lvlName->Data() << "]" << std::endl; + this->activeLevels.insert(level); + } } - // Erase from vector - this->activeLevels.erase(pendingEnd, this->activeLevels.end()); } ///////////////////////////////////////////////// @@ -815,7 +648,10 @@ void LevelManager::LoadActiveEntities(const std::set &_namesToLoad) { GZ_PROFILE("LevelManager::LoadActiveEntities"); - if (this->worldEntity == kNullEntity) + auto worldEntity = + this->runner->entityCompMgr.EntityByComponents(components::World()); + + if (worldEntity == kNullEntity) { gzerr << "Could not find the world entity while loading levels\n"; return; @@ -823,57 +659,60 @@ void LevelManager::LoadActiveEntities(const std::set &_namesToLoad) // Models for (uint64_t modelIndex = 0; - modelIndex < this->runner->sdfWorld->ModelCount(); ++modelIndex) + modelIndex < this->runner->sdfWorld.ModelCount(); ++modelIndex) { // There is no sdf::World::ModelByName so we have to iterate by index and // check if the model is in this level - auto model = this->runner->sdfWorld->ModelByIndex(modelIndex); - if (_namesToLoad.find(model->Name()) != _namesToLoad.end()) + auto model = this->runner->sdfWorld.ModelByIndex(modelIndex); + if (_namesToLoad.find(model->Name()) != _namesToLoad.end() && + this->runner->EntityByName(model->Name()) == std::nullopt) { Entity modelEntity = this->entityCreator->CreateEntities(model); - this->entityCreator->SetParent(modelEntity, this->worldEntity); + this->entityCreator->SetParent(modelEntity, worldEntity); } } // Actors for (uint64_t actorIndex = 0; - actorIndex < this->runner->sdfWorld->ActorCount(); ++actorIndex) + actorIndex < this->runner->sdfWorld.ActorCount(); ++actorIndex) { // There is no sdf::World::ActorByName so we have to iterate by index and // check if the actor is in this level - auto actor = this->runner->sdfWorld->ActorByIndex(actorIndex); - if (_namesToLoad.find(actor->Name()) != _namesToLoad.end()) + auto actor = this->runner->sdfWorld.ActorByIndex(actorIndex); + if (_namesToLoad.find(actor->Name()) != _namesToLoad.end() && + this->runner->EntityByName(actor->Name()) == std::nullopt) { Entity actorEntity = this->entityCreator->CreateEntities(actor); - this->entityCreator->SetParent(actorEntity, this->worldEntity); + this->entityCreator->SetParent(actorEntity, worldEntity); } } // Lights for (uint64_t lightIndex = 0; - lightIndex < this->runner->sdfWorld->LightCount(); ++lightIndex) + lightIndex < this->runner->sdfWorld.LightCount(); ++lightIndex) { - auto light = this->runner->sdfWorld->LightByIndex(lightIndex); - if (_namesToLoad.find(light->Name()) != _namesToLoad.end()) + auto light = this->runner->sdfWorld.LightByIndex(lightIndex); + if (_namesToLoad.find(light->Name()) != _namesToLoad.end() && + this->runner->EntityByName(light->Name()) == std::nullopt) { Entity lightEntity = this->entityCreator->CreateEntities(light); - this->entityCreator->SetParent(lightEntity, this->worldEntity); + this->entityCreator->SetParent(lightEntity, worldEntity); } } // Joints for (uint64_t jointIndex = 0; - jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex) + jointIndex < this->runner->sdfWorld.JointCount(); ++jointIndex) { - auto joint = this->runner->sdfWorld->JointByIndex(jointIndex); + auto joint = this->runner->sdfWorld.JointByIndex(jointIndex); if (_namesToLoad.find(joint->Name()) != _namesToLoad.end()) { Entity jointEntity = this->entityCreator->CreateEntities(joint); - this->entityCreator->SetParent(jointEntity, this->worldEntity); + this->entityCreator->SetParent(jointEntity, worldEntity); } } @@ -938,8 +777,7 @@ void LevelManager::UnloadInactiveEntities( ///////////////////////////////////////////////// bool LevelManager::IsLevelActive(const Entity _entity) const { - return std::find(this->activeLevels.begin(), this->activeLevels.end(), - _entity) != this->activeLevels.end(); + return this->activeLevels.find(_entity) != this->activeLevels.end(); } ///////////////////////////////////////////////// @@ -982,3 +820,31 @@ int LevelManager::CreatePerformerEntity(const std::string &_name, this->entityCreator->SetParent(this->performerMap[_name], modelEntity); return 0; } + +////////////////////////////////////////////////// +void LevelManager::UnloadLevel(const Entity &_entity, + const std::set &_entityNamesMarked) +{ + auto entityNames = this->runner->entityCompMgr + .Component(_entity) + ->Data(); + + std::set entityNamesToUnload; + for (const auto &name : entityNames) + { + if (_entityNamesMarked.find(name) == _entityNamesMarked.end()) + { + entityNamesToUnload.insert(name); + } + } + + if (entityNamesToUnload.size() > 0) + { + this->UnloadInactiveEntities(entityNamesToUnload); + } + this->activeLevels.erase(_entity); + const components::Name *lvlName = + this->runner->entityCompMgr.Component(_entity); + + gzmsg << "Unloaded level [" << lvlName->Data() << "]" << std::endl; +} diff --git a/src/LevelManager.hh b/src/LevelManager.hh index 63e0466533..69e398bd90 100644 --- a/src/LevelManager.hh +++ b/src/LevelManager.hh @@ -88,6 +88,15 @@ namespace gz /// every update cycle public: void UpdateLevelsState(); + /// \brief Read level and performer information from the sdf::World + /// object + /// \param[in] _world The SDF world + public: void ReadLevelPerformerInfo(const sdf::World &_world); + + /// \brief Determine which entities belong to the default level and + /// schedule them to be loaded + public: void ConfigureDefaultLevel(); + /// \brief Load entities that have been marked for loading. /// \param[in] _namesToLoad List of of entity names to load private: void LoadActiveEntities( @@ -98,27 +107,15 @@ namespace gz private: void UnloadInactiveEntities( const std::set &_namesToUnload); - /// \brief Read level and performer information from the sdf::World - /// object - private: void ReadLevelPerformerInfo(); - - /// \brief Create performers - /// Assuming that a simulation runner is performer-centered - private: void CreatePerformers(); - /// \brief Read information about performers from the sdf Element and /// create performer entities - /// \param[in] _sdf sdf::ElementPtr of the gz::sim plugin tag - private: void ReadPerformers(const sdf::ElementPtr &_sdf); + /// \param[in] _plugin sdf::Plugin of the gz::sim plugin tag + private: void ReadPerformers(const sdf::Plugin &_plugin); /// \brief Read information about levels from the sdf Element and /// create level entities - /// \param[in] _sdf sdf::ElementPtr of the gz::sim plugin tag - private: void ReadLevels(const sdf::ElementPtr &_sdf); - - /// \brief Determine which entities belong to the default level and - /// schedule them to be loaded - private: void ConfigureDefaultLevel(); + /// \param[in] _plugin sdf::Plugin of the gz::sim plugin tag + private: void ReadLevels(const sdf::Plugin &_plugin); /// \brief Determine if a level is active /// \param[in] _entity Entity of level to be checked @@ -145,8 +142,11 @@ namespace gz private: int CreatePerformerEntity(const std::string &_name, const sdf::Geometry &_geom); + private: void UnloadLevel(const Entity &_entity, + const std::set &_entityNamesMarked = {}); + /// \brief List of currently active levels - private: std::vector activeLevels; + private: std::set activeLevels; /// \brief Names of entities that are currently active (loaded). private: std::set activeEntityNames; @@ -161,9 +161,6 @@ namespace gz /// \brief Names of all entities that have assigned levels private: std::set entityNamesInLevels; - /// \brief Entity of the world. - private: Entity worldEntity{kNullEntity}; - /// \brief Flag whether to use levels or not. private: bool useLevels{false}; diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index af340444f9..afe266c5cd 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -57,11 +57,14 @@ #include "gz/sim/components/JointAxis.hh" #include "gz/sim/components/JointType.hh" #include "gz/sim/components/LaserRetro.hh" +#include "gz/sim/components/Level.hh" +#include "gz/sim/components/LevelEntityNames.hh" #include "gz/sim/components/Lidar.hh" #include "gz/sim/components/Light.hh" #include "gz/sim/components/LightType.hh" #include "gz/sim/components/LinearAcceleration.hh" #include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/components/LinearVelocitySeed.hh" #include "gz/sim/components/Link.hh" #include "gz/sim/components/LogicalCamera.hh" #include "gz/sim/components/MagneticField.hh" @@ -73,7 +76,9 @@ #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParentLinkName.hh" #include +#include "gz/sim/components/Performer.hh" #include "gz/sim/components/Physics.hh" +#include "gz/sim/components/PhysicsEnginePlugin.hh" #include "gz/sim/components/Pose.hh" #include #include "gz/sim/components/RgbdCamera.hh" @@ -91,6 +96,7 @@ #include "gz/sim/components/Visibility.hh" #include "gz/sim/components/Visual.hh" #include "gz/sim/components/WideAngleCamera.hh" +#include "gz/sim/components/Wind.hh" #include "gz/sim/components/WindMode.hh" #include "gz/sim/components/World.hh" #endif @@ -239,34 +245,86 @@ SdfEntityCreator &SdfEntityCreator::operator=(SdfEntityCreator &&_creator) ////////////////////////////////////////////////// Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) { - GZ_PROFILE("SdfEntityCreator::CreateEntities(sdf::World)"); // World entity Entity worldEntity = this->dataPtr->ecm->CreateEntity(); - // World components - this->dataPtr->ecm->CreateComponent(worldEntity, components::World()); - this->dataPtr->ecm->CreateComponent(worldEntity, + this->CreateEntities(_world, worldEntity); + return worldEntity; +} + +////////////////////////////////////////////////// +void SdfEntityCreator::CreateEntities(const sdf::World *_world, + Entity _worldEntity) +{ + GZ_PROFILE("SdfEntityCreator::CreateEntities(sdf::World)"); + + if (!this->dataPtr->ecm->EntityHasComponentType( + _worldEntity, components::World::typeId)) + { + this->dataPtr->ecm->CreateComponent(_worldEntity, components::World()); + } + + this->dataPtr->ecm->CreateComponent(_worldEntity, components::Name(_world->Name())); + // Gravity + this->dataPtr->ecm->CreateComponent(_worldEntity, + components::Gravity(_world->Gravity())); + + // MagneticField + this->dataPtr->ecm->CreateComponent(_worldEntity, + components::MagneticField(_world->MagneticField())); + + // Create Wind + auto windEntity = this->dataPtr->ecm->CreateEntity(); + this->SetParent(windEntity, _worldEntity); + this->dataPtr->ecm->CreateComponent(windEntity, components::Wind()); + this->dataPtr->ecm->CreateComponent(windEntity, + components::WorldLinearVelocity(_world->WindLinearVelocity())); + // Initially the wind linear velocity is used as the seed velocity + this->dataPtr->ecm->CreateComponent(windEntity, + components::WorldLinearVelocitySeed(_world->WindLinearVelocity())); + + // Set the parent of each level to the world + this->dataPtr->ecm->Each([&]( + const Entity &_entity, + const components::Level *) -> bool + { + this->SetParent(_entity, _worldEntity); + return true; + }); + + // Get the entities that should be loaded based on level information. + std::set levelEntityNames; + this->dataPtr->ecm->Each ([&]( + const Entity &, + const components::DefaultLevel *, + const components::LevelEntityNames *_names) -> bool + { + levelEntityNames = _names->Data(); + return true; + }); + // scene if (_world->Scene()) { - this->dataPtr->ecm->CreateComponent(worldEntity, + this->dataPtr->ecm->CreateComponent(_worldEntity, components::Scene(*_world->Scene())); } // atmosphere if (_world->Atmosphere()) { - this->dataPtr->ecm->CreateComponent(worldEntity, + this->dataPtr->ecm->CreateComponent(_worldEntity, components::Atmosphere(*_world->Atmosphere())); } // spherical coordinates if (_world->SphericalCoordinates()) { - this->dataPtr->ecm->CreateComponent(worldEntity, + this->dataPtr->ecm->CreateComponent(_worldEntity, components::SphericalCoordinates(*_world->SphericalCoordinates())); } @@ -274,35 +332,84 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) for (uint64_t modelIndex = 0; modelIndex < _world->ModelCount(); ++modelIndex) { - auto model = _world->ModelByIndex(modelIndex); - auto modelEntity = this->CreateEntities(model); + const sdf::Model *model = _world->ModelByIndex(modelIndex); + if (levelEntityNames.empty() || + levelEntityNames.find(model->Name()) != levelEntityNames.end()) + + { + Entity modelEntity = this->CreateEntities(model, false); - this->SetParent(modelEntity, worldEntity); + this->SetParent(modelEntity, _worldEntity); + } } // Actors for (uint64_t actorIndex = 0; actorIndex < _world->ActorCount(); ++actorIndex) { - auto actor = _world->ActorByIndex(actorIndex); - auto actorEntity = this->CreateEntities(actor); - - this->SetParent(actorEntity, worldEntity); + const sdf::Actor *actor = _world->ActorByIndex(actorIndex); + if (levelEntityNames.empty() || + levelEntityNames.find(actor->Name()) != levelEntityNames.end()) + { + Entity actorEntity = this->CreateEntities(actor); + this->SetParent(actorEntity, _worldEntity); + } } // Lights for (uint64_t lightIndex = 0; lightIndex < _world->LightCount(); ++lightIndex) { - auto light = _world->LightByIndex(lightIndex); - auto lightEntity = this->CreateEntities(light); + const sdf::Light *light = _world->LightByIndex(lightIndex); + if (levelEntityNames.empty() || + levelEntityNames.find(light->Name()) != levelEntityNames.end()) + { + Entity lightEntity = this->CreateEntities(light); - this->SetParent(lightEntity, worldEntity); + this->SetParent(lightEntity, _worldEntity); + } } - // Gravity - this->dataPtr->ecm->CreateComponent(worldEntity, - components::Gravity(_world->Gravity())); + // Attach performers to their parent entity + this->dataPtr->ecm->Each< + components::Performer, + components::PerformerRef>([&]( + const Entity &_entity, + const components::Performer *, + const components::PerformerRef *_ref) -> bool + { + std::optional parentEntity = + this->dataPtr->ecm->EntityByName(_ref->Data()); + if (!parentEntity) + { + // Performers have not been created yet. Try to create the model + // or actor and attach the peformer. + if (_world->ModelNameExists(_ref->Data())) + { + const sdf::Model *model = _world->ModelByName(_ref->Data()); + Entity modelEntity = this->CreateEntities(model, false); + this->SetParent(modelEntity, _worldEntity); + this->SetParent(_entity, modelEntity); + } + else if (_world->ActorNameExists(_ref->Data())) + { + const sdf::Actor *actor = _world->ActorByName(_ref->Data()); + Entity actorEntity = this->CreateEntities(actor); + this->SetParent(actorEntity, _worldEntity); + this->SetParent(_entity, actorEntity); + } + else + { + gzerr << "Unable to find performer parent entity with name[" << + _ref->Data() << "]. This performer will not adhere to levels.\n"; + } + } + else + { + this->SetParent(_entity, *parentEntity); + } + return true; + }); // Physics // \todo(anyone) Support picking a specific physics profile @@ -311,7 +418,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) { physics = _world->PhysicsDefault(); } - this->dataPtr->ecm->CreateComponent(worldEntity, + this->dataPtr->ecm->CreateComponent(_worldEntity, components::Physics(*physics)); // Populate physics options that aren't accessible outside the Element() @@ -325,7 +432,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) auto collisionDetector = dartElem->Get("collision_detector"); - this->dataPtr->ecm->CreateComponent(worldEntity, + this->dataPtr->ecm->CreateComponent(_worldEntity, components::PhysicsCollisionDetector(collisionDetector)); } if (dartElem->HasElement("solver") && @@ -334,30 +441,26 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) auto solver = dartElem->GetElement("solver")->Get("solver_type"); - this->dataPtr->ecm->CreateComponent(worldEntity, + this->dataPtr->ecm->CreateComponent(_worldEntity, components::PhysicsSolver(solver)); } } - // MagneticField - this->dataPtr->ecm->CreateComponent(worldEntity, - components::MagneticField(_world->MagneticField())); + // Store the world's SDF DOM to be used when saving the world to file + this->dataPtr->ecm->CreateComponent( + _worldEntity, components::WorldSdf(*_world)); - this->dataPtr->eventManager->Emit(worldEntity, + // Load world plugins first. + this->dataPtr->eventManager->Emit(_worldEntity, _world->Plugins()); - for (const sdf::Plugin &p : _world->Plugins()) - { - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - this->dataPtr->eventManager->Emit(worldEntity, - p.ToElement()); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - } - // Store the world's SDF DOM to be used when saving the world to file - this->dataPtr->ecm->CreateComponent( - worldEntity, components::WorldSdf(*_world)); + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + this->dataPtr->eventManager->Emit(_worldEntity, + _world->ToElement()); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - return worldEntity; + // Load model plugins after the world plugin. + this->LoadModelPlugins(); } ////////////////////////////////////////////////// @@ -368,6 +471,14 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) auto ent = this->CreateEntities(_model, false); // Load all model plugins afterwards, so we get scoped name for nested models. + this->LoadModelPlugins(); + + return ent; +} + +////////////////////////////////////////////////// +void SdfEntityCreator::LoadModelPlugins() +{ for (const auto &[entity, plugins] : this->dataPtr->newModels) { this->dataPtr->eventManager->Emit(entity, plugins); @@ -408,8 +519,6 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) } } this->dataPtr->newVisuals.clear(); - - return ent; } ////////////////////////////////////////////////// diff --git a/src/SdfEntityCreator_TEST.cc b/src/SdfEntityCreator_TEST.cc index d4cad99d84..e7f7527dc4 100644 --- a/src/SdfEntityCreator_TEST.cc +++ b/src/SdfEntityCreator_TEST.cc @@ -107,9 +107,9 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) EXPECT_TRUE(this->ecm.HasComponentType(components::LaserRetro::typeId)); // Check entities - // 1 x world + 5 x model + 5 x link + 5 x collision + 5 x visual + + // 1 x world + 1 wind + 5 x model + 5 x link + 5 x collision + 5 x visual + // 1 x light (light + visual) - EXPECT_EQ(23u, this->ecm.EntityCount()); + EXPECT_EQ(24u, this->ecm.EntityCount()); // Check worlds unsigned int worldCount{0}; @@ -685,8 +685,9 @@ TEST_F(SdfEntityCreatorTest, CreateLights) creator.CreateEntities(root.WorldByIndex(0)); // Check entities - // 1 x world + 1 x model + 1 x link + 1 x visual + 4 x light (light + visual) - EXPECT_EQ(12u, this->ecm.EntityCount()); + // 1 x world + 1 wind + 1 x model + 1 x link + 1 x visual + + // 4 x light (light + visual) + EXPECT_EQ(13u, this->ecm.EntityCount()); // Check worlds unsigned int worldCount{0}; @@ -1107,9 +1108,9 @@ TEST_F(SdfEntityCreatorTest, RemoveEntities) creator.CreateEntities(root.WorldByIndex(0)); // Check entities - // 1 x world + 4 x model + 4 x link + 4 x collision + 4 x visual + // 1 x world + 1 wind + 4 x model + 4 x link + 4 x collision + 4 x visual // + 1 x light (light + visual) - EXPECT_EQ(23u, this->ecm.EntityCount()); + EXPECT_EQ(24u, this->ecm.EntityCount()); auto world = this->ecm.EntityByComponents(components::World()); EXPECT_NE(kNullEntity, world); @@ -1135,7 +1136,7 @@ TEST_F(SdfEntityCreatorTest, RemoveEntities) creator.RequestRemoveEntity(models.front()); this->ecm.ProcessEntityRemovals(); - EXPECT_EQ(19u, this->ecm.EntityCount()); + EXPECT_EQ(20u, this->ecm.EntityCount()); models = this->ecm.ChildrenByComponents(world, components::Model()); ASSERT_EQ(4u, models.size()); @@ -1158,7 +1159,7 @@ TEST_F(SdfEntityCreatorTest, RemoveEntities) creator.RequestRemoveEntity(models.front(), false); this->ecm.ProcessEntityRemovals(); - EXPECT_EQ(18u, this->ecm.EntityCount()); + EXPECT_EQ(19u, this->ecm.EntityCount()); // There's only 1 model left models = this->ecm.ChildrenByComponents(world, components::Model()); diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 97b3930c55..62b3e329c4 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -286,14 +286,14 @@ void ServerPrivate::CreateEntities() for (uint64_t worldIndex = 0; worldIndex < this->sdfRoot.WorldCount(); ++worldIndex) { - auto world = this->sdfRoot.WorldByIndex(worldIndex); + sdf::World *world = this->sdfRoot.WorldByIndex(worldIndex); { std::lock_guard lock(this->worldsMutex); this->worldNames.push_back(world->Name()); } auto runner = std::make_unique( - world, this->systemLoader, this->config); + *world, this->systemLoader, this->config); runner->SetFuelUriMap(this->fuelUriMap); this->simRunners.push_back(std::move(runner)); } diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 922b3e1470..39de1ec5f7 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -441,7 +441,8 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigLogRecord)) EXPECT_EQ(0u, *server.IterationCount()); EXPECT_EQ(3u, *server.EntityCount()); - EXPECT_EQ(4u, *server.SystemCount()); + // Only the log record system is needed and therefore loaded. + EXPECT_EQ(1u, *server.SystemCount()); EXPECT_TRUE(serverConfig.LogRecordTopics().empty()); serverConfig.AddLogRecordTopic("test_topic1"); @@ -480,7 +481,9 @@ TEST_P(ServerFixture, sim::Server server(serverConfig); EXPECT_EQ(0u, *server.IterationCount()); EXPECT_EQ(3u, *server.EntityCount()); - EXPECT_EQ(4u, *server.SystemCount()); + + // Only the log record system is needed and therefore loaded. + EXPECT_EQ(1u, *server.SystemCount()); } EXPECT_FALSE(common::exists(logFile)); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 650fe35087..0f2d5a9b05 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -40,7 +40,11 @@ #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Physics.hh" #include "gz/sim/components/PhysicsCmd.hh" +#include "gz/sim/components/PhysicsEnginePlugin.hh" #include "gz/sim/components/Recreate.hh" +#include "gz/sim/components/RenderEngineGuiPlugin.hh" +#include "gz/sim/components/RenderEngineServerHeadless.hh" +#include "gz/sim/components/RenderEngineServerPlugin.hh" #include "gz/sim/Events.hh" #include "gz/sim/SdfEntityCreator.hh" #include "gz/sim/Util.hh" @@ -55,21 +59,13 @@ using StringSet = std::unordered_set; ////////////////////////////////////////////////// -SimulationRunner::SimulationRunner(const sdf::World *_world, +SimulationRunner::SimulationRunner(const sdf::World &_world, const SystemLoaderPtr &_systemLoader, const ServerConfig &_config) - // \todo(nkoenig) Either copy the world, or add copy constructor to the - // World and other elements. - : sdfWorld(_world), serverConfig(_config) + : sdfWorld(_world), serverConfig(_config) { - if (nullptr == _world) - { - gzerr << "Can't start simulation runner with null world." << std::endl; - return; - } - // Keep world name - this->worldName = _world->Name(); + this->worldName = _world.Name(); this->parametersRegistry = std::make_unique< gz::transport::parameters::ParametersRegistry>( @@ -77,10 +73,10 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // Get the physics profile // TODO(luca): remove duplicated logic in SdfEntityCreator and LevelManager - auto physics = _world->PhysicsByIndex(0); + const sdf::Physics *physics = _world.PhysicsByIndex(0); if (!physics) { - physics = _world->PhysicsDefault(); + physics = _world.PhysicsDefault(); } // Step size @@ -166,9 +162,6 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, std::bind(&SimulationRunner::LoadPlugins, this, std::placeholders::_1, std::placeholders::_2)); - // Create the level manager - this->levelMgr = std::make_unique(this, _config.UseLevels()); - // Check if this is going to be a distributed runner // Attempt to create the manager based on environment variables. // If the configuration is invalid, then networkMgr will be `nullptr`. @@ -206,27 +199,11 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, } } - // Load the active levels - this->levelMgr->UpdateLevelsState(); - - // Store the initial state of the ECM; - this->initialEntityCompMgr.CopyFrom(this->entityCompMgr); - - // Load any additional plugins from the Server Configuration - this->LoadServerPlugins(this->serverConfig.Plugins()); - - // If we have reached this point and no world systems have been loaded, then - // load a default set of systems. - if (this->systemMgr->TotalByEntity( - worldEntity(this->entityCompMgr)).empty()) - { - gzmsg << "No systems loaded from SDF, loading defaults" << std::endl; - bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); - auto plugins = sim::loadPluginInfo(isPlayback); - this->LoadServerPlugins(plugins); - } + // Create the level manager + this->levelMgr = std::make_unique(this, + this->serverConfig.UseLevels()); - this->LoadLoggingPlugins(this->serverConfig); + this->CreateEntities(_world); // TODO(louise) Combine both messages into one. this->node->Advertise("control", &SimulationRunner::OnWorldControl, this); @@ -241,9 +218,9 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // Publish empty GUI messages for worlds that have no GUI in the beginning. // In the future, support modifying GUI from the server at runtime. - if (_world->Gui()) + if (_world.Gui()) { - this->guiMsg = convert(*_world->Gui()); + this->guiMsg = convert(*_world.Gui()); } std::string infoService{"gui/info"}; @@ -252,7 +229,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, gzmsg << "Serving GUI information on [" << opts.NameSpace() << "/" << infoService << "]" << std::endl; - gzmsg << "World [" << _world->Name() << "] initialized with [" + gzmsg << "World [" << this->worldName << "] initialized with [" << physics->Name() << "] physics profile." << std::endl; std::string genWorldSdfService{"generate_world_sdf"}; @@ -1417,19 +1394,7 @@ bool SimulationRunner::RequestRemoveEntity(const std::string &_name, std::optional SimulationRunner::EntityByName( const std::string &_name) const { - std::optional entity; - this->entityCompMgr.Each([&](const Entity _entity, - const components::Name *_entityName)->bool - { - if (_entityName->Data() == _name) - { - entity = _entity; - return false; - } - return true; - }); - - return entity; + return this->entityCompMgr.EntityByName(_name); } ///////////////////////////////////////////////// @@ -1497,3 +1462,80 @@ void SimulationRunner::SetNextStepAsBlockingPaused(const bool value) { this->blockingPausedStepPending = value; } + +////////////////////////////////////////////////// +void SimulationRunner::CreateEntities(const sdf::World &_world) +{ + this->sdfWorld = _world; + + // Instantiate the SDF creator + auto creator = std::make_unique(this->entityCompMgr, + this->eventMgr); + + // We create the world entity so that the simulation runner can inject + // some components + Entity worldEntity = this->entityCompMgr.CreateEntity(); + this->entityCompMgr.CreateComponent(worldEntity, components::World()); + + // 1. Level manager read performers and levels. Add components to the + // performers and levels so that the SdfEntityCreator knows whether to + // create them or not. Make sure to set parents properly + // 2. Create entities. + + // Read the level information. This will create components containing + // information about which entities should be created for the current + // level. + this->levelMgr->ReadLevelPerformerInfo(this->sdfWorld); + + // Configure the default level + this->levelMgr->ConfigureDefaultLevel(); + + // Create components based on the contents of the server configuration. + this->entityCompMgr.CreateComponent(worldEntity, + components::PhysicsEnginePlugin(this->serverConfig.PhysicsEngine())); + + this->entityCompMgr.CreateComponent(worldEntity, + components::RenderEngineServerPlugin( + this->serverConfig.RenderEngineServer())); + + this->entityCompMgr.CreateComponent(worldEntity, + components::RenderEngineServerHeadless( + this->serverConfig.HeadlessRendering())); + + this->entityCompMgr.CreateComponent(worldEntity, + components::RenderEngineGuiPlugin( + this->serverConfig.RenderEngineGui())); + + // Load the world entities from SDF + creator->CreateEntities(&this->sdfWorld, worldEntity); + + // Load the active levels + this->levelMgr->UpdateLevelsState(); + + // Some entities and component should be removed based on the levels. + this->entityCompMgr.ProcessRemoveEntityRequests(); + this->entityCompMgr.ClearRemovedComponents(); + + this->LoadLoggingPlugins(this->serverConfig); + + // Load any additional plugins from the Server Configuration + this->LoadServerPlugins(this->serverConfig.Plugins()); + + // If we have reached this point and no world systems have been loaded, then + // load a default set of systems. + if (this->systemMgr->TotalByEntity(worldEntity).empty()) + { + gzmsg << "No systems loaded from SDF, loading defaults" << std::endl; + bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); + auto plugins = gz::sim::loadPluginInfo(isPlayback); + this->LoadServerPlugins(plugins); + } + + // Store the initial state of the ECM; + this->initialEntityCompMgr.CopyFrom(this->entityCompMgr); + + // Publish empty GUI messages for worlds that have no GUI in the beginning. + // In the future, support modifying GUI from the server at runtime. + if (_world.Gui()) + this->guiMsg = convert(*_world.Gui()); +} diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 438fc329ba..0af4a0138f 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -77,7 +77,7 @@ namespace gz /// \param[in] _world Pointer to the SDF world. /// \param[in] _systemLoader Reference to system manager. /// \param[in] _useLevels Whether to use levles or not. False by default. - public: explicit SimulationRunner(const sdf::World *_world, + public: explicit SimulationRunner(const sdf::World &_world, const SystemLoaderPtr &_systemLoader, const ServerConfig &_config = ServerConfig()); @@ -151,8 +151,8 @@ namespace gz const sdf::Plugins &_plugins); /// \brief Load server plugins for a given entity. - /// \param[in] _config Configuration to load plugins from. - /// plugins based on the _config contents + /// \param[in] _plugins Load any additional plugins from the + /// Server Configuration public: void LoadServerPlugins( const std::list &_plugins); @@ -373,6 +373,11 @@ namespace gz /// Physics component of the world, if any. public: void UpdatePhysicsParams(); + /// \brief Create entities for the world simulated by this runner based + /// on the provided SDF Root object. + /// \param[in] _world SDF world created entities from. + public: void CreateEntities(const sdf::World &_world); + /// \brief Process entities with the components::Recreate component. /// Put in a request to make them as removed private: void ProcessRecreateEntitiesRemove(); @@ -477,8 +482,8 @@ namespace gz /// \brief Connection to the load plugins event. private: common::ConnectionPtr loadPluginsConn; - /// \brief Pointer to the sdf::World object of this runner - private: const sdf::World *sdfWorld; + /// \brief The sdf::World object of this runner + private: sdf::World sdfWorld; /// \brief The real time factor calculated based on sim and real time /// averages. diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index e507877a6c..75457345b2 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -118,7 +118,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(root.WorldByIndex(0), systemLoader); + SimulationRunner runner(*root.WorldByIndex(0), systemLoader); // Check component types EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( @@ -652,7 +652,7 @@ TEST_P(SimulationRunnerTest, CreateLights) // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(root.WorldByIndex(0), systemLoader); + SimulationRunner runner(*root.WorldByIndex(0), systemLoader); // Check entities // 1 x world + 1 x (default) level + 1 x wind + 1 x model + 1 x link + 1 x @@ -922,7 +922,7 @@ TEST_P(SimulationRunnerTest, CreateJointEntities) // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(root.WorldByIndex(0), systemLoader); + SimulationRunner runner(*root.WorldByIndex(0), systemLoader); // Check component types EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( @@ -1067,7 +1067,7 @@ TEST_P(SimulationRunnerTest, Time) // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(root.WorldByIndex(0), systemLoader); + SimulationRunner runner(*root.WorldByIndex(0), systemLoader); // Check state EXPECT_TRUE(runner.Paused()); @@ -1196,7 +1196,7 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(root.WorldByIndex(0), systemLoader); + SimulationRunner runner(*root.WorldByIndex(0), systemLoader); // Get world entity Entity worldId{kNullEntity}; @@ -1309,7 +1309,7 @@ TEST_P(SimulationRunnerTest, // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader, + SimulationRunner runner(*rootWithout.WorldByIndex(0), systemLoader, serverConfig); ASSERT_EQ(2u, runner.SystemCount()); @@ -1340,7 +1340,7 @@ TEST_P(SimulationRunnerTest, // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader, + SimulationRunner runner(*rootWithout.WorldByIndex(0), systemLoader, serverConfig); // Get world entity @@ -1433,7 +1433,7 @@ TEST_P(SimulationRunnerTest, // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader); + SimulationRunner runner(*rootWithout.WorldByIndex(0), systemLoader); ASSERT_EQ(3u, runner.SystemCount()); common::unsetenv(kServerConfigPathEnv); } @@ -1450,7 +1450,7 @@ TEST_P(SimulationRunnerTest, // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader); + SimulationRunner runner(*rootWithout.WorldByIndex(0), systemLoader); runner.SetPaused(false); // Get model entities @@ -1549,7 +1549,7 @@ TEST_P(SimulationRunnerTest, // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader, + SimulationRunner runner(*rootWithout.WorldByIndex(0), systemLoader, serverConfig); // 1 model plugin from SDF and 2 world plugins from config @@ -1568,7 +1568,7 @@ TEST_P(SimulationRunnerTest, GuiInfo) // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(root.WorldByIndex(0), systemLoader); + SimulationRunner runner(*root.WorldByIndex(0), systemLoader); // Create requester transport::Node node; @@ -1605,7 +1605,7 @@ TEST_P(SimulationRunnerTest, GenerateWorldSdf) // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(root.WorldByIndex(0), systemLoader); + SimulationRunner runner(*root.WorldByIndex(0), systemLoader); msgs::SdfGeneratorConfig req; msgs::StringMsg genWorldSdf; @@ -1654,7 +1654,7 @@ TEST_P(SimulationRunnerTest, GeneratedSdfHasNoSpuriousPlugins) // Create simulation runner auto systemLoader = std::make_shared(); - SimulationRunner runner(root.WorldByIndex(0), systemLoader); + SimulationRunner runner(*root.WorldByIndex(0), systemLoader); msgs::SdfGeneratorConfig req; msgs::StringMsg genWorldSdf; diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc index c0836401dd..fab58be000 100644 --- a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc @@ -405,7 +405,7 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( }; // create services for this source - const auto validName = topicFromScopedName(entity, _ecm, false); + const auto validName = topicFromScopedName(entity, _ecm, true); if (validName.empty()) { gzerr << "Failed to create valid topics with entity scoped name [" @@ -503,7 +503,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( // create the detection publisher for this microphone auto pub = this->node.Advertise( - topicFromScopedName(entity, _ecm, false) + "/detection"); + topicFromScopedName(entity, _ecm, true) + "/detection"); if (!pub) { gzerr << "Error creating a detection publisher for microphone " diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index fd167a882e..d4c9bc0d39 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -250,7 +250,7 @@ void PosePublisher::Configure(const Entity &_entity, this->dataPtr->usePoseV = _sdf->Get("use_pose_vector_msg", this->dataPtr->usePoseV).first; - std::string poseTopic = topicFromScopedName(_entity, _ecm, false) + "/pose"; + std::string poseTopic = topicFromScopedName(_entity, _ecm, true) + "/pose"; if (poseTopic.empty()) { poseTopic = "/pose";