Use parentEntity available in systems instead of storing it for each interface

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

Author: azeey

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())

src/SystemManager.cc

@@ -18,7 +18,6 @@
 #include <list>
 #include <mutex>
 #include <set>
-#include <unordered_set>
 
 #include <gz/common/StringUtils.hh>
 
@@ -110,31 +109,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 +160,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 +281,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;
 }
@@ -445,6 +430,13 @@ void RemoveFromVectorIf(std::vector<Tp>& vec,
 {
   vec.erase(std::remove_if(vec.begin(), vec.end(), pred), vec.end());
 }
+template <typename Tp>
+void removeFromVector(std::vector<Tp> &_vec, const Tp &_value)
+{
+  _vec.erase(std::remove_if(_vec.begin(), _vec.end(),
+                            [&](const auto &_arg) { return _arg == _value; }),
+             _vec.end());
+}
 
 //////////////////////////////////////////////////
 void SystemManager::ProcessRemovedEntities(
@@ -460,58 +452,40 @@ void SystemManager::ProcessRemovedEntities(
     return;
   }
 
-  RemoveFromVectorIf(this->systemsReset,
-    [&](const SystemIfaceWithParent<ISystemReset>& system) {
-      return _ecm.IsMarkedForRemoval(system.parent);
-    });
-  RemoveFromVectorIf(this->systemsPreupdate,
-    [&](const SystemIfaceWithParent<ISystemPreUpdate>& system) {
-      return _ecm.IsMarkedForRemoval(system.parent);
-    });
-  RemoveFromVectorIf(this->systemsUpdate,
-    [&](const SystemIfaceWithParent<ISystemUpdate>& system) {
-      return _ecm.IsMarkedForRemoval(system.parent);
-    });
+  std::vector<SystemInternal> markedForRemoval;
+  for (const auto &system: this->systems)
+  {
+    if (_ecm.IsMarkedForRemoval(system.parentEntity))
+    {
+      markedForRemoval.push_back(system);
+    }
+  }
 
-  std::unordered_set<ISystemPostUpdate *> markedForRemoval;
-  for (std::size_t i = 0; i < this->systemsPostupdate.size(); i++)
+  for (const auto &system: markedForRemoval)
   {
-    if(_ecm.IsMarkedForRemoval(postUpdateParents[i]))
+    removeFromVector(this->systemsReset, system.reset);
+    removeFromVector(this->systemsPreupdate, system.preupdate);
+    removeFromVector(this->systemsUpdate, system.update);
+
+    if (system.postupdate)
     {
       // 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)) {
-        return true;
-      }
-      return false;
-    });
+    removeFromVector(this->systemsPostupdate, system.postupdate);
+    removeFromVector(this->systemsConfigure, system.configure);
+    removeFromVector(this->systemsConfigureParameters, system.configureParameters);
+    RemoveFromVectorIf(this->systems,
+                       [&](const SystemInternal& _arg) {
+                       return _arg.parentEntity == system.parentEntity;
+                       });
+  }
 
-  RemoveFromVectorIf(this->postUpdateParents,
-    [&](const Entity& entity){
-      return _ecm.IsMarkedForRemoval(entity);
-    }
-  );
-  RemoveFromVectorIf(this->systemsConfigure,
-    [&](const SystemIfaceWithParent<ISystemConfigure>& system) {
-      return _ecm.IsMarkedForRemoval(system.parent);
-    });
-  RemoveFromVectorIf(this->systemsConfigureParameters,
-    [&](const SystemIfaceWithParent<ISystemConfigureParameters>& system) {
-      return _ecm.IsMarkedForRemoval(system.parent);
-    });
-  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);
+    [&](const SystemInternal& _system) {
+      return _ecm.IsMarkedForRemoval(_system.parentEntity);
     });
 }

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;