Support adding systems that don't come from a plugin (#936)

Signed-off-by: Louise Poubel <[email protected]>

include/ignition/gazebo/Server.hh

@@ -19,6 +19,7 @@
 
 #include <cstdint>
 #include <memory>
+#include <optional>
 #include <string>
 #include <ignition/gazebo/config.hh>
 #include <ignition/gazebo/EntityComponentManager.hh>
@@ -218,6 +219,16 @@ namespace ignition
                   const SystemPluginPtr &_system,
                   const unsigned int _worldIndex = 0);
 
+      /// \brief Add a System to the server. The server must not be running when
+      /// calling this.
+      /// \param[in] _system System to be added
+      /// \param[in] _worldIndex Index of the world to add to.
+      /// \return Whether the system was added successfully, or std::nullopt
+      /// if _worldIndex is invalid.
+      public: std::optional<bool> AddSystem(
+                  const std::shared_ptr<System> &_system,
+                  const unsigned int _worldIndex = 0);
+
       /// \brief Get an Entity based on a name.
       /// \details If multiple entities with the same name exist, the first
       /// entity found will be returned.

include/ignition/gazebo/Util.hh

@@ -130,6 +130,12 @@ namespace ignition
     Entity IGNITION_GAZEBO_VISIBLE worldEntity(const Entity &_entity,
         const EntityComponentManager &_ecm);
 
+    /// \brief Get the first world entity that's found.
+    /// \param[in] _ecm Immutable reference to ECM.
+    /// \return World entity ID.
+    Entity IGNITION_GAZEBO_VISIBLE worldEntity(
+        const EntityComponentManager &_ecm);
+
     /// \brief Helper function to remove a parent scope from a given name.
     /// This removes the first name found before the delimiter.
     /// \param[in] _name Input name possibly generated by scopedName.

src/Server.cc

@@ -354,6 +354,26 @@ std::optional<bool> Server::AddSystem(const SystemPluginPtr &_system,
   return std::nullopt;
 }
 
+//////////////////////////////////////////////////
+std::optional<bool> Server::AddSystem(const std::shared_ptr<System> &_system,
+                                      const unsigned int _worldIndex)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->runMutex);
+  if (this->dataPtr->running)
+  {
+    ignerr << "Cannot add system while the server is runnnng.\n";
+    return false;
+  }
+
+  if (_worldIndex < this->dataPtr->simRunners.size())
+  {
+    this->dataPtr->simRunners[_worldIndex]->AddSystem(_system);
+    return true;
+  }
+
+  return std::nullopt;
+}
+
 //////////////////////////////////////////////////
 bool Server::HasEntity(const std::string &_name,
                        const unsigned int _worldIndex) const

src/Server_TEST.cc

@@ -721,6 +721,7 @@ TEST_P(ServerFixture, AddSystemWhileRunning)
 
   EXPECT_EQ(3u, *server.SystemCount());
 
+  // Add system from plugin
   gazebo::SystemLoader systemLoader;
   auto mockSystemPlugin = systemLoader.LoadPlugin("libMockSystem.so",
       "ignition::gazebo::MockSystem", nullptr);
@@ -731,6 +732,13 @@ TEST_P(ServerFixture, AddSystemWhileRunning)
   EXPECT_FALSE(result.value());
   EXPECT_EQ(3u, *server.SystemCount());
 
+  // Add system pointer
+  auto mockSystem = std::make_shared<MockSystem>();
+  result = server.AddSystem(mockSystem);
+  ASSERT_TRUE(result.has_value());
+  EXPECT_FALSE(result.value());
+  EXPECT_EQ(3u, *server.SystemCount());
+
   // Stop the server
   std::raise(SIGTERM);
 
@@ -750,28 +758,55 @@ TEST_P(ServerFixture, AddSystemAfterLoad)
   EXPECT_FALSE(server.Running());
   EXPECT_FALSE(*server.Running(0));
 
+  // Add system from plugin
   gazebo::SystemLoader systemLoader;
   auto mockSystemPlugin = systemLoader.LoadPlugin("libMockSystem.so",
       "ignition::gazebo::MockSystem", nullptr);
   ASSERT_TRUE(mockSystemPlugin.has_value());
 
