Prevent hanging when world has only non-world plugins (#1383)

Signed-off-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

src/SimulationRunner.cc

@@ -172,9 +172,10 @@ SimulationRunner::SimulationRunner(const sdf::World *_world,
   // Load any additional plugins from the Server Configuration
   this->LoadServerPlugins(this->serverConfig.Plugins());
 
-  // If we have reached this point and no systems have been loaded, then load
-  // a default set of systems.
-  if (this->systemMgr->TotalCount() == 0)
+  // If we have reached this point and no world systems have been loaded, then
+  // load a default set of systems.
+  if (this->systemMgr->TotalByEntity(
+      worldEntity(this->entityCompMgr)).empty())
   {
     ignmsg << "No systems loaded from SDF, loading defaults" << std::endl;
     bool isPlayback = !this->serverConfig.LogPlaybackPath().empty();

src/SimulationRunner_TEST.cc

@@ -1532,6 +1532,30 @@ TEST_P(SimulationRunnerTest,
       componentId)) << componentId;
 }
 
+/////////////////////////////////////////////////
+TEST_P(SimulationRunnerTest,
+       IGN_UTILS_TEST_DISABLED_ON_WIN32(LoadOnlyModelPlugin) )
+{
+  sdf::Root rootWithout;
+  rootWithout.Load(common::joinPaths(PROJECT_SOURCE_PATH,
+      "test", "worlds", "model_plugin_only.sdf"));
+  ASSERT_EQ(1u, rootWithout.WorldCount());
+
+  // ServerConfig will fall back to environment variable
+  auto config = common::joinPaths(PROJECT_SOURCE_PATH,
+    "test", "worlds", "server_valid2.config");
+  ASSERT_EQ(true, common::setenv(gazebo::kServerConfigPathEnv, config));
+  ServerConfig serverConfig;
+
+  // Create simulation runner
+  auto systemLoader = std::make_shared<SystemLoader>();
+  SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader,
+      serverConfig);
+
+  // 1 model plugin from SDF and 2 world plugins from config
+  ASSERT_EQ(3u, runner.SystemCount());
+}
+
 /////////////////////////////////////////////////
 TEST_P(SimulationRunnerTest, GuiInfo)
 {

src/SystemInternal.hh

@@ -39,25 +39,31 @@ namespace ignition
     {
       /// \brief Constructor
       /// \param[in] _systemPlugin A system loaded from a plugin.
-      public: explicit SystemInternal(SystemPluginPtr _systemPlugin)
+      /// \param[in] _entity The entity that this system is attached to.
+      public: explicit SystemInternal(SystemPluginPtr _systemPlugin,
+          Entity _entity)
               : systemPlugin(std::move(_systemPlugin)),
                 system(systemPlugin->QueryInterface<System>()),
                 configure(systemPlugin->QueryInterface<ISystemConfigure>()),
                 preupdate(systemPlugin->QueryInterface<ISystemPreUpdate>()),
                 update(systemPlugin->QueryInterface<ISystemUpdate>()),
-                postupdate(systemPlugin->QueryInterface<ISystemPostUpdate>())
+                postupdate(systemPlugin->QueryInterface<ISystemPostUpdate>()),
+                parentEntity(_entity)
       {
       }
 
       /// \brief Constructor
       /// \param[in] _system Pointer to a system.
-      public: explicit SystemInternal(const std::shared_ptr<System> &_system)
+      /// \param[in] _entity The entity that this system is attached to.
+      public: explicit SystemInternal(const std::shared_ptr<System> &_system,
+          Entity _entity)
               : 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()))
+                postupdate(dynamic_cast<ISystemPostUpdate *>(_system.get())),
+                parentEntity(_entity)
       {
       }
 
@@ -89,9 +95,9 @@ namespace ignition
       /// Will be nullptr if the System doesn't implement this interface.
       public: ISystemPostUpdate *postupdate = nullptr;
 
-      /// \brief Cached entity that was used to call `Configure` on the system
-      /// Useful for if a system needs to be reconfigured at runtime
-      public: Entity configureEntity = {kNullEntity};
+      /// \brief Entity that the system is attached to. It's passed to the
+      /// system during the `Configure` call.
+      public: Entity parentEntity = {kNullEntity};
 
       /// \brief Cached sdf that was used to call `Configure` on the system
       /// Useful for if a system needs to be reconfigured at runtime

src/SystemManager.cc

@@ -103,7 +103,7 @@ void SystemManager::AddSystem(const SystemPluginPtr &_system,
       Entity _entity,
       std::shared_ptr<const sdf::Element> _sdf)
 {
-  this->AddSystemImpl(SystemInternal(_system), _entity, _sdf);
+  this->AddSystemImpl(SystemInternal(_system, _entity), _sdf);
 }
 
 //////////////////////////////////////////////////
@@ -112,19 +112,18 @@ void SystemManager::AddSystem(
       Entity _entity,
       std::shared_ptr<const sdf::Element> _sdf)
 {
-  this->AddSystemImpl(SystemInternal(_system), _entity, _sdf);
+  this->AddSystemImpl(SystemInternal(_system, _entity), _sdf);
 }
 
 //////////////////////////////////////////////////
 void SystemManager::AddSystemImpl(
       SystemInternal _system,
-      Entity _entity,
       std::shared_ptr<const sdf::Element> _sdf)
 {
   // Configure the system, if necessary
   if (_system.configure && this->entityCompMgr && this->eventMgr)
   {
-    _system.configure->Configure(_entity, _sdf,
+    _system.configure->Configure(_system.parentEntity, _sdf,
                                  *this->entityCompMgr,
                                  *this->eventMgr);
   }
@@ -157,3 +156,20 @@ const std::vector<ISystemPostUpdate *>& SystemManager::SystemsPostUpdate()
 {
   return this->systemsPostupdate;
 }
+
+//////////////////////////////////////////////////
+std::vector<SystemInternal> SystemManager::TotalByEntity(Entity _entity)
+{
+  std::vector<SystemInternal> result;
+  for (auto system : this->systems)
+  {
+    if (system.parentEntity == _entity)
+      result.push_back(system);
+  }
+  for (auto system : this->pendingSystems)
+  {
+    if (system.parentEntity == _entity)
+      result.push_back(system);
+  }
+  return result;
+}

src/SystemManager.hh

@@ -92,26 +92,32 @@ namespace ignition
       /// \return The number of newly-active systems
       public: size_t ActivatePendingSystems();
 
-      /// \brief Get an vector of all systems implementing "Configure"
+      /// \brief Get an vector of all active systems implementing "Configure"
+      /// \return Vector of systems's configure interfaces.
       public: const std::vector<ISystemConfigure *>& SystemsConfigure();
 
-      /// \brief Get an vector of all systems implementing "PreUpdate"
+      /// \brief Get an vector of all active systems implementing "PreUpdate"
+      /// \return Vector of systems's pre-update interfaces.
       public: const std::vector<ISystemPreUpdate *>& SystemsPreUpdate();
 
-      /// \brief Get an vector of all systems implementing "Update"
+      /// \brief Get an vector of all active systems implementing "Update"
+      /// \return Vector of systems's update interfaces.
       public: const std::vector<ISystemUpdate *>& SystemsUpdate();
 
-      /// \brief Get an vector of all systems implementing "PostUpdate"
+      /// \brief Get an vector of all active systems implementing "PostUpdate"
+      /// \return Vector of systems's post-update interfaces.
       public: const std::vector<ISystemPostUpdate *>& SystemsPostUpdate();
 
+      /// \brief Get an vector of all systems attached to a given entity.
+      /// \return Vector of systems.
+      public: std::vector<SystemInternal> TotalByEntity(Entity _entity);
+
       /// \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,
-                                  Entity _entity,
                                   std::shared_ptr<const sdf::Element> _sdf);
 
       /// \brief All the systems.

src/SystemManager_TEST.cc

@@ -93,7 +93,8 @@ TEST(SystemManager, AddSystemNoEcm)
   EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
 
   auto configSystem = std::make_shared<SystemWithConfigure>();
-  systemMgr.AddSystem(configSystem, kNullEntity, nullptr);
+  Entity configEntity{123u};
+  systemMgr.AddSystem(configSystem, configEntity, nullptr);
 
   // SystemManager without an ECM/EventmManager will mean no config occurs
   EXPECT_EQ(0, configSystem->configured);
@@ -105,6 +106,7 @@ TEST(SystemManager, AddSystemNoEcm)
   EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
   EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
   EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+  EXPECT_EQ(1u, systemMgr.TotalByEntity(configEntity).size());
 
   systemMgr.ActivatePendingSystems();
   EXPECT_EQ(1u, systemMgr.ActiveCount());
@@ -114,16 +116,19 @@ TEST(SystemManager, AddSystemNoEcm)
   EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
   EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
   EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
+  EXPECT_EQ(1u, systemMgr.TotalByEntity(configEntity).size());
 
   auto updateSystem = std::make_shared<SystemWithUpdates>();
-  systemMgr.AddSystem(updateSystem, kNullEntity, nullptr);
+  Entity updateEntity{456u};
+  systemMgr.AddSystem(updateSystem, updateEntity, 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());
+  EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size());
 
   systemMgr.ActivatePendingSystems();
   EXPECT_EQ(2u, systemMgr.ActiveCount());
@@ -133,6 +138,7 @@ TEST(SystemManager, AddSystemNoEcm)
   EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size());
   EXPECT_EQ(1u, systemMgr.SystemsUpdate().size());
   EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size());
+  EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size());
 }
 
 /////////////////////////////////////////////////

test/worlds/model_plugin_only.sdf

@@ -0,0 +1,44 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="model_plugin_only">
+
+    <light type="directional" name="sun">
+    </light>
+
+    <model name='box'>
+      <link name='link'>
+        <inertial>
+          <mass>1.14395</mass>
+          <inertia>
+            <ixx>0.126164</ixx>
+            <iyy>0.416519</iyy>
+            <izz>0.481014</izz>
+          </inertia>
+        </inertial>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>2.01142 1 0.568726</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>2.01142 1 0.568726</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+
+      <plugin
+        filename="ignition-gazebo-velocity-control-system"
+        name="ignition::gazebo::systems::VelocityControl">
+        <initial_linear>0.3 0 0</initial_linear>
+        <initial_angular>0 0 -0.1</initial_angular>
+      </plugin>
+
+    </model>
+
+  </world>
+</sdf>