From 33f47e869fe377845719a10a5226c04c07b6fc00 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 7 Nov 2023 17:42:26 +0800 Subject: [PATCH 01/51] Remove systems if their parent entity is removed This commit tries to address #2217. In particular if a user despawns an entity, the associated plugin gets removed. This should prevent issues like #2165. TBH I'm not sure if this is the right way forward as a system should technically be able to access any entity in a traditional ECS. I also recognize that there may be some performance impact. I will need to quantify this. Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- include/gz/sim/EntityComponentManager.hh | 3 ++ src/SimulationRunner.cc | 8 +-- src/SystemManager.cc | 69 +++++++++++++++++++++--- src/SystemManager.hh | 28 +++++++--- 4 files changed, 89 insertions(+), 19 deletions(-) diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index b87ada778c..7fff308c76 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 a5ba51e96c..9a546addd3 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 fd43f5330d..61dfeb7cc2 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 acd82c09dc..1c6c8231e1 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; From b93235484000fa08132c3edd16fdd67cec281932 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 Nov 2023 10:04:36 +0800 Subject: [PATCH 02/51] Fix vector removal routine. Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SystemManager.cc | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 61dfeb7cc2..19c22ca141 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -423,20 +423,19 @@ 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]; + auto originalSize = vec.size(); + int j = 0; + + for(std::size_t i = 0; i < vec.size(); ++i) { + if (pred(vec[i])) { j++; - resizedVec--; + } + else { + vec[i-j] = vec[i]; } } - while (vec.size() > resizedVec) { + while (vec.size() > originalSize - j) { vec.pop_back(); } } From 45c4910742a53fe9392324ff167b419d631d2d63 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 Nov 2023 10:06:44 +0800 Subject: [PATCH 03/51] Reduce performace impact by only running when there are entities to remove. This makes sense because for the most part, we will not be removing entities very often so it does not make sense to iterate unless there are entities which do need removing. Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SystemManager.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 19c22ca141..58a311fd0c 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -444,6 +444,11 @@ void RemoveFromVectorIf(std::vector& vec, void SystemManager::ProcessRemovedEntities( const EntityComponentManager &_ecm) { + if (!_ecm.HasEntitiesMarkedForRemoval()) + { + return; + } + RemoveFromVectorIf(this->systemsReset, [&](const SystemHolder& system) { return _ecm.IsMarkedForRemoval(system.parent); From 0a7c065a149673d0175d1073d2a1a4c7b6b347bb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 Nov 2023 13:03:28 +0800 Subject: [PATCH 04/51] style Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SystemManager.hh | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 1c6c8231e1..149a0056de 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -42,12 +42,18 @@ namespace gz // Inline bracket to help doxygen filtering. inline namespace GZ_SIM_VERSION_NAMESPACE { + /// \brief Helper container to keep track of + /// system interfaces and their parents template struct SystemHolder { + /// Parent entity of system Entity parent; + + /// Interface pointer IFace* system; - SystemHolder(Entity _parent, IFace* _iface): parent(_parent), system(_iface) {}; + /// \brief constructor + SystemHolder(Entity _parent, IFace* _iface): parent(_parent), system(_iface) {}; }; /// \brief Used to load / unload sysetms as well as iterate over them. From 1e7416f5e80a8a8b0409c744468f3fecc6a5cfc3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 Nov 2023 14:44:39 +0800 Subject: [PATCH 05/51] style Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SimulationRunner_TEST.cc | 3 +-- src/SystemManager.cc | 12 ++++++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 10ca3a9406..9905c41747 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1484,8 +1484,7 @@ TEST_P(SimulationRunnerTest, EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sphereEntity, componentId)) << componentId; - // Remove entities that have plugin - this is not unloading or destroying - // the plugin though! + // Remove entities that have plugin auto entityCount = runner.EntityCompMgr().EntityCount(); const_cast( runner.EntityCompMgr()).RequestRemoveEntity(boxEntity); diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 58a311fd0c..1a21e4c478 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -113,16 +113,20 @@ size_t SystemManager::ActivatePendingSystems() this->systemsConfigureParameters.push_back(system.configureParameters); if (system.reset) - this->systemsReset.emplace_back(system.parentEntity, system.reset); + this->systemsReset.emplace_back(system.parentEntity, + system.reset); if (system.preupdate) - this->systemsPreupdate.emplace_back(system.parentEntity, system.preupdate); + this->systemsPreupdate.emplace_back(system.parentEntity, + system.preupdate); if (system.update) - this->systemsUpdate.emplace_back(system.parentEntity, system.update); + this->systemsUpdate.emplace_back(system.parentEntity, + system.update); if (system.postupdate) - this->systemsPostupdate.emplace_back(system.parentEntity, system.postupdate); + this->systemsPostupdate.emplace_back(system.parentEntity, + system.postupdate); } this->pendingSystems.clear(); From 453b54b2d42874daafdccd8d4aa6fd32ed0ebb0e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 Nov 2023 15:31:37 +0800 Subject: [PATCH 06/51] Style Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SystemManager.cc | 21 ++++++++++++++------- src/SystemManager.hh | 9 ++++++--- 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 1a21e4c478..425ee968a6 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -299,19 +299,22 @@ const std::vector> &SystemManager::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; } @@ -430,16 +433,20 @@ void RemoveFromVectorIf(std::vector& vec, auto originalSize = vec.size(); int j = 0; - for(std::size_t i = 0; i < vec.size(); ++i) { - if (pred(vec[i])) { + for(std::size_t i = 0; i < vec.size(); ++i) + { + if (pred(vec[i])) + { j++; } - else { + else + { vec[i-j] = vec[i]; } } - while (vec.size() > originalSize - j) { + while (vec.size() > originalSize - j) + { vec.pop_back(); } } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 149a0056de..becfb122b0 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -53,7 +53,8 @@ namespace gz IFace* system; /// \brief constructor - SystemHolder(Entity _parent, IFace* _iface): parent(_parent), system(_iface) {}; + SystemHolder(Entity _parent, IFace* _iface): + parent(_parent), system(_iface) {} }; /// \brief Used to load / unload sysetms as well as iterate over them. @@ -143,7 +144,8 @@ namespace gz /// \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. @@ -151,7 +153,8 @@ namespace gz /// \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. From 4d9809a35a90686090ac9a91f97b49bb15c6b39b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 9 Nov 2023 13:28:37 +0800 Subject: [PATCH 07/51] Stopped things from going :boom: Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/Barrier.cc | 9 ++++++++ src/Barrier.hh | 2 ++ src/SimulationRunner.cc | 19 ++++++++++++++-- src/SimulationRunner.hh | 3 +++ src/SystemManager.cc | 48 ++++++++++++++++++++++++++++++++++++----- src/SystemManager.hh | 16 ++++++++------ 6 files changed, 84 insertions(+), 13 deletions(-) diff --git a/src/Barrier.cc b/src/Barrier.cc index ca6b4347bd..dfd3e0067c 100644 --- a/src/Barrier.cc +++ b/src/Barrier.cc @@ -97,3 +97,12 @@ void Barrier::Cancel() this->dataPtr->cancelled = true; this->dataPtr->cv.notify_all(); } + +#include +////////////////////////////////////////////////// +void Barrier::Drop() +{ + std::unique_lock lock(this->dataPtr->mutex); + this->dataPtr->threadCount--; + this->dataPtr->cv.notify_all(); +} \ No newline at end of file diff --git a/src/Barrier.hh b/src/Barrier.hh index d013b04e75..4d5a10de56 100644 --- a/src/Barrier.hh +++ b/src/Barrier.hh @@ -81,6 +81,8 @@ namespace gz /// reflect that. public: ExitStatus Wait(); + public: void Drop(); + /// \brief Cancel the barrier, causing all threads to unblock and /// return CANCELLED public: void Cancel(); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 9a546addd3..b197f03b69 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -560,6 +560,7 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateThreads.push_back(std::thread([&, id]() { + auto parentEntity = system.parent; std::stringstream ss; ss << "PostUpdateThread: " << id; GZ_PROFILE_THREAD_NAME(ss.str().c_str()); @@ -568,11 +569,24 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateStartBarrier->Wait(); if (this->postUpdateThreadsRunning) { + auto terminate = this->threadsToTerminate.find(parentEntity); + if (terminate != this->threadsToTerminate.end()) { + terminate->second--; + if (terminate->second == 0) { + this->threadsToTerminate.erase(terminate); + } + gzerr << "Terminating thread " << id << ", " << parentEntity <<"\n"; + this->postUpdateStartBarrier->Drop(); + this->postUpdateStopBarrier->Wait(); + this->postUpdateStopBarrier->Drop(); + break; + } system.system->PostUpdate(this->currentInfo, this->entityCompMgr); } this->postUpdateStopBarrier->Wait(); } - gzdbg << "Exiting postupdate worker thread (" + + gzerr << "Exiting postupdate worker thread (" << id << ")" << std::endl; })); id++; @@ -923,7 +937,8 @@ void SimulationRunner::Step(const UpdateInfo &_info) this->ProcessRecreateEntitiesCreate(); // Process entity removals. - this->systemMgr->ProcessRemovedEntities(this->entityCompMgr); + this->systemMgr->ProcessRemovedEntities(this->entityCompMgr, + this->threadsToTerminate); this->entityCompMgr.ProcessRemoveEntityRequests(); // Process components removals diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 438fc329ba..773bb2136e 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -543,6 +543,9 @@ namespace gz /// at the appropriate time. private: std::unique_ptr newWorldControlState; + + private: std::unordered_map threadsToTerminate; + private: bool resetInitiated{false}; friend class LevelManager; }; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 425ee968a6..fa1ca37b3c 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -16,10 +16,12 @@ */ #include +#include #include #include +#include "SystemInternal.hh" #include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/Conversions.hh" #include "SystemManager.hh" @@ -107,10 +109,14 @@ size_t SystemManager::ActivatePendingSystems() this->systems.push_back(system); if (system.configure) - this->systemsConfigure.push_back(system.configure); + this->systemsConfigure.emplace_back( + system.parentEntity, + system.configure); if (system.configureParameters) - this->systemsConfigureParameters.push_back(system.configureParameters); + this->systemsConfigureParameters.emplace_back( + system.parentEntity, + system.configureParameters); if (system.reset) this->systemsReset.emplace_back(system.parentEntity, @@ -281,12 +287,12 @@ void SystemManager::AddSystemImpl( } ////////////////////////////////////////////////// -const std::vector& SystemManager::SystemsConfigure() +const std::vector>& SystemManager::SystemsConfigure() { return this->systemsConfigure; } -const std::vector& +const std::vector>& SystemManager::SystemsConfigureParameters() { return this->systemsConfigureParameters; @@ -453,7 +459,8 @@ void RemoveFromVectorIf(std::vector& vec, ////////////////////////////////////////////////// void SystemManager::ProcessRemovedEntities( - const EntityComponentManager &_ecm) + const EntityComponentManager &_ecm, + std::unordered_map &_threadsToTerminate) { if (!_ecm.HasEntitiesMarkedForRemoval()) { @@ -474,6 +481,37 @@ void SystemManager::ProcessRemovedEntities( }); RemoveFromVectorIf(this->systemsPostupdate, [&](const SystemHolder& system) { + // Post update system. Make sure that the threadsToTerminate + if (_ecm.IsMarkedForRemoval(system.parent)) { + auto threads = _threadsToTerminate.find(system.parent); + if (threads == _threadsToTerminate.end()) { + _threadsToTerminate.emplace(system.parent, 1); + } + else { + threads->second++; + } + gzerr << "Terminating system for" << system.parent <<"\n"; + return true; + } + return false; + }); + RemoveFromVectorIf(this->systemsConfigure, + [&](const SystemHolder& system) { return _ecm.IsMarkedForRemoval(system.parent); }); + RemoveFromVectorIf(this->systemsConfigureParameters, + [&](const SystemHolder& system) { + return _ecm.IsMarkedForRemoval(system.parent); + }); + + /// NOTE: We can't do this because the pointers get messed up. + /*RemoveFromVectorIf(this->systems, + [&](const SystemInternal& system) { + return _ecm.IsMarkedForRemoval(system.parentEntity); + });*/ + std::lock_guard lock(this->pendingSystemsMutex); + RemoveFromVectorIf(this->pendingSystems, + [&](const SystemInternal& system) { + return _ecm.IsMarkedForRemoval(system.parentEntity); + }); } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index becfb122b0..65e14a4569 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -130,13 +130,14 @@ namespace gz /// \brief Get a vector of all systems implementing "Configure" /// \return Vector of systems' configure interfaces. - public: const std::vector& SystemsConfigure(); + public: const std::vector>& + SystemsConfigure(); /// \brief Get an vector of all active systems implementing /// "ConfigureParameters" /// \return Vector of systems's configure interfaces. - public: const std::vector& - SystemsConfigureParameters(); + public: const std::vector>& + SystemsConfigureParameters(); /// \brief Get an vector of all active systems implementing "Reset" /// \return Vector of systems' reset interfaces. @@ -163,8 +164,11 @@ namespace gz /// \brief Process system messages and add systems to entities public: void ProcessPendingEntitySystems(); + /// \brief Remove systems that are attached to removed entities + /// \param[in] _entityCompMgr - ECM with entities marked for removal public: void ProcessRemovedEntities( - const EntityComponentManager &_entityCompMgr); + const EntityComponentManager &_entityCompMgr, + std::unordered_map &_threadsToTerminate); /// \brief Implementation for AddSystem functions that takes an SDF /// element. This calls the AddSystemImpl that accepts an SDF Plugin. @@ -208,10 +212,10 @@ namespace gz private: mutable std::mutex pendingSystemsMutex; /// \brief Systems implementing Configure - private: std::vector systemsConfigure; + private: std::vector> systemsConfigure; /// \brief Systems implementing ConfigureParameters - private: std::vector + private: std::vector> systemsConfigureParameters; /// \brief Systems implementing Reset From 968c3ac5db6e1c7fc2448d098f760f630fac4718 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 9 Nov 2023 14:18:04 +0800 Subject: [PATCH 08/51] Now the PosePublisher system likes to go :boom: Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/Barrier.cc | 7 +++++-- src/SimulationRunner.cc | 1 + 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/Barrier.cc b/src/Barrier.cc index dfd3e0067c..825a6e83e0 100644 --- a/src/Barrier.cc +++ b/src/Barrier.cc @@ -16,6 +16,7 @@ */ #include "Barrier.hh" +#include class gz::sim::BarrierPrivate { @@ -68,9 +69,10 @@ Barrier::ExitStatus Barrier::Wait() this->dataPtr->generation++; this->dataPtr->count = this->dataPtr->threadCount; this->dataPtr->cv.notify_all(); + std::cout << "Barrier reset " << this->dataPtr->count << std::endl; return Barrier::ExitStatus::DONE_LAST; } - + std::cout << "Barrier waiting " << this->dataPtr->count << std::endl; while (gen == this->dataPtr->generation && !this->dataPtr->cancelled) { // All threads haven't reached, so wait until generation is reached @@ -98,11 +100,12 @@ void Barrier::Cancel() this->dataPtr->cv.notify_all(); } -#include ////////////////////////////////////////////////// void Barrier::Drop() { std::unique_lock lock(this->dataPtr->mutex); this->dataPtr->threadCount--; + this->dataPtr->count--; + //std::cout << "Dropping " << this->dataPtr->threadCount << ", " << this->dataPtr->count <dataPtr->cv.notify_all(); } \ No newline at end of file diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index b197f03b69..360c188436 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -638,6 +638,7 @@ void SimulationRunner::UpdateSystems() } this->entityCompMgr.LockAddingEntitiesToViews(false); } + gzerr << "here" <<"\n"; } ///////////////////////////////////////////////// From 862ff3ddd30b7e86c87f6c058f5c87d9862d1cc3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 16 Nov 2023 11:18:35 +0800 Subject: [PATCH 09/51] Debugging inputs Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/Barrier.cc | 4 ---- src/SimulationRunner.cc | 1 - src/systems/pose_publisher/PosePublisher.cc | 7 +++++++ 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/Barrier.cc b/src/Barrier.cc index 825a6e83e0..a52159c029 100644 --- a/src/Barrier.cc +++ b/src/Barrier.cc @@ -16,7 +16,6 @@ */ #include "Barrier.hh" -#include class gz::sim::BarrierPrivate { @@ -69,10 +68,8 @@ Barrier::ExitStatus Barrier::Wait() this->dataPtr->generation++; this->dataPtr->count = this->dataPtr->threadCount; this->dataPtr->cv.notify_all(); - std::cout << "Barrier reset " << this->dataPtr->count << std::endl; return Barrier::ExitStatus::DONE_LAST; } - std::cout << "Barrier waiting " << this->dataPtr->count << std::endl; while (gen == this->dataPtr->generation && !this->dataPtr->cancelled) { // All threads haven't reached, so wait until generation is reached @@ -106,6 +103,5 @@ void Barrier::Drop() std::unique_lock lock(this->dataPtr->mutex); this->dataPtr->threadCount--; this->dataPtr->count--; - //std::cout << "Dropping " << this->dataPtr->threadCount << ", " << this->dataPtr->count <dataPtr->cv.notify_all(); } \ No newline at end of file diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 360c188436..b197f03b69 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -638,7 +638,6 @@ void SimulationRunner::UpdateSystems() } this->entityCompMgr.LockAddingEntitiesToViews(false); } - gzerr << "here" <<"\n"; } ///////////////////////////////////////////////// diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 58661d4816..a9ff8e1f76 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -293,6 +293,11 @@ void PosePublisher::PostUpdate(const UpdateInfo &_info, { GZ_PROFILE("PosePublisher::PostUpdate"); + if (!this->dataPtr->model.Valid(_ecm)) + { + gzerr << "Should not have reached here"; + return; + } // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { @@ -531,6 +536,8 @@ void PosePublisherPrivate::PublishPoses( { GZ_PROFILE("PosePublisher::PublishPoses"); + gzerr << "Model " << this->model.Entity() << "\n"; + // publish poses msgs::Pose *msg = nullptr; if (this->usePoseV) From b4e07a34b05da9d94dd27771461c42ea62746fb7 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 24 Nov 2023 12:46:59 +0800 Subject: [PATCH 10/51] Remove "optimization" This commit removes an "optimization" in the PosePublisher that was causing the system to segfault when an entity was removed. In reality this commit is unrelated to the previous few commits. However, without this change the previous change would cause removal of an entity to crash demos that used pose publishers. I think the crux of the issue is that protobufs takeover memory management of objects leading to a scenario where if a reference was freed elsewhere, the protobuf would continue to hold said reference. When we try to do operations on the protobuf we end up double-freeing the memory and hence there is a :boom:. Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/systems/pose_publisher/PosePublisher.cc | 27 ++++++--------------- 1 file changed, 7 insertions(+), 20 deletions(-) diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index a9ff8e1f76..5d0958cb9f 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -155,16 +155,6 @@ class gz::sim::systems::PosePublisherPrivate /// improves performance by avoiding memory allocation public: std::vector> staticPoses; - /// \brief A variable that gets populated with poses. This also here as a - /// member variable to avoid repeated memory allocations and improve - /// performance. - public: msgs::Pose poseMsg; - - /// \brief A variable that gets populated with poses. This also here as a - /// member variable to avoid repeated memory allocations and improve - /// performance. - public: msgs::Pose_V poseVMsg; - /// \brief True to publish a vector of poses. False to publish individual pose /// msgs. public: bool usePoseV = false; @@ -535,13 +525,10 @@ void PosePublisherPrivate::PublishPoses( transport::Node::Publisher &_publisher) { GZ_PROFILE("PosePublisher::PublishPoses"); - - gzerr << "Model " << this->model.Entity() << "\n"; - + msgs::Pose poseMsg; + msgs::Pose_V poseVMsg; // publish poses msgs::Pose *msg = nullptr; - if (this->usePoseV) - this->poseVMsg.Clear(); for (const auto &[entity, pose] : _poses) { @@ -551,12 +538,12 @@ void PosePublisherPrivate::PublishPoses( if (this->usePoseV) { - msg = this->poseVMsg.add_pose(); + msg = poseVMsg.add_pose(); } else { - this->poseMsg.Clear(); - msg = &this->poseMsg; + poseMsg.Clear(); + msg = &poseMsg; } // fill pose msg @@ -583,12 +570,12 @@ void PosePublisherPrivate::PublishPoses( // publish individual pose msgs if (!this->usePoseV) - _publisher.Publish(this->poseMsg); + _publisher.Publish(poseMsg); } // publish pose vector msg if (this->usePoseV) - _publisher.Publish(this->poseVMsg); + _publisher.Publish(poseVMsg); } GZ_ADD_PLUGIN(PosePublisher, From f67db9e4f75bcd4090b8568326babc4e0da6c0c9 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 24 Nov 2023 13:06:17 +0800 Subject: [PATCH 11/51] Style Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/Barrier.cc | 2 +- src/SimulationRunner.cc | 2 +- src/SystemManager.cc | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/Barrier.cc b/src/Barrier.cc index a52159c029..fd13bf1212 100644 --- a/src/Barrier.cc +++ b/src/Barrier.cc @@ -104,4 +104,4 @@ void Barrier::Drop() this->dataPtr->threadCount--; this->dataPtr->count--; this->dataPtr->cv.notify_all(); -} \ No newline at end of file +} diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index b197f03b69..69a60c3cd3 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -585,7 +585,7 @@ void SimulationRunner::ProcessSystemQueue() } this->postUpdateStopBarrier->Wait(); } - + gzerr << "Exiting postupdate worker thread (" << id << ")" << std::endl; })); diff --git a/src/SystemManager.cc b/src/SystemManager.cc index fa1ca37b3c..97e24e08b9 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -287,7 +287,8 @@ void SystemManager::AddSystemImpl( } ////////////////////////////////////////////////// -const std::vector>& SystemManager::SystemsConfigure() +const std::vector>& + SystemManager::SystemsConfigure() { return this->systemsConfigure; } @@ -486,8 +487,7 @@ void SystemManager::ProcessRemovedEntities( auto threads = _threadsToTerminate.find(system.parent); if (threads == _threadsToTerminate.end()) { _threadsToTerminate.emplace(system.parent, 1); - } - else { + } else { threads->second++; } gzerr << "Terminating system for" << system.parent <<"\n"; From f16bad121d129b66ab059dc73b4ef578c0620569 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 15 Mar 2024 14:01:50 +0800 Subject: [PATCH 12/51] Adds a container to handle system removal better. Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/CMakeLists.txt | 1 + src/SystemCointainer.hpp | 162 ++++++++++++++++++++ src/SystemContainer_TEST.cc | 89 +++++++++++ src/SystemManager.cc | 15 +- src/SystemManager.hh | 3 +- src/systems/pose_publisher/PosePublisher.cc | 5 - 6 files changed, 262 insertions(+), 13 deletions(-) create mode 100644 src/SystemCointainer.hpp create mode 100644 src/SystemContainer_TEST.cc diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index d4a5589b19..ae1abe698e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -122,6 +122,7 @@ set (gtest_sources ServerConfig_TEST.cc Server_TEST.cc SimulationRunner_TEST.cc + SystemContainer_TEST.cc SystemLoader_TEST.cc SystemManager_TEST.cc TestFixture_TEST.cc diff --git a/src/SystemCointainer.hpp b/src/SystemCointainer.hpp new file mode 100644 index 0000000000..b863b381f7 --- /dev/null +++ b/src/SystemCointainer.hpp @@ -0,0 +1,162 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_SIM_SYSTEM_CONTAINER_HH_ +#define GZ_SIM_SYSTEM_CONTAINER_HH_ + +#include +#include +#include + +namespace gz +{ + namespace sim + { + ////////////////////////////////////////////////// + /// This container implements a simple masked vector. + /// Using a masked vector for systems ensures that + /// when a system is deleted, its memory location stays + /// the same. This is important because there are a number + /// of places where we refer to systems by raw pointer + /// values. + /// + /// Operation times: + /// push - O(1) + /// removeIf - O(n) + /// iterate - O(n) where n is the max number of systems + /// -> TODO(arjoc): It should be possible to use a cache + /// to make each iterator increment have O(1) time. + /// For now worst case iterator increment is O(n). + /// That being said for most cases the increment + /// should be O(1) + template + class SystemContainer + { + ////////////////////////////////////////// + /// Push an item onto the container + public: void Push(T internal) + { + if (freeSpots.size() == 0) + { + this->systems.push_back(internal); + this->occupied.push_back(true); + return; + } + + auto freeIdx = freeSpots.back(); + freeSpots.pop_back(); + this->systems[freeIdx] = internal; + this->occupied[freeIdx] = true; + } + + ////////////////////////////////////////// + /// Clear the container + public: void Clear() + { + this->systems.clear(); + this->freeSpots.clear(); + this->occupied.clear(); + } + + ////////////////////////////////////////// + /// Number of active systems + public: std::size_t Size() const + { + return this->systems.size() - this->freeSpots.size(); + } + + ////////////////////////////////////////// + /// Remove an item if a condition is met. + public: void RemoveIf(std::function fn) + { + for (std::size_t i = 0; i < systems.size(); i++) + { + if (occupied[i] && fn(systems[i])) + { + occupied[i] = false; + freeSpots.push_back(i); + } + } + } + + private: std::vector systems; + + private: std::vector occupied; + + private: std::vector freeSpots; + + /// Simulates a linked list. + /// TODO(arjoc) use this to build a cache. + //private: std::vector> nextPtr, prevPtr; + + ////////////////////////////////////////// + class iterator : public std::iterator< + std::input_iterator_tag, // iterator_category + T, // value_type + long, // difference_type + T*, // pointer + T& // reference + > { + std::size_t num; + SystemContainer* parent; + public: + explicit iterator(SystemContainer* parent, std::size_t _num = 0) : + num(_num), parent(parent) + { + + } + iterator& operator++() { + // O(n) for now + do { + ++num; + } while(num < parent->Size() && !parent->occupied[num]); + return *this; + } + bool operator==(iterator other) const { return num == other.num; } + bool operator!=(iterator other) const { return !(*this == other); } + T& operator*() const { + return parent->systems[num]; + } + }; + + ////////////////////////////////////////// + public: iterator begin() + { + return iterator(this); + } + + ////////////////////////////////////////// + public: iterator end() + { + auto lastIdx = this->occupied.size(); + + if (lastIdx == 0) + { + return iterator(this); + } + + + while(!this->occupied[lastIdx-1] && lastIdx != 0) + { + lastIdx--; + } + return iterator(this, lastIdx); + } + }; + } +} + +#endif \ No newline at end of file diff --git a/src/SystemContainer_TEST.cc b/src/SystemContainer_TEST.cc new file mode 100644 index 0000000000..dbeec5c922 --- /dev/null +++ b/src/SystemContainer_TEST.cc @@ -0,0 +1,89 @@ +#include +#include +#include "SystemCointainer.hpp" + +#include "iostream" +using namespace gz::sim; + +////////////////////////////////////////////////// +TEST(SystemContainer, InsertionAndIteration) +{ + SystemContainer cont; + cont.Push(1); + cont.Push(2); + cont.Push(3); + + int idx = 0; + for (auto &i: cont) + { + idx++; + ASSERT_EQ(i, idx); + } + ASSERT_EQ(idx, 3); + cont.RemoveIf([](int i) { + return i%2 == 0; + }); + + auto newCount = 0; + std::unordered_set vals; + for (auto &i: cont) + { + newCount++; + ASSERT_NE(i, 2); + vals.insert(i); + } + ASSERT_TRUE(vals.count(1) && vals.count(3)); + ASSERT_EQ(vals.size(), 2); + + std::unordered_set finalVals; + cont.Push(2); + for (auto &i: cont) + { + finalVals.insert(i); + } + ASSERT_TRUE(finalVals.count(1)); + ASSERT_TRUE(finalVals.count(2)); + ASSERT_TRUE(finalVals.count(3)); + ASSERT_EQ(finalVals.size(), 3); +} + +////////////////////////////////////////////////// +TEST(SystemContainer, RemoveAtEnd) +{ + SystemContainer cont; + cont.Push(1); + cont.Push(2); + cont.Push(3); + + cont.RemoveIf([](int i) { + return i == 5; + }); + ASSERT_EQ(cont.Size(), 3); + + cont.RemoveIf([](int i) { + return i == 3; + }); + ASSERT_EQ(cont.Size(), 2); + + int idx = 0; + for (auto &i: cont) + { + idx++; + ASSERT_EQ(i, idx); + } + ASSERT_EQ(idx, 2); +} + +////////////////////////////////////////////////// +TEST(SystemContainer, CheckEmptyProperties) +{ + SystemContainer cont; + ASSERT_EQ(cont.Size(), 0); + int idx = 0; + for (auto &i: cont) + { + idx++; + ASSERT_EQ(i, idx); + } + ASSERT_EQ(idx, 0); +} \ No newline at end of file diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 97e24e08b9..886a0adf79 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -21,6 +21,7 @@ #include +#include "SystemCointainer.hpp" #include "SystemInternal.hh" #include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/Conversions.hh" @@ -87,7 +88,7 @@ size_t SystemManager::TotalCount() const ////////////////////////////////////////////////// size_t SystemManager::ActiveCount() const { - return this->systems.size(); + return this->systems.Size(); } ////////////////////////////////////////////////// @@ -106,7 +107,7 @@ size_t SystemManager::ActivatePendingSystems() for (const auto& system : this->pendingSystems) { - this->systems.push_back(system); + this->systems.Push(system); if (system.configure) this->systemsConfigure.emplace_back( @@ -202,7 +203,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) } } - this->systems.clear(); + this->systems.Clear(); // Load plugins which do not implement reset after clearing this->systems // to ensure the previous instance is destroyed before the new one is created @@ -429,6 +430,7 @@ 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. @@ -504,11 +506,10 @@ void SystemManager::ProcessRemovedEntities( return _ecm.IsMarkedForRemoval(system.parent); }); - /// NOTE: We can't do this because the pointers get messed up. - /*RemoveFromVectorIf(this->systems, - [&](const SystemInternal& system) { + this->systems.RemoveIf([&](const SystemInternal& system) { return _ecm.IsMarkedForRemoval(system.parentEntity); - });*/ + }); + std::lock_guard lock(this->pendingSystemsMutex); RemoveFromVectorIf(this->pendingSystems, [&](const SystemInternal& system) { diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 65e14a4569..09ce9606e2 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -33,6 +33,7 @@ #include "gz/sim/SystemLoader.hh" #include "gz/sim/Types.hh" +#include "SystemCointainer.hpp" #include "SystemInternal.hh" namespace gz @@ -203,7 +204,7 @@ namespace gz msgs::EntityPlugin_V &_res); /// \brief All the systems. - private: std::vector systems; + private: SystemContainer systems; /// \brief Pending systems to be added to systems. private: std::vector pendingSystems; diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 5d0958cb9f..7fe638408d 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -283,11 +283,6 @@ void PosePublisher::PostUpdate(const UpdateInfo &_info, { GZ_PROFILE("PosePublisher::PostUpdate"); - if (!this->dataPtr->model.Valid(_ecm)) - { - gzerr << "Should not have reached here"; - return; - } // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { From 829488886f36d9ba7a3ee565581e392ebfaa88cd Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 18 Mar 2024 15:06:31 +0800 Subject: [PATCH 13/51] Add unit tests Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 4 +-- src/SimulationRunner_TEST.cc | 14 +++++--- src/SystemCointainer.hpp | 10 +++--- src/SystemManager.cc | 7 ++-- src/SystemManager_TEST.cc | 66 ++++++++++++++++++++++++++++++++++++ 5 files changed, 89 insertions(+), 12 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 69a60c3cd3..205cbd2b1d 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -575,7 +575,7 @@ void SimulationRunner::ProcessSystemQueue() if (terminate->second == 0) { this->threadsToTerminate.erase(terminate); } - gzerr << "Terminating thread " << id << ", " << parentEntity <<"\n"; + gzdbg << "Terminating thread " << id << ", " << parentEntity <<"\n"; this->postUpdateStartBarrier->Drop(); this->postUpdateStopBarrier->Wait(); this->postUpdateStopBarrier->Drop(); @@ -586,7 +586,7 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateStopBarrier->Wait(); } - gzerr << "Exiting postupdate worker thread (" + gzdbg << "Exiting postupdate worker thread (" << id << ")" << std::endl; })); id++; diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 9905c41747..bee0a7f04e 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -16,7 +16,6 @@ */ #include - #include #include @@ -111,7 +110,6 @@ void rootClockCb(const msgs::Clock &_msg) rootClockMsgs.push_back(_msg); } - ///////////////////////////////////////////////// TEST_P(SimulationRunnerTest, CreateEntities) { @@ -1532,8 +1530,16 @@ TEST_P(SimulationRunnerTest, SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader, serverConfig); - // 1 model plugin from SDF and 2 world plugins from config - ASSERT_EQ(3u, runner.SystemCount()); + // 1 model plugin from SDF and 1 world plugin from config + // and 1 model plugin from theconfig + EXPECT_EQ(3u, runner.SystemCount()); + runner.SetPaused(false); + runner.Run(1); + + //Remove the model. Only 1 world plugin should remain. + EXPECT_TRUE(runner.RequestRemoveEntity("box")); + runner.Run(2); + EXPECT_EQ(1u, runner.SystemCount()); } ///////////////////////////////////////////////// diff --git a/src/SystemCointainer.hpp b/src/SystemCointainer.hpp index b863b381f7..c566c3a595 100644 --- a/src/SystemCointainer.hpp +++ b/src/SystemCointainer.hpp @@ -41,7 +41,10 @@ namespace gz /// to make each iterator increment have O(1) time. /// For now worst case iterator increment is O(n). /// That being said for most cases the increment - /// should be O(1) + /// should be O(1). Eitherways, run does not iterate + /// through systems but rather ISystem* interfaces. + /// The only two places we iterate over this is during + /// reset and TotalByEntity in the tests. template class SystemContainer { @@ -116,7 +119,7 @@ namespace gz explicit iterator(SystemContainer* parent, std::size_t _num = 0) : num(_num), parent(parent) { - + } iterator& operator++() { // O(n) for now @@ -145,9 +148,8 @@ namespace gz if (lastIdx == 0) { - return iterator(this); + return iterator(this); } - while(!this->occupied[lastIdx-1] && lastIdx != 0) { diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 886a0adf79..91387d2c1e 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -465,6 +465,10 @@ void SystemManager::ProcessRemovedEntities( const EntityComponentManager &_ecm, std::unordered_map &_threadsToTerminate) { + // Note: This function has O(n) time when an entity is removed + // where n is number of systems. Ideally we would only iterate + // over entities marked for removal but that would involve having + // a key value map. This can be marked as a future improvement. if (!_ecm.HasEntitiesMarkedForRemoval()) { return; @@ -492,7 +496,6 @@ void SystemManager::ProcessRemovedEntities( } else { threads->second++; } - gzerr << "Terminating system for" << system.parent <<"\n"; return true; } return false; @@ -509,7 +512,7 @@ void SystemManager::ProcessRemovedEntities( this->systems.RemoveIf([&](const SystemInternal& system) { return _ecm.IsMarkedForRemoval(system.parentEntity); }); - + std::lock_guard lock(this->pendingSystemsMutex); RemoveFromVectorIf(this->pendingSystems, [&](const SystemInternal& system) { diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 5026842a9f..4708b7bf80 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -214,6 +214,72 @@ TEST(SystemManager, AddSystemEcm) EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); } +///////////////////////////////////////////////// +TEST(SystemManager, AddAndRemoveSystemEcm) +{ + auto loader = std::make_shared(); + + auto ecm = EntityComponentManager(); + auto eventManager = EventManager(); + + auto paramRegistry = std::make_unique< + gz::transport::parameters::ParametersRegistry>("SystemManager_TEST"); + SystemManager systemMgr( + loader, &ecm, &eventManager, std::string(), paramRegistry.get()); + + EXPECT_EQ(0u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(0u, systemMgr.TotalCount()); + EXPECT_EQ(0u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + + auto configSystem = std::make_shared(); + systemMgr.AddSystem(configSystem, kNullEntity, nullptr); + + auto entity = ecm.CreateEntity(); + + auto updateSystemWithChild = std::make_shared(); + systemMgr.AddSystem(updateSystemWithChild, entity, nullptr); + + // Configure called during AddSystem + EXPECT_EQ(1, configSystem->configured); + EXPECT_EQ(1, configSystem->configuredParameters); + + EXPECT_EQ(0u, systemMgr.ActiveCount()); + EXPECT_EQ(2u, systemMgr.PendingCount()); + EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(0u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + + systemMgr.ActivatePendingSystems(); + EXPECT_EQ(2u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); + + // Remove the entity + ecm.RequestRemoveEntity(entity); + std::unordered_map entities_to_remove; + systemMgr.ProcessRemovedEntities(ecm, entities_to_remove); + + EXPECT_EQ(entities_to_remove.size(), 1); + EXPECT_EQ(entities_to_remove[entity], 1); + EXPECT_EQ(1u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(1u, systemMgr.TotalCount()); + EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); +} + ///////////////////////////////////////////////// TEST(SystemManager, AddSystemWithInfo) { From 53e951ae3cf7fdebf82650b950a6d6b699baea1f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 18 Mar 2024 15:22:08 +0800 Subject: [PATCH 14/51] Style Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SystemCointainer.hpp | 2 +- src/SystemContainer_TEST.cc | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/SystemCointainer.hpp b/src/SystemCointainer.hpp index c566c3a595..1fa9a4005c 100644 --- a/src/SystemCointainer.hpp +++ b/src/SystemCointainer.hpp @@ -161,4 +161,4 @@ namespace gz } } -#endif \ No newline at end of file +#endif diff --git a/src/SystemContainer_TEST.cc b/src/SystemContainer_TEST.cc index dbeec5c922..a2da2808e7 100644 --- a/src/SystemContainer_TEST.cc +++ b/src/SystemContainer_TEST.cc @@ -12,7 +12,7 @@ TEST(SystemContainer, InsertionAndIteration) cont.Push(1); cont.Push(2); cont.Push(3); - + int idx = 0; for (auto &i: cont) { @@ -25,7 +25,7 @@ TEST(SystemContainer, InsertionAndIteration) }); auto newCount = 0; - std::unordered_set vals; + std::unordered_set vals; for (auto &i: cont) { newCount++; @@ -35,7 +35,7 @@ TEST(SystemContainer, InsertionAndIteration) ASSERT_TRUE(vals.count(1) && vals.count(3)); ASSERT_EQ(vals.size(), 2); - std::unordered_set finalVals; + std::unordered_set finalVals; cont.Push(2); for (auto &i: cont) { @@ -86,4 +86,4 @@ TEST(SystemContainer, CheckEmptyProperties) ASSERT_EQ(i, idx); } ASSERT_EQ(idx, 0); -} \ No newline at end of file +} From 446e816386348210edf0ffd9bccefd530993eaae Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 18 Mar 2024 15:39:17 +0800 Subject: [PATCH 15/51] Style Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SimulationRunner_TEST.cc | 2 +- src/SystemCointainer.hpp | 2 +- src/SystemContainer_TEST.cc | 24 ++++++++++++++++++++---- 3 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index bee0a7f04e..a127383b8a 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1536,7 +1536,7 @@ TEST_P(SimulationRunnerTest, runner.SetPaused(false); runner.Run(1); - //Remove the model. Only 1 world plugin should remain. + // Remove the model. Only 1 world plugin should remain. EXPECT_TRUE(runner.RequestRemoveEntity("box")); runner.Run(2); EXPECT_EQ(1u, runner.SystemCount()); diff --git a/src/SystemCointainer.hpp b/src/SystemCointainer.hpp index 1fa9a4005c..fae5c2f979 100644 --- a/src/SystemCointainer.hpp +++ b/src/SystemCointainer.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Open Source Robotics Foundation + * Copyright (C) 2024 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/src/SystemContainer_TEST.cc b/src/SystemContainer_TEST.cc index a2da2808e7..926036fa90 100644 --- a/src/SystemContainer_TEST.cc +++ b/src/SystemContainer_TEST.cc @@ -1,3 +1,19 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ #include #include #include "SystemCointainer.hpp" @@ -14,7 +30,7 @@ TEST(SystemContainer, InsertionAndIteration) cont.Push(3); int idx = 0; - for (auto &i: cont) + for (auto &i : cont) { idx++; ASSERT_EQ(i, idx); @@ -26,7 +42,7 @@ TEST(SystemContainer, InsertionAndIteration) auto newCount = 0; std::unordered_set vals; - for (auto &i: cont) + for (auto &i : cont) { newCount++; ASSERT_NE(i, 2); @@ -37,7 +53,7 @@ TEST(SystemContainer, InsertionAndIteration) std::unordered_set finalVals; cont.Push(2); - for (auto &i: cont) + for (auto &i : cont) { finalVals.insert(i); } @@ -80,7 +96,7 @@ TEST(SystemContainer, CheckEmptyProperties) SystemContainer cont; ASSERT_EQ(cont.Size(), 0); int idx = 0; - for (auto &i: cont) + for (auto &i : cont) { idx++; ASSERT_EQ(i, idx); From 7ecefdc7a6d0e15095009db72c89767622615403 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 18 Mar 2024 15:50:48 +0800 Subject: [PATCH 16/51] Style Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SystemContainer_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SystemContainer_TEST.cc b/src/SystemContainer_TEST.cc index 926036fa90..066c2d79cb 100644 --- a/src/SystemContainer_TEST.cc +++ b/src/SystemContainer_TEST.cc @@ -82,7 +82,7 @@ TEST(SystemContainer, RemoveAtEnd) ASSERT_EQ(cont.Size(), 2); int idx = 0; - for (auto &i: cont) + for (auto &i : cont) { idx++; ASSERT_EQ(i, idx); From 19b418c6735358316ae21761da3d19fdd88a989a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 19 Mar 2024 11:18:52 +0800 Subject: [PATCH 17/51] Update barrier documentation. Signed-off-by: Arjo Chakravarty --- src/Barrier.hh | 4 ++++ src/SystemCointainer.hpp | 8 ++------ 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/Barrier.hh b/src/Barrier.hh index 4d5a10de56..8ecc0fb728 100644 --- a/src/Barrier.hh +++ b/src/Barrier.hh @@ -81,6 +81,10 @@ namespace gz /// reflect that. public: ExitStatus Wait(); + /// \brief Similar to C++20's std::barrier::await_and_drop + /// decrements both the initial expected count for subsequent phases and + /// the expected count for current phase by one. Unlike C++20, this + /// does not wait. public: void Drop(); /// \brief Cancel the barrier, causing all threads to unblock and diff --git a/src/SystemCointainer.hpp b/src/SystemCointainer.hpp index fae5c2f979..55c917765c 100644 --- a/src/SystemCointainer.hpp +++ b/src/SystemCointainer.hpp @@ -101,10 +101,6 @@ namespace gz private: std::vector freeSpots; - /// Simulates a linked list. - /// TODO(arjoc) use this to build a cache. - //private: std::vector> nextPtr, prevPtr; - ////////////////////////////////////////// class iterator : public std::iterator< std::input_iterator_tag, // iterator_category @@ -116,8 +112,8 @@ namespace gz std::size_t num; SystemContainer* parent; public: - explicit iterator(SystemContainer* parent, std::size_t _num = 0) : - num(_num), parent(parent) + explicit iterator(SystemContainer* _parent, std::size_t _num = 0) : + num(_num), parent(_parent) { } From e84d9dc057248ed925f7f8cf26c785a38aba927a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 19 Mar 2024 13:27:40 +0800 Subject: [PATCH 18/51] Rework barrier and add test. Signed-off-by: Arjo Chakravarty --- src/Barrier.cc | 12 ++++++-- src/Barrier.hh | 3 +- src/Barrier_TEST.cc | 75 +++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 86 insertions(+), 4 deletions(-) diff --git a/src/Barrier.cc b/src/Barrier.cc index fd13bf1212..8c813db673 100644 --- a/src/Barrier.cc +++ b/src/Barrier.cc @@ -16,6 +16,9 @@ */ #include "Barrier.hh" +#include +#include +#include class gz::sim::BarrierPrivate { @@ -102,6 +105,11 @@ void Barrier::Drop() { std::unique_lock lock(this->dataPtr->mutex); this->dataPtr->threadCount--; - this->dataPtr->count--; - this->dataPtr->cv.notify_all(); + if (--this->dataPtr->count == 0) + { + // All threads have reached the wait, so reset the barrier. + this->dataPtr->generation++; + this->dataPtr->count = this->dataPtr->threadCount; + this->dataPtr->cv.notify_all(); + } } diff --git a/src/Barrier.hh b/src/Barrier.hh index 8ecc0fb728..7a1bda5312 100644 --- a/src/Barrier.hh +++ b/src/Barrier.hh @@ -83,8 +83,7 @@ namespace gz /// \brief Similar to C++20's std::barrier::await_and_drop /// decrements both the initial expected count for subsequent phases and - /// the expected count for current phase by one. Unlike C++20, this - /// does not wait. + /// the expected count for current phase by one. public: void Drop(); /// \brief Cancel the barrier, causing all threads to unblock and diff --git a/src/Barrier_TEST.cc b/src/Barrier_TEST.cc index 1af0c73f8b..b7750b0152 100644 --- a/src/Barrier_TEST.cc +++ b/src/Barrier_TEST.cc @@ -17,6 +17,7 @@ #include +#include #include #include "Barrier.hh" @@ -174,3 +175,77 @@ TEST(Barrier, Cancel) t.join(); } + + +////////////////////////////////////////////////// +TEST(Barrier, Drop) +{ + auto barrier = std::make_unique(3); + + unsigned int preBarrier { 0 }; + unsigned int postBarrier { 0 }; + + + std::mutex mutex; + std::condition_variable cv; + + auto t1 = std::thread([&]() + { + { + std::lock_guard lock(mutex); + preBarrier++; + cv.notify_one(); + } + + barrier->Wait(); + + { + std::lock_guard lock(mutex); + postBarrier++; + cv.notify_one(); + } + barrier->Wait(); + + }); + + auto t2 = std::thread([&]() + { + { + std::lock_guard lock(mutex); + preBarrier++; + cv.notify_one(); + } + + barrier->Drop(); + + }); + + + barrier->Wait(); + { + std::unique_lock lock(mutex); + auto ret = cv.wait_for(lock, std::chrono::milliseconds(1000), + [&]() + { + return preBarrier == 2; + }); + ASSERT_TRUE(ret); + } + EXPECT_EQ(preBarrier, 2); + // t2 should terminate. + t2.join(); + + + barrier->Wait(); + { + std::unique_lock lock(mutex); + auto ret = cv.wait_for(lock, std::chrono::milliseconds(1000), + [&]() + { + return postBarrier == 1; + }); + ASSERT_TRUE(ret); + } + EXPECT_EQ(postBarrier, 1); + t1.join(); +} From b5878e5620dff9adfb297c497fbd46bbe3405401 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 19 Mar 2024 13:41:43 +0800 Subject: [PATCH 19/51] Use the new simplified drop API. Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 205cbd2b1d..2e278d6051 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -577,7 +577,6 @@ void SimulationRunner::ProcessSystemQueue() } gzdbg << "Terminating thread " << id << ", " << parentEntity <<"\n"; this->postUpdateStartBarrier->Drop(); - this->postUpdateStopBarrier->Wait(); this->postUpdateStopBarrier->Drop(); break; } From 3dd43956852e4d36f1764285f42dec1964992e54 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 7 May 2024 14:29:49 +0800 Subject: [PATCH 20/51] Fixed systemcontainer logic bug. Signed-off-by: Arjo Chakravarty --- src/SystemCointainer.hpp | 7 ++++++- src/SystemContainer_TEST.cc | 33 +++++++++++++++++++++++++++++++++ 2 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/SystemCointainer.hpp b/src/SystemCointainer.hpp index 55c917765c..a34f50c8cb 100644 --- a/src/SystemCointainer.hpp +++ b/src/SystemCointainer.hpp @@ -121,7 +121,12 @@ namespace gz // O(n) for now do { ++num; - } while(num < parent->Size() && !parent->occupied[num]); + } while(num < parent->systems.size() && !parent->occupied[num]); + + auto res = parent->end(); + if (num >= parent->systems.size()){ + num = res.num; + } return *this; } bool operator==(iterator other) const { return num == other.num; } diff --git a/src/SystemContainer_TEST.cc b/src/SystemContainer_TEST.cc index 066c2d79cb..bda1f4261d 100644 --- a/src/SystemContainer_TEST.cc +++ b/src/SystemContainer_TEST.cc @@ -90,6 +90,39 @@ TEST(SystemContainer, RemoveAtEnd) ASSERT_EQ(idx, 2); } +////////////////////////////////////////////////// +TEST(SystemContainer, RemoveFromMiddle) +{ + SystemContainer cont; + for (int i = 0; i < 10; i++) + { + cont.Push(i); + } + + ASSERT_EQ(cont.Size(), 10); + + cont.RemoveIf([](int i) { + return i == 5; + }); + ASSERT_EQ(cont.Size(), 9); + + + for (auto &i : cont) + { + ASSERT_NE(i, 5); + } + + cont.RemoveIf([](int i) { + return i % 2 == 1; + }); + + for (auto &i : cont) + { + ASSERT_NE(i % 2, 1); + } + ASSERT_EQ(cont.Size(), 5); +} + ////////////////////////////////////////////////// TEST(SystemContainer, CheckEmptyProperties) { From 4d104ec412eef8fe4733b4de9c2e13976725c51d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 7 May 2024 15:20:26 +0800 Subject: [PATCH 21/51] Cleaner version of the fix. Signed-off-by: Arjo Chakravarty --- src/SystemCointainer.hpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/SystemCointainer.hpp b/src/SystemCointainer.hpp index a34f50c8cb..c732713cc8 100644 --- a/src/SystemCointainer.hpp +++ b/src/SystemCointainer.hpp @@ -118,15 +118,12 @@ namespace gz } iterator& operator++() { + auto end = parent->end(); // O(n) for now do { ++num; - } while(num < parent->systems.size() && !parent->occupied[num]); + } while(num < end.num && !parent->occupied[num]); - auto res = parent->end(); - if (num >= parent->systems.size()){ - num = res.num; - } return *this; } bool operator==(iterator other) const { return num == other.num; } From 483e19a771305c5077ab2254b36b62500b2db037 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 7 May 2024 15:47:10 +0800 Subject: [PATCH 22/51] Rename according to @scpeters feedback. Signed-off-by: Arjo Chakravarty --- src/SystemManager.cc | 24 ++++++++++++------------ src/SystemManager.hh | 28 ++++++++++++++-------------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 91387d2c1e..de79bcc5a4 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -288,40 +288,40 @@ void SystemManager::AddSystemImpl( } ////////////////////////////////////////////////// -const std::vector>& +const std::vector>& SystemManager::SystemsConfigure() { return this->systemsConfigure; } -const std::vector>& +const std::vector>& SystemManager::SystemsConfigureParameters() { return this->systemsConfigureParameters; } ////////////////////////////////////////////////// -const std::vector> &SystemManager::SystemsReset() +const std::vector> &SystemManager::SystemsReset() { return this->systemsReset; } ////////////////////////////////////////////////// -const std::vector>& +const std::vector>& SystemManager::SystemsPreUpdate() { return this->systemsPreupdate; } ////////////////////////////////////////////////// -const std::vector>& +const std::vector>& SystemManager::SystemsUpdate() { return this->systemsUpdate; } ////////////////////////////////////////////////// -const std::vector>& +const std::vector>& SystemManager::SystemsPostUpdate() { return this->systemsPostupdate; @@ -475,19 +475,19 @@ void SystemManager::ProcessRemovedEntities( } RemoveFromVectorIf(this->systemsReset, - [&](const SystemHolder& system) { + [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); }); RemoveFromVectorIf(this->systemsPreupdate, - [&](const SystemHolder& system) { + [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); }); RemoveFromVectorIf(this->systemsUpdate, - [&](const SystemHolder& system) { + [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); }); RemoveFromVectorIf(this->systemsPostupdate, - [&](const SystemHolder& system) { + [&](const SystemIfaceWithParent& system) { // Post update system. Make sure that the threadsToTerminate if (_ecm.IsMarkedForRemoval(system.parent)) { auto threads = _threadsToTerminate.find(system.parent); @@ -501,11 +501,11 @@ void SystemManager::ProcessRemovedEntities( return false; }); RemoveFromVectorIf(this->systemsConfigure, - [&](const SystemHolder& system) { + [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); }); RemoveFromVectorIf(this->systemsConfigureParameters, - [&](const SystemHolder& system) { + [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); }); diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 09ce9606e2..7589c40c2d 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -46,7 +46,7 @@ namespace gz /// \brief Helper container to keep track of /// system interfaces and their parents template - struct SystemHolder { + struct SystemIfaceWithParent { /// Parent entity of system Entity parent; @@ -54,7 +54,7 @@ namespace gz IFace* system; /// \brief constructor - SystemHolder(Entity _parent, IFace* _iface): + SystemIfaceWithParent(Entity _parent, IFace* _iface): parent(_parent), system(_iface) {} }; @@ -131,31 +131,31 @@ namespace gz /// \brief Get a vector of all systems implementing "Configure" /// \return Vector of systems' configure interfaces. - public: const std::vector>& + public: const std::vector>& SystemsConfigure(); /// \brief Get an vector of all active systems implementing /// "ConfigureParameters" /// \return Vector of systems's configure interfaces. - public: const std::vector>& + public: const std::vector>& SystemsConfigureParameters(); /// \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>& + 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>& + public: const std::vector>& SystemsPostUpdate(); /// \brief Get an vector of all systems attached to a given entity. @@ -213,23 +213,23 @@ namespace gz private: mutable std::mutex pendingSystemsMutex; /// \brief Systems implementing Configure - private: std::vector> systemsConfigure; + private: std::vector> systemsConfigure; /// \brief Systems implementing ConfigureParameters - private: std::vector> + private: std::vector> 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; From 5ded1142091bedd8fe9e549d697235e185698f38 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 7 May 2024 16:31:09 +0800 Subject: [PATCH 23/51] Fix spelling error and style Signed-off-by: Arjo Chakravarty --- src/{SystemCointainer.hpp => SystemContainer.hpp} | 0 src/SystemContainer_TEST.cc | 3 +-- src/SystemManager.cc | 2 +- src/SystemManager.hh | 11 +++++++---- 4 files changed, 9 insertions(+), 7 deletions(-) rename src/{SystemCointainer.hpp => SystemContainer.hpp} (100%) diff --git a/src/SystemCointainer.hpp b/src/SystemContainer.hpp similarity index 100% rename from src/SystemCointainer.hpp rename to src/SystemContainer.hpp diff --git a/src/SystemContainer_TEST.cc b/src/SystemContainer_TEST.cc index bda1f4261d..c002913de3 100644 --- a/src/SystemContainer_TEST.cc +++ b/src/SystemContainer_TEST.cc @@ -16,9 +16,8 @@ */ #include #include -#include "SystemCointainer.hpp" +#include "SystemContainer.hpp" -#include "iostream" using namespace gz::sim; ////////////////////////////////////////////////// diff --git a/src/SystemManager.cc b/src/SystemManager.cc index de79bcc5a4..54fbf102e6 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -21,7 +21,7 @@ #include -#include "SystemCointainer.hpp" +#include "SystemContainer.hpp" #include "SystemInternal.hh" #include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/Conversions.hh" diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 7589c40c2d..c9fb64209a 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -33,7 +33,7 @@ #include "gz/sim/SystemLoader.hh" #include "gz/sim/Types.hh" -#include "SystemCointainer.hpp" +#include "SystemContainer.hpp" #include "SystemInternal.hh" namespace gz @@ -137,12 +137,14 @@ namespace gz /// \brief Get an vector of all active systems implementing /// "ConfigureParameters" /// \return Vector of systems's configure interfaces. - public: const std::vector>& + public: const std::vector< + SystemIfaceWithParent>& SystemsConfigureParameters(); /// \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. @@ -151,7 +153,8 @@ namespace gz /// \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. From 378399dfb13df2f421f9baa232539e24385dd469 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 7 May 2024 16:38:16 +0800 Subject: [PATCH 24/51] Style Signed-off-by: Arjo Chakravarty --- src/SystemManager.cc | 3 ++- src/SystemManager.hh | 9 ++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 54fbf102e6..dde20f8c9d 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -301,7 +301,8 @@ SystemManager::SystemsConfigureParameters() } ////////////////////////////////////////////////// -const std::vector> &SystemManager::SystemsReset() +const std::vector>& +SystemManager::SystemsReset() { return this->systemsReset; } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index c9fb64209a..92a2f2d254 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -216,7 +216,8 @@ namespace gz private: mutable std::mutex pendingSystemsMutex; /// \brief Systems implementing Configure - private: std::vector> systemsConfigure; + private: std::vector> + systemsConfigure; /// \brief Systems implementing ConfigureParameters private: std::vector> @@ -226,13 +227,15 @@ namespace gz private: std::vector> systemsReset; /// \brief Systems implementing PreUpdate - private: std::vector> systemsPreupdate; + private: std::vector> + systemsPreupdate; /// \brief Systems implementing Update 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; From 391496fe77da4a9c0f08b3c6a528e103911b0c1b Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 May 2024 10:08:41 +0800 Subject: [PATCH 25/51] Remove the use of `std::iterator` Signed-off-by: Arjo Chakravarty --- src/Barrier.cc | 3 --- src/SystemContainer.hpp | 14 +++++++------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/Barrier.cc b/src/Barrier.cc index 8c813db673..3b8e1b30f0 100644 --- a/src/Barrier.cc +++ b/src/Barrier.cc @@ -16,9 +16,6 @@ */ #include "Barrier.hh" -#include -#include -#include class gz::sim::BarrierPrivate { diff --git a/src/SystemContainer.hpp b/src/SystemContainer.hpp index c732713cc8..6027b05926 100644 --- a/src/SystemContainer.hpp +++ b/src/SystemContainer.hpp @@ -17,6 +17,7 @@ #ifndef GZ_SIM_SYSTEM_CONTAINER_HH_ #define GZ_SIM_SYSTEM_CONTAINER_HH_ +#include #include #include #include @@ -102,16 +103,15 @@ namespace gz private: std::vector freeSpots; ////////////////////////////////////////// - class iterator : public std::iterator< - std::input_iterator_tag, // iterator_category - T, // value_type - long, // difference_type - T*, // pointer - T& // reference - > { + class iterator { std::size_t num; SystemContainer* parent; public: + using iterator_category = std::input_iterator_tag; + using value_type = T; + using difference_type = long; + using pointer = T*; + using reference = T&; explicit iterator(SystemContainer* _parent, std::size_t _num = 0) : num(_num), parent(_parent) { From 61141756506d464a39a29433b7065f587fde8427 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 May 2024 15:27:15 +0800 Subject: [PATCH 26/51] Fix thread safety concern Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 7 +++---- src/SimulationRunner.hh | 6 ++++-- src/SystemManager.cc | 9 ++------- src/SystemManager.hh | 2 +- src/SystemManager_TEST.cc | 4 ++-- 5 files changed, 12 insertions(+), 16 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 2e278d6051..0407e7ddac 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -571,10 +571,6 @@ void SimulationRunner::ProcessSystemQueue() { auto terminate = this->threadsToTerminate.find(parentEntity); if (terminate != this->threadsToTerminate.end()) { - terminate->second--; - if (terminate->second == 0) { - this->threadsToTerminate.erase(terminate); - } gzdbg << "Terminating thread " << id << ", " << parentEntity <<"\n"; this->postUpdateStartBarrier->Drop(); this->postUpdateStopBarrier->Drop(); @@ -903,6 +899,9 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Update all the systems. this->UpdateSystems(); + // Remove any threads that have been terminated + this->threadsToTerminate.clear(); + if (!this->Paused() && this->requestedRunToSimTime && this->requestedRunToSimTime.value() > this->simTimeEpoch && this->currentInfo.simTime >= this->requestedRunToSimTime.value()) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 773bb2136e..73a1b4de6f 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -543,8 +543,10 @@ namespace gz /// at the appropriate time. private: std::unique_ptr newWorldControlState; - - private: std::unordered_map threadsToTerminate; + /// \brief Lists which threads are supposed to be removed on the next iteration. + /// Note: We read from this during the `postUpdate` the set is cleared after + /// `postUpdate`. + private: std::unordered_set threadsToTerminate; private: bool resetInitiated{false}; friend class LevelManager; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index dde20f8c9d..ee34e43c50 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -464,7 +464,7 @@ void RemoveFromVectorIf(std::vector& vec, ////////////////////////////////////////////////// void SystemManager::ProcessRemovedEntities( const EntityComponentManager &_ecm, - std::unordered_map &_threadsToTerminate) + std::unordered_set &_threadsToTerminate) { // Note: This function has O(n) time when an entity is removed // where n is number of systems. Ideally we would only iterate @@ -491,12 +491,7 @@ void SystemManager::ProcessRemovedEntities( [&](const SystemIfaceWithParent& system) { // Post update system. Make sure that the threadsToTerminate if (_ecm.IsMarkedForRemoval(system.parent)) { - auto threads = _threadsToTerminate.find(system.parent); - if (threads == _threadsToTerminate.end()) { - _threadsToTerminate.emplace(system.parent, 1); - } else { - threads->second++; - } + _threadsToTerminate.emplace(system.parent); return true; } return false; diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 92a2f2d254..06ae81605d 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -172,7 +172,7 @@ namespace gz /// \param[in] _entityCompMgr - ECM with entities marked for removal public: void ProcessRemovedEntities( const EntityComponentManager &_entityCompMgr, - std::unordered_map &_threadsToTerminate); + std::unordered_set &_threadsToTerminate); /// \brief Implementation for AddSystem functions that takes an SDF /// element. This calls the AddSystemImpl that accepts an SDF Plugin. diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 4708b7bf80..4b4f366ce0 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -266,11 +266,11 @@ TEST(SystemManager, AddAndRemoveSystemEcm) // Remove the entity ecm.RequestRemoveEntity(entity); - std::unordered_map entities_to_remove; + std::unordered_set entities_to_remove; systemMgr.ProcessRemovedEntities(ecm, entities_to_remove); EXPECT_EQ(entities_to_remove.size(), 1); - EXPECT_EQ(entities_to_remove[entity], 1); + EXPECT_EQ(entities_to_remove.count(entity), 1); EXPECT_EQ(1u, systemMgr.ActiveCount()); EXPECT_EQ(0u, systemMgr.PendingCount()); EXPECT_EQ(1u, systemMgr.TotalCount()); From f486fb8877d217414f999b8bfa23182dba6a10f1 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 8 May 2024 16:06:02 +0800 Subject: [PATCH 27/51] Style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.hh | 8 +++++--- src/SystemManager.cc | 1 + src/SystemManager.hh | 1 + 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 73a1b4de6f..4ba03be128 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -543,9 +543,11 @@ namespace gz /// at the appropriate time. private: std::unique_ptr newWorldControlState; - /// \brief Lists which threads are supposed to be removed on the next iteration. - /// Note: We read from this during the `postUpdate` the set is cleared after - /// `postUpdate`. + /// \brief Lists which threads are supposed to be removed on the next + /// iteration. + /// Note: We read from this during the `postUpdate` the set is cleared + /// after `postUpdate` and then we set it when processing + /// removals. private: std::unordered_set threadsToTerminate; private: bool resetInitiated{false}; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index ee34e43c50..e4f34d63ca 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 06ae81605d..4b1ff39717 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -22,6 +22,7 @@ #include #include #include +#include #include #include From 574ab2d72b68f32e76e8443123789507cd3f6423 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 18 Jun 2024 12:54:57 +0800 Subject: [PATCH 28/51] Apply suggestions from code review Co-authored-by: Steve Peters Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.hh | 6 +++--- src/SystemManager.cc | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 4ba03be128..4e5332ed74 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -545,9 +545,9 @@ namespace gz /// \brief Lists which threads are supposed to be removed on the next /// iteration. - /// Note: We read from this during the `postUpdate` the set is cleared - /// after `postUpdate` and then we set it when processing - /// removals. + /// Note: We read from this during the `PostUpdate`, the set is cleared + /// after `PostUpdate`, and then we set it when processing + /// removals at the end of `Step`. private: std::unordered_set threadsToTerminate; private: bool resetInitiated{false}; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index e4f34d63ca..b9f7c518b1 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -490,7 +490,8 @@ void SystemManager::ProcessRemovedEntities( }); RemoveFromVectorIf(this->systemsPostupdate, [&](const SystemIfaceWithParent& system) { - // Post update system. Make sure that the threadsToTerminate + // If system with a PostUpdate is marked for removal, mark its thread for + // termination. if (_ecm.IsMarkedForRemoval(system.parent)) { _threadsToTerminate.emplace(system.parent); return true; From f5b3878142d83976874974081a3bd92840b500e1 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 19 Jun 2024 08:44:11 +0800 Subject: [PATCH 29/51] Rename and update class Signed-off-by: Arjo Chakravarty --- ...SystemContainer.hpp => SystemContainer.hh} | 24 +++++++++++-------- src/SystemContainer_TEST.cc | 2 +- src/SystemManager.cc | 2 +- src/SystemManager.hh | 2 +- 4 files changed, 17 insertions(+), 13 deletions(-) rename src/{SystemContainer.hpp => SystemContainer.hh} (89%) diff --git a/src/SystemContainer.hpp b/src/SystemContainer.hh similarity index 89% rename from src/SystemContainer.hpp rename to src/SystemContainer.hh index 6027b05926..45ed26d231 100644 --- a/src/SystemContainer.hpp +++ b/src/SystemContainer.hh @@ -22,10 +22,13 @@ #include #include +#include "gz/sim/config.hh" + namespace gz { namespace sim { + inline namespace GZ_SIM_VERSION_NAMESPACE { ////////////////////////////////////////////////// /// This container implements a simple masked vector. /// Using a masked vector for systems ensures that @@ -103,7 +106,7 @@ namespace gz private: std::vector freeSpots; ////////////////////////////////////////// - class iterator { + class Iterator { std::size_t num; SystemContainer* parent; public: @@ -112,12 +115,12 @@ namespace gz using difference_type = long; using pointer = T*; using reference = T&; - explicit iterator(SystemContainer* _parent, std::size_t _num = 0) : + explicit Iterator(SystemContainer* _parent, std::size_t _num = 0) : num(_num), parent(_parent) { } - iterator& operator++() { + Iterator& operator++() { auto end = parent->end(); // O(n) for now do { @@ -126,36 +129,37 @@ namespace gz return *this; } - bool operator==(iterator other) const { return num == other.num; } - bool operator!=(iterator other) const { return !(*this == other); } + bool operator==(Iterator other) const { return num == other.num; } + bool operator!=(Iterator other) const { return !(*this == other); } T& operator*() const { return parent->systems[num]; } }; ////////////////////////////////////////// - public: iterator begin() + public: Iterator begin() { - return iterator(this); + return Iterator(this); } ////////////////////////////////////////// - public: iterator end() + public: Iterator end() { auto lastIdx = this->occupied.size(); if (lastIdx == 0) { - return iterator(this); + return Iterator(this); } while(!this->occupied[lastIdx-1] && lastIdx != 0) { lastIdx--; } - return iterator(this, lastIdx); + return Iterator(this, lastIdx); } }; + } } } diff --git a/src/SystemContainer_TEST.cc b/src/SystemContainer_TEST.cc index c002913de3..857b29af26 100644 --- a/src/SystemContainer_TEST.cc +++ b/src/SystemContainer_TEST.cc @@ -16,7 +16,7 @@ */ #include #include -#include "SystemContainer.hpp" +#include "SystemContainer.hh" using namespace gz::sim; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index b9f7c518b1..86628965f5 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -22,7 +22,7 @@ #include -#include "SystemContainer.hpp" +#include "SystemContainer.hh" #include "SystemInternal.hh" #include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/Conversions.hh" diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 4b1ff39717..430a9f2fb3 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -34,7 +34,7 @@ #include "gz/sim/SystemLoader.hh" #include "gz/sim/Types.hh" -#include "SystemContainer.hpp" +#include "SystemContainer.hh" #include "SystemInternal.hh" namespace gz From 55815255e878481b6a45c73cc5c475529ca212dd Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 19 Jun 2024 09:34:04 +0800 Subject: [PATCH 30/51] Simplified logic using stl algorithms Signed-off-by: Arjo Chakravarty --- src/SystemManager.cc | 20 +------------------- 1 file changed, 1 insertion(+), 19 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 86628965f5..57a2c8b0fc 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -441,25 +441,7 @@ template void RemoveFromVectorIf(std::vector& vec, typename identity>::type pred) { - auto originalSize = vec.size(); - int j = 0; - - for(std::size_t i = 0; i < vec.size(); ++i) - { - if (pred(vec[i])) - { - j++; - } - else - { - vec[i-j] = vec[i]; - } - } - - while (vec.size() > originalSize - j) - { - vec.pop_back(); - } + vec.erase(std::remove_if(vec.begin(), vec.end(), pred), vec.end()); } ////////////////////////////////////////////////// From 3a39ff58fdcc2a8a68490e3e034d60bd1f5ed6aa Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 19 Jun 2024 10:13:43 +0800 Subject: [PATCH 31/51] Make cpplint happy Signed-off-by: Arjo Chakravarty --- src/SystemContainer.hh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/SystemContainer.hh b/src/SystemContainer.hh index 45ed26d231..8c66739415 100644 --- a/src/SystemContainer.hh +++ b/src/SystemContainer.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include #include "gz/sim/config.hh" @@ -112,7 +113,7 @@ namespace gz public: using iterator_category = std::input_iterator_tag; using value_type = T; - using difference_type = long; + using difference_type = int64_t; using pointer = T*; using reference = T&; explicit Iterator(SystemContainer* _parent, std::size_t _num = 0) : From 9dafb7d6e914903f77449b8594a4040dae71e8df Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 19 Jun 2024 14:37:42 +0800 Subject: [PATCH 32/51] Destroy system immedately Signed-off-by: Arjo Chakravarty --- src/SystemContainer.hh | 7 ++++--- src/SystemManager.cc | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/SystemContainer.hh b/src/SystemContainer.hh index 8c66739415..7dc44cd8bd 100644 --- a/src/SystemContainer.hh +++ b/src/SystemContainer.hh @@ -88,14 +88,15 @@ namespace gz ////////////////////////////////////////// /// Remove an item if a condition is met. - public: void RemoveIf(std::function fn) + public: void RemoveIf(std::function fn, T null_type=T(0)) { for (std::size_t i = 0; i < systems.size(); i++) { if (occupied[i] && fn(systems[i])) { - occupied[i] = false; - freeSpots.push_back(i); + this->occupied[i] = false; + this->freeSpots.push_back(i); + this->systems[i] = null_type; } } } diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 57a2c8b0fc..e36d608daf 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -488,10 +488,10 @@ void SystemManager::ProcessRemovedEntities( [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); }); - + SystemInternal null_sys(nullptr, kNullEntity); this->systems.RemoveIf([&](const SystemInternal& system) { return _ecm.IsMarkedForRemoval(system.parentEntity); - }); + }, null_sys); std::lock_guard lock(this->pendingSystemsMutex); RemoveFromVectorIf(this->pendingSystems, From 6eb2d2ff0f011f7b462bea42198117f6db4a01c7 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 19 Jun 2024 14:45:43 +0800 Subject: [PATCH 33/51] Style Signed-off-by: Arjo Chakravarty --- src/SystemContainer.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SystemContainer.hh b/src/SystemContainer.hh index 7dc44cd8bd..d105ab362d 100644 --- a/src/SystemContainer.hh +++ b/src/SystemContainer.hh @@ -88,7 +88,7 @@ namespace gz ////////////////////////////////////////// /// Remove an item if a condition is met. - public: void RemoveIf(std::function fn, T null_type=T(0)) + public: void RemoveIf(std::function fn, T null_type = T(0)) { for (std::size_t i = 0; i < systems.size(); i++) { From 91521b74702abc92d7106f8276ef3a48ff183dbd Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 19 Jun 2024 14:51:22 +0800 Subject: [PATCH 34/51] Style Signed-off-by: Arjo Chakravarty --- src/SystemContainer.hh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/SystemContainer.hh b/src/SystemContainer.hh index d105ab362d..e410a68e35 100644 --- a/src/SystemContainer.hh +++ b/src/SystemContainer.hh @@ -88,7 +88,8 @@ namespace gz ////////////////////////////////////////// /// Remove an item if a condition is met. - public: void RemoveIf(std::function fn, T null_type = T(0)) + public: void RemoveIf(std::function fn, + T null_type = T(0)) { for (std::size_t i = 0; i < systems.size(); i++) { From 36ed0adfe878241c9015ca53a40472132a8cf1e5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 4 Jul 2024 15:28:54 +0800 Subject: [PATCH 35/51] Fix crash and revert pose-publisher changes. Thanks @azeey I found the source of the crash. It comes from the fact that the PostUpdate system list changes with time. The reference taken by the thread to the PostUpdate interface will change over time. Thus we copy the interface pointer. Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 4 ++- src/systems/pose_publisher/PosePublisher.cc | 34 +++++++++++++-------- 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 0407e7ddac..662475fd45 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -561,6 +561,7 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateThreads.push_back(std::thread([&, id]() { auto parentEntity = system.parent; + auto this_thread_system = system.system; std::stringstream ss; ss << "PostUpdateThread: " << id; GZ_PROFILE_THREAD_NAME(ss.str().c_str()); @@ -576,9 +577,10 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateStopBarrier->Drop(); break; } - system.system->PostUpdate(this->currentInfo, this->entityCompMgr); + this_thread_system->PostUpdate(this->currentInfo, this->entityCompMgr); } this->postUpdateStopBarrier->Wait(); + } gzdbg << "Exiting postupdate worker thread (" diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 7fe638408d..d7e082155c 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -155,6 +155,16 @@ class gz::sim::systems::PosePublisherPrivate /// improves performance by avoiding memory allocation public: std::vector> staticPoses; + /// \brief A variable that gets populated with poses. This also here as a + /// member variable to avoid repeated memory allocations and improve + /// performance. + public: msgs::Pose poseMsg; + + /// \brief A variable that gets populated with poses. This also here as a + /// member variable to avoid repeated memory allocations and improve + /// performance. + public: msgs::Pose_V poseVMsg; + /// \brief True to publish a vector of poses. False to publish individual pose /// msgs. public: bool usePoseV = false; @@ -242,8 +252,7 @@ void PosePublisher::Configure(const Entity &_entity, this->dataPtr->usePoseV = _sdf->Get("use_pose_vector_msg", this->dataPtr->usePoseV).first; - std::string poseTopic = scopedName(_entity, _ecm) + "/pose"; - poseTopic = transport::TopicUtils::AsValidTopic(poseTopic); + std::string poseTopic = topicFromScopedName(_entity, _ecm, false) + "/pose"; if (poseTopic.empty()) { poseTopic = "/pose"; @@ -287,8 +296,8 @@ void PosePublisher::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Nothing left to do if paused. @@ -520,10 +529,11 @@ void PosePublisherPrivate::PublishPoses( transport::Node::Publisher &_publisher) { GZ_PROFILE("PosePublisher::PublishPoses"); - msgs::Pose poseMsg; - msgs::Pose_V poseVMsg; + // publish poses msgs::Pose *msg = nullptr; + if (this->usePoseV) + this->poseVMsg.Clear(); for (const auto &[entity, pose] : _poses) { @@ -533,12 +543,12 @@ void PosePublisherPrivate::PublishPoses( if (this->usePoseV) { - msg = poseVMsg.add_pose(); + msg = this->poseVMsg.add_pose(); } else { - poseMsg.Clear(); - msg = &poseMsg; + this->poseMsg.Clear(); + msg = &this->poseMsg; } // fill pose msg @@ -565,12 +575,12 @@ void PosePublisherPrivate::PublishPoses( // publish individual pose msgs if (!this->usePoseV) - _publisher.Publish(poseMsg); + _publisher.Publish(this->poseMsg); } // publish pose vector msg if (this->usePoseV) - _publisher.Publish(poseVMsg); + _publisher.Publish(this->poseVMsg); } GZ_ADD_PLUGIN(PosePublisher, @@ -579,4 +589,4 @@ GZ_ADD_PLUGIN(PosePublisher, PosePublisher::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(PosePublisher, - "gz::sim::systems::PosePublisher") + "gz::sim::systems::PosePublisher") //14,16 \ No newline at end of file From b7a7d8cc1092b81b69fca096ea4c8c6fa9716a4e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 4 Jul 2024 15:41:20 +0800 Subject: [PATCH 36/51] Revert debug changes Signed-off-by: Arjo Chakravarty --- src/systems/pose_publisher/PosePublisher.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index d7e082155c..d3c283eb7c 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -252,7 +252,8 @@ 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 = scopedName(_entity, _ecm) + "/pose"; + poseTopic = transport::TopicUtils::AsValidTopic(poseTopic); if (poseTopic.empty()) { poseTopic = "/pose"; @@ -296,8 +297,8 @@ void PosePublisher::PostUpdate(const UpdateInfo &_info, if (_info.dt < std::chrono::steady_clock::duration::zero()) { gzwarn << "Detected jump back in time [" - << std::chrono::duration(_info.dt).count() - << "s]. System may not work properly." << std::endl; + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; } // Nothing left to do if paused. @@ -589,4 +590,4 @@ GZ_ADD_PLUGIN(PosePublisher, PosePublisher::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(PosePublisher, - "gz::sim::systems::PosePublisher") //14,16 \ No newline at end of file + "gz::sim::systems::PosePublisher") \ No newline at end of file From 1e73b053cecd924c786b413b490584c5e87666a6 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 4 Jul 2024 15:46:25 +0800 Subject: [PATCH 37/51] Style Signed-off-by: Arjo Chakravarty --- src/systems/pose_publisher/PosePublisher.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index d3c283eb7c..58661d4816 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -590,4 +590,4 @@ GZ_ADD_PLUGIN(PosePublisher, PosePublisher::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(PosePublisher, - "gz::sim::systems::PosePublisher") \ No newline at end of file + "gz::sim::systems::PosePublisher") From e0b3a9ec38faea94fc0d425953f7d3d8692d4e7e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 8 Jul 2024 10:57:46 +0800 Subject: [PATCH 38/51] style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 662475fd45..4ed4712d1b 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -561,7 +561,7 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateThreads.push_back(std::thread([&, id]() { auto parentEntity = system.parent; - auto this_thread_system = system.system; + auto thisThreadSystem = system.system; std::stringstream ss; ss << "PostUpdateThread: " << id; GZ_PROFILE_THREAD_NAME(ss.str().c_str()); @@ -577,7 +577,8 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateStopBarrier->Drop(); break; } - this_thread_system->PostUpdate(this->currentInfo, this->entityCompMgr); + thisThreadSystem->PostUpdate(this->currentInfo, + this->entityCompMgr); } this->postUpdateStopBarrier->Wait(); @@ -967,7 +968,7 @@ void SimulationRunner::LoadServerPlugins( // // Check plugins from the ServerConfig for matching entities. - for (const ServerConfig::PluginInfo &plugin : _plugins) + for (const ServerConfig::PluginInfo &plugin : _plugins)4:00pm { // \todo(anyone) Type + name is not enough to uniquely identify an entity // \todo(louise) The runner shouldn't care about specific components, this From 2d6f115b45290991f8870de85e1f559fc94cc0c2 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 8 Jul 2024 11:13:36 +0800 Subject: [PATCH 39/51] style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 4ed4712d1b..6f5a975da5 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -968,7 +968,7 @@ void SimulationRunner::LoadServerPlugins( // // Check plugins from the ServerConfig for matching entities. - for (const ServerConfig::PluginInfo &plugin : _plugins)4:00pm + for (const ServerConfig::PluginInfo &plugin : _plugins) { // \todo(anyone) Type + name is not enough to uniquely identify an entity // \todo(louise) The runner shouldn't care about specific components, this From 5e27af85d863b723a91503e3d78013e0199f7bdb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 9 Jul 2024 10:52:38 +0800 Subject: [PATCH 40/51] Avoid cross-thread communication Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 6f5a975da5..de8bd6eb67 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -534,12 +534,15 @@ void SimulationRunner::ProcessSystemQueue() { auto pending = this->systemMgr->PendingCount(); - if (0 == pending) + if (0 == pending && this->threadsToTerminate.size() == 0) return; - // If additional systems are to be added, stop the existing threads. + gzerr << "Pausing all systems to rebuild\n"; + // If additional systems are to be added or removed, stop the existing threads. this->StopWorkerThreads(); + this->threadsToTerminate.clear(); + this->systemMgr->ActivatePendingSystems(); unsigned int threadCount = @@ -560,7 +563,6 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateThreads.push_back(std::thread([&, id]() { - auto parentEntity = system.parent; auto thisThreadSystem = system.system; std::stringstream ss; ss << "PostUpdateThread: " << id; @@ -570,13 +572,6 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateStartBarrier->Wait(); if (this->postUpdateThreadsRunning) { - auto terminate = this->threadsToTerminate.find(parentEntity); - if (terminate != this->threadsToTerminate.end()) { - gzdbg << "Terminating thread " << id << ", " << parentEntity <<"\n"; - this->postUpdateStartBarrier->Drop(); - this->postUpdateStopBarrier->Drop(); - break; - } thisThreadSystem->PostUpdate(this->currentInfo, this->entityCompMgr); } From 18d122c256d67eb54422357e4347504636606571 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 9 Jul 2024 13:17:12 +0800 Subject: [PATCH 41/51] stashing temporarily Signed-off-by: Arjo Chakravarty --- src/SystemContainer.hh | 3 +++ src/SystemManager.cc | 18 +++++++++++------- src/SystemManager.hh | 2 +- test/integration/CMakeLists.txt | 3 ++- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/SystemContainer.hh b/src/SystemContainer.hh index e410a68e35..70a4dae3c9 100644 --- a/src/SystemContainer.hh +++ b/src/SystemContainer.hh @@ -25,6 +25,8 @@ #include "gz/sim/config.hh" +#include "gz/common/Console.hh" + namespace gz { namespace sim @@ -91,6 +93,7 @@ namespace gz public: void RemoveIf(std::function fn, T null_type = T(0)) { +gzerr << "Removing"; for (std::size_t i = 0; i < systems.size(); i++) { if (occupied[i] && fn(systems[i])) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index e36d608daf..35aaa5a243 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -89,7 +89,7 @@ size_t SystemManager::TotalCount() const ////////////////////////////////////////////////// size_t SystemManager::ActiveCount() const { - return this->systems.Size(); + return this->systems.size(); } ////////////////////////////////////////////////// @@ -108,7 +108,7 @@ size_t SystemManager::ActivatePendingSystems() for (const auto& system : this->pendingSystems) { - this->systems.Push(system); + this->systems.push_back(system); if (system.configure) this->systemsConfigure.emplace_back( @@ -204,7 +204,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) } } - this->systems.Clear(); + this->systems.clear(); // Load plugins which do not implement reset after clearing this->systems // to ensure the previous instance is destroyed before the new one is created @@ -458,6 +458,8 @@ void SystemManager::ProcessRemovedEntities( return; } + _threadsToTerminate.emplace(10); + RemoveFromVectorIf(this->systemsReset, [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); @@ -475,7 +477,6 @@ void SystemManager::ProcessRemovedEntities( // If system with a PostUpdate is marked for removal, mark its thread for // termination. if (_ecm.IsMarkedForRemoval(system.parent)) { - _threadsToTerminate.emplace(system.parent); return true; } return false; @@ -488,11 +489,14 @@ void SystemManager::ProcessRemovedEntities( [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); }); - SystemInternal null_sys(nullptr, kNullEntity); - this->systems.RemoveIf([&](const SystemInternal& system) { + //SystemInternal null_sys(nullptr, kNullEntity); + /*this->systems.RemoveIf([&](const SystemInternal& system) { return _ecm.IsMarkedForRemoval(system.parentEntity); - }, null_sys); + }, null_sys);*/ + RemoveFromVectorIf(this->systems, [&](const SystemInternal& system) { + return _ecm.IsMarkedForRemoval(system.parentEntity); + }); std::lock_guard lock(this->pendingSystemsMutex); RemoveFromVectorIf(this->pendingSystems, [&](const SystemInternal& system) { diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 430a9f2fb3..468e53810f 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -208,7 +208,7 @@ namespace gz msgs::EntityPlugin_V &_res); /// \brief All the systems. - private: SystemContainer systems; + private: std::vector systems; /// \brief Pending systems to be added to systems. private: std::vector pendingSystems; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 639674e4a3..faeabda3e0 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -88,6 +88,7 @@ set(tests wind_effects.cc world.cc world_control_state.cc + reset_sensors.cc ) # elevator system causes compile erros on windows @@ -111,7 +112,7 @@ set(tests_needing_display markers.cc mesh_uri.cc optical_tactile_plugin.cc - reset_sensors.cc + rgbd_camera.cc sensors_system.cc sensors_system_battery.cc From ff833ce1a11eecc4de6695450d9d22f0792b8804 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 9 Jul 2024 15:45:17 +0800 Subject: [PATCH 42/51] Significantly reworked how things work Instead of using barriers simply stop all PostUpdate threads. This makes the PR much smaller and easier to reason about. Additionally it addresses unit test failures caught by @scpeters. Signed-off-by: Arjo Chakravarty --- src/Barrier.cc | 15 +--- src/Barrier_TEST.cc | 74 ---------------- src/CMakeLists.txt | 1 - src/SimulationRunner.cc | 12 +-- src/SimulationRunner.hh | 8 +- src/SystemContainer.hh | 172 ------------------------------------ src/SystemContainer_TEST.cc | 137 ---------------------------- src/SystemManager.cc | 34 +++++-- src/SystemManager.hh | 11 +-- src/SystemManager_TEST.cc | 7 +- 10 files changed, 42 insertions(+), 429 deletions(-) delete mode 100644 src/SystemContainer.hh delete mode 100644 src/SystemContainer_TEST.cc diff --git a/src/Barrier.cc b/src/Barrier.cc index 3b8e1b30f0..ca6b4347bd 100644 --- a/src/Barrier.cc +++ b/src/Barrier.cc @@ -70,6 +70,7 @@ Barrier::ExitStatus Barrier::Wait() this->dataPtr->cv.notify_all(); return Barrier::ExitStatus::DONE_LAST; } + while (gen == this->dataPtr->generation && !this->dataPtr->cancelled) { // All threads haven't reached, so wait until generation is reached @@ -96,17 +97,3 @@ void Barrier::Cancel() this->dataPtr->cancelled = true; this->dataPtr->cv.notify_all(); } - -////////////////////////////////////////////////// -void Barrier::Drop() -{ - std::unique_lock lock(this->dataPtr->mutex); - this->dataPtr->threadCount--; - if (--this->dataPtr->count == 0) - { - // All threads have reached the wait, so reset the barrier. - this->dataPtr->generation++; - this->dataPtr->count = this->dataPtr->threadCount; - this->dataPtr->cv.notify_all(); - } -} diff --git a/src/Barrier_TEST.cc b/src/Barrier_TEST.cc index b7750b0152..12609132a8 100644 --- a/src/Barrier_TEST.cc +++ b/src/Barrier_TEST.cc @@ -175,77 +175,3 @@ TEST(Barrier, Cancel) t.join(); } - - -////////////////////////////////////////////////// -TEST(Barrier, Drop) -{ - auto barrier = std::make_unique(3); - - unsigned int preBarrier { 0 }; - unsigned int postBarrier { 0 }; - - - std::mutex mutex; - std::condition_variable cv; - - auto t1 = std::thread([&]() - { - { - std::lock_guard lock(mutex); - preBarrier++; - cv.notify_one(); - } - - barrier->Wait(); - - { - std::lock_guard lock(mutex); - postBarrier++; - cv.notify_one(); - } - barrier->Wait(); - - }); - - auto t2 = std::thread([&]() - { - { - std::lock_guard lock(mutex); - preBarrier++; - cv.notify_one(); - } - - barrier->Drop(); - - }); - - - barrier->Wait(); - { - std::unique_lock lock(mutex); - auto ret = cv.wait_for(lock, std::chrono::milliseconds(1000), - [&]() - { - return preBarrier == 2; - }); - ASSERT_TRUE(ret); - } - EXPECT_EQ(preBarrier, 2); - // t2 should terminate. - t2.join(); - - - barrier->Wait(); - { - std::unique_lock lock(mutex); - auto ret = cv.wait_for(lock, std::chrono::milliseconds(1000), - [&]() - { - return postBarrier == 1; - }); - ASSERT_TRUE(ret); - } - EXPECT_EQ(postBarrier, 1); - t1.join(); -} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ae1abe698e..d4a5589b19 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -122,7 +122,6 @@ set (gtest_sources ServerConfig_TEST.cc Server_TEST.cc SimulationRunner_TEST.cc - SystemContainer_TEST.cc SystemLoader_TEST.cc SystemManager_TEST.cc TestFixture_TEST.cc diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index de8bd6eb67..a882f9556b 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -534,14 +534,13 @@ void SimulationRunner::ProcessSystemQueue() { auto pending = this->systemMgr->PendingCount(); - if (0 == pending && this->threadsToTerminate.size() == 0) + if (0 == pending && !this->needsCleanUp) return; - gzerr << "Pausing all systems to rebuild\n"; // If additional systems are to be added or removed, stop the existing threads. this->StopWorkerThreads(); - this->threadsToTerminate.clear(); + this->needsCleanUp = false; this->systemMgr->ActivatePendingSystems(); @@ -563,7 +562,6 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateThreads.push_back(std::thread([&, id]() { - auto thisThreadSystem = system.system; std::stringstream ss; ss << "PostUpdateThread: " << id; GZ_PROFILE_THREAD_NAME(ss.str().c_str()); @@ -572,7 +570,7 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateStartBarrier->Wait(); if (this->postUpdateThreadsRunning) { - thisThreadSystem->PostUpdate(this->currentInfo, + system->PostUpdate(this->currentInfo, this->entityCompMgr); } this->postUpdateStopBarrier->Wait(); @@ -897,8 +895,6 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Update all the systems. this->UpdateSystems(); - // Remove any threads that have been terminated - this->threadsToTerminate.clear(); if (!this->Paused() && this->requestedRunToSimTime && this->requestedRunToSimTime.value() > this->simTimeEpoch && @@ -934,7 +930,7 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Process entity removals. this->systemMgr->ProcessRemovedEntities(this->entityCompMgr, - this->threadsToTerminate); + this->needsCleanUp); this->entityCompMgr.ProcessRemoveEntityRequests(); // Process components removals diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 4e5332ed74..3b4be7cc7b 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -543,12 +543,8 @@ namespace gz /// at the appropriate time. private: std::unique_ptr newWorldControlState; - /// \brief Lists which threads are supposed to be removed on the next - /// iteration. - /// Note: We read from this during the `PostUpdate`, the set is cleared - /// after `PostUpdate`, and then we set it when processing - /// removals at the end of `Step`. - private: std::unordered_set threadsToTerminate; + /// \brief Set if we need to remove systems due to entity removal + private: bool needsCleanUp; private: bool resetInitiated{false}; friend class LevelManager; diff --git a/src/SystemContainer.hh b/src/SystemContainer.hh deleted file mode 100644 index 70a4dae3c9..0000000000 --- a/src/SystemContainer.hh +++ /dev/null @@ -1,172 +0,0 @@ -/* - * Copyright (C) 2024 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#ifndef GZ_SIM_SYSTEM_CONTAINER_HH_ -#define GZ_SIM_SYSTEM_CONTAINER_HH_ - -#include -#include -#include -#include -#include - -#include "gz/sim/config.hh" - -#include "gz/common/Console.hh" - -namespace gz -{ - namespace sim - { - inline namespace GZ_SIM_VERSION_NAMESPACE { - ////////////////////////////////////////////////// - /// This container implements a simple masked vector. - /// Using a masked vector for systems ensures that - /// when a system is deleted, its memory location stays - /// the same. This is important because there are a number - /// of places where we refer to systems by raw pointer - /// values. - /// - /// Operation times: - /// push - O(1) - /// removeIf - O(n) - /// iterate - O(n) where n is the max number of systems - /// -> TODO(arjoc): It should be possible to use a cache - /// to make each iterator increment have O(1) time. - /// For now worst case iterator increment is O(n). - /// That being said for most cases the increment - /// should be O(1). Eitherways, run does not iterate - /// through systems but rather ISystem* interfaces. - /// The only two places we iterate over this is during - /// reset and TotalByEntity in the tests. - template - class SystemContainer - { - ////////////////////////////////////////// - /// Push an item onto the container - public: void Push(T internal) - { - if (freeSpots.size() == 0) - { - this->systems.push_back(internal); - this->occupied.push_back(true); - return; - } - - auto freeIdx = freeSpots.back(); - freeSpots.pop_back(); - this->systems[freeIdx] = internal; - this->occupied[freeIdx] = true; - } - - ////////////////////////////////////////// - /// Clear the container - public: void Clear() - { - this->systems.clear(); - this->freeSpots.clear(); - this->occupied.clear(); - } - - ////////////////////////////////////////// - /// Number of active systems - public: std::size_t Size() const - { - return this->systems.size() - this->freeSpots.size(); - } - - ////////////////////////////////////////// - /// Remove an item if a condition is met. - public: void RemoveIf(std::function fn, - T null_type = T(0)) - { -gzerr << "Removing"; - for (std::size_t i = 0; i < systems.size(); i++) - { - if (occupied[i] && fn(systems[i])) - { - this->occupied[i] = false; - this->freeSpots.push_back(i); - this->systems[i] = null_type; - } - } - } - - private: std::vector systems; - - private: std::vector occupied; - - private: std::vector freeSpots; - - ////////////////////////////////////////// - class Iterator { - std::size_t num; - SystemContainer* parent; - public: - using iterator_category = std::input_iterator_tag; - using value_type = T; - using difference_type = int64_t; - using pointer = T*; - using reference = T&; - explicit Iterator(SystemContainer* _parent, std::size_t _num = 0) : - num(_num), parent(_parent) - { - - } - Iterator& operator++() { - auto end = parent->end(); - // O(n) for now - do { - ++num; - } while(num < end.num && !parent->occupied[num]); - - return *this; - } - bool operator==(Iterator other) const { return num == other.num; } - bool operator!=(Iterator other) const { return !(*this == other); } - T& operator*() const { - return parent->systems[num]; - } - }; - - ////////////////////////////////////////// - public: Iterator begin() - { - return Iterator(this); - } - - ////////////////////////////////////////// - public: Iterator end() - { - auto lastIdx = this->occupied.size(); - - if (lastIdx == 0) - { - return Iterator(this); - } - - while(!this->occupied[lastIdx-1] && lastIdx != 0) - { - lastIdx--; - } - return Iterator(this, lastIdx); - } - }; - } - } -} - -#endif diff --git a/src/SystemContainer_TEST.cc b/src/SystemContainer_TEST.cc deleted file mode 100644 index 857b29af26..0000000000 --- a/src/SystemContainer_TEST.cc +++ /dev/null @@ -1,137 +0,0 @@ -/* - * Copyright (C) 2024 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#include -#include -#include "SystemContainer.hh" - -using namespace gz::sim; - -////////////////////////////////////////////////// -TEST(SystemContainer, InsertionAndIteration) -{ - SystemContainer cont; - cont.Push(1); - cont.Push(2); - cont.Push(3); - - int idx = 0; - for (auto &i : cont) - { - idx++; - ASSERT_EQ(i, idx); - } - ASSERT_EQ(idx, 3); - cont.RemoveIf([](int i) { - return i%2 == 0; - }); - - auto newCount = 0; - std::unordered_set vals; - for (auto &i : cont) - { - newCount++; - ASSERT_NE(i, 2); - vals.insert(i); - } - ASSERT_TRUE(vals.count(1) && vals.count(3)); - ASSERT_EQ(vals.size(), 2); - - std::unordered_set finalVals; - cont.Push(2); - for (auto &i : cont) - { - finalVals.insert(i); - } - ASSERT_TRUE(finalVals.count(1)); - ASSERT_TRUE(finalVals.count(2)); - ASSERT_TRUE(finalVals.count(3)); - ASSERT_EQ(finalVals.size(), 3); -} - -////////////////////////////////////////////////// -TEST(SystemContainer, RemoveAtEnd) -{ - SystemContainer cont; - cont.Push(1); - cont.Push(2); - cont.Push(3); - - cont.RemoveIf([](int i) { - return i == 5; - }); - ASSERT_EQ(cont.Size(), 3); - - cont.RemoveIf([](int i) { - return i == 3; - }); - ASSERT_EQ(cont.Size(), 2); - - int idx = 0; - for (auto &i : cont) - { - idx++; - ASSERT_EQ(i, idx); - } - ASSERT_EQ(idx, 2); -} - -////////////////////////////////////////////////// -TEST(SystemContainer, RemoveFromMiddle) -{ - SystemContainer cont; - for (int i = 0; i < 10; i++) - { - cont.Push(i); - } - - ASSERT_EQ(cont.Size(), 10); - - cont.RemoveIf([](int i) { - return i == 5; - }); - ASSERT_EQ(cont.Size(), 9); - - - for (auto &i : cont) - { - ASSERT_NE(i, 5); - } - - cont.RemoveIf([](int i) { - return i % 2 == 1; - }); - - for (auto &i : cont) - { - ASSERT_NE(i % 2, 1); - } - ASSERT_EQ(cont.Size(), 5); -} - -////////////////////////////////////////////////// -TEST(SystemContainer, CheckEmptyProperties) -{ - SystemContainer cont; - ASSERT_EQ(cont.Size(), 0); - int idx = 0; - for (auto &i : cont) - { - idx++; - ASSERT_EQ(i, idx); - } - ASSERT_EQ(idx, 0); -} diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 35aaa5a243..96075e3207 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -22,7 +22,6 @@ #include -#include "SystemContainer.hh" #include "SystemInternal.hh" #include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/Conversions.hh" @@ -133,8 +132,10 @@ size_t SystemManager::ActivatePendingSystems() system.update); if (system.postupdate) - this->systemsPostupdate.emplace_back(system.parentEntity, - system.postupdate); + { + this->systemsPostupdate.push_back(system.postupdate); + this->postUpdateParent.push_back(system.parentEntity); + } } this->pendingSystems.clear(); @@ -168,6 +169,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) this->systemsPreupdate.clear(); this->systemsUpdate.clear(); this->systemsPostupdate.clear(); + this->postUpdateParent.clear(); std::vector pluginsToBeLoaded; @@ -323,7 +325,7 @@ const std::vector>& } ////////////////////////////////////////////////// -const std::vector>& +const std::vector& SystemManager::SystemsPostUpdate() { return this->systemsPostupdate; @@ -447,7 +449,7 @@ void RemoveFromVectorIf(std::vector& vec, ////////////////////////////////////////////////// void SystemManager::ProcessRemovedEntities( const EntityComponentManager &_ecm, - std::unordered_set &_threadsToTerminate) + bool &_needsCleanUp) { // Note: This function has O(n) time when an entity is removed // where n is number of systems. Ideally we would only iterate @@ -458,7 +460,7 @@ void SystemManager::ProcessRemovedEntities( return; } - _threadsToTerminate.emplace(10); + _needsCleanUp = true; RemoveFromVectorIf(this->systemsReset, [&](const SystemIfaceWithParent& system) { @@ -472,15 +474,31 @@ void SystemManager::ProcessRemovedEntities( [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); }); + + std::unordered_set markedForRemoval; + for (std::size_t i = 0; i < this->systemsPostupdate.size(); i++) + { + if(_ecm.IsMarkedForRemoval(postUpdateParent[i])) + { + markedForRemoval.insert(this->systemsPostupdate[i]); + } + } + RemoveFromVectorIf(this->systemsPostupdate, - [&](const SystemIfaceWithParent& system) { + [&](const auto system) { // If system with a PostUpdate is marked for removal, mark its thread for // termination. - if (_ecm.IsMarkedForRemoval(system.parent)) { + if (markedForRemoval.count(system)) { return true; } return false; }); + + RemoveFromVectorIf(this->postUpdateParent, + [&](const Entity& entity){ + return _ecm.IsMarkedForRemoval(entity); + } + ); RemoveFromVectorIf(this->systemsConfigure, [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 468e53810f..9ae2e6f1d1 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -34,7 +34,6 @@ #include "gz/sim/SystemLoader.hh" #include "gz/sim/Types.hh" -#include "SystemContainer.hh" #include "SystemInternal.hh" namespace gz @@ -159,7 +158,7 @@ namespace gz /// \brief Get an vector of all active systems implementing "PostUpdate" /// \return Vector of systems's post-update interfaces. - public: const std::vector>& + public: const std::vector& SystemsPostUpdate(); /// \brief Get an vector of all systems attached to a given entity. @@ -173,7 +172,7 @@ namespace gz /// \param[in] _entityCompMgr - ECM with entities marked for removal public: void ProcessRemovedEntities( const EntityComponentManager &_entityCompMgr, - std::unordered_set &_threadsToTerminate); + bool &_threadsToTerminate); /// \brief Implementation for AddSystem functions that takes an SDF /// element. This calls the AddSystemImpl that accepts an SDF Plugin. @@ -235,8 +234,10 @@ namespace gz private: std::vector> systemsUpdate; /// \brief Systems implementing PostUpdate - private: std::vector> - systemsPostupdate; + private: std::vector systemsPostupdate; + + /// \brief Parents of post update system. + private: std::vector postUpdateParent; /// \brief System loader, for loading system plugins. private: SystemLoaderPtr systemLoader; diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 4b4f366ce0..1265cc5a5f 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -266,11 +266,10 @@ TEST(SystemManager, AddAndRemoveSystemEcm) // Remove the entity ecm.RequestRemoveEntity(entity); - std::unordered_set entities_to_remove; - systemMgr.ProcessRemovedEntities(ecm, entities_to_remove); + bool needsCleanUp; + systemMgr.ProcessRemovedEntities(ecm, needsCleanUp); - EXPECT_EQ(entities_to_remove.size(), 1); - EXPECT_EQ(entities_to_remove.count(entity), 1); + EXPECT_TRUE(needsCleanUp); EXPECT_EQ(1u, systemMgr.ActiveCount()); EXPECT_EQ(0u, systemMgr.PendingCount()); EXPECT_EQ(1u, systemMgr.TotalCount()); From 33d5d4f77c235a6f3046c40f2c2b73862bb2f023 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 9 Jul 2024 15:51:51 +0800 Subject: [PATCH 43/51] Remove unrelated changes Signed-off-by: Arjo Chakravarty --- src/Barrier.hh | 5 ----- src/Barrier_TEST.cc | 1 - 2 files changed, 6 deletions(-) diff --git a/src/Barrier.hh b/src/Barrier.hh index 7a1bda5312..d013b04e75 100644 --- a/src/Barrier.hh +++ b/src/Barrier.hh @@ -81,11 +81,6 @@ namespace gz /// reflect that. public: ExitStatus Wait(); - /// \brief Similar to C++20's std::barrier::await_and_drop - /// decrements both the initial expected count for subsequent phases and - /// the expected count for current phase by one. - public: void Drop(); - /// \brief Cancel the barrier, causing all threads to unblock and /// return CANCELLED public: void Cancel(); diff --git a/src/Barrier_TEST.cc b/src/Barrier_TEST.cc index 12609132a8..1af0c73f8b 100644 --- a/src/Barrier_TEST.cc +++ b/src/Barrier_TEST.cc @@ -17,7 +17,6 @@ #include -#include #include #include "Barrier.hh" From e4174c47ab93b15d73ed54cff1006f187f9c7d7e Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 9 Jul 2024 16:02:15 +0800 Subject: [PATCH 44/51] Style Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 3 ++- src/SystemManager.cc | 5 ----- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index a882f9556b..67b36dd77e 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -537,7 +537,8 @@ void SimulationRunner::ProcessSystemQueue() if (0 == pending && !this->needsCleanUp) return; - // If additional systems are to be added or removed, stop the existing threads. + // If additional systems are to be added or removed, stop the existing + // threads. this->StopWorkerThreads(); this->needsCleanUp = false; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 96075e3207..2339910d04 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -507,11 +507,6 @@ void SystemManager::ProcessRemovedEntities( [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); }); - //SystemInternal null_sys(nullptr, kNullEntity); - /*this->systems.RemoveIf([&](const SystemInternal& system) { - return _ecm.IsMarkedForRemoval(system.parentEntity); - }, null_sys);*/ - RemoveFromVectorIf(this->systems, [&](const SystemInternal& system) { return _ecm.IsMarkedForRemoval(system.parentEntity); }); From 26aa10625375d2015f201c7ed428ac727867bf43 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 9 Jul 2024 16:05:35 +0800 Subject: [PATCH 45/51] Style Signed-off-by: Arjo Chakravarty --- test/integration/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index faeabda3e0..639674e4a3 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -88,7 +88,6 @@ set(tests wind_effects.cc world.cc world_control_state.cc - reset_sensors.cc ) # elevator system causes compile erros on windows @@ -112,7 +111,7 @@ set(tests_needing_display markers.cc mesh_uri.cc optical_tactile_plugin.cc - + reset_sensors.cc rgbd_camera.cc sensors_system.cc sensors_system_battery.cc From 401e501dfeeef109a762e13eb21c2d553c279695 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 10 Jul 2024 09:42:59 +0800 Subject: [PATCH 46/51] Change variable name Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 6 +++--- src/SimulationRunner.hh | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 67b36dd77e..a7859635f5 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -534,14 +534,14 @@ void SimulationRunner::ProcessSystemQueue() { auto pending = this->systemMgr->PendingCount(); - if (0 == pending && !this->needsCleanUp) + if (0 == pending && !this->threadsNeedCleanUp) return; // If additional systems are to be added or removed, stop the existing // threads. this->StopWorkerThreads(); - this->needsCleanUp = false; + this->threadsNeedCleanUp = false; this->systemMgr->ActivatePendingSystems(); @@ -931,7 +931,7 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Process entity removals. this->systemMgr->ProcessRemovedEntities(this->entityCompMgr, - this->needsCleanUp); + this->threadsNeedCleanUp); this->entityCompMgr.ProcessRemoveEntityRequests(); // Process components removals diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 3b4be7cc7b..7230ed9b86 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -544,7 +544,7 @@ namespace gz private: std::unique_ptr newWorldControlState; /// \brief Set if we need to remove systems due to entity removal - private: bool needsCleanUp; + private: bool threadsNeedCleanUp; private: bool resetInitiated{false}; friend class LevelManager; From 21a18cf4a19eacbce49b174e51bb006d9f270139 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 10 Jul 2024 09:51:52 +0800 Subject: [PATCH 47/51] Stylistic feedback Signed-off-by: Arjo Chakravarty --- src/SimulationRunner.cc | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index a7859635f5..099f3ce23e 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -537,8 +537,8 @@ void SimulationRunner::ProcessSystemQueue() if (0 == pending && !this->threadsNeedCleanUp) return; - // If additional systems are to be added or removed, stop the existing - // threads. + // If additional systems are to be added or have been removed, + // stop the existing threads. this->StopWorkerThreads(); this->threadsNeedCleanUp = false; @@ -575,7 +575,6 @@ void SimulationRunner::ProcessSystemQueue() this->entityCompMgr); } this->postUpdateStopBarrier->Wait(); - } gzdbg << "Exiting postupdate worker thread (" From 7b370c2b79292c334289f8cb4c22e65bad0204ec Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 10 Jul 2024 09:58:58 +0800 Subject: [PATCH 48/51] Move logical check Signed-off-by: Arjo Chakravarty --- src/SystemManager.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 2339910d04..879ea3aeff 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -460,8 +460,6 @@ void SystemManager::ProcessRemovedEntities( return; } - _needsCleanUp = true; - RemoveFromVectorIf(this->systemsReset, [&](const SystemIfaceWithParent& system) { return _ecm.IsMarkedForRemoval(system.parent); @@ -480,14 +478,15 @@ void SystemManager::ProcessRemovedEntities( { if(_ecm.IsMarkedForRemoval(postUpdateParent[i])) { + // If system with a PostUpdate is marked for removal + // mark all worker threads for removal. + _needsCleanUp = true; markedForRemoval.insert(this->systemsPostupdate[i]); } } RemoveFromVectorIf(this->systemsPostupdate, [&](const auto system) { - // If system with a PostUpdate is marked for removal, mark its thread for - // termination. if (markedForRemoval.count(system)) { return true; } From b792bfa100df68fdd66fdfbcdf42a1eac8cdb96a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 10 Jul 2024 10:07:05 +0800 Subject: [PATCH 49/51] Rename Variable Signed-off-by: Arjo Chakravarty --- src/SystemManager.cc | 8 ++++---- src/SystemManager.hh | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 879ea3aeff..15a8764419 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -134,7 +134,7 @@ size_t SystemManager::ActivatePendingSystems() if (system.postupdate) { this->systemsPostupdate.push_back(system.postupdate); - this->postUpdateParent.push_back(system.parentEntity); + this->postUpdateParents.push_back(system.parentEntity); } } @@ -169,7 +169,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) this->systemsPreupdate.clear(); this->systemsUpdate.clear(); this->systemsPostupdate.clear(); - this->postUpdateParent.clear(); + this->postUpdateParents.clear(); std::vector pluginsToBeLoaded; @@ -476,7 +476,7 @@ void SystemManager::ProcessRemovedEntities( std::unordered_set markedForRemoval; for (std::size_t i = 0; i < this->systemsPostupdate.size(); i++) { - if(_ecm.IsMarkedForRemoval(postUpdateParent[i])) + if(_ecm.IsMarkedForRemoval(postUpdateParents[i])) { // If system with a PostUpdate is marked for removal // mark all worker threads for removal. @@ -493,7 +493,7 @@ void SystemManager::ProcessRemovedEntities( return false; }); - RemoveFromVectorIf(this->postUpdateParent, + RemoveFromVectorIf(this->postUpdateParents, [&](const Entity& entity){ return _ecm.IsMarkedForRemoval(entity); } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 9ae2e6f1d1..c0b15541c0 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -236,8 +236,8 @@ namespace gz /// \brief Systems implementing PostUpdate private: std::vector systemsPostupdate; - /// \brief Parents of post update system. - private: std::vector postUpdateParent; + /// \brief Parents of post update systems. + private: std::vector postUpdateParents; /// \brief System loader, for loading system plugins. private: SystemLoaderPtr systemLoader; From 00a6e470480e145b1a18125f17643f0139b736c3 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 10 Jul 2024 10:09:09 +0800 Subject: [PATCH 50/51] Docstring Signed-off-by: Arjo Chakravarty --- src/SystemManager.hh | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/SystemManager.hh b/src/SystemManager.hh index c0b15541c0..bd77a0b8d2 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -170,9 +170,11 @@ namespace gz /// \brief Remove systems that are attached to removed entities /// \param[in] _entityCompMgr - ECM with entities marked for removal + /// \param[out] _needsCleanUp - Set to true if a system with a + /// PostUpdate was removed, and its thread needs to be terminated public: void ProcessRemovedEntities( const EntityComponentManager &_entityCompMgr, - bool &_threadsToTerminate); + bool &_needsCleanUp); /// \brief Implementation for AddSystem functions that takes an SDF /// element. This calls the AddSystemImpl that accepts an SDF Plugin. From 7026b5714f0f88764ad058920f766c07096f0fb7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 12 Jul 2024 16:51:13 -0500 Subject: [PATCH 51/51] Use parentEntity available in `systems` instead of storing it for each interface (#2473) --------- Signed-off-by: Addisu Z. Taddese Signed-off-by: Steve Peters Signed-off-by: Arjo Chakravarty Co-authored-by: Steve Peters Co-authored-by: Arjo Chakravarty --- src/SimulationRunner.cc | 9 +-- src/SystemManager.cc | 145 ++++++++++++++++++++++++---------------- src/SystemManager.hh | 52 ++++---------- 3 files changed, 101 insertions(+), 105 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 099f3ce23e..87f72d1ce2 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -571,12 +571,10 @@ void SimulationRunner::ProcessSystemQueue() this->postUpdateStartBarrier->Wait(); if (this->postUpdateThreadsRunning) { - system->PostUpdate(this->currentInfo, - this->entityCompMgr); + system->PostUpdate(this->currentInfo, this->entityCompMgr); } this->postUpdateStopBarrier->Wait(); } - gzdbg << "Exiting postupdate worker thread (" << id << ")" << std::endl; })); @@ -604,13 +602,13 @@ void SimulationRunner::UpdateSystems() { GZ_PROFILE("PreUpdate"); for (auto& system : this->systemMgr->SystemsPreUpdate()) - system.system->PreUpdate(this->currentInfo, this->entityCompMgr); + system->PreUpdate(this->currentInfo, this->entityCompMgr); } { GZ_PROFILE("Update"); for (auto& system : this->systemMgr->SystemsUpdate()) - system.system->Update(this->currentInfo, this->entityCompMgr); + system->Update(this->currentInfo, this->entityCompMgr); } { @@ -895,7 +893,6 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Update all the systems. this->UpdateSystems(); - if (!this->Paused() && this->requestedRunToSimTime && this->requestedRunToSimTime.value() > this->simTimeEpoch && this->currentInfo.simTime >= this->requestedRunToSimTime.value()) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 15a8764419..d1b66346e3 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -110,31 +110,23 @@ size_t SystemManager::ActivatePendingSystems() this->systems.push_back(system); if (system.configure) - this->systemsConfigure.emplace_back( - system.parentEntity, - system.configure); + this->systemsConfigure.push_back(system.configure); if (system.configureParameters) - this->systemsConfigureParameters.emplace_back( - system.parentEntity, - system.configureParameters); + this->systemsConfigureParameters.push_back(system.configureParameters); if (system.reset) - this->systemsReset.emplace_back(system.parentEntity, - system.reset); + this->systemsReset.push_back(system.reset); if (system.preupdate) - this->systemsPreupdate.emplace_back(system.parentEntity, - system.preupdate); + this->systemsPreupdate.push_back(system.preupdate); if (system.update) - this->systemsUpdate.emplace_back(system.parentEntity, - system.update); + this->systemsUpdate.push_back(system.update); if (system.postupdate) { this->systemsPostupdate.push_back(system.postupdate); - this->postUpdateParents.push_back(system.parentEntity); } } @@ -169,7 +161,6 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) this->systemsPreupdate.clear(); this->systemsUpdate.clear(); this->systemsPostupdate.clear(); - this->postUpdateParents.clear(); std::vector pluginsToBeLoaded; @@ -291,42 +282,37 @@ void SystemManager::AddSystemImpl( } ////////////////////////////////////////////////// -const std::vector>& - SystemManager::SystemsConfigure() +const std::vector& SystemManager::SystemsConfigure() { return this->systemsConfigure; } -const std::vector>& +const std::vector& SystemManager::SystemsConfigureParameters() { return this->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; } @@ -460,58 +446,99 @@ void SystemManager::ProcessRemovedEntities( return; } + std::unordered_set resetSystemsToBeRemoved; + std::unordered_set preupdateSystemsToBeRemoved; + std::unordered_set updateSystemsToBeRemoved; + std::unordered_set postupdateSystemsToBeRemoved; + std::unordered_set configureSystemsToBeRemoved; + std::unordered_set + configureParametersSystemsToBeRemoved; + for (const auto &system : this->systems) + { + if (_ecm.IsMarkedForRemoval(system.parentEntity)) + { + if (system.reset) + { + resetSystemsToBeRemoved.insert(system.reset); + } + if (system.preupdate) + { + preupdateSystemsToBeRemoved.insert(system.preupdate); + } + if (system.update) + { + updateSystemsToBeRemoved.insert(system.update); + } + if (system.postupdate) + { + postupdateSystemsToBeRemoved.insert(system.postupdate); + // If system with a PostUpdate is marked for removal + // mark all worker threads for removal. + _needsCleanUp = true; + } + if (system.configure) + { + configureSystemsToBeRemoved.insert(system.configure); + } + if (system.configureParameters) + { + configureParametersSystemsToBeRemoved.insert( + system.configureParameters); + } + } + } + RemoveFromVectorIf(this->systemsReset, - [&](const SystemIfaceWithParent& system) { - return _ecm.IsMarkedForRemoval(system.parent); + [&](const auto system) { + if (resetSystemsToBeRemoved.count(system)) { + return true; + } + return false; }); RemoveFromVectorIf(this->systemsPreupdate, - [&](const SystemIfaceWithParent& system) { - return _ecm.IsMarkedForRemoval(system.parent); + [&](const auto& system) { + if (preupdateSystemsToBeRemoved.count(system)) { + return true; + } + return false; }); RemoveFromVectorIf(this->systemsUpdate, - [&](const SystemIfaceWithParent& system) { - return _ecm.IsMarkedForRemoval(system.parent); + [&](const auto& system) { + if (updateSystemsToBeRemoved.count(system)) { + return true; + } + return false; }); - std::unordered_set markedForRemoval; - for (std::size_t i = 0; i < this->systemsPostupdate.size(); i++) - { - if(_ecm.IsMarkedForRemoval(postUpdateParents[i])) - { - // If system with a PostUpdate is marked for removal - // mark all worker threads for removal. - _needsCleanUp = true; - markedForRemoval.insert(this->systemsPostupdate[i]); - } - } - RemoveFromVectorIf(this->systemsPostupdate, - [&](const auto system) { - if (markedForRemoval.count(system)) { + [&](const auto& system) { + if (postupdateSystemsToBeRemoved.count(system)) { return true; } return false; }); - - RemoveFromVectorIf(this->postUpdateParents, - [&](const Entity& entity){ - return _ecm.IsMarkedForRemoval(entity); - } - ); RemoveFromVectorIf(this->systemsConfigure, - [&](const SystemIfaceWithParent& system) { - return _ecm.IsMarkedForRemoval(system.parent); + [&](const auto& system) { + if (configureSystemsToBeRemoved.count(system)) { + return true; + } + return false; }); RemoveFromVectorIf(this->systemsConfigureParameters, - [&](const SystemIfaceWithParent& system) { - return _ecm.IsMarkedForRemoval(system.parent); + [&](const auto& system) { + if (configureParametersSystemsToBeRemoved.count(system)) { + return true; + } + return false; }); - RemoveFromVectorIf(this->systems, [&](const SystemInternal& system) { - return _ecm.IsMarkedForRemoval(system.parentEntity); + RemoveFromVectorIf(this->systems, + [&](const SystemInternal& _arg) { + return _ecm.IsMarkedForRemoval(_arg.parentEntity); }); + std::lock_guard lock(this->pendingSystemsMutex); RemoveFromVectorIf(this->pendingSystems, - [&](const SystemInternal& system) { - return _ecm.IsMarkedForRemoval(system.parentEntity); + [&](const SystemInternal& _system) { + return _ecm.IsMarkedForRemoval(_system.parentEntity); }); } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index bd77a0b8d2..d523c4e740 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -21,8 +21,6 @@ #include #include -#include -#include #include #include @@ -43,21 +41,6 @@ namespace gz // Inline bracket to help doxygen filtering. inline namespace GZ_SIM_VERSION_NAMESPACE { - /// \brief Helper container to keep track of - /// system interfaces and their parents - template - struct SystemIfaceWithParent { - /// Parent entity of system - Entity parent; - - /// Interface pointer - IFace* system; - - /// \brief constructor - SystemIfaceWithParent(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 { @@ -131,35 +114,29 @@ namespace gz /// \brief Get a vector of all systems implementing "Configure" /// \return Vector of systems' configure interfaces. - public: const std::vector>& - SystemsConfigure(); + public: const std::vector& SystemsConfigure(); /// \brief Get an vector of all active systems implementing /// "ConfigureParameters" /// \return Vector of systems's configure interfaces. - public: const std::vector< - SystemIfaceWithParent>& - SystemsConfigureParameters(); + public: const std::vector& + SystemsConfigureParameters(); /// \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. @@ -218,29 +195,24 @@ namespace gz private: mutable std::mutex pendingSystemsMutex; /// \brief Systems implementing Configure - private: std::vector> - systemsConfigure; + private: std::vector systemsConfigure; /// \brief Systems implementing ConfigureParameters - private: std::vector> + private: std::vector 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; - /// \brief Parents of post update systems. - private: std::vector postUpdateParents; - /// \brief System loader, for loading system plugins. private: SystemLoaderPtr systemLoader;