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 <[email protected]>
gazebosim · Nov 7, 2023 · a7b1f22 · a7b1f22
1 parent 5d1c505
commit a7b1f22
Show file tree

Hide file tree

Showing 4 changed files with 89 additions and 19 deletions.
diff --git 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
@@ -30,6 +30,7 @@
 #include <gz/msgs/world_stats.pb.h>
 
 #include <sdf/Root.hh>
+#include <vector>
 
 #include "gz/common/Profiler.hh"
 #include "gz/sim/components/Model.hh"
@@ -523,7 +524,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();
       }
@@ -554,13 +555,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);
   }
 
   {
@@ -872,6 +873,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
@@ -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<ISystemReset *> &SystemManager::SystemsReset()
+const std::vector<SystemHolder<ISystemReset>> &SystemManager::SystemsReset()
 {
   return this->systemsReset;
 }
 
 //////////////////////////////////////////////////
-const std::vector<ISystemPreUpdate *>& SystemManager::SystemsPreUpdate()
+const std::vector<SystemHolder<ISystemPreUpdate>>& SystemManager::SystemsPreUpdate()
 {
   return this->systemsPreupdate;
 }
 
 //////////////////////////////////////////////////
-const std::vector<ISystemUpdate *>& SystemManager::SystemsUpdate()
+const std::vector<SystemHolder<ISystemUpdate>>& SystemManager::SystemsUpdate()
 {
   return this->systemsUpdate;
 }
 
 //////////////////////////////////////////////////
-const std::vector<ISystemPostUpdate *>& SystemManager::SystemsPostUpdate()
+const std::vector<SystemHolder<ISystemPostUpdate>>& SystemManager::SystemsPostUpdate()
 {
   return this->systemsPostupdate;
 }
@@ -409,3 +409,56 @@ void SystemManager::ProcessPendingEntitySystems()
   }
   this->systemsToAdd.clear();
 }
+
+template <typename T>
+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<typename Tp>
+void RemoveFromVectorIf(std::vector<Tp>& vec,
+  typename identity<std::function<bool(const Tp&)>>::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<ISystemReset>& system) {
+      return _ecm.IsMarkedForRemoval(system.parent);
+    });
+  RemoveFromVectorIf(this->systemsPreupdate,
+    [&](const SystemHolder<ISystemPreUpdate>& system) {
+      return _ecm.IsMarkedForRemoval(system.parent);
+    });
+  RemoveFromVectorIf(this->systemsUpdate,
+    [&](const SystemHolder<ISystemUpdate>& system) {
+      return _ecm.IsMarkedForRemoval(system.parent);
+    });
+  RemoveFromVectorIf(this->systemsPostupdate,
+    [&](const SystemHolder<ISystemPostUpdate>& system) {
+      return _ecm.IsMarkedForRemoval(system.parent);
+    });
+}
diff --git a/src/SystemManager.hh b/src/SystemManager.hh
@@ -21,6 +21,7 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include <sdf/Plugin.hh>
@@ -41,6 +42,14 @@ namespace gz
     // Inline bracket to help doxygen filtering.
     inline namespace GZ_SIM_VERSION_NAMESPACE {
 
+    template<typename IFace>
+    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<ISystemReset *>& SystemsReset();
+      public: const std::vector<SystemHolder<ISystemReset>>& SystemsReset();
 
       /// \brief Get an vector of all active systems implementing "PreUpdate"
       /// \return Vector of systems's pre-update interfaces.
-      public: const std::vector<ISystemPreUpdate *>& SystemsPreUpdate();
+      public: const std::vector<SystemHolder<ISystemPreUpdate>>& SystemsPreUpdate();
 
       /// \brief Get an vector of all active systems implementing "Update"
       /// \return Vector of systems's update interfaces.
-      public: const std::vector<ISystemUpdate *>& SystemsUpdate();
+      public: const std::vector<SystemHolder<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<SystemHolder<ISystemPostUpdate>>& 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<ISystemReset *> systemsReset;
+      private: std::vector<SystemHolder<ISystemReset>> systemsReset;
 
       /// \brief Systems implementing PreUpdate
-      private: std::vector<ISystemPreUpdate *> systemsPreupdate;
+      private: std::vector<SystemHolder<ISystemPreUpdate>> systemsPreupdate;
 
       /// \brief Systems implementing Update
-      private: std::vector<ISystemUpdate *> systemsUpdate;
+      private: std::vector<SystemHolder<ISystemUpdate>> systemsUpdate;
 
       /// \brief Systems implementing PostUpdate
-      private: std::vector<ISystemPostUpdate *> systemsPostupdate;
+      private: std::vector<SystemHolder<ISystemPostUpdate>> systemsPostupdate;
 
       /// \brief System loader, for loading system plugins.
       private: SystemLoaderPtr systemLoader;