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..2840b3f0ea 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,10 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, static_cast(this->stepSize.count() / this->desiredRtf)); } + // Create the system manager + this->systemMgr = std::make_unique(_systemLoader, + &this->entityCompMgr, &this->eventMgr); + this->pauseConn = this->eventMgr.Connect( std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1)); @@ -173,7 +174,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 +456,9 @@ void SimulationRunner::AddSystem(const SystemPluginPtr &_system, std::optional _entity, std::optional> _sdf) { - this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); + auto entity = _entity.value_or(worldEntity(this->entityCompMgr)); + auto sdf = _sdf.value_or(this->sdfWorld->Element()); + this->systemMgr->AddSystem(_system, entity, sdf); } ////////////////////////////////////////////////// @@ -464,104 +467,57 @@ void SimulationRunner::AddSystem( std::optional _entity, std::optional> _sdf) { - this->AddSystemImpl(SystemInternal(_system), _entity, _sdf); -} - -////////////////////////////////////////////////// -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); + auto entity = _entity.value_or(worldEntity(this->entityCompMgr)); + auto sdf = _sdf.value_or(this->sdfWorld->Element()); + this->systemMgr->AddSystem(_system, entity, sdf); } ///////////////////////////////////////////////// -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 +533,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 +859,7 @@ 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); } ////////////////////////////////////////////////// @@ -1085,8 +1029,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 5da194e209..351dfe6fa0 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 "SystemManager.hh" #include "Barrier.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 @@ -457,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(); @@ -486,25 +389,16 @@ 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. + /// 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. @@ -536,12 +430,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 new file mode 100644 index 0000000000..13158688c3 --- /dev/null +++ b/src/SystemInternal.hh @@ -0,0 +1,107 @@ +/* + * 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 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; + }; + } + } // namespace gazebo +} // namespace ignition +#endif // IGNITION_GAZEBO_SYSTEMINTERNAL_HH_ + diff --git a/src/SystemManager.cc b/src/SystemManager.cc new file mode 100644 index 0000000000..ba716f99a9 --- /dev/null +++ b/src/SystemManager.cc @@ -0,0 +1,159 @@ +/* + * 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, + EntityComponentManager *_entityCompMgr, + EventManager *_eventMgr) + : systemLoader(_systemLoader), + entityCompMgr(_entityCompMgr), + eventMgr(_eventMgr) +{ +} + +////////////////////////////////////////////////// +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 +{ + return this->ActiveCount() + this->PendingCount(); +} + +////////////////////////////////////////////////// +size_t SystemManager::ActiveCount() const +{ + return this->systems.size(); +} + +////////////////////////////////////////////////// +size_t SystemManager::PendingCount() const +{ + std::lock_guard lock(this->pendingSystemsMutex); + return this->pendingSystems.size(); +} + +////////////////////////////////////////////////// +size_t SystemManager::ActivatePendingSystems() +{ + std::lock_guard lock(this->pendingSystemsMutex); + + 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(); + 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) +{ + // 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->pendingSystemsMutex); + this->pendingSystems.push_back(_system); +} + +////////////////////////////////////////////////// +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..9c38505922 --- /dev/null +++ b/src/SystemManager.hh @@ -0,0 +1,153 @@ +/* + * 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 IGNITION_GAZEBO_VISIBLE SystemManager + { + /// \brief Constructor + /// \param[in] _systemLoader A pointer to a SystemLoader to load plugins + /// from files + /// \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 + /// \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 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 Mutex to protect pendingSystems + private: mutable std::mutex pendingSystemsMutex; + + /// \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; + + /// \brief Pointer to associated entity component manager + private: EntityComponentManager *entityCompMgr; + + /// \brief Pointer to associated event manager + private: EventManager *eventMgr; + }; + } + } // 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..4fc7288580 --- /dev/null +++ b/src/SystemManager_TEST.cc @@ -0,0 +1,198 @@ +/* + * 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 SystemWithConfigure: + public System, + public ISystemConfigure +{ + // Documentation inherited + public: void Configure( + const Entity &, + const std::shared_ptr &, + EntityComponentManager &, + EventManager &) override { configured++; }; + + public: int configured = 0; +}; + +///////////////////////////////////////////////// +class SystemWithUpdates: + 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, AddSystemNoEcm) +{ + 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); + + // 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, AddSystemEcm) +{ + 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()); + 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()); +} + diff --git a/src/WorldControl.hh b/src/WorldControl.hh new file mode 100644 index 0000000000..9016e8c8f4 --- /dev/null +++ b/src/WorldControl.hh @@ -0,0 +1,61 @@ +/* + * 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_ + +#include +#include + +#include "ignition/gazebo/config.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_