Adjustments for Fortress

Signed-off-by: Michael Carroll <michael@openrobotics.org>
gazebosim · Feb 17, 2022 · bb0f013 · bb0f013
1 parent 0230667
commit bb0f013
Show file tree

Hide file tree

Showing 5 changed files with 95 additions and 44 deletions.
diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc
@@ -113,7 +113,8 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
   }
 
   // Create the system manager
-  this->systemMgr = std::make_unique<SystemManager>(_systemLoader);
+  this->systemMgr = std::make_unique<SystemManager>(_systemLoader,
+      &this->entityCompMgr, &this->eventMgr);
 
   this->pauseConn = this->eventMgr.Connect<events::Pause>(
       std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1));
@@ -460,7 +461,6 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system,
   auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element();
 
   this->systemMgr->AddSystem(_system, entity, sdf);
-  this->systemMgr->ConfigurePendingSystems(this->entityCompMgr, this->eventMgr);
 }
 
 //////////////////////////////////////////////////
@@ -474,7 +474,6 @@ void SimulationRunner::AddSystem(
   auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element();
 
   this->systemMgr->AddSystem(_system, entity, sdf);
-  this->systemMgr->ConfigurePendingSystems(this->entityCompMgr, this->eventMgr);
 }
 
 /////////////////////////////////////////////////
@@ -865,8 +864,6 @@ void SimulationRunner::LoadPlugin(const Entity _entity,
                                   const sdf::ElementPtr &_sdf)
 {
   this->systemMgr->LoadPlugin(_entity, _fname, _name, _sdf);
-  this->systemMgr->ConfigurePendingSystems(
-      this->entityCompMgr, this->eventMgr);
 }
 
 //////////////////////////////////////////////////

diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh
@@ -389,9 +389,6 @@ namespace ignition
       /// server is in the run state.
       private: std::atomic<bool> running{false};
 
-      /// \brief Manager of all systems.
-      private: std::unique_ptr<SystemManager> systemMgr;
-
       /// \brief Manager of all events.
       /// Note: must be before EntityComponentManager
       private: EventManager eventMgr;

diff --git a/src/SystemManager.cc b/src/SystemManager.cc
@@ -21,8 +21,12 @@ using namespace ignition;
 using namespace gazebo;
 
 //////////////////////////////////////////////////
-SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader)
-  : systemLoader(_systemLoader)
+SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader,
+                             EntityComponentManager *_entityCompMgr,
+                             EventManager *_eventMgr)
+  : systemLoader(_systemLoader),
+    entityCompMgr(_entityCompMgr),
+    eventMgr(_eventMgr)
 {
 }
 
@@ -68,28 +72,6 @@ size_t SystemManager::PendingCount() const
   return this->pendingSystems.size();
 }
 
-//////////////////////////////////////////////////
-void SystemManager::ConfigurePendingSystems(EntityComponentManager &_ecm,
-                                            EventManager &_eventMgr)
-{
-  std::lock_guard<std::mutex> lock(this->systemsMutex);
-  for (size_t ii = 0; ii < this->pendingSystems.size(); ++ii)
-  {
-    if (this->pendingSystemsConfigured[ii])
-      continue;
-
-    const auto& system = this->pendingSystems[ii];
-
-    if (system.configure)
-    {
-      system.configure->Configure(system.configureEntity,
-                                  system.configureSdf,
-                                  _ecm, _eventMgr);
-      this->pendingSystemsConfigured[ii] = true;
-    }
-  }
-}
-
 //////////////////////////////////////////////////
 size_t SystemManager::ActivatePendingSystems()
 {
@@ -115,7 +97,6 @@ size_t SystemManager::ActivatePendingSystems()
   }
 
   this->pendingSystems.clear();
-  this->pendingSystemsConfigured.clear();
   return count;
 }
 
@@ -142,13 +123,17 @@ void SystemManager::AddSystemImpl(
       Entity _entity,
       std::shared_ptr<const sdf::Element> _sdf)
 {
-  _system.configureEntity = _entity;
-  _system.configureSdf = _sdf;
+  // Configure the system, if necessary
+  if (_system.configure && this->entityCompMgr && this->eventMgr)
+  {
+    _system.configure->Configure(_entity, _sdf,
+                                 *this->entityCompMgr,
+                                 *this->eventMgr);
+  }
 
   // Update callbacks will be handled later, add to queue
   std::lock_guard<std::mutex> lock(this->systemsMutex);
   this->pendingSystems.push_back(_system);
-  this->pendingSystemsConfigured.push_back(false);
 }
 
 //////////////////////////////////////////////////