+  auto system = mockSystemPlugin.value()->QueryInterface<gazebo::System>();
+  EXPECT_NE(system, nullptr);
+  auto mockSystem = dynamic_cast<gazebo::MockSystem*>(system);
+  ASSERT_NE(mockSystem, nullptr);
+
   EXPECT_EQ(3u, *server.SystemCount());
+  EXPECT_EQ(0u, mockSystem->configureCallCount);
+
   EXPECT_TRUE(*server.AddSystem(mockSystemPlugin.value()));
+
   EXPECT_EQ(4u, *server.SystemCount());
+  EXPECT_EQ(1u, mockSystem->configureCallCount);
 
-  auto system = mockSystemPlugin.value()->QueryInterface<gazebo::System>();
-  EXPECT_NE(system, nullptr);
-  auto mockSystem = dynamic_cast<gazebo::MockSystem*>(system);
-  EXPECT_NE(mockSystem, nullptr);
+  // Add system pointer
+  auto mockSystemLocal = std::make_shared<MockSystem>();
+  EXPECT_EQ(0u, mockSystemLocal->configureCallCount);
+
+  EXPECT_TRUE(server.AddSystem(mockSystemLocal));
+  EXPECT_EQ(5u, *server.SystemCount());
+  EXPECT_EQ(1u, mockSystemLocal->configureCallCount);
 
+  // Check that update callbacks are called
   server.SetUpdatePeriod(1us);
   EXPECT_EQ(0u, mockSystem->preUpdateCallCount);
   EXPECT_EQ(0u, mockSystem->updateCallCount);
   EXPECT_EQ(0u, mockSystem->postUpdateCallCount);
+  EXPECT_EQ(0u, mockSystemLocal->preUpdateCallCount);
+  EXPECT_EQ(0u, mockSystemLocal->updateCallCount);
+  EXPECT_EQ(0u, mockSystemLocal->postUpdateCallCount);
   server.Run(true, 1, false);
   EXPECT_EQ(1u, mockSystem->preUpdateCallCount);
   EXPECT_EQ(1u, mockSystem->updateCallCount);
   EXPECT_EQ(1u, mockSystem->postUpdateCallCount);
+  EXPECT_EQ(1u, mockSystemLocal->preUpdateCallCount);
+  EXPECT_EQ(1u, mockSystemLocal->updateCallCount);
+  EXPECT_EQ(1u, mockSystemLocal->postUpdateCallCount);
+
+  // Add to inexistent world
+  auto result = server.AddSystem(mockSystemPlugin.value(), 100);
+  EXPECT_FALSE(result.has_value());
+
+  result = server.AddSystem(mockSystemLocal, 100);
+  EXPECT_FALSE(result.has_value());
 }
 
 /////////////////////////////////////////////////

src/SimulationRunner.cc

@@ -416,28 +416,61 @@ void SimulationRunner::PublishStats()
     this->rootClockPub.Publish(clockMsg);
 }
 
-/////////////////////////////////////////////////
-void SimulationRunner::AddSystem(const SystemPluginPtr &_system)
+//////////////////////////////////////////////////
+void SimulationRunner::AddSystem(const SystemPluginPtr &_system,
+      std::optional<Entity> _entity,
+      std::optional<std::shared_ptr<const sdf::Element>> _sdf)
+{
+  this->AddSystemImpl(SystemInternal(_system), _entity, _sdf);
+}
+
+//////////////////////////////////////////////////
+void SimulationRunner::AddSystem(
+      const std::shared_ptr<System> &_system,
+      std::optional<Entity> _entity,
+      std::optional<std::shared_ptr<const sdf::Element>> _sdf)
+{
+  this->AddSystemImpl(SystemInternal(_system), _entity, _sdf);
+}
+
+//////////////////////////////////////////////////
+void SimulationRunner::AddSystemImpl(
+      SystemInternal _system,
+      std::optional<Entity> _entity,
+      std::optional<std::shared_ptr<const sdf::Element>> _sdf)
 {
+  // Call configure
+  if (_system.configure)
+  {
+    // Default to world entity and SDF
+    auto entity = _entity.has_value() ? _entity.value()
+        : worldEntity(this->entityCompMgr);
+    auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element();
+
+    _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->pendingSystemsMutex);
   this->pendingSystems.push_back(_system);
 }
 
 /////////////////////////////////////////////////
