Use parentEntity available in systems instead of storing it for eac…

…h interface (#2473) --------- Signed-off-by: Addisu Z. Taddese <[email protected]> Signed-off-by: Steve Peters <[email protected]> Signed-off-by: Arjo Chakravarty <[email protected]> Co-authored-by: Steve Peters <[email protected]> Co-authored-by: Arjo Chakravarty <[email protected]>
gazebosim · Jul 12, 2024 · 7026b57 · 7026b57
1 parent 00a6e47
commit 7026b57
Show file tree

Hide file tree

Showing 3 changed files with 101 additions and 105 deletions.
diff --git 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
@@ -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<PluginInfo> pluginsToBeLoaded;
 
@@ -291,42 +282,37 @@ void SystemManager::AddSystemImpl(
 }
 
 //////////////////////////////////////////////////
-const std::vector<SystemIfaceWithParent<ISystemConfigure>>&
-  SystemManager::SystemsConfigure()
+const std::vector<ISystemConfigure *>& SystemManager::SystemsConfigure()
 {
   return this->systemsConfigure;
 }
 
-const std::vector<SystemIfaceWithParent<ISystemConfigureParameters>>&
+const std::vector<ISystemConfigureParameters *>&
 SystemManager::SystemsConfigureParameters()
 {
   return this->systemsConfigureParameters;
 }
 
 //////////////////////////////////////////////////
-const std::vector<SystemIfaceWithParent<ISystemReset>>&
-SystemManager::SystemsReset()
+const std::vector<ISystemReset *> &SystemManager::SystemsReset()
 {
   return this->systemsReset;
 }
 
 //////////////////////////////////////////////////
-const std::vector<SystemIfaceWithParent<ISystemPreUpdate>>&
-  SystemManager::SystemsPreUpdate()
+const std::vector<ISystemPreUpdate *>& SystemManager::SystemsPreUpdate()
 {
   return this->systemsPreupdate;
 }
 
 //////////////////////////////////////////////////
-const std::vector<SystemIfaceWithParent<ISystemUpdate>>&
-  SystemManager::SystemsUpdate()
+const std::vector<ISystemUpdate *>& SystemManager::SystemsUpdate()
 {
   return this->systemsUpdate;
 }
 
 //////////////////////////////////////////////////
-const std::vector<ISystemPostUpdate *>&
-  SystemManager::SystemsPostUpdate()
+const std::vector<ISystemPostUpdate *>& SystemManager::SystemsPostUpdate()
 {
   return this->systemsPostupdate;
 }
@@ -460,58 +446,99 @@ void SystemManager::ProcessRemovedEntities(
     return;
   }
 
+  std::unordered_set<ISystemReset *> resetSystemsToBeRemoved;
+  std::unordered_set<ISystemPreUpdate *> preupdateSystemsToBeRemoved;
+  std::unordered_set<ISystemUpdate *> updateSystemsToBeRemoved;
+  std::unordered_set<ISystemPostUpdate *> postupdateSystemsToBeRemoved;
+  std::unordered_set<ISystemConfigure *> configureSystemsToBeRemoved;
+  std::unordered_set<ISystemConfigureParameters *>
+    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<ISystemReset>& system) {
-      return _ecm.IsMarkedForRemoval(system.parent);
+    [&](const auto system) {
+      if (resetSystemsToBeRemoved.count(system)) {
+        return true;
+      }
+      return false;
     });
   RemoveFromVectorIf(this->systemsPreupdate,
-    [&](const SystemIfaceWithParent<ISystemPreUpdate>& system) {
-      return _ecm.IsMarkedForRemoval(system.parent);
+    [&](const auto& system) {
+      if (preupdateSystemsToBeRemoved.count(system)) {
+        return true;
+      }
+      return false;
     });
   RemoveFromVectorIf(this->systemsUpdate,
-    [&](const SystemIfaceWithParent<ISystemUpdate>& system) {
-      return _ecm.IsMarkedForRemoval(system.parent);
+    [&](const auto& system) {
+      if (updateSystemsToBeRemoved.count(system)) {
+        return true;
+      }
+      return false;
     });
 
-  std::unordered_set<ISystemPostUpdate *> 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<ISystemConfigure>& system) {
-      return _ecm.IsMarkedForRemoval(system.parent);
+    [&](const auto& system) {
+      if (configureSystemsToBeRemoved.count(system)) {
+        return true;
+      }
+      return false;
     });
   RemoveFromVectorIf(this->systemsConfigureParameters,
-    [&](const SystemIfaceWithParent<ISystemConfigureParameters>& 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
@@ -21,8 +21,6 @@
 
 #include <memory>
 #include <string>
-#include <unordered_map>
-#include <unordered_set>
 #include <vector>
 
 #include <sdf/Plugin.hh>
@@ -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<typename IFace>
-    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<SystemIfaceWithParent<ISystemConfigure>>&
-        SystemsConfigure();
+      public: const std::vector<ISystemConfigure *>& SystemsConfigure();
 
       /// \brief Get an vector of all active systems implementing
       ///   "ConfigureParameters"
       /// \return Vector of systems's configure interfaces.
-      public: const std::vector<
-        SystemIfaceWithParent<ISystemConfigureParameters>>&
-        SystemsConfigureParameters();
+      public: const std::vector<ISystemConfigureParameters *>&
+      SystemsConfigureParameters();
 
       /// \brief Get an vector of all active systems implementing "Reset"
       /// \return Vector of systems' reset interfaces.
-      public: const std::vector<SystemIfaceWithParent<ISystemReset>>&
-        SystemsReset();
+      public: const std::vector<ISystemReset *>& SystemsReset();
 
       /// \brief Get an vector of all active systems implementing "PreUpdate"
       /// \return Vector of systems's pre-update interfaces.
-      public: const std::vector<SystemIfaceWithParent<ISystemPreUpdate>>&
-        SystemsPreUpdate();
+      public: const std::vector<ISystemPreUpdate *>& SystemsPreUpdate();
 
       /// \brief Get an vector of all active systems implementing "Update"
       /// \return Vector of systems's update interfaces.
-      public: const std::vector<SystemIfaceWithParent<ISystemUpdate>>&
-        SystemsUpdate();
+      public: const std::vector<ISystemUpdate *>& SystemsUpdate();
 
       /// \brief Get an vector of all active systems implementing "PostUpdate"
       /// \return Vector of systems's post-update interfaces.
-      public: const std::vector<ISystemPostUpdate *>&
-        SystemsPostUpdate();
+      public: const std::vector<ISystemPostUpdate *>& 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<SystemIfaceWithParent<ISystemConfigure>>
-        systemsConfigure;
+      private: std::vector<ISystemConfigure *> systemsConfigure;
 
       /// \brief Systems implementing ConfigureParameters
-      private: std::vector<SystemIfaceWithParent<ISystemConfigureParameters>>
+      private: std::vector<ISystemConfigureParameters *>
         systemsConfigureParameters;
 
       /// \brief Systems implementing Reset
-      private: std::vector<SystemIfaceWithParent<ISystemReset>> systemsReset;
+      private: std::vector<ISystemReset *> systemsReset;
 
       /// \brief Systems implementing PreUpdate
-      private: std::vector<SystemIfaceWithParent<ISystemPreUpdate>>
-        systemsPreupdate;
+      private: std::vector<ISystemPreUpdate *> systemsPreupdate;
 
       /// \brief Systems implementing Update
-      private: std::vector<SystemIfaceWithParent<ISystemUpdate>> systemsUpdate;
+      private: std::vector<ISystemUpdate *> systemsUpdate;
 
       /// \brief Systems implementing PostUpdate
       private: std::vector<ISystemPostUpdate *> systemsPostupdate;
 
-      /// \brief Parents of post update systems.
-      private: std::vector<Entity> postUpdateParents;
-
       /// \brief System loader, for loading system plugins.
       private: SystemLoaderPtr systemLoader;