From 5b974b36e761917f0c2e6a5ae94ec72c7dbe771e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 15 Feb 2022 11:22:54 -0600 Subject: [PATCH 1/6] SystemInternal and WorldControl into own headers Reduces the size of simulationrunner header a bit Signed-off-by: Michael Carroll --- src/SimulationRunner.hh | 91 +------------------------------------ src/SystemInternal.hh | 99 +++++++++++++++++++++++++++++++++++++++++ src/WorldControl.hh | 56 +++++++++++++++++++++++ 3 files changed, 157 insertions(+), 89 deletions(-) create mode 100644 src/SystemInternal.hh create mode 100644 src/WorldControl.hh diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 5da194e209..2600bef722 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -48,14 +48,14 @@ #include "ignition/gazebo/EventManager.hh" #include "ignition/gazebo/Export.hh" #include "ignition/gazebo/ServerConfig.hh" -#include "ignition/gazebo/System.hh" #include "ignition/gazebo/SystemLoader.hh" -#include "ignition/gazebo/SystemPluginPtr.hh" #include "ignition/gazebo/Types.hh" #include "network/NetworkManager.hh" #include "LevelManager.hh" #include "Barrier.hh" +#include "SystemInternal.hh" +#include "WorldControl.hh" using namespace std::chrono_literals; @@ -68,93 +68,6 @@ namespace ignition // Forward declarations. class SimulationRunnerPrivate; - /// \brief Helper struct to control world time. It's used to hold - /// input from either msgs::WorldControl or msgs::LogPlaybackControl. - struct WorldControl - { - /// \brief True to pause simulation. - // cppcheck-suppress unusedStructMember - bool pause{false}; // NOLINT - - /// \biref Run a given number of simulation iterations. - // cppcheck-suppress unusedStructMember - uint64_t multiStep{0u}; // NOLINT - - /// \brief Reset simulation back to time zero. Rewinding resets sim time, - /// real time and iterations. - // cppcheck-suppress unusedStructMember - bool rewind{false}; // NOLINT - - /// \brief A simulation time in the future to run to and then pause. - /// A negative number indicates that this variable it not being used. - std::chrono::steady_clock::duration runToSimTime{-1}; // NOLINT - - /// \brief Sim time to jump to. A negative value means don't seek. - /// Seeking changes sim time but doesn't affect real time. - /// It also resets iterations back to zero. - std::chrono::steady_clock::duration seek{-1}; - }; - - /// \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()), - configure(systemPlugin->QueryInterface()), - preupdate(systemPlugin->QueryInterface()), - update(systemPlugin->QueryInterface()), - postupdate(systemPlugin->QueryInterface()) - { - } - - /// \brief Constructor - /// \param[in] _system Pointer to a system. - public: explicit SystemInternal(const std::shared_ptr &_system) - : 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())) - { - } - - /// \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 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; - - /// \brief Access this system via the ISystemUpdate interface - /// Will be nullptr if the System doesn't implement this interface. - public: ISystemUpdate *update = nullptr; - - /// \brief Access this system via the ISystemPostUpdate interface - /// Will be nullptr if the System doesn't implement this interface. - public: ISystemPostUpdate *postupdate = nullptr; - - /// \brief Vector of queries and callbacks - public: std::vector updates; - }; - class IGNITION_GAZEBO_VISIBLE SimulationRunner { /// \brief Constructor diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh new file mode 100644 index 0000000000..64285a25c6 --- /dev/null +++ b/src/SystemInternal.hh @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_SYSTEMINTERNAL_HH_ +#define IGNITION_GAZEBO_SYSTEMINTERNAL_HH_ + +#include +#include +#include +#include + +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/System.hh" +#include "ignition/gazebo/SystemPluginPtr.hh" + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + + /// \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()), + configure(systemPlugin->QueryInterface()), + preupdate(systemPlugin->QueryInterface()), + update(systemPlugin->QueryInterface()), + postupdate(systemPlugin->QueryInterface()) + { + } + + /// \brief Constructor + /// \param[in] _system Pointer to a system. + public: explicit SystemInternal(const std::shared_ptr &_system) + : 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())) + { + } + + /// \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 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; + + /// \brief Access this system via the ISystemUpdate interface + /// Will be nullptr if the System doesn't implement this interface. + public: ISystemUpdate *update = nullptr; + + /// \brief Access this system via the ISystemPostUpdate interface + /// Will be nullptr if the System doesn't implement this interface. + public: ISystemPostUpdate *postupdate = nullptr; + + /// \brief Vector of queries and callbacks + public: std::vector updates; + }; + } + } // namespace gazebo +} // namespace ignition +#endif // IGNITION_GAZEBO_SYSTEMINTERNAL_HH_ + diff --git a/src/WorldControl.hh b/src/WorldControl.hh new file mode 100644 index 0000000000..bb0005e093 --- /dev/null +++ b/src/WorldControl.hh @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_WORLDCONTROL_HH_ +#define IGNITION_GAZEBO_WORLDCONTROL_HH_ + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + + /// \brief Helper struct to control world time. It's used to hold + /// input from either msgs::WorldControl or msgs::LogPlaybackControl. + struct WorldControl + { + /// \brief True to pause simulation. + // cppcheck-suppress unusedStructMember + bool pause{false}; // NOLINT + + /// \biref Run a given number of simulation iterations. + // cppcheck-suppress unusedStructMember + uint64_t multiStep{0u}; // NOLINT + + /// \brief Reset simulation back to time zero. Rewinding resets sim time, + /// real time and iterations. + // cppcheck-suppress unusedStructMember + bool rewind{false}; // NOLINT + + /// \brief A simulation time in the future to run to and then pause. + /// A negative number indicates that this variable it not being used. + std::chrono::steady_clock::duration runToSimTime{-1}; // NOLINT + + /// \brief Sim time to jump to. A negative value means don't seek. + /// Seeking changes sim time but doesn't affect real time. + /// It also resets iterations back to zero. + std::chrono::steady_clock::duration seek{-1}; + }; + } + } // namespace gazebo +} // namespace ignition +#endif // IGNITION_GAZEBO_WORLDCONTROL_HH_ From f885cf5f02ad4511c04945f9b790f1edc16dee86 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 15 Feb 2022 13:24:58 -0600 Subject: [PATCH 2/6] Move System interaction to SystemManager Signed-off-by: Michael Carroll --- src/CMakeLists.txt | 2 + src/SimulationRunner.cc | 154 +++++++++++---------------------- src/SimulationRunner.hh | 42 ++------- src/SystemInternal.hh | 8 ++ src/SystemManager.cc | 176 ++++++++++++++++++++++++++++++++++++++ src/SystemManager.hh | 150 ++++++++++++++++++++++++++++++++ src/SystemManager_TEST.cc | 132 ++++++++++++++++++++++++++++ 7 files changed, 528 insertions(+), 136 deletions(-) create mode 100644 src/SystemManager.cc create mode 100644 src/SystemManager.hh create mode 100644 src/SystemManager_TEST.cc diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 358256f1e3..c609bec040 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -60,6 +60,7 @@ set (sources ServerPrivate.cc SimulationRunner.cc SystemLoader.cc + SystemManager.cc TestFixture.cc Util.cc World.cc @@ -86,6 +87,7 @@ set (gtest_sources Server_TEST.cc SimulationRunner_TEST.cc SystemLoader_TEST.cc + SystemManager_TEST.cc System_TEST.cc TestFixture_TEST.cc Util_TEST.cc diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 9264a41713..cd724b512f 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -61,9 +61,6 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // Keep world name this->worldName = _world->Name(); - // Keep system loader so plugins can be loaded at runtime - this->systemLoader = _systemLoader; - // Get the physics profile // TODO(luca): remove duplicated logic in SdfEntityCreator and LevelManager auto physics = _world->PhysicsByIndex(0); @@ -115,6 +112,9 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, static_cast(this->stepSize.count() / this->desiredRtf)); } + // Create the system manager + this->systemMgr = std::make_unique(_systemLoader); + this->pauseConn = this->eventMgr.Connect( std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1)); @@ -173,7 +173,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // If we have reached this point and no systems have been loaded, then load // a default set of systems. - if (this->systems.empty() && this->pendingSystems.empty()) + if (this->systemMgr->TotalCount() == 0) { ignmsg << "No systems loaded from SDF, loading defaults" << std::endl; bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); @@ -455,7 +455,12 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system, std::optional _entity, std::optional> _sdf) { - this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); + auto entity = _entity.has_value() ? _entity.value() + : worldEntity(this->entityCompMgr); + auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element(); + + this->systemMgr->AddSystem(_system, entity, sdf); + this->systemMgr->ConfigurePendingSystems(this->entityCompMgr, this->eventMgr); } ////////////////////////////////////////////////// @@ -464,104 +469,60 @@ void SimulationRunner::AddSystem( std::optional _entity, std::optional> _sdf) { - this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); -} + auto entity = _entity.has_value() ? _entity.value() + : worldEntity(this->entityCompMgr); + auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element(); -////////////////////////////////////////////////// -void SimulationRunner::AddSystemImpl( - SystemInternal _system, - std::optional _entity, - std::optional> _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 lock(this->pendingSystemsMutex); - this->pendingSystems.push_back(_system); + this->systemMgr->AddSystem(_system, entity, sdf); + this->systemMgr->ConfigurePendingSystems(this->entityCompMgr, this->eventMgr); } ///////////////////////////////////////////////// -void SimulationRunner::AddSystemToRunner(SystemInternal _system) +void SimulationRunner::ProcessSystemQueue() { - this->systems.push_back(_system); + auto pending = this->systemMgr->PendingCount(); - if (_system.preupdate) - this->systemsPreupdate.push_back(_system.preupdate); + if (0 == pending) + return; - if (_system.update) - this->systemsUpdate.push_back(_system.update); + // If additional systems are to be added, stop the existing threads. + this->StopWorkerThreads(); - if (_system.postupdate) - this->systemsPostupdate.push_back(_system.postupdate); -} + this->systemMgr->ActivatePendingSystems(); -///////////////////////////////////////////////// -void SimulationRunner::ProcessSystemQueue() -{ - std::lock_guard lock(this->pendingSystemsMutex); - auto pending = this->pendingSystems.size(); - - if (pending > 0) - { - // If additional systems are to be added, stop the existing threads. - this->StopWorkerThreads(); - } + auto threadCount = this->systemMgr->SystemsPostUpdate().size() + 1u; - for (const auto &system : this->pendingSystems) - { - this->AddSystemToRunner(system); - } - this->pendingSystems.clear(); + igndbg << "Creating PostUpdate worker threads: " + << threadCount << std::endl; - // If additional systems were added, recreate the worker threads. - if (pending > 0) - { - igndbg << "Creating PostUpdate worker threads: " - << this->systemsPostupdate.size() + 1 << std::endl; + this->postUpdateStartBarrier = std::make_unique(threadCount); + this->postUpdateStopBarrier = std::make_unique(threadCount); - this->postUpdateStartBarrier = - std::make_unique(this->systemsPostupdate.size() + 1u); - this->postUpdateStopBarrier = - std::make_unique(this->systemsPostupdate.size() + 1u); + this->postUpdateThreadsRunning = true; + int id = 0; - this->postUpdateThreadsRunning = true; - int id = 0; + for (auto &system : this->systemMgr->SystemsPostUpdate()) + { + igndbg << "Creating postupdate worker thread (" << id << ")" << std::endl; - for (auto &system : this->systemsPostupdate) + this->postUpdateThreads.push_back(std::thread([&, id]() { - igndbg << "Creating postupdate worker thread (" << id << ")" << std::endl; - - this->postUpdateThreads.push_back(std::thread([&, id]() + std::stringstream ss; + ss << "PostUpdateThread: " << id; + IGN_PROFILE_THREAD_NAME(ss.str().c_str()); + while (this->postUpdateThreadsRunning) { - std::stringstream ss; - ss << "PostUpdateThread: " << id; - IGN_PROFILE_THREAD_NAME(ss.str().c_str()); - while (this->postUpdateThreadsRunning) + this->postUpdateStartBarrier->Wait(); + if (this->postUpdateThreadsRunning) { - this->postUpdateStartBarrier->Wait(); - if (this->postUpdateThreadsRunning) - { - system->PostUpdate(this->currentInfo, this->entityCompMgr); - } - this->postUpdateStopBarrier->Wait(); + system->PostUpdate(this->currentInfo, this->entityCompMgr); } - igndbg << "Exiting postupdate worker thread (" - << id << ")" << std::endl; - })); - id++; - } + this->postUpdateStopBarrier->Wait(); + } + igndbg << "Exiting postupdate worker thread (" + << id << ")" << std::endl; + })); + id++; } } @@ -577,13 +538,13 @@ void SimulationRunner::UpdateSystems() { IGN_PROFILE("PreUpdate"); - for (auto& system : this->systemsPreupdate) + for (auto& system : this->systemMgr->SystemsPreUpdate()) system->PreUpdate(this->currentInfo, this->entityCompMgr); } { IGN_PROFILE("Update"); - for (auto& system : this->systemsUpdate) + for (auto& system : this->systemMgr->SystemsUpdate()) system->Update(this->currentInfo, this->entityCompMgr); } @@ -903,19 +864,9 @@ void SimulationRunner::LoadPlugin(const Entity _entity, const std::string &_name, const sdf::ElementPtr &_sdf) { - std::optional system; - { - std::lock_guard lock(this->systemLoaderMutex); - system = this->systemLoader->LoadPlugin(_fname, _name, _sdf); - } - - // System correctly loaded from library - if (system) - { - this->AddSystem(system.value(), _entity, _sdf); - igndbg << "Loaded system [" << _name - << "] for entity [" << _entity << "]" << std::endl; - } + this->systemMgr->LoadPlugin(_entity, _fname, _name, _sdf); + this->systemMgr->ConfigurePendingSystems( + this->entityCompMgr, this->eventMgr); } ////////////////////////////////////////////////// @@ -1085,8 +1036,7 @@ size_t SimulationRunner::EntityCount() const ///////////////////////////////////////////////// size_t SimulationRunner::SystemCount() const { - std::lock_guard lock(this->pendingSystemsMutex); - return this->systems.size() + this->pendingSystems.size(); + return this->systemMgr->TotalCount(); } ///////////////////////////////////////////////// diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 2600bef722..add7a6b63b 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -53,8 +53,8 @@ #include "network/NetworkManager.hh" #include "LevelManager.hh" +#include "SystemManager.hh" #include "Barrier.hh" -#include "SystemInternal.hh" #include "WorldControl.hh" using namespace std::chrono_literals; @@ -370,16 +370,6 @@ 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 = std::nullopt, - std::optional> _sdf = std::nullopt); - /// \brief Process entities with the components::Recreate component. /// Put in a request to make them as removed private: void ProcessRecreateEntitiesRemove(); @@ -399,27 +389,17 @@ namespace ignition /// server is in the run state. private: std::atomic running{false}; - /// \brief All the systems. - private: std::vector systems; - - /// \brief Pending systems to be added to systems. - private: std::vector pendingSystems; - - /// \brief Mutex to protect pendingSystems - private: mutable std::mutex pendingSystemsMutex; - - /// \brief Systems implementing PreUpdate - private: std::vector systemsPreupdate; - - /// \brief Systems implementing Update - private: std::vector systemsUpdate; - - /// \brief Systems implementing PostUpdate - private: std::vector systemsPostupdate; + /// \brief Manager of all systems. + private: std::unique_ptr systemMgr; /// \brief Manager of all events. + /// Note: must be before EntityComponentManager private: EventManager eventMgr; + /// \brief Manager of all systems. + /// Note: must be before EntityComponentManager + private: std::unique_ptr systemMgr; + /// \brief Manager of all components. private: EntityComponentManager entityCompMgr; @@ -449,12 +429,6 @@ namespace ignition /// \brief List of real times used to compute averages. private: std::list realTimes; - /// \brief System loader, for loading system plugins. - private: SystemLoaderPtr systemLoader; - - /// \brief Mutex to protect systemLoader - private: std::mutex systemLoaderMutex; - /// \brief Node for communication. private: std::unique_ptr node{nullptr}; diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 64285a25c6..13158688c3 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -89,6 +89,14 @@ 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 Cached sdf that was used to call `Configure` on the system + /// Useful for if a system needs to be reconfigured at runtime + public: std::shared_ptr configureSdf = nullptr; + /// \brief Vector of queries and callbacks public: std::vector updates; }; diff --git a/src/SystemManager.cc b/src/SystemManager.cc new file mode 100644 index 0000000000..b3dfa2b392 --- /dev/null +++ b/src/SystemManager.cc @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "SystemManager.hh" + +using namespace ignition; +using namespace gazebo; + +////////////////////////////////////////////////// +SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader) + : systemLoader(_systemLoader) +{ +} + +////////////////////////////////////////////////// +void SystemManager::LoadPlugin(const Entity _entity, + const std::string &_fname, + const std::string &_name, + const sdf::ElementPtr &_sdf) +{ + std::optional system; + { + std::lock_guard lock(this->systemLoaderMutex); + system = this->systemLoader->LoadPlugin(_fname, _name, _sdf); + } + + // System correctly loaded from library + if (system) + { + this->AddSystem(system.value(), _entity, _sdf); + igndbg << "Loaded system [" << _name + << "] for entity [" << _entity << "]" << std::endl; + } +} + +////////////////////////////////////////////////// +size_t SystemManager::TotalCount() const +{ + std::lock_guard lock(this->systemsMutex); + return this->systems.size() + this->pendingSystems.size(); +} + +////////////////////////////////////////////////// +size_t SystemManager::ActiveCount() const +{ + std::lock_guard lock(this->systemsMutex); + return this->systems.size(); +} + +////////////////////////////////////////////////// +size_t SystemManager::PendingCount() const +{ + std::lock_guard lock(this->systemsMutex); + return this->pendingSystems.size(); +} + +////////////////////////////////////////////////// +void SystemManager::ConfigurePendingSystems(EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + std::lock_guard lock(this->systemsMutex); + for (size_t ii = 0; ii < this->pendingSystems.size(); ++ii) + { + if (this->pendingSystemsConfigured[ii]) + continue; + + const auto& system = this->pendingSystems[ii]; + + if (system.configure) + { + system.configure->Configure(system.configureEntity, + system.configureSdf, + _ecm, _eventMgr); + this->pendingSystemsConfigured[ii] = true; + } + } +} + +////////////////////////////////////////////////// +size_t SystemManager::ActivatePendingSystems() +{ + std::lock_guard lock(this->systemsMutex); + + auto count = this->pendingSystems.size(); + + for (const auto& system : this->pendingSystems) + { + this->systems.push_back(system); + + if (system.configure) + this->systemsConfigure.push_back(system.configure); + + if (system.preupdate) + this->systemsPreupdate.push_back(system.preupdate); + + if (system.update) + this->systemsUpdate.push_back(system.update); + + if (system.postupdate) + this->systemsPostupdate.push_back(system.postupdate); + } + + this->pendingSystems.clear(); + this->pendingSystemsConfigured.clear(); + return count; +} + +////////////////////////////////////////////////// +void SystemManager::AddSystem(const SystemPluginPtr &_system, + Entity _entity, + std::shared_ptr _sdf) +{ + this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); +} + +////////////////////////////////////////////////// +void SystemManager::AddSystem( + const std::shared_ptr &_system, + Entity _entity, + std::shared_ptr _sdf) +{ + this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); +} + +////////////////////////////////////////////////// +void SystemManager::AddSystemImpl( + SystemInternal _system, + Entity _entity, + std::shared_ptr _sdf) +{ + _system.configureEntity = _entity; + _system.configureSdf = _sdf; + + // Update callbacks will be handled later, add to queue + std::lock_guard lock(this->systemsMutex); + this->pendingSystems.push_back(_system); + this->pendingSystemsConfigured.push_back(false); +} + +////////////////////////////////////////////////// +const std::vector& SystemManager::SystemsConfigure() +{ + return this->systemsConfigure; +} + +////////////////////////////////////////////////// +const std::vector& SystemManager::SystemsPreUpdate() +{ + return this->systemsPreupdate; +} + +////////////////////////////////////////////////// +const std::vector& SystemManager::SystemsUpdate() +{ + return this->systemsUpdate; +} + +////////////////////////////////////////////////// +const std::vector& SystemManager::SystemsPostUpdate() +{ + return this->systemsPostupdate; +} diff --git a/src/SystemManager.hh b/src/SystemManager.hh new file mode 100644 index 0000000000..f9991cca4a --- /dev/null +++ b/src/SystemManager.hh @@ -0,0 +1,150 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_SYSTEMMANAGER_HH_ +#define IGNITION_GAZEBO_SYSTEMMANAGER_HH_ + +#include +#include +#include + +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Export.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/Types.hh" + +#include "SystemInternal.hh" + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + + /// \brief Used to load / unload sysetms as well as iterate over them. + class SystemManager + { + /// \brief Constructor + /// \param[in] _systemLoader A pointer to a SystemLoader to load plugins + /// from files + public: explicit SystemManager(const SystemLoaderPtr &_systemLoader); + + /// \brief Load system plugin for a given entity. + /// \param[in] _entity Entity + /// \param[in] _fname Filename of the plugin library + /// \param[in] _name Name of the plugin + /// \param[in] _sdf SDF element (content of plugin tag) + public: void LoadPlugin(const Entity _entity, + const std::string &_fname, + const std::string &_name, + const sdf::ElementPtr &_sdf); + + /// \brief Add a system to the manager + /// \param[in] _system SystemPluginPtr to be added + /// \param[in] _entity Entity that system is attached to. + /// \param[in] _sdf Pointer to the SDF of the entity. + public: void AddSystem(const SystemPluginPtr &_system, + Entity _entity, + std::shared_ptr _sdf); + + /// \brief Add a system to the manager + /// \param[in] _system SystemPluginPtr to be added + /// \param[in] _entity Entity that system is attached to. + /// \param[in] _sdf Pointer to the SDF of the entity. + public: void AddSystem(const std::shared_ptr &_system, + Entity _entity, + std::shared_ptr _sdf); + + /// \brief Get the count of currently active systems. + /// \return The active systems count. + public: size_t ActiveCount() const; + + /// \brief Get the count of currently pending systems. + /// \return The pending systems count. + public: size_t PendingCount() const; + + /// \brief Get the count of all (pending + active) managed systems + /// \return The count. + public: size_t TotalCount() const; + + /// \brief Call the configure call on all pending systems + /// \param[in] _ecm The entity component manager to configure with + /// \param[in] _evetnMgr The event manager to configure with + public: void ConfigurePendingSystems(EntityComponentManager &_ecm, + EventManager &_eventMgr); + + /// \brief Move all "pending" systems to "active" state + /// \return The number of newly-active systems + public: size_t ActivatePendingSystems(); + + /// \brief Get an vector of all systems implementing "Configure" + public: const std::vector& SystemsConfigure(); + + /// \brief Get an vector of all systems implementing "PreUpdate" + public: const std::vector& SystemsPreUpdate(); + + /// \brief Get an vector of all systems implementing "Update" + public: const std::vector& SystemsUpdate(); + + /// \brief Get an vector of all systems implementing "PostUpdate" + public: const std::vector& SystemsPostUpdate(); + + /// \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. + private: std::vector systems; + + /// \brief Pending systems to be added to systems. + private: std::vector pendingSystems; + + /// \brief Mark if a pending system has been configured + private: std::vector pendingSystemsConfigured; + + /// \brief Mutex to protect pendingSystems + private: mutable std::mutex systemsMutex; + + /// \brief Systems implementing Configure + private: std::vector systemsConfigure; + + /// \brief Systems implementing PreUpdate + private: std::vector systemsPreupdate; + + /// \brief Systems implementing Update + private: std::vector systemsUpdate; + + /// \brief Systems implementing PostUpdate + private: std::vector systemsPostupdate; + + /// \brief System loader, for loading system plugins. + private: SystemLoaderPtr systemLoader; + + /// \brief Mutex to protect systemLoader + private: std::mutex systemLoaderMutex; + }; + } + } // namespace gazebo +} // namespace ignition +#endif // IGNITION_GAZEBO_SYSTEMINTERNAL_HH_ diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc new file mode 100644 index 0000000000..ac0a292aeb --- /dev/null +++ b/src/SystemManager_TEST.cc @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/System.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/test_config.hh" // NOLINT(build/include) + +#include "SystemManager.hh" + +using namespace ignition::gazebo; + +///////////////////////////////////////////////// +class System_WithConfigure: + public System, + public ISystemConfigure +{ + // Documentation inherited + public: void Configure( + const Entity &, + const std::shared_ptr &, + EntityComponentManager &, + EventManager &) override { configured++; }; + + public: int configured = 0; +}; + +///////////////////////////////////////////////// +class System_WithUpdates: + public System, + public ISystemPreUpdate, + public ISystemUpdate, + public ISystemPostUpdate +{ + // Documentation inherited + public: void PreUpdate(const UpdateInfo &, + EntityComponentManager &) override {}; + + // Documentation inherited + public: void Update(const UpdateInfo &, + EntityComponentManager &) override {}; + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &, + const EntityComponentManager &) override {}; +}; + +///////////////////////////////////////////////// +TEST(SystemManager, Constructor) +{ + auto loader = std::make_shared(); + SystemManager systemMgr(loader); + + ASSERT_EQ(0u, systemMgr.ActiveCount()); + ASSERT_EQ(0u, systemMgr.PendingCount()); + ASSERT_EQ(0u, systemMgr.TotalCount()); + + ASSERT_EQ(0u, systemMgr.SystemsConfigure().size()); + ASSERT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + ASSERT_EQ(0u, systemMgr.SystemsUpdate().size()); + ASSERT_EQ(0u, systemMgr.SystemsPostUpdate().size()); +} + +///////////////////////////////////////////////// +TEST(SystemManager, AddSystem) +{ + auto loader = std::make_shared(); + SystemManager systemMgr(loader); + + EXPECT_EQ(0u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(0u, systemMgr.TotalCount()); + EXPECT_EQ(0u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + + auto configSystem = std::make_shared(); + systemMgr.AddSystem(configSystem, kNullEntity, nullptr); + EXPECT_EQ(0u, systemMgr.ActiveCount()); + EXPECT_EQ(1u, systemMgr.PendingCount()); + EXPECT_EQ(1u, systemMgr.TotalCount()); + EXPECT_EQ(0u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + + systemMgr.ActivatePendingSystems(); + EXPECT_EQ(1u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(1u, 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()); + + auto updateSystem = std::make_shared(); + systemMgr.AddSystem(updateSystem, kNullEntity, 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()); + + systemMgr.ActivatePendingSystems(); + EXPECT_EQ(2u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); +} From af90891942d343f728289feceead1db28d8f5df0 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 16 Feb 2022 12:16:32 -0600 Subject: [PATCH 3/6] Adjustments for Fortress Signed-off-by: Michael Carroll --- src/SimulationRunner.cc | 7 ++-- src/SimulationRunner.hh | 3 -- src/SystemManager.cc | 41 ++++++++--------------- src/SystemManager.hh | 20 ++++++++---- src/SystemManager_TEST.cc | 68 ++++++++++++++++++++++++++++++++++++++- 5 files changed, 95 insertions(+), 44 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index cd724b512f..f34a21cafc 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -113,7 +113,8 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, } // Create the system manager - this->systemMgr = std::make_unique(_systemLoader); + this->systemMgr = std::make_unique(_systemLoader, + &this->entityCompMgr, &this->eventMgr); this->pauseConn = this->eventMgr.Connect( std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1)); @@ -460,7 +461,6 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system, auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element(); this->systemMgr->AddSystem(_system, entity, sdf); - this->systemMgr->ConfigurePendingSystems(this->entityCompMgr, this->eventMgr); } ////////////////////////////////////////////////// @@ -474,7 +474,6 @@ void SimulationRunner::AddSystem( auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element(); this->systemMgr->AddSystem(_system, entity, sdf); - this->systemMgr->ConfigurePendingSystems(this->entityCompMgr, this->eventMgr); } ///////////////////////////////////////////////// @@ -865,8 +864,6 @@ void SimulationRunner::LoadPlugin(const Entity _entity, const sdf::ElementPtr &_sdf) { this->systemMgr->LoadPlugin(_entity, _fname, _name, _sdf); - this->systemMgr->ConfigurePendingSystems( - this->entityCompMgr, this->eventMgr); } ////////////////////////////////////////////////// diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index add7a6b63b..d3bc2de963 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -389,9 +389,6 @@ namespace ignition /// server is in the run state. private: std::atomic running{false}; - /// \brief Manager of all systems. - private: std::unique_ptr systemMgr; - /// \brief Manager of all events. /// Note: must be before EntityComponentManager private: EventManager eventMgr; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index b3dfa2b392..c159a2515d 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -21,8 +21,12 @@ using namespace ignition; using namespace gazebo; ////////////////////////////////////////////////// -SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader) - : systemLoader(_systemLoader) +SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, + EntityComponentManager *_entityCompMgr, + EventManager *_eventMgr) + : systemLoader(_systemLoader), + entityCompMgr(_entityCompMgr), + eventMgr(_eventMgr) { } @@ -68,28 +72,6 @@ size_t SystemManager::PendingCount() const return this->pendingSystems.size(); } -////////////////////////////////////////////////// -void SystemManager::ConfigurePendingSystems(EntityComponentManager &_ecm, - EventManager &_eventMgr) -{ - std::lock_guard lock(this->systemsMutex); - for (size_t ii = 0; ii < this->pendingSystems.size(); ++ii) - { - if (this->pendingSystemsConfigured[ii]) - continue; - - const auto& system = this->pendingSystems[ii]; - - if (system.configure) - { - system.configure->Configure(system.configureEntity, - system.configureSdf, - _ecm, _eventMgr); - this->pendingSystemsConfigured[ii] = true; - } - } -} - ////////////////////////////////////////////////// size_t SystemManager::ActivatePendingSystems() { @@ -115,7 +97,6 @@ size_t SystemManager::ActivatePendingSystems() } this->pendingSystems.clear(); - this->pendingSystemsConfigured.clear(); return count; } @@ -142,13 +123,17 @@ void SystemManager::AddSystemImpl( Entity _entity, std::shared_ptr _sdf) { - _system.configureEntity = _entity; - _system.configureSdf = _sdf; + // Configure the system, if necessary + if (_system.configure && this->entityCompMgr && this->eventMgr) + { + _system.configure->Configure(_entity, _sdf, + *this->entityCompMgr, + *this->eventMgr); + } // Update callbacks will be handled later, add to queue std::lock_guard lock(this->systemsMutex); this->pendingSystems.push_back(_system); - this->pendingSystemsConfigured.push_back(false); } ////////////////////////////////////////////////// diff --git a/src/SystemManager.hh b/src/SystemManager.hh index f9991cca4a..8b882e50b6 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -42,7 +42,13 @@ namespace ignition /// \brief Constructor /// \param[in] _systemLoader A pointer to a SystemLoader to load plugins /// from files - public: explicit SystemManager(const SystemLoaderPtr &_systemLoader); + /// \param[in] _entityCompMgr Pointer to the entity component manager to + /// be used when configuring new systems + /// \param[in] _eventMgr Pointer to the event manager to be used when + /// configuring new systems + public: explicit SystemManager(const SystemLoaderPtr &_systemLoader, + EntityComponentManager *_entityCompMgr = nullptr, + EventManager *_eventMgr = nullptr); /// \brief Load system plugin for a given entity. /// \param[in] _entity Entity @@ -82,12 +88,6 @@ namespace ignition /// \return The count. public: size_t TotalCount() const; - /// \brief Call the configure call on all pending systems - /// \param[in] _ecm The entity component manager to configure with - /// \param[in] _evetnMgr The event manager to configure with - public: void ConfigurePendingSystems(EntityComponentManager &_ecm, - EventManager &_eventMgr); - /// \brief Move all "pending" systems to "active" state /// \return The number of newly-active systems public: size_t ActivatePendingSystems(); @@ -143,6 +143,12 @@ namespace ignition /// \brief Mutex to protect systemLoader private: std::mutex systemLoaderMutex; + + /// \brief Pointer to associated entity component manager + private: EntityComponentManager *entityCompMgr; + + /// \brief Pointer to associated event manager + private: EventManager *eventMgr; }; } } // namespace gazebo diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index ac0a292aeb..0aeb55f331 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -79,7 +79,7 @@ TEST(SystemManager, Constructor) } ///////////////////////////////////////////////// -TEST(SystemManager, AddSystem) +TEST(SystemManager, AddSystem_NoEcm) { auto loader = std::make_shared(); SystemManager systemMgr(loader); @@ -94,6 +94,71 @@ TEST(SystemManager, AddSystem) auto configSystem = std::make_shared(); systemMgr.AddSystem(configSystem, kNullEntity, nullptr); + + // SystemManager without an ECM/EventmManager will mean no config occurs + EXPECT_EQ(0, configSystem->configured); + + EXPECT_EQ(0u, systemMgr.ActiveCount()); + EXPECT_EQ(1u, systemMgr.PendingCount()); + EXPECT_EQ(1u, systemMgr.TotalCount()); + EXPECT_EQ(0u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + + systemMgr.ActivatePendingSystems(); + EXPECT_EQ(1u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(1u, 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()); + + auto updateSystem = std::make_shared(); + systemMgr.AddSystem(updateSystem, kNullEntity, 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()); + + systemMgr.ActivatePendingSystems(); + EXPECT_EQ(2u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(2u, systemMgr.TotalCount()); + EXPECT_EQ(1u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); +} + +///////////////////////////////////////////////// +TEST(SystemManager, AddSystem_Ecm) +{ + auto loader = std::make_shared(); + + auto ecm = EntityComponentManager(); + auto eventManager = EventManager(); + + SystemManager systemMgr(loader, &ecm, &eventManager); + + EXPECT_EQ(0u, systemMgr.ActiveCount()); + EXPECT_EQ(0u, systemMgr.PendingCount()); + EXPECT_EQ(0u, systemMgr.TotalCount()); + EXPECT_EQ(0u, systemMgr.SystemsConfigure().size()); + EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); + EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); + + auto configSystem = std::make_shared(); + systemMgr.AddSystem(configSystem, kNullEntity, nullptr); + + // Configure called during AddSystem + EXPECT_EQ(1, configSystem->configured); + EXPECT_EQ(0u, systemMgr.ActiveCount()); EXPECT_EQ(1u, systemMgr.PendingCount()); EXPECT_EQ(1u, systemMgr.TotalCount()); @@ -130,3 +195,4 @@ TEST(SystemManager, AddSystem) EXPECT_EQ(1u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); } + From 4cb543ee859fbedb793bf4c18d521652a15dd10c Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 17 Feb 2022 15:00:16 -0600 Subject: [PATCH 4/6] Address reviewer feedback Signed-off-by: Michael Carroll --- src/SimulationRunner.cc | 12 ++++-------- src/SystemManager.cc | 10 ++++------ src/SystemManager.hh | 5 +---- src/SystemManager_TEST.cc | 16 ++++++++-------- src/WorldControl.hh | 5 +++++ 5 files changed, 22 insertions(+), 26 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index f34a21cafc..2840b3f0ea 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -456,10 +456,8 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system, std::optional _entity, std::optional> _sdf) { - auto entity = _entity.has_value() ? _entity.value() - : worldEntity(this->entityCompMgr); - auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element(); - + auto entity = _entity.value_or(worldEntity(this->entityCompMgr)); + auto sdf = _sdf.value_or(this->sdfWorld->Element()); this->systemMgr->AddSystem(_system, entity, sdf); } @@ -469,10 +467,8 @@ void SimulationRunner::AddSystem( std::optional _entity, std::optional> _sdf) { - auto entity = _entity.has_value() ? _entity.value() - : worldEntity(this->entityCompMgr); - auto sdf = _sdf.has_value() ? _sdf.value() : this->sdfWorld->Element(); - + auto entity = _entity.value_or(worldEntity(this->entityCompMgr)); + auto sdf = _sdf.value_or(this->sdfWorld->Element()); this->systemMgr->AddSystem(_system, entity, sdf); } diff --git a/src/SystemManager.cc b/src/SystemManager.cc index c159a2515d..ba716f99a9 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -54,28 +54,26 @@ void SystemManager::LoadPlugin(const Entity _entity, ////////////////////////////////////////////////// size_t SystemManager::TotalCount() const { - std::lock_guard lock(this->systemsMutex); - return this->systems.size() + this->pendingSystems.size(); + return this->ActiveCount() + this->PendingCount(); } ////////////////////////////////////////////////// size_t SystemManager::ActiveCount() const { - std::lock_guard lock(this->systemsMutex); return this->systems.size(); } ////////////////////////////////////////////////// size_t SystemManager::PendingCount() const { - std::lock_guard lock(this->systemsMutex); + std::lock_guard lock(this->pendingSystemsMutex); return this->pendingSystems.size(); } ////////////////////////////////////////////////// size_t SystemManager::ActivatePendingSystems() { - std::lock_guard lock(this->systemsMutex); + std::lock_guard lock(this->pendingSystemsMutex); auto count = this->pendingSystems.size(); @@ -132,7 +130,7 @@ void SystemManager::AddSystemImpl( } // Update callbacks will be handled later, add to queue - std::lock_guard lock(this->systemsMutex); + std::lock_guard lock(this->pendingSystemsMutex); this->pendingSystems.push_back(_system); } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 8b882e50b6..a1813b1f14 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -120,11 +120,8 @@ namespace ignition /// \brief Pending systems to be added to systems. private: std::vector pendingSystems; - /// \brief Mark if a pending system has been configured - private: std::vector pendingSystemsConfigured; - /// \brief Mutex to protect pendingSystems - private: mutable std::mutex systemsMutex; + private: mutable std::mutex pendingSystemsMutex; /// \brief Systems implementing Configure private: std::vector systemsConfigure; diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index 0aeb55f331..4fc7288580 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -28,7 +28,7 @@ using namespace ignition::gazebo; ///////////////////////////////////////////////// -class System_WithConfigure: +class SystemWithConfigure: public System, public ISystemConfigure { @@ -43,7 +43,7 @@ class System_WithConfigure: }; ///////////////////////////////////////////////// -class System_WithUpdates: +class SystemWithUpdates: public System, public ISystemPreUpdate, public ISystemUpdate, @@ -79,7 +79,7 @@ TEST(SystemManager, Constructor) } ///////////////////////////////////////////////// -TEST(SystemManager, AddSystem_NoEcm) +TEST(SystemManager, AddSystemNoEcm) { auto loader = std::make_shared(); SystemManager systemMgr(loader); @@ -92,7 +92,7 @@ TEST(SystemManager, AddSystem_NoEcm) EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); - auto configSystem = std::make_shared(); + auto configSystem = std::make_shared(); systemMgr.AddSystem(configSystem, kNullEntity, nullptr); // SystemManager without an ECM/EventmManager will mean no config occurs @@ -115,7 +115,7 @@ TEST(SystemManager, AddSystem_NoEcm) EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); - auto updateSystem = std::make_shared(); + auto updateSystem = std::make_shared(); systemMgr.AddSystem(updateSystem, kNullEntity, nullptr); EXPECT_EQ(1u, systemMgr.ActiveCount()); EXPECT_EQ(1u, systemMgr.PendingCount()); @@ -136,7 +136,7 @@ TEST(SystemManager, AddSystem_NoEcm) } ///////////////////////////////////////////////// -TEST(SystemManager, AddSystem_Ecm) +TEST(SystemManager, AddSystemEcm) { auto loader = std::make_shared(); @@ -153,7 +153,7 @@ TEST(SystemManager, AddSystem_Ecm) EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); - auto configSystem = std::make_shared(); + auto configSystem = std::make_shared(); systemMgr.AddSystem(configSystem, kNullEntity, nullptr); // Configure called during AddSystem @@ -176,7 +176,7 @@ TEST(SystemManager, AddSystem_Ecm) EXPECT_EQ(0u, systemMgr.SystemsUpdate().size()); EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size()); - auto updateSystem = std::make_shared(); + auto updateSystem = std::make_shared(); systemMgr.AddSystem(updateSystem, kNullEntity, nullptr); EXPECT_EQ(1u, systemMgr.ActiveCount()); EXPECT_EQ(1u, systemMgr.PendingCount()); diff --git a/src/WorldControl.hh b/src/WorldControl.hh index bb0005e093..9016e8c8f4 100644 --- a/src/WorldControl.hh +++ b/src/WorldControl.hh @@ -17,6 +17,11 @@ #ifndef IGNITION_GAZEBO_WORLDCONTROL_HH_ #define IGNITION_GAZEBO_WORLDCONTROL_HH_ +#include +#include + +#include "ignition/gazebo/config.hh" + namespace ignition { namespace gazebo From 74ce41ba7612402466d6fe0d07085657b672aad9 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 25 Feb 2022 11:14:57 -0600 Subject: [PATCH 5/6] Fix another sequencing issue Signed-off-by: Michael Carroll --- src/SimulationRunner.hh | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index d3bc2de963..351dfe6fa0 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -389,14 +389,18 @@ namespace ignition /// server is in the run state. private: std::atomic running{false}; - /// \brief Manager of all events. - /// Note: must be before EntityComponentManager - private: EventManager eventMgr; - /// \brief Manager of all systems. /// Note: must be before EntityComponentManager + /// Note: must be before EventMgr + /// Because systems have access to the ECM and Events, they need to be + /// cleanly stopped and destructed before destroying the event manager + /// and entity component manager. private: std::unique_ptr systemMgr; + /// \brief Manager of all events. + /// Note: must be before EntityComponentManager + private: EventManager eventMgr; + /// \brief Manager of all components. private: EntityComponentManager entityCompMgr; From 4d05ad5a20153a6dea2632d2e7b89d8e1125fc17 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 25 Feb 2022 12:13:07 -0600 Subject: [PATCH 6/6] Hide the system manager on Windows Signed-off-by: Michael Carroll --- src/SystemManager.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SystemManager.hh b/src/SystemManager.hh index a1813b1f14..9c38505922 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -37,7 +37,7 @@ namespace ignition inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Used to load / unload sysetms as well as iterate over them. - class SystemManager + class IGNITION_GAZEBO_VISIBLE SystemManager { /// \brief Constructor /// \param[in] _systemLoader A pointer to a SystemLoader to load plugins