-void SimulationRunner::AddSystemToRunner(const SystemPluginPtr &_system)
+void SimulationRunner::AddSystemToRunner(SystemInternal _system)
 {
-  this->systems.push_back(SystemInternal(_system));
-
-  const auto &system = this->systems.back();
+  this->systems.push_back(_system);
 
-  if (system.preupdate)
-    this->systemsPreupdate.push_back(system.preupdate);
+  if (_system.preupdate)
+    this->systemsPreupdate.push_back(_system.preupdate);
 
-  if (system.update)
-    this->systemsUpdate.push_back(system.update);
+  if (_system.update)
+    this->systemsUpdate.push_back(_system.update);
 
-  if (system.postupdate)
-    this->systemsPostupdate.push_back(system.postupdate);
+  if (_system.postupdate)
+    this->systemsPostupdate.push_back(_system.postupdate);
 }
 
 /////////////////////////////////////////////////
@@ -456,7 +489,6 @@ void SimulationRunner::ProcessSystemQueue()
   {
     this->AddSystemToRunner(system);
   }
-
   this->pendingSystems.clear();
 
   // If additional systems were added, recreate the worker threads.
@@ -466,9 +498,9 @@ void SimulationRunner::ProcessSystemQueue()
       << this->systemsPostupdate.size() + 1 << std::endl;
 
     this->postUpdateStartBarrier =
-      std::make_unique<Barrier>(this->systemsPostupdate.size() + 1);
+      std::make_unique<Barrier>(this->systemsPostupdate.size() + 1u);
     this->postUpdateStopBarrier =
-      std::make_unique<Barrier>(this->systemsPostupdate.size() + 1);
+      std::make_unique<Barrier>(this->systemsPostupdate.size() + 1u);
 
     this->postUpdateThreadsRunning = true;
     int id = 0;
@@ -807,18 +839,10 @@ void SimulationRunner::LoadPlugin(const Entity _entity,
     system = this->systemLoader->LoadPlugin(_fname, _name, _sdf);
   }
 
-  // System correctly loaded from library, try to configure
+  // System correctly loaded from library
   if (system)
   {
-    auto systemConfig = system.value()->QueryInterface<ISystemConfigure>();
-    if (systemConfig != nullptr)
-    {
-      systemConfig->Configure(_entity, _sdf,
-          this->entityCompMgr,
-          this->eventMgr);
-    }
-
-    this->AddSystem(system.value());
+    this->AddSystem(system.value(), _entity, _sdf);
     igndbg << "Loaded system [" << _name
            << "] for entity [" << _entity << "]" << std::endl;
   }

src/SimulationRunner.hh

@@ -26,6 +26,7 @@
 #include <functional>
 #include <list>
 #include <memory>
+#include <optional>
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
@@ -88,26 +89,50 @@ namespace ignition
       std::chrono::steady_clock::duration seek{-1};
     };
 
