From 9b3dfca6022a0785e40b2554a2fff9224a85fcf6 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 10 Mar 2022 13:27:15 -0800 Subject: [PATCH 1/3] Initial SystemManager implementation Signed-off-by: Louise Poubel --- src/SimulationRunner.cc | 7 ++--- src/SystemInternal.hh | 17 +++++++----- src/SystemManager.cc | 19 ++++++++++--- src/SystemManager.hh | 18 ++++++++----- src/SystemManager_TEST.cc | 10 +++++-- test/worlds/model_plugin_only.sdf | 44 +++++++++++++++++++++++++++++++ 6 files changed, 93 insertions(+), 22 deletions(-) create mode 100644 test/worlds/model_plugin_only.sdf diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 2840b3f0ea5..403358c4d69 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->SystemsByEntity( + worldEntity(this->entityCompMgr)).empty()) { ignmsg << "No systems loaded from SDF, loading defaults" << std::endl; bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 13158688c34..abb63c3161b 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -39,25 +39,28 @@ namespace ignition { /// \brief Constructor /// \param[in] _systemPlugin A system loaded from a plugin. - public: explicit SystemInternal(SystemPluginPtr _systemPlugin) + 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) + 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 +92,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 ba716f99a91..62a761e2d8d 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,15 @@ const std::vector& SystemManager::SystemsPostUpdate() { return this->systemsPostupdate; } + +////////////////////////////////////////////////// +std::vector SystemManager::SystemsByEntity(Entity _entity) +{ + std::vector result; + for (auto system : this->systems) + { + if (system.parentEntity == _entity) + result.push_back(system); + } + return result; +} diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 9c38505922c..27c010bbeab 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 active systems attached to a given entity. + /// \return Vector of systems. + public: std::vector SystemsByEntity(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 4fc7288580f..330375d05ab 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(0u, systemMgr.SystemsByEntity(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.SystemsByEntity(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(0u, systemMgr.SystemsByEntity(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.SystemsByEntity(updateEntity).size()); } ///////////////////////////////////////////////// diff --git a/test/worlds/model_plugin_only.sdf b/test/worlds/model_plugin_only.sdf new file mode 100644 index 00000000000..6293cf50043 --- /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 + + + + + + From 892283519e2c6c914151fd6a83368351e4e775f8 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 10 Mar 2022 14:03:13 -0800 Subject: [PATCH 2/3] Total systems, add test Signed-off-by: Louise Poubel --- src/SimulationRunner.cc | 2 +- src/SimulationRunner_TEST.cc | 24 ++++++++++++++++++++++++ src/SystemInternal.hh | 1 + src/SystemManager.cc | 7 ++++++- src/SystemManager.hh | 4 ++-- src/SystemManager_TEST.cc | 8 ++++---- 6 files changed, 38 insertions(+), 8 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 403358c4d69..715df805d3e 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -174,7 +174,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // If we have reached this point and no world systems have been loaded, then // load a default set of systems. - if (this->systemMgr->SystemsByEntity( + if (this->systemMgr->TotalByEntity( worldEntity(this->entityCompMgr)).empty()) { ignmsg << "No systems loaded from SDF, loading defaults" << std::endl; diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 17641a873ce..3d525f0f230 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 abb63c3161b..ff054a7e4eb 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -39,6 +39,7 @@ namespace ignition { /// \brief Constructor /// \param[in] _systemPlugin A system loaded from a plugin. + /// \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()), diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 62a761e2d8d..5f75fc3ec9b 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -158,7 +158,7 @@ const std::vector& SystemManager::SystemsPostUpdate() } ////////////////////////////////////////////////// -std::vector SystemManager::SystemsByEntity(Entity _entity) +std::vector SystemManager::TotalByEntity(Entity _entity) { std::vector result; for (auto system : this->systems) @@ -166,5 +166,10 @@ std::vector SystemManager::SystemsByEntity(Entity _entity) 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 27c010bbeab..1d83272787f 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -108,9 +108,9 @@ namespace ignition /// \return Vector of systems's post-update interfaces. public: const std::vector& SystemsPostUpdate(); - /// \brief Get an vector of all active systems attached to a given entity. + /// \brief Get an vector of all systems attached to a given entity. /// \return Vector of systems. - public: std::vector SystemsByEntity(Entity _entity); + 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 diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 330375d05ab..454f53e45a7 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -106,7 +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(0u, systemMgr.SystemsByEntity(configEntity).size()); + EXPECT_EQ(1u, systemMgr.TotalByEntity(configEntity).size()); systemMgr.ActivatePendingSystems(); EXPECT_EQ(1u, systemMgr.ActiveCount()); @@ -116,7 +116,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.SystemsByEntity(configEntity).size()); + EXPECT_EQ(1u, systemMgr.TotalByEntity(configEntity).size()); auto updateSystem = std::make_shared(); Entity updateEntity{456u}; @@ -128,7 +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(0u, systemMgr.SystemsByEntity(updateEntity).size()); + EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size()); systemMgr.ActivatePendingSystems(); EXPECT_EQ(2u, systemMgr.ActiveCount()); @@ -138,7 +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.SystemsByEntity(updateEntity).size()); + EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size()); } ///////////////////////////////////////////////// From 1e5acea2723be6f4baa955bd195c7d9c3a0fb504 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 10 Mar 2022 14:16:24 -0800 Subject: [PATCH 3/3] cpopcehck Signed-off-by: Louise Poubel --- src/SystemInternal.hh | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index ff054a7e4eb..66277cf0268 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -40,7 +40,8 @@ namespace ignition /// \brief Constructor /// \param[in] _systemPlugin A system loaded from a plugin. /// \param[in] _entity The entity that this system is attached to. - public: explicit SystemInternal(SystemPluginPtr _systemPlugin, Entity _entity) + public: explicit SystemInternal(SystemPluginPtr _systemPlugin, + Entity _entity) : systemPlugin(std::move(_systemPlugin)), system(systemPlugin->QueryInterface()), configure(systemPlugin->QueryInterface()), @@ -53,6 +54,7 @@ namespace ignition /// \brief Constructor /// \param[in] _system Pointer to a system. + /// \param[in] _entity The entity that this system is attached to. public: explicit SystemInternal(const std::shared_ptr &_system, Entity _entity) : systemShared(_system),