diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index b87ada778c7..7fff308c768 100644 --- a/include/gz/sim/EntityComponentManager.hh +++ b/include/gz/sim/EntityComponentManager.hh @@ -828,6 +828,9 @@ namespace gz friend class GuiRunner; friend class SimulationRunner; + // Make SystemManager friend so it has access to removals + friend class SystemManager; + // Make network managers friends so they have control over component // states. Like the runners, the managers are internal. friend class NetworkManagerPrimary; diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index a5ba51e96cf..9a546addd37 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -33,6 +33,7 @@ #include #include +#include #include "gz/common/Profiler.hh" #include "gz/sim/components/Model.hh" @@ -567,7 +568,7 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateStartBarrier->Wait(); if (this->postUpdateThreadsRunning) { - system->PostUpdate(this->currentInfo, this->entityCompMgr); + system.system->PostUpdate(this->currentInfo, this->entityCompMgr); } this->postUpdateStopBarrier->Wait(); } @@ -598,13 +599,13 @@ void SimulationRunner::UpdateSystems() { GZ_PROFILE("PreUpdate"); for (auto& system : this->systemMgr->SystemsPreUpdate()) - system->PreUpdate(this->currentInfo, this->entityCompMgr); + system.system->PreUpdate(this->currentInfo, this->entityCompMgr); } { GZ_PROFILE("Update"); for (auto& system : this->systemMgr->SystemsUpdate()) - system->Update(this->currentInfo, this->entityCompMgr); + system.system->Update(this->currentInfo, this->entityCompMgr); } { @@ -922,6 +923,7 @@ void SimulationRunner::Step(const UpdateInfo &_info) this->ProcessRecreateEntitiesCreate(); // Process entity removals. + this->systemMgr->ProcessRemovedEntities(this->entityCompMgr); this->entityCompMgr.ProcessRemoveEntityRequests(); // Process components removals diff --git a/src/SystemManager.cc b/src/SystemManager.cc index fd43f5330d0..61dfeb7cc22 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -113,16 +113,16 @@ size_t SystemManager::ActivatePendingSystems() this->systemsConfigureParameters.push_back(system.configureParameters); if (system.reset) - this->systemsReset.push_back(system.reset); + this->systemsReset.emplace_back(system.parentEntity, system.reset); if (system.preupdate) - this->systemsPreupdate.push_back(system.preupdate); + this->systemsPreupdate.emplace_back(system.parentEntity, system.preupdate); if (system.update) - this->systemsUpdate.push_back(system.update); + this->systemsUpdate.emplace_back(system.parentEntity, system.update); if (system.postupdate) - this->systemsPostupdate.push_back(system.postupdate); + this->systemsPostupdate.emplace_back(system.parentEntity, system.postupdate); } this->pendingSystems.clear(); @@ -289,25 +289,25 @@ SystemManager::SystemsConfigureParameters() } ////////////////////////////////////////////////// -const std::vector &SystemManager::SystemsReset() +const std::vector> &SystemManager::SystemsReset() { return this->systemsReset; } ////////////////////////////////////////////////// -const std::vector& SystemManager::SystemsPreUpdate() +const std::vector>& SystemManager::SystemsPreUpdate() { return this->systemsPreupdate; } ////////////////////////////////////////////////// -const std::vector& SystemManager::SystemsUpdate() +const std::vector>& SystemManager::SystemsUpdate() { return this->systemsUpdate; } ////////////////////////////////////////////////// -const std::vector& SystemManager::SystemsPostUpdate() +const std::vector>& SystemManager::SystemsPostUpdate() { return this->systemsPostupdate; } @@ -409,3 +409,56 @@ void SystemManager::ProcessPendingEntitySystems() } this->systemsToAdd.clear(); } + +template +struct identity +{ + using type = T; +}; +////////////////////////////////////////////////// +/// TODO(arjo) - When we move to C++20 we can just use erase_if +/// Removes all items that match the given predicate. +/// This function runs in O(n) time and uses memory in-place +template +void RemoveFromVectorIf(std::vector& vec, + typename identity>::type pred) +{ + auto resizedVec = vec.size(); + for(std::size_t i = 0; i < resizedVec; ++i) { + int j = 1; + while (pred(vec[i])) { + if (i+j >= vec.size()) { + break; + } + vec[i] = vec[i + j]; + j++; + resizedVec--; + } + } + + while (vec.size() > resizedVec) { + vec.pop_back(); + } +} + +////////////////////////////////////////////////// +void SystemManager::ProcessRemovedEntities( + const EntityComponentManager &_ecm) +{ + RemoveFromVectorIf(this->systemsReset, + [&](const SystemHolder& system) { + return _ecm.IsMarkedForRemoval(system.parent); + }); + RemoveFromVectorIf(this->systemsPreupdate, + [&](const SystemHolder& system) { + return _ecm.IsMarkedForRemoval(system.parent); + }); + RemoveFromVectorIf(this->systemsUpdate, + [&](const SystemHolder& system) { + return _ecm.IsMarkedForRemoval(system.parent); + }); + RemoveFromVectorIf(this->systemsPostupdate, + [&](const SystemHolder& system) { + return _ecm.IsMarkedForRemoval(system.parent); + }); +} diff --git a/src/SystemManager.hh b/src/SystemManager.hh index acd82c09dc7..1c6c8231e1a 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -41,6 +42,14 @@ namespace gz // Inline bracket to help doxygen filtering. inline namespace GZ_SIM_VERSION_NAMESPACE { + template + struct SystemHolder { + Entity parent; + IFace* system; + + SystemHolder(Entity _parent, IFace* _iface): parent(_parent), system(_iface) {}; + }; + /// \brief Used to load / unload sysetms as well as iterate over them. class GZ_SIM_VISIBLE SystemManager { @@ -124,19 +133,19 @@ namespace gz /// \brief Get an vector of all active systems implementing "Reset" /// \return Vector of systems' reset interfaces. - public: const std::vector& SystemsReset(); + public: const std::vector>& SystemsReset(); /// \brief Get an vector of all active systems implementing "PreUpdate" /// \return Vector of systems's pre-update interfaces. - public: const std::vector& SystemsPreUpdate(); + public: const std::vector>& SystemsPreUpdate(); /// \brief Get an vector of all active systems implementing "Update" /// \return Vector of systems's update interfaces. - public: const std::vector& SystemsUpdate(); + public: const std::vector>& SystemsUpdate(); /// \brief Get an vector of all active systems implementing "PostUpdate" /// \return Vector of systems's post-update interfaces. - public: const std::vector& SystemsPostUpdate(); + public: const std::vector>& SystemsPostUpdate(); /// \brief Get an vector of all systems attached to a given entity. /// \return Vector of systems. @@ -145,6 +154,9 @@ namespace gz /// \brief Process system messages and add systems to entities public: void ProcessPendingEntitySystems(); + public: void ProcessRemovedEntities( + const EntityComponentManager &_entityCompMgr); + /// \brief Implementation for AddSystem functions that takes an SDF /// element. This calls the AddSystemImpl that accepts an SDF Plugin. /// \param[in] _system Generic representation of a system. @@ -194,16 +206,16 @@ namespace gz systemsConfigureParameters; /// \brief Systems implementing Reset - private: std::vector systemsReset; + private: std::vector> systemsReset; /// \brief Systems implementing PreUpdate - private: std::vector systemsPreupdate; + private: std::vector> systemsPreupdate; /// \brief Systems implementing Update - private: std::vector systemsUpdate; + private: std::vector> systemsUpdate; /// \brief Systems implementing PostUpdate - private: std::vector systemsPostupdate; + private: std::vector> systemsPostupdate; /// \brief System loader, for loading system plugins. private: SystemLoaderPtr systemLoader;