-    /// \brief Class to hold systems internally
+    /// \brief Class to hold systems internally. It supports systems loaded
+    /// from plugins, as well as systems created at runtime.
     class SystemInternal
     {
       /// \brief Constructor
+      /// \param[in] _systemPlugin A system loaded from a plugin.
       public: explicit SystemInternal(SystemPluginPtr _systemPlugin)
               : systemPlugin(std::move(_systemPlugin)),
                 system(systemPlugin->QueryInterface<System>()),
+                configure(systemPlugin->QueryInterface<ISystemConfigure>()),
                 preupdate(systemPlugin->QueryInterface<ISystemPreUpdate>()),
                 update(systemPlugin->QueryInterface<ISystemUpdate>()),
                 postupdate(systemPlugin->QueryInterface<ISystemPostUpdate>())
       {
       }
 
+      /// \brief Constructor
+      /// \param[in] _system Pointer to a system.
+      public: explicit SystemInternal(const std::shared_ptr<System> &_system)
+              : systemShared(_system),
+                system(_system.get()),
+                configure(dynamic_cast<ISystemConfigure *>(_system.get())),
+                preupdate(dynamic_cast<ISystemPreUpdate *>(_system.get())),
+                update(dynamic_cast<ISystemUpdate *>(_system.get())),
+                postupdate(dynamic_cast<ISystemPostUpdate *>(_system.get()))
+      {
+      }
+
       /// \brief Plugin object. This manages the lifecycle of the instantiated
       /// class as well as the shared library.
+      /// This will be null if the system wasn't loaded from a plugin.
       public: SystemPluginPtr systemPlugin;
 
+      /// \brief Pointer to a system.
+      /// This will be null if the system wasn't loaded from a pointer.
+      public: std::shared_ptr<System> systemShared{nullptr};
+
       /// \brief Access this system via the `System` interface
       public: System *system = nullptr;
 
+      /// \brief Access this system via the ISystemConfigure interface
+      /// Will be nullptr if the System doesn't implement this interface.
+      public: ISystemConfigure *configure = nullptr;
+
       /// \brief Access this system via the ISystemPreUpdate interface
       /// Will be nullptr if the System doesn't implement this interface.
       public: ISystemPreUpdate *preupdate = nullptr;
@@ -160,9 +185,31 @@ namespace ignition
 
       /// \brief Add system after the simulation runner has been instantiated
       /// \note This actually adds system to a queue. The system is added to the
-      /// runner at the begining of the a simulation cycle (call to Run)
+      /// runner at the begining of the a simulation cycle (call to Run). It is
+      /// also responsible for calling `Configure` on the system.
+      /// \param[in] _system SystemPluginPtr to be added
+      /// \param[in] _entity Entity that system is attached to. If nullopt,
+      /// system is attached to a world.
+      /// \param[in] _sdf Pointer to the SDF of the entity. Nullopt defaults to
+      /// SDF of the entire world.
+      public: void AddSystem(const SystemPluginPtr &_system,
+          std::optional<Entity> _entity = std::nullopt,
+          std::optional<std::shared_ptr<const sdf::Element>> _sdf =
+              std::nullopt);
+
+      /// \brief Add system after the simulation runner has been instantiated
+      /// \note This actually adds system to a queue. The system is added to the
+      /// runner at the begining of the a simulation cycle (call to Run). It is
+      /// also responsible for calling `Configure` on the system.
       /// \param[in] _system System to be added
-      public: void AddSystem(const SystemPluginPtr &_system);
+      /// \param[in] _entity Entity of system to be added. Nullopt if system
+      /// doesn't connect to an entity.
+      /// \param[in] _sdf Pointer to the SDF of the entity. Nullopt defaults to
+      /// world.
+      public: void AddSystem(const std::shared_ptr<System> &_system,
+          std::optional<Entity> _entity = std::nullopt,
+          std::optional<std::shared_ptr<const sdf::Element>> _sdf =
+              std::nullopt);
 
       /// \brief Update all the systems
       public: void UpdateSystems();
@@ -339,7 +386,7 @@ namespace ignition
 
       /// \brief Actually add system to the runner
       /// \param[in] _system System to be added
-      public: void AddSystemToRunner(const SystemPluginPtr &_system);
+      public: void AddSystemToRunner(SystemInternal _system);
 
       /// \brief Calls AddSystemToRunner to each system that is pending to be
       /// added.
@@ -376,6 +423,16 @@ namespace ignition
       /// Physics component of the world, if any.
       public: void UpdatePhysicsParams();
 
+      /// \brief Implementation for AddSystem functions. This only adds systems
+      /// to a queue, the actual addition is performed by `AddSystemToRunner` at
+      /// the appropriate time.
+      /// \param[in] _system Generic representation of a system.
+      /// \param[in] _entity Entity received from AddSystem.
+      /// \param[in] _sdf SDF received from AddSystem.
+      private: void AddSystemImpl(SystemInternal _system,
+        std::optional<Entity> _entity = std::nullopt,
+        std::optional<std::shared_ptr<const sdf::Element>> _sdf = std::nullopt);
+
       /// \brief This is used to indicate that a stop event has been received.
       private: std::atomic<bool> stopReceived{false};
 
@@ -387,7 +444,7 @@ namespace ignition
       private: std::vector<SystemInternal> systems;
 
       /// \brief Pending systems to be added to systems.
-      private: std::vector<SystemPluginPtr> pendingSystems;
+      private: std::vector<SystemInternal> pendingSystems;
 
       /// \brief Mutex to protect pendingSystems
       private: mutable std::mutex pendingSystemsMutex;