diff --git a/src/SystemManager.hh b/src/SystemManager.hh
@@ -42,7 +42,13 @@ namespace ignition
       /// \brief Constructor
       /// \param[in] _systemLoader A pointer to a SystemLoader to load plugins
       ///  from files
-      public: explicit SystemManager(const SystemLoaderPtr &_systemLoader);
+      /// \param[in] _entityCompMgr Pointer to the entity component manager to
+      ///  be used when configuring new systems
+      /// \param[in] _eventMgr Pointer to the event manager to be used when
+      ///  configuring new systems
+      public: explicit SystemManager(const SystemLoaderPtr &_systemLoader,
+                            EntityComponentManager *_entityCompMgr = nullptr,
+                            EventManager *_eventMgr = nullptr);
 
       /// \brief Load system plugin for a given entity.
       /// \param[in] _entity Entity
@@ -82,12 +88,6 @@ namespace ignition
       /// \return The count.
       public: size_t TotalCount() const;
 
-      /// \brief Call the configure call on all pending systems
-      /// \param[in] _ecm The entity component manager to configure with
-      /// \param[in] _evetnMgr The event manager to configure with
-      public: void ConfigurePendingSystems(EntityComponentManager &_ecm,
-                                           EventManager &_eventMgr);
-
       /// \brief Move all "pending" systems to "active" state
       /// \return The number of newly-active systems
       public: size_t ActivatePendingSystems();
@@ -143,6 +143,12 @@ namespace ignition
 
       /// \brief Mutex to protect systemLoader
       private: std::mutex systemLoaderMutex;
+
+      /// \brief Pointer to associated entity component manager
+      private: EntityComponentManager *entityCompMgr;
+
+      /// \brief Pointer to associated event manager
+      private: EventManager *eventMgr;
     };
     }
   }  // namespace gazebo

diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc
@@ -79,7 +79,7 @@ TEST(SystemManager, Constructor)
 }
 
 /////////////////////////////////////////////////
-TEST(SystemManager, AddSystem)
+TEST(SystemManager, AddSystem_NoEcm)
 {
   auto loader = std::make_shared<SystemLoader>();
   SystemManager systemMgr(loader);
@@ -94,6 +94,71 @@ TEST(SystemManager, AddSystem)
 
   auto configSystem = std::make_shared<System_WithConfigure>();
   systemMgr.AddSystem(configSystem, kNullEntity, nullptr);
+
+  // SystemManager without an ECM/EventmManager will mean no config occurs
+  EXPECT_EQ(0, configSystem->configured);
+
+  EXPECT_EQ(0u, systemMgr.ActiveCount());
+  EXPECT_EQ(1u, systemMgr.PendingCount());
+  EXPECT_EQ(1u, 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(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());
+
+  auto updateSystem = std::make_shared<System_WithUpdates>();
+  systemMgr.AddSystem(updateSystem, kNullEntity, nullptr);
+  EXPECT_EQ(1u, systemMgr.ActiveCount());
+  EXPECT_EQ(1u, systemMgr.PendingCount());
+  EXPECT_EQ(2u, 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());
+
+  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());
+}
+
+/////////////////////////////////////////////////
+TEST(SystemManager, AddSystem_Ecm)
+{
+  auto loader = std::make_shared<SystemLoader>();
+
+  auto ecm = EntityComponentManager();
+  auto eventManager = EventManager();
+
+  SystemManager systemMgr(loader, &ecm, &eventManager);
+
+  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<System_WithConfigure>();
+  systemMgr.AddSystem(configSystem, kNullEntity, nullptr);
+
+  // Configure called during AddSystem
+  EXPECT_EQ(1, configSystem->configured);
+
   EXPECT_EQ(0u, systemMgr.ActiveCount());
   EXPECT_EQ(1u, systemMgr.PendingCount());
   EXPECT_EQ(1u, systemMgr.TotalCount());
@@ -130,3 +195,4 @@ TEST(SystemManager, AddSystem)
   EXPECT_EQ(1u, systemMgr.SystemsUpdate().size());
   EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size());
 }
+