From 310668726b81271b5597845ae528c1e3ffa0379a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 18 Mar 2022 17:10:41 -0700 Subject: [PATCH] Prevent hanging when world has only non-world plugins (#1383) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel Co-authored-by: Alejandro Hernández Cordero --- src/SimulationRunner.cc | 7 ++--- src/SimulationRunner_TEST.cc | 24 +++++++++++++++++ src/SystemInternal.hh | 20 +++++++++----- src/SystemManager.cc | 24 ++++++++++++++--- src/SystemManager.hh | 18 ++++++++----- src/SystemManager_TEST.cc | 10 +++++-- test/worlds/model_plugin_only.sdf | 44 +++++++++++++++++++++++++++++++ 7 files changed, 125 insertions(+), 22 deletions(-) create mode 100644 test/worlds/model_plugin_only.sdf diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 2840b3f0ea..715df805d3 100644 --- a/src/SimulationRunner.cc +++ b/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(); diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 17641a873c..3d525f0f23 100644 --- a/src/SimulationRunner_TEST.cc +++ b/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(); + 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) { diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 13158688c3..66277cf026 100644 --- a/src/SystemInternal.hh +++ b/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()), configure(systemPlugin->QueryInterface()), preupdate(systemPlugin->QueryInterface()), update(systemPlugin->QueryInterface()), - postupdate(systemPlugin->QueryInterface()) + postupdate(systemPlugin->QueryInterface()), + parentEntity(_entity) { } /// \brief Constructor /// \param[in] _system Pointer to a system. - public: explicit SystemInternal(const std::shared_ptr &_system) + /// \param[in] _entity The entity that this system is attached to. + public: explicit SystemInternal(const std::shared_ptr &_system, + Entity _entity) : systemShared(_system), system(_system.get()), configure(dynamic_cast(_system.get())), preupdate(dynamic_cast(_system.get())), update(dynamic_cast(_system.get())), - postupdate(dynamic_cast(_system.get())) + postupdate(dynamic_cast(_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 diff --git a/src/SystemManager.cc b/src/SystemManager.cc index ba716f99a9..5f75fc3ec9 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -103,7 +103,7 @@ void SystemManager::AddSystem(const SystemPluginPtr &_system, Entity _entity, std::shared_ptr _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 _sdf) { - this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); + this->AddSystemImpl(SystemInternal(_system, _entity), _sdf); } ////////////////////////////////////////////////// void SystemManager::AddSystemImpl( SystemInternal _system, - Entity _entity, std::shared_ptr _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& SystemManager::SystemsPostUpdate() { return this->systemsPostupdate; } + +////////////////////////////////////////////////// +std::vector SystemManager::TotalByEntity(Entity _entity) +{ + std::vector 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; +} diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 9c38505922..1d83272787 100644 --- a/src/SystemManager.hh +++ b/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& 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& 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& 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& SystemsPostUpdate(); + /// \brief Get an vector of all systems attached to a given entity. + /// \return Vector of systems. + public: std::vector 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 _sdf); /// \brief All the systems. diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 4fc7288580..454f53e45a 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -93,7 +93,8 @@ TEST(SystemManager, AddSystemNoEcm) EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); auto configSystem = std::make_shared(); - 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,9 +116,11 @@ 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(); - 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()); @@ -124,6 +128,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(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()); } ///////////////////////////////////////////////// diff --git a/test/worlds/model_plugin_only.sdf b/test/worlds/model_plugin_only.sdf new file mode 100644 index 0000000000..6293cf5004 --- /dev/null +++ b/test/worlds/model_plugin_only.sdf @@ -0,0 +1,44 @@ + + + + + + + + + + + 1.14395 + + 0.126164 + 0.416519 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + + + + 2.01142 1 0.568726 + + + + + + + 0.3 0 0 + 0 0 -0.1 + + + + + +