Add entity and sdf parameters to Server's AddSystem interface

Signed-off-by: Gabriel Arjones <[email protected]>
gazebosim · Feb 26, 2024 · 6d3211d · 6d3211d
1 parent 633ce72
commit 6d3211d
Show file tree

Hide file tree

Showing 3 changed files with 79 additions and 7 deletions.
diff --git a/include/gz/sim/Server.hh b/include/gz/sim/Server.hh
@@ -225,14 +225,42 @@ namespace gz
                   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] _entity Entity of system to be added.
+      /// \param[in] _sdf Pointer to the SDF of the entity.
+      /// \param[in] _worldIndex Index of the world to query.
+      /// \return Whether the system was added successfully, or std::nullopt
+      /// if _worldIndex is invalid.
+      public: std::optional<bool> AddSystem(
+                  const SystemPluginPtr &_system,
+                  std::optional<Entity> _entity,
+                  std::optional<std::shared_ptr<const sdf::Element>> _sdf,
+                  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 Add a System to the server. The server must not be running when
       /// calling this.
       /// \param[in] _system System to be added
+      /// \param[in] _entity Entity of system to be added.
+      /// \param[in] _sdf Pointer to the SDF of the entity.
       /// \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,
+                  std::optional<Entity> _entity,
+                  std::optional<std::shared_ptr<const sdf::Element>> _sdf,
                   const unsigned int _worldIndex = 0);
 
       /// \brief Get an Entity based on a name.

diff --git a/src/Server.cc b/src/Server.cc
@@ -352,6 +352,16 @@ std::optional<size_t> Server::SystemCount(const unsigned int _worldIndex) const
 //////////////////////////////////////////////////
 std::optional<bool> Server::AddSystem(const SystemPluginPtr &_system,
                                       const unsigned int _worldIndex)
+{
+  return this->AddSystem(_system, std::nullopt, std::nullopt, _worldIndex);
+}
+
+//////////////////////////////////////////////////
+std::optional<bool> Server::AddSystem(
+    const SystemPluginPtr &_system,
+    std::optional<Entity> _entity,
+    std::optional<std::shared_ptr<const sdf::Element>> _sdf,
+    const unsigned int _worldIndex)
 {
   // Check the current state, and return early if preconditions are not met.
   std::lock_guard<std::mutex> lock(this->dataPtr->runMutex);
@@ -364,7 +374,7 @@ std::optional<bool> Server::AddSystem(const SystemPluginPtr &_system,
 
   if (_worldIndex < this->dataPtr->simRunners.size())
   {
-    this->dataPtr->simRunners[_worldIndex]->AddSystem(_system);
+    this->dataPtr->simRunners[_worldIndex]->AddSystem(_system, _entity, _sdf);
     return true;
   }
 
@@ -374,6 +384,16 @@ std::optional<bool> Server::AddSystem(const SystemPluginPtr &_system,
 //////////////////////////////////////////////////
 std::optional<bool> Server::AddSystem(const std::shared_ptr<System> &_system,
                                       const unsigned int _worldIndex)
+{
+  return this->AddSystem(_system, std::nullopt, std::nullopt, _worldIndex);
+}
+
+//////////////////////////////////////////////////
+std::optional<bool> Server::AddSystem(
+    const std::shared_ptr<System> &_system,
+    std::optional<Entity> _entity,
+    std::optional<std::shared_ptr<const sdf::Element>> _sdf,
+    const unsigned int _worldIndex)
 {
   std::lock_guard<std::mutex> lock(this->dataPtr->runMutex);
   if (this->dataPtr->running)
@@ -384,7 +404,7 @@ std::optional<bool> Server::AddSystem(const std::shared_ptr<System> &_system,
 
   if (_worldIndex < this->dataPtr->simRunners.size())
   {
-    this->dataPtr->simRunners[_worldIndex]->AddSystem(_system);
+    this->dataPtr->simRunners[_worldIndex]->AddSystem(_system, _entity, _sdf);
     return true;
   }
 

diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc
@@ -625,17 +625,41 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RunOnceUnpaused))
   auto mockSystemPlugin = systemLoader.LoadPlugin(sdfPlugin);
   ASSERT_TRUE(mockSystemPlugin.has_value());
 
-  // Check that it was loaded
-  const size_t systemCount = *server.SystemCount();
-  EXPECT_TRUE(*server.AddSystem(mockSystemPlugin.value()));
-  EXPECT_EQ(systemCount + 1, *server.SystemCount());
-
   // Query the interface from the plugin
   auto system = mockSystemPlugin.value()->QueryInterface<sim::System>();
   EXPECT_NE(system, nullptr);
   auto mockSystem = dynamic_cast<sim::MockSystem*>(system);
   EXPECT_NE(mockSystem, nullptr);
 
+  Entity entity = server.EntityByName("default").value();
+  size_t configureCallCount = 0;
+  auto configureCallback = [&sdfPlugin, &entity, &configureCallCount](
+    const Entity &_entity,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    EntityComponentManager &,
+    EventManager &)
+  {
+    configureCallCount++;
+    EXPECT_EQ(entity, _entity);
+    EXPECT_EQ(sdfPlugin.ToElement()->ToString(""), _sdf->ToString(""));
+  };
+
+  mockSystem->configureCallback = configureCallback;
+  const size_t systemCount = *server.SystemCount();
+  EXPECT_TRUE(
+    *server.AddSystem(
+      mockSystemPlugin.value(), entity, sdfPlugin.ToElement()
+  ));
+
+  // Add pointer
+  auto mockSystemPtr = std::make_shared<MockSystem>();
+  mockSystemPtr->configureCallback = configureCallback;
+  EXPECT_TRUE(*server.AddSystem(mockSystemPtr, entity, sdfPlugin.ToElement()));
+
+  // Check that it was loaded
+  EXPECT_EQ(systemCount + 2, *server.SystemCount());
+  EXPECT_EQ(2u, configureCallCount);
+
   // No steps should have been executed
   EXPECT_EQ(0u, mockSystem->preUpdateCallCount);
   EXPECT_EQ(0u, mockSystem->updateCallCount);