From 11e029a0f47bbf04c584e5a7ce770b44b24b3c5b Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 15 Feb 2022 11:22:54 -0600 Subject: [PATCH 01/20] 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 a52210425e..0dfabbfbba 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 56f28bd487d822f563ce322b89a528c1592e59ab Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 15 Feb 2022 13:24:58 -0600 Subject: [PATCH 02/20] Move System interaction to SystemManager Signed-off-by: Michael Carroll --- src/CMakeLists.txt | 2 + src/SimulationRunner.cc | 154 +++++++++++---------------------- src/SimulationRunner.hh | 44 ++-------- src/SystemInternal.hh | 8 ++ src/SystemManager.cc | 176 ++++++++++++++++++++++++++++++++++++++ src/SystemManager.hh | 150 ++++++++++++++++++++++++++++++++ src/SystemManager_TEST.cc | 132 ++++++++++++++++++++++++++++ 7 files changed, 526 insertions(+), 140 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 0dfabbfbba..d3bc2de963 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,30 +389,14 @@ 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 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 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; @@ -452,12 +426,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 c35e3ce3f49f491c6fa2238ca2ab91d727f8047c Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 16 Feb 2022 12:16:32 -0600 Subject: [PATCH 03/20] Adjustments for Fortress Signed-off-by: Michael Carroll --- src/SimulationRunner.cc | 7 ++-- src/SystemManager.cc | 41 ++++++++--------------- src/SystemManager.hh | 20 ++++++++---- src/SystemManager_TEST.cc | 68 ++++++++++++++++++++++++++++++++++++++- 4 files changed, 95 insertions(+), 41 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/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 8266e73b37deb9ceea277b74bc33778b33fcdf90 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 17 Feb 2022 15:00:16 -0600 Subject: [PATCH 04/20] 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 ca9102309598e948a19adfed7dc6d465e57ed373 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 8 Feb 2022 11:58:52 -0600 Subject: [PATCH 05/20] Introduce System::Reset API Co-authored-by: Addisu Z. Taddese Signed-off-by: Michael Carroll --- include/ignition/gazebo/System.hh | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/ignition/gazebo/System.hh b/include/ignition/gazebo/System.hh index 67d7f1a00b..178dea43f9 100644 --- a/include/ignition/gazebo/System.hh +++ b/include/ignition/gazebo/System.hh @@ -100,6 +100,11 @@ namespace ignition EventManager &_eventMgr) = 0; }; + class ISystemReset { + public: virtual void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) = 0; + }; + /// \class ISystemPreUpdate ISystem.hh ignition/gazebo/System.hh /// \brief Interface for a system that uses the PreUpdate phase class ISystemPreUpdate { From 85cb36e6bbf39b4f981f6e7bdaf4190d02ab774c Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 8 Feb 2022 12:01:26 -0600 Subject: [PATCH 06/20] SimulationRunner Reset implementation Co-authored-by: Addisu Z. Taddese Signed-off-by: Michael Carroll --- .../ignition/gazebo/EntityComponentManager.hh | 28 +++ include/ignition/gazebo/detail/BaseView.hh | 3 + include/ignition/gazebo/detail/View.hh | 10 + src/CMakeLists.txt | 1 + src/EntityComponentManager.cc | 144 +++++++++++++++ src/EntityComponentManagerDiff.cc | 59 ++++++ src/EntityComponentManagerDiff.hh | 52 ++++++ src/EntityComponentManager_TEST.cc | 174 ++++++++++++++++++ src/SimulationRunner.cc | 20 +- src/SimulationRunner.hh | 5 + src/SystemInternal.hh | 6 + src/SystemManager.cc | 9 + src/SystemManager.hh | 6 + 13 files changed, 516 insertions(+), 1 deletion(-) create mode 100644 src/EntityComponentManagerDiff.cc create mode 100644 src/EntityComponentManagerDiff.hh diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 87fdb22c4e..24b79d139d 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -49,6 +49,7 @@ namespace ignition inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { // Forward declarations. class IGNITION_GAZEBO_HIDDEN EntityComponentManagerPrivate; + class EntityComponentManagerDiff; /// \brief Type alias for the graph that holds entities. /// Each vertex is an entity, and the direction points from the parent to @@ -71,6 +72,8 @@ namespace ignition /// \brief Destructor public: ~EntityComponentManager(); + public: void Copy(const EntityComponentManager &_fromEcm); + /// \brief Creates a new Entity. /// \return An id for the Entity, or kNullEntity on failure. public: Entity CreateEntity(); @@ -669,6 +672,13 @@ namespace ignition /// \param[in] _offset Offset value. public: void SetEntityCreateOffset(uint64_t _offset); + /// \brief Given a diff, apply it to this ECM. Note that for removed + /// entities, this would mark them for removal instead of actually + /// removing the entities. + /// \param[in] _other Original EntityComponentManager from which the diff + /// was computed. + public: void ResetTo(const EntityComponentManager &_other); + /// \brief Return true if there are components marked for removal. /// \return True if there are components marked for removal. public: bool HasRemovedComponents() const; @@ -690,6 +700,24 @@ namespace ignition /// \brief Mark all components as not changed. protected: void SetAllComponentsUnchanged(); + /// Compute the diff between this EntityComponentManager and _other at the + /// entity level. + /// * If an entity is in `_other`, but not in `this`, insert the entity + /// as an "added" entity. + /// * If an entity is in `this`, but not in `other`, insert the entity + /// as a "removed" entity. + /// \return Data structure containing the added and removed entities + protected: EntityComponentManagerDiff ComputeDiff( + const EntityComponentManager &_other) const; + + /// \brief Given a diff, apply it to this ECM. Note that for removed + /// entities, this would mark them for removal instead of actually + /// removing the entities. + /// \param[in] _other Original EntityComponentManager from which the diff + /// was computed. + protected: void ApplyDiff(const EntityComponentManager &_other, + const EntityComponentManagerDiff &_diff); + /// \brief Get whether an Entity exists and is new. /// /// Entities are considered new in the time between their creation and a diff --git a/include/ignition/gazebo/detail/BaseView.hh b/include/ignition/gazebo/detail/BaseView.hh index eea6755316..7f08090b80 100644 --- a/include/ignition/gazebo/detail/BaseView.hh +++ b/include/ignition/gazebo/detail/BaseView.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_DETAIL_BASEVIEW_HH_ #include +#include #include #include #include @@ -187,6 +188,8 @@ class IGNITION_GAZEBO_VISIBLE BaseView /// \sa ToAddEntities public: void ClearToAddEntities(); + public: virtual std::unique_ptr Clone() const = 0; + // TODO(adlarkin) make this a std::unordered_set for better performance. // We need to make sure nothing else depends on the ordered preserved by // std::set first diff --git a/include/ignition/gazebo/detail/View.hh b/include/ignition/gazebo/detail/View.hh index 5589fbc02f..23eccf7303 100644 --- a/include/ignition/gazebo/detail/View.hh +++ b/include/ignition/gazebo/detail/View.hh @@ -17,6 +17,7 @@ #ifndef IGNITION_GAZEBO_DETAIL_VIEW_HH_ #define IGNITION_GAZEBO_DETAIL_VIEW_HH_ +#include #include #include #include @@ -105,6 +106,8 @@ class View : public BaseView /// \brief Documentation inherited public: void Reset() override; + public: std::unique_ptr Clone() const override; + /// \brief A map of entities to their component data. Since tuples are defined /// at compile time, we need separate containers that have tuples for both /// non-const and const component pointers (calls to ECM::Each can have a @@ -329,6 +332,13 @@ void View::Reset() this->invalidConstData.clear(); this->missingCompTracker.clear(); } + +////////////////////////////////////////////////// +template +std::unique_ptr View::Clone() const +{ + return std::make_unique>(*this); +} } // namespace detail } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE } // namespace gazebo diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c609bec040..11b979f42b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -49,6 +49,7 @@ set (sources BaseView.cc Conversions.cc EntityComponentManager.cc + EntityComponentManagerDiff.cc LevelManager.cc Link.cc Model.cc diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index c4e195cd47..5575fc2b8f 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -16,6 +16,7 @@ */ #include "ignition/gazebo/EntityComponentManager.hh" +#include "EntityComponentManagerDiff.hh" #include #include @@ -71,6 +72,8 @@ class ignition::gazebo::EntityComponentManagerPrivate /// `AddEntityToMessage`. public: void CalculateStateThreadLoad(); + public: void Copy(const EntityComponentManagerPrivate &_from); + /// \brief Create a message for the removed components /// \param[in] _entity Entity with the removed components /// \param[in, out] _msg Entity message @@ -287,6 +290,47 @@ EntityComponentManager::EntityComponentManager() ////////////////////////////////////////////////// EntityComponentManager::~EntityComponentManager() = default; +// ////////////////////////////////////////////////// +// EntityComponentManager::EntityComponentManager( +// EntityComponentManager &&_ecm) noexcept = default; + +////////////////////////////////////////////////// +void EntityComponentManagerPrivate::Copy( + const EntityComponentManagerPrivate &_from) +{ + this->createdCompTypes = _from.createdCompTypes; + this->entities = _from.entities; + this->periodicChangedComponents = _from.periodicChangedComponents; + this->oneTimeChangedComponents = _from.oneTimeChangedComponents; + this->newlyCreatedEntities = _from.newlyCreatedEntities; + this->toRemoveEntities = _from.toRemoveEntities; + this->modifiedComponents = _from.modifiedComponents; + this->removeAllEntities = _from.removeAllEntities; + this->views.clear(); + this->lockAddEntitiesToViews = _from.lockAddEntitiesToViews; + this->descendantCache.clear(); + this->entityCount = _from.entityCount; + this->removedComponents = _from.removedComponents; + this->componentsMarkedAsRemoved = _from.componentsMarkedAsRemoved; + + for (const auto &[entity, comps] : _from.componentStorage) + { + this->componentStorage[entity].clear(); + for (const auto &comp : comps) + { + this->componentStorage[entity].emplace_back(comp->Clone()); + } + } + this->componentTypeIndex = _from.componentTypeIndex; + this->componentTypeIndexIterators.clear(); + this->componentTypeIndexDirty = true; + + // Not copying maps related to cloning since they are transient variables + // that are used as return values of some member functions. + + this->pinnedEntities = _from.pinnedEntities; +} + ////////////////////////////////////////////////// size_t EntityComponentManager::EntityCount() const { @@ -2068,3 +2112,103 @@ void EntityComponentManager::UnpinAllEntities() { this->dataPtr->pinnedEntities.clear(); } + +///////////////////////////////////////////////// +void EntityComponentManager::Copy(const EntityComponentManager &_fromEcm) +{ + this->dataPtr->Copy(*_fromEcm.dataPtr); +} + +///////////////////////////////////////////////// +EntityComponentManagerDiff EntityComponentManager::ComputeDiff( + const EntityComponentManager &_other) const +{ + EntityComponentManagerDiff diff; + for (const auto &item : _other.dataPtr->entities.Vertices()) + { + const auto &v = item.second.get(); + if (!this->dataPtr->entities.VertexFromId(v.Id()).Valid()) + { + // In `_other` but not in `this`, so insert the entity as an "added" + // entity. + diff.InsertAddedEntity(v.Data()); + } + } + + for (const auto &item : this->dataPtr->entities.Vertices()) + { + const auto &v = item.second.get(); + if (!_other.dataPtr->entities.VertexFromId(v.Id()).Valid()) + { + // In `this` but not in `other`, so insert the entity as a "removed" + // entity. + diff.InsertRemovedEntity(v.Data()); + } + } + return diff; +} + +///////////////////////////////////////////////// +void EntityComponentManager::ApplyDiff(const EntityComponentManager &_other, + const EntityComponentManagerDiff &_diff) +{ + auto copyComponents = [&](Entity _entity) + { + for (const auto compTypeId : _other.ComponentTypes(_entity)) + { + const components::BaseComponent *data = + _other.ComponentImplementation(_entity, compTypeId); + this->CreateComponentImplementation(_entity, compTypeId, + data->Clone().get()); + } + }; + + for(auto entity : _diff.AddedEntities()) + { + if (!this->HasEntity(entity)) + { + this->dataPtr->CreateEntityImplementation(entity); + if (entity >= this->dataPtr->entityCount) + { + this->dataPtr->entityCount = entity; + } + copyComponents(entity); + this->SetParentEntity(entity, _other.ParentEntity(entity)); + } + } + + for (const auto &entity : _diff.RemovedEntities()) + { + // if the entity is not in this ECM, add it before requesting for its + // removal. + if (!this->HasEntity(entity)) + { + this->dataPtr->CreateEntityImplementation(entity); + this->RequestRemoveEntity(entity, false); + // We want to set this entity as "removed", but + // CreateEntityImplementation sets it as "newlyCreated", + // so remove it from that list. + { + std::lock_guard lock(this->dataPtr->entityCreatedMutex); + this->dataPtr->newlyCreatedEntities.erase(entity); + } + // Copy components so that EachRemoved match correctly + if (entity >= this->dataPtr->entityCount) + { + this->dataPtr->entityCount = entity; + } + copyComponents(entity); + this->SetParentEntity(entity, _other.ParentEntity(entity)); + } + } +} + +///////////////////////////////////////////////// +void EntityComponentManager::ResetTo(const EntityComponentManager &_other) +{ + auto ecmDiff = this->ComputeDiff(_other); + EntityComponentManager tmpCopy; + tmpCopy.Copy(_other); + tmpCopy.ApplyDiff(*this, ecmDiff); + this->Copy(tmpCopy); +} diff --git a/src/EntityComponentManagerDiff.cc b/src/EntityComponentManagerDiff.cc new file mode 100644 index 0000000000..6be7a87b95 --- /dev/null +++ b/src/EntityComponentManagerDiff.cc @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2021 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 "EntityComponentManagerDiff.hh" + +#include "ignition/gazebo/Entity.hh" + +using namespace ignition; +using namespace gazebo; + +////////////////////////////////////////////////// +void EntityComponentManagerDiff::InsertAddedEntity(const Entity &_entity) +{ + this->addedEntities.push_back(_entity); +} + +////////////////////////////////////////////////// +void EntityComponentManagerDiff::InsertRemovedEntity(const Entity &_entity) +{ + this->removedEntities.push_back(_entity); +} + +////////////////////////////////////////////////// +const std::vector &EntityComponentManagerDiff::AddedEntities() const +{ + return this->addedEntities; +} + +////////////////////////////////////////////////// +const std::vector &EntityComponentManagerDiff::RemovedEntities() const +{ + return this->removedEntities; +} + +////////////////////////////////////////////////// +void EntityComponentManagerDiff::ClearAddedEntities() +{ + this->addedEntities.clear(); +} + +////////////////////////////////////////////////// +void EntityComponentManagerDiff::ClearRemovedEntities() +{ + this->removedEntities.clear(); +} diff --git a/src/EntityComponentManagerDiff.hh b/src/EntityComponentManagerDiff.hh new file mode 100644 index 0000000000..fce282493d --- /dev/null +++ b/src/EntityComponentManagerDiff.hh @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2021 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_ENTITYCOMPONENTMANAGER_DIFF_HH_ +#define IGNITION_GAZEBO_ENTITYCOMPONENTMANAGER_DIFF_HH_ + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Export.hh" +#include "ignition/gazebo/Types.hh" + +#include + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + + class EntityComponentManagerDiff + { + public: void InsertAddedEntity(const Entity &_entity); + public: void InsertRemovedEntity(const Entity &_entity); + + public: const std::vector &RemovedEntities() const; + public: const std::vector &AddedEntities() const; + + public: void ClearAddedEntities(); + public: void ClearRemovedEntities(); + + private: std::vector addedEntities; + private: std::vector removedEntities; + }; + } + } +} + +#endif diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 925a556b51..366c4e1202 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -35,6 +35,7 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/config.hh" +#include "EntityComponentManagerDiff.hh" #include "../test/helpers/EnvTestFixture.hh" using namespace ignition; @@ -106,6 +107,18 @@ class EntityCompMgrTest : public EntityComponentManager { this->ClearRemovedComponents(); } + + public: EntityComponentManagerDiff RunComputeDiff( + const EntityComponentManager &_other) const + { + return this->ComputeDiff(_other); + } + + public: void RunApplyDiff(const EntityComponentManager &_other, + const EntityComponentManagerDiff &_diff) + { + this->ApplyDiff(_other, _diff); + } }; class EntityComponentManagerFixture @@ -3150,6 +3163,167 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(1, foundEntities); } +TEST_P(EntityComponentManagerFixture, CopyEcm) +{ + Entity entity = manager.CreateEntity(); + math::Pose3d testPose{1, 2, 3, 0.1, 0.2, 0.3}; + manager.CreateComponent(entity, components::Pose{testPose}); + + EntityCompMgrTest managerCopy; + managerCopy.Copy(manager); + EXPECT_EQ(manager.EntityCount(), managerCopy.EntityCount()); + EXPECT_TRUE(managerCopy.HasEntity(entity)); + EXPECT_TRUE( + managerCopy.EntityHasComponentType(entity, components::Pose::typeId)); + managerCopy.EachNew( + [&](const Entity &_entity, const components::Pose *_pose) + { + EXPECT_EQ(_entity, entity); + EXPECT_EQ(testPose, _pose->Data()); + return true; + }); +} + +TEST_P(EntityComponentManagerFixture, ComputDiff) +{ + Entity entity1 = manager.CreateEntity(); + math::Pose3d testPose{1, 2, 3, 0.1, 0.2, 0.3}; + manager.CreateComponent(entity1, components::Pose{testPose}); + + EntityCompMgrTest managerCopy; + managerCopy.Copy(manager); + + Entity entity2 = manager.CreateEntity(); + manager.CreateComponent(entity2, components::StringComponent{"Entity2"}); + + manager.RunClearNewlyCreatedEntities(); + + // manager now has: + // - entity1 [Pose] + // - entity2 [StringComponent] + // managerCopy has + // - entity1 [Pose] + + { + EntityComponentManagerDiff diff = managerCopy.RunComputeDiff(manager); + EXPECT_EQ(1u, diff.AddedEntities().size()); + EXPECT_EQ(0u, diff.RemovedEntities().size()); + } + + // Now add another component to managerCopy. We should expect one more entity + // in RemovedEntities + managerCopy.SetEntityCreateOffset(10); + managerCopy.CreateEntity(); + { + EntityComponentManagerDiff diff = managerCopy.RunComputeDiff(manager); + EXPECT_EQ(1u, diff.AddedEntities().size()); + EXPECT_EQ(1u, diff.RemovedEntities().size()); + + diff.ClearRemovedEntities(); + EXPECT_EQ(1u, diff.AddedEntities().size()); + EXPECT_EQ(0u, diff.RemovedEntities().size()); + } + + { + EntityComponentManagerDiff diff = managerCopy.RunComputeDiff(manager); + EXPECT_EQ(1u, diff.RemovedEntities().size()); + managerCopy.RunApplyDiff(manager, diff); + EXPECT_TRUE(managerCopy.HasEntitiesMarkedForRemoval()); + } +} + +TEST_P(EntityComponentManagerFixture, ResetToWithDeletedEntity) +{ + Entity entity1 = manager.CreateEntity(); + math::Pose3d testPose{1, 2, 3, 0.1, 0.2, 0.3}; + manager.CreateComponent(entity1, components::Pose{testPose}); + manager.CreateComponent(entity1, components::Name{"entity1"}); + + Entity entity2 = manager.CreateEntity(); + manager.CreateComponent(entity2, components::Name{"entity2"}); + + { + std::vector newEntities; + manager.EachNew( + [&](const Entity &_entity, const components::Name *) + { + newEntities.push_back(_entity); + return true; + }); + ASSERT_EQ(2u, newEntities.size()); + } + + EntityCompMgrTest managerCopy; + managerCopy.Copy(manager); + + manager.RequestRemoveEntity(entity1); + + // Emulate a step so that entity1 can be actually removed. + manager.RunClearNewlyCreatedEntities(); + manager.ProcessEntityRemovals(); + manager.RunClearRemovedComponents(); + manager.RunSetAllComponentsUnchanged(); + + EXPECT_FALSE(manager.HasNewEntities()); + + // Now reset to the copy + manager.ResetTo(managerCopy); + EXPECT_TRUE(manager.HasNewEntities()); + + { + std::vector newEntities; + manager.EachNew( + [&](const Entity &_entity, const components::Name *) + { + newEntities.push_back(_entity); + return true; + }); + ASSERT_EQ(2u, newEntities.size()); + } +} + +TEST_P(EntityComponentManagerFixture, ResetToWithAddedEntity) +{ + Entity entity1 = manager.CreateEntity(); + math::Pose3d testPose{1, 2, 3, 0.1, 0.2, 0.3}; + manager.CreateComponent(entity1, components::Pose{testPose}); + manager.CreateComponent(entity1, components::Name{"entity1"}); + + Entity entity2 = manager.CreateEntity(); + manager.CreateComponent(entity2, components::Name{"entity2"}); + + EntityCompMgrTest managerCopy; + managerCopy.Copy(manager); + + // Add entity3 after a copy has been made. + Entity entity3 = manager.CreateEntity(); + manager.CreateComponent(entity3, components::Name{"entity3"}); + + // Emulate a step so that entity1 can be actually removed. + manager.RunClearNewlyCreatedEntities(); + manager.ProcessEntityRemovals(); + manager.RunClearRemovedComponents(); + manager.RunSetAllComponentsUnchanged(); + + EXPECT_FALSE(manager.HasNewEntities()); + + // Now reset to the copy + manager.ResetTo(managerCopy); + EXPECT_TRUE(manager.HasNewEntities()); + + { + std::vector removedEntities; + manager.EachRemoved( + [&](const Entity &_entity, const components::Name *) + { + removedEntities.push_back(_entity); + return true; + }); + ASSERT_EQ(1u, removedEntities.size()); + EXPECT_EQ(entity3, removedEntities.front()); + } +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat, diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 2840b3f0ea..2f90442ce0 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -169,6 +169,9 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // Load the active levels this->levelMgr->UpdateLevelsState(); + // Store the initial state of the ECM; + this->initialEntityCompMgr.Copy(this->entityCompMgr); + // Load any additional plugins from the Server Configuration this->LoadServerPlugins(this->serverConfig.Plugins()); @@ -264,7 +267,7 @@ void SimulationRunner::UpdateCurrentInfo() this->realTimeWatch.Reset(); if (!this->currentInfo.paused) this->realTimeWatch.Start(); - + this->resetInitiated = true; this->requestedRewind = false; return; @@ -531,6 +534,14 @@ void SimulationRunner::UpdateSystems() // WorkerPool.cc). We could turn on parallel updates in the future, and/or // turn it on if there are sufficient systems. More testing is required. + if (this->resetInitiated) + { + IGN_PROFILE("Reset"); + for (auto &system : this->systemsReset) + system->Reset(this->currentInfo, this->entityCompMgr); + return; + } + { IGN_PROFILE("PreUpdate"); for (auto& system : this->systemMgr->SystemsPreUpdate()) @@ -748,6 +759,11 @@ bool SimulationRunner::Run(const uint64_t _iterations) // Update time information. This will update the iteration count, RTF, // and other values. this->UpdateCurrentInfo(); + if (this->resetInitiated) + { + this->entityCompMgr.ResetTo(this->initialEntityCompMgr); + } + if (!this->currentInfo.paused) { processedIterations++; @@ -772,6 +788,8 @@ bool SimulationRunner::Run(const uint64_t _iterations) this->currentInfo.iterations++; this->blockingPausedStepPending = false; } + + this->resetInitiated = false; } this->running = false; diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index d3bc2de963..a9fa9dc7c9 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -400,6 +400,10 @@ namespace ignition /// \brief Manager of all components. private: EntityComponentManager entityCompMgr; + /// \brief Copy of the EntityComponentManager immediately after the + /// initial entity creation/world load. + private: EntityComponentManager initialEntityCompMgr; + /// \brief Manager of all levels. private: std::unique_ptr levelMgr; @@ -528,6 +532,7 @@ namespace ignition /// at the appropriate time. private: std::unique_ptr newWorldControlState; + private: bool resetInitiated{false}; friend class LevelManager; }; } diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 13158688c3..f8703f098f 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -43,6 +43,7 @@ namespace ignition : systemPlugin(std::move(_systemPlugin)), system(systemPlugin->QueryInterface()), configure(systemPlugin->QueryInterface()), + reset(systemPlugin->QueryInterface()), preupdate(systemPlugin->QueryInterface()), update(systemPlugin->QueryInterface()), postupdate(systemPlugin->QueryInterface()) @@ -55,6 +56,7 @@ namespace ignition : systemShared(_system), system(_system.get()), configure(dynamic_cast(_system.get())), + reset(dynamic_cast(_system.get())), preupdate(dynamic_cast(_system.get())), update(dynamic_cast(_system.get())), postupdate(dynamic_cast(_system.get())) @@ -77,6 +79,10 @@ namespace ignition /// Will be nullptr if the System doesn't implement this interface. public: ISystemConfigure *configure = nullptr; + /// \brief Access this system via the ISystemReset interface + /// Will be nullptr if the System doesn't implement this interface. + public: ISystemReset *reset = nullptr; + /// \brief Access this system via the ISystemPreUpdate interface /// Will be nullptr if the System doesn't implement this interface. public: ISystemPreUpdate *preupdate = nullptr; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index ba716f99a9..53085641e4 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -84,6 +84,9 @@ size_t SystemManager::ActivatePendingSystems() if (system.configure) this->systemsConfigure.push_back(system.configure); + if (system.reset) + this->systemsReset.push_back(system.reset); + if (system.preupdate) this->systemsPreupdate.push_back(system.preupdate); @@ -140,6 +143,12 @@ const std::vector& SystemManager::SystemsConfigure() return this->systemsConfigure; } +////////////////////////////////////////////////// +const std::vector& SystemManager::SystemsReset() +{ + return this->systemsReset; +} + ////////////////////////////////////////////////// const std::vector& SystemManager::SystemsPreUpdate() { diff --git a/src/SystemManager.hh b/src/SystemManager.hh index a1813b1f14..02c56c5f4d 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -95,6 +95,9 @@ namespace ignition /// \brief Get an vector of all systems implementing "Configure" public: const std::vector& SystemsConfigure(); + /// \brief Get an vector of all systems implementing "Reset" + public: const std::vector& SystemsReset(); + /// \brief Get an vector of all systems implementing "PreUpdate" public: const std::vector& SystemsPreUpdate(); @@ -126,6 +129,9 @@ namespace ignition /// \brief Systems implementing Configure private: std::vector systemsConfigure; + /// \brief Systems implementing Reset + private: std::vector systemsReset; + /// \brief Systems implementing PreUpdate private: std::vector systemsPreupdate; From 144556d423b9a7ad594a80044e1d85c1bfd50592 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 8 Feb 2022 12:03:00 -0600 Subject: [PATCH 07/20] Simulation Reset implementation in Physics system Co-authored-by: Addisu Z. Taddese Signed-off-by: Michael Carroll --- .../physics/CanonicalLinkModelTracker.hh | 19 +- src/systems/physics/Physics.cc | 252 +++++++++++++++--- src/systems/physics/Physics.hh | 5 + 3 files changed, 230 insertions(+), 46 deletions(-) diff --git a/src/systems/physics/CanonicalLinkModelTracker.hh b/src/systems/physics/CanonicalLinkModelTracker.hh index 4f6f01443e..1e2fdd10e9 100644 --- a/src/systems/physics/CanonicalLinkModelTracker.hh +++ b/src/systems/physics/CanonicalLinkModelTracker.hh @@ -58,6 +58,10 @@ namespace systems::physics_system /// \param[in] _ecm EntityComponentManager public: void AddNewModels(const EntityComponentManager &_ecm); + /// \brief Save mappings for all models and their canonical links + /// \param[in] _ecm EntityComponentManager + public: void AddAllModels(const EntityComponentManager &_ecm); + /// \brief Get a topological ordering of models that have a particular /// canonical link /// \param[in] _canonicalLink The canonical link @@ -79,7 +83,7 @@ namespace systems::physics_system /// \brief An empty set of models that is returned from the /// CanonicalLinkModels method for links that map to no models - private: const std::set emptyModelOrdering{}; + private: static inline const std::set kEmptyModelOrdering{}; }; void CanonicalLinkModelTracker::AddNewModels( @@ -93,6 +97,17 @@ namespace systems::physics_system return true; }); } + void CanonicalLinkModelTracker::AddAllModels( + const EntityComponentManager &_ecm) + { + _ecm.Each( + [this](const Entity &_model, const components::Model *, + const components::ModelCanonicalLink *_canonicalLinkComp) + { + this->linkModelMap[_canonicalLinkComp->Data()].insert(_model); + return true; + }); + } const std::set &CanonicalLinkModelTracker::CanonicalLinkModels( const Entity _canonicalLink) const @@ -102,7 +117,7 @@ namespace systems::physics_system return it->second; // if an invalid entity was given, it maps to no models - return this->emptyModelOrdering; + return this->kEmptyModelOrdering; } void CanonicalLinkModelTracker::RemoveLink(const Entity &_link) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 18df36ba58..83d6c7db0d 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -189,27 +189,32 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Create physics entities /// \param[in] _ecm Constant reference to ECM. - public: void CreatePhysicsEntities(const EntityComponentManager &_ecm); + public: void CreatePhysicsEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create world entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateWorldEntities(const EntityComponentManager &_ecm); - + public: void CreateWorldEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create model entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateModelEntities(const EntityComponentManager &_ecm); + public: void CreateModelEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create link entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateLinkEntities(const EntityComponentManager &_ecm); + public: void CreateLinkEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create collision entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateCollisionEntities(const EntityComponentManager &_ecm); + public: void CreateCollisionEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create joint entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateJointEntities(const EntityComponentManager &_ecm); + public: void CreateJointEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create Battery entities /// \param[in] _ecm Constant reference to ECM. @@ -223,6 +228,10 @@ class ignition::gazebo::systems::PhysicsPrivate /// \param[in] _ecm Mutable reference to ECM. public: void UpdatePhysics(EntityComponentManager &_ecm); + /// \brief Reset physics from components + /// \param[in] _ecm Constant reference to ECM. + public: void ResetPhysics(EntityComponentManager &_ecm); + /// \brief Step the simulation for each world /// \param[in] _dt Duration /// \returns Output data from the physics engine (this currently contains @@ -824,14 +833,6 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("Physics::Update"); - // \TODO(anyone) Support rewind - if (_info.dt < std::chrono::steady_clock::duration::zero()) - { - ignwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; - } - if (this->dataPtr->engine) { this->dataPtr->CreatePhysicsEntities(_ecm); @@ -853,23 +854,37 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) +void Physics::Reset(const UpdateInfo &, EntityComponentManager &_ecm) +{ + IGN_PROFILE("Physics::Reset"); + + if (this->dataPtr->engine) + { + igndbg << "Resetting Physics\n"; + this->dataPtr->ResetPhysics(_ecm); + } +} + +////////////////////////////////////////////////// +void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { // Clear the set of links that were added to a model. this->linkAddedToModel.clear(); this->jointAddedToModel.clear(); - this->CreateWorldEntities(_ecm); - this->CreateModelEntities(_ecm); - this->CreateLinkEntities(_ecm); + this->CreateWorldEntities(_ecm, _warnIfEntityExists); + this->CreateModelEntities(_ecm, _warnIfEntityExists); + this->CreateLinkEntities(_ecm, _warnIfEntityExists); // We don't need to add visuals to the physics engine. - this->CreateCollisionEntities(_ecm); - this->CreateJointEntities(_ecm); + this->CreateCollisionEntities(_ecm, _warnIfEntityExists); + this->CreateJointEntities(_ecm, _warnIfEntityExists); this->CreateBatteryEntities(_ecm); } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { // Get all the new worlds _ecm.EachNew( @@ -881,9 +896,12 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) // Check if world already exists if (this->entityWorldMap.HasEntity(_entity)) { - ignwarn << "World entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "World entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -950,7 +968,8 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { _ecm.EachNew( @@ -966,9 +985,12 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) // Check if model already exists if (this->entityModelMap.HasEntity(_entity)) { - ignwarn << "Model entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Model entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } // TODO(anyone) Don't load models unless they have collisions @@ -1085,7 +1107,8 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { _ecm.EachNew( @@ -1110,9 +1133,12 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) // Check if link already exists if (this->entityLinkMap.HasEntity(_entity)) { - ignwarn << "Link entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Link entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -1155,7 +1181,8 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { _ecm.EachNewentityCollisionMap.HasEntity(_entity)) { - ignwarn << "Collision entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Collision entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -1354,7 +1384,8 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { _ecm.EachNewentityJointMap.HasEntity(_entity)) { - ignwarn << "Joint entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Joint entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -1466,9 +1500,12 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) // Check if joint already exists if (this->entityJointMap.HasEntity(_entity)) { - ignwarn << "Joint entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Joint entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -1628,6 +1665,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) } } this->entityLinkMap.Remove(childLink); + this->entityFreeGroupMap.Remove(childLink); this->topLevelModelMap.erase(childLink); this->staticEntities.erase(childLink); this->linkWorldPoses.erase(childLink); @@ -2354,6 +2392,131 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) } // NOLINT readability/fn_size // TODO (azeey) Reduce size of function and remove the NOLINT above +////////////////////////////////////////////////// +void PhysicsPrivate::ResetPhysics(EntityComponentManager &_ecm) +{ + IGN_PROFILE("PhysicsPrivate::ResetPhysics"); + // Clear worldPoseCmdsToRemove because pose commands that were issued before + // the reset will be ignored. + this->linkWorldPoses.clear(); + this->canonicalLinkModelTracker = CanonicalLinkModelTracker(); + this->modelWorldPoses.clear(); + this->worldPoseCmdsToRemove.clear(); + + this->CreatePhysicsEntities(_ecm, false); + this->canonicalLinkModelTracker.AddAllModels(_ecm); + + // Update link pose, linear velocity, and angular velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *) + { + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + { + ignwarn << "Failed to find link [" << _entity << "]." << std::endl; + return true; + } + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + if (freeGroup->RootLink() == linkPtrPhys) + { + auto linkWorldPose = worldPose(_entity, _ecm); + freeGroup->SetWorldPose(math::eigen3::convert(linkWorldPose)); + } + + auto worldAngularVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + + if (!worldAngularVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to reset link angular velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be reset." + << std::endl; + informed = true; + } + return true; + } + else + { + worldAngularVelFeature->SetWorldAngularVelocity( + Eigen::Vector3d::Zero()); + } + + auto worldLinearVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + if (!worldLinearVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link linear velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + else + { + worldLinearVelFeature->SetWorldLinearVelocity( + Eigen::Vector3d::Zero()); + } + + return true; + }); + + // Handle joint state + _ecm.Each( + [&](const Entity &_entity, const components::Joint *) + { + auto jointPhys = this->entityJointMap.Get(_entity); + if (nullptr == jointPhys) + { + ignwarn << "Failed to find joint [" << _entity << "]." << std::endl; + return true; + } + + // Assume initial joint position and velocities are zero + // Reset the velocity + for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); ++i) + { + jointPhys->SetVelocity(i, 0.0); + jointPhys->SetPosition(i, 0.0); + } + + return true; + }); + + // Also update modelWorldPoses. This is a workaround to the problem that we + // don't have a way to reset the physics engine and clear its internal cache + // of link poses. In the event that a model's canonical link's pose hasn't + // changed after reset, the parent model's world pose won't be recorded in + // the modelWorldPoses map. If any of the model's other links have changed, + // however, we try to look for the parent model's world pose in + // modelWorldPoses and fail. So the workaround here is to update the world + // poses of all models. + _ecm.Each( + [&](const Entity &_entity, const components::Model *) + { + this->modelWorldPoses[_entity] = gazebo::worldPose(_entity, _ecm); + return true; + }); + + this->RemovePhysicsEntities(_ecm); +} + ////////////////////////////////////////////////// ignition::physics::ForwardStep::Output PhysicsPrivate::Step( const std::chrono::steady_clock::duration &_dt) @@ -3384,6 +3547,7 @@ void PhysicsPrivate::DisableContactSurfaceCustomization(const Entity &_world) IGNITION_ADD_PLUGIN(Physics, ignition::gazebo::System, Physics::ISystemConfigure, + Physics::ISystemReset, Physics::ISystemUpdate) IGNITION_ADD_PLUGIN_ALIAS(Physics, "ignition::gazebo::systems::Physics") diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index 2877bef288..15e6ea8054 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -67,6 +67,7 @@ namespace systems class Physics: public System, public ISystemConfigure, + public ISystemReset, public ISystemUpdate { /// \brief Constructor @@ -81,6 +82,10 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; + /// Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + /// Documentation inherited public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) final; From 4b023fade9d6a0cb64affb6789a5b87521dfdf66 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 8 Feb 2022 12:03:31 -0600 Subject: [PATCH 08/20] Simulation reset example plugin Co-authored-by: Addisu Z. Taddese Signed-off-by: Michael Carroll --- examples/plugin/reset_plugin/CMakeLists.txt | 9 +++ examples/plugin/reset_plugin/README.md | 34 +++++++++++ .../reset_plugin/joint_position_randomizer.cc | 61 +++++++++++++++++++ .../joint_position_randomizer.sdf | 44 +++++++++++++ 4 files changed, 148 insertions(+) create mode 100644 examples/plugin/reset_plugin/CMakeLists.txt create mode 100644 examples/plugin/reset_plugin/README.md create mode 100644 examples/plugin/reset_plugin/joint_position_randomizer.cc create mode 100644 examples/plugin/reset_plugin/joint_position_randomizer.sdf diff --git a/examples/plugin/reset_plugin/CMakeLists.txt b/examples/plugin/reset_plugin/CMakeLists.txt new file mode 100644 index 0000000000..686a6dc780 --- /dev/null +++ b/examples/plugin/reset_plugin/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-cmake2 REQUIRED) + +project(ResetPlugins) +find_package(ignition-gazebo7 REQUIRED) +add_library(JointPositionRandomizer SHARED joint_position_randomizer.cc) +target_link_libraries(JointPositionRandomizer + PRIVATE ignition-gazebo7::core) diff --git a/examples/plugin/reset_plugin/README.md b/examples/plugin/reset_plugin/README.md new file mode 100644 index 0000000000..9a1c25b207 --- /dev/null +++ b/examples/plugin/reset_plugin/README.md @@ -0,0 +1,34 @@ +# System Reset API + +This example uses the JointPositionRandomizer system to randomize the joint +positions of a robot arm at every reset. + + +## Build + +~~~ +cd examples/plugin/reset_plugin +mkdir build +cd build +cmake .. +make +~~~ + +This will generate the `JointPositionRandomizer` library under `build`. + +## Run + +Add the library to the path: + +~~~ +cd examples/plugin/reset_plugin +export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/build +~~~ + +Then run a world that loads the plugin as follows: + + ign gazebo -r -v 4 joint_position_randomizer.sdf + +In another terminal, run the following to reset the world. + + ign service -s /world/default/control --reqtype ignition.msgs.WorldControl --reptype ignition.msgs.Boolean --timeout 3000 --req 'reset: {all: true}' diff --git a/examples/plugin/reset_plugin/joint_position_randomizer.cc b/examples/plugin/reset_plugin/joint_position_randomizer.cc new file mode 100644 index 0000000000..70a1fab8ec --- /dev/null +++ b/examples/plugin/reset_plugin/joint_position_randomizer.cc @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2021 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 +#include +#include +#include +#include + +using namespace ignition; +using namespace gazebo; +namespace reset_plugin +{ +class JointPositionRandomizer : public System, + public ISystemConfigure, + public ISystemReset +{ + void Configure(const Entity &_entity, + const std::shared_ptr &, + EntityComponentManager &, EventManager &) override + { + this->targetEntity = _entity; + } + + void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) override + { + std::cout << "Reset joints\n"; + auto joints = _ecm.EntitiesByComponents( + components::ParentEntity(this->targetEntity), components::Joint()); + for (auto joint : joints) + { + double pos = math::Rand::DblUniform(0, IGN_PI); + std::cout << "joint " << joint << " pos: " << pos << std::endl; + _ecm.SetComponentData(joint, {pos}); + } + } + + private: Entity targetEntity; +}; +} + + +IGNITION_ADD_PLUGIN(reset_plugin::JointPositionRandomizer, + ignition::gazebo::System, + reset_plugin::JointPositionRandomizer::ISystemConfigure, + reset_plugin::JointPositionRandomizer::ISystemReset) diff --git a/examples/plugin/reset_plugin/joint_position_randomizer.sdf b/examples/plugin/reset_plugin/joint_position_randomizer.sdf new file mode 100644 index 0000000000..0d2055fdb5 --- /dev/null +++ b/examples/plugin/reset_plugin/joint_position_randomizer.sdf @@ -0,0 +1,44 @@ + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Simple Arm + + + + From 75e57fb7f1973915a5f9c17f643d9be8fd1e1f8f Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 8 Feb 2022 15:22:02 -0600 Subject: [PATCH 09/20] Add integration test for resetting physics Signed-off-by: Michael Carroll --- test/integration/CMakeLists.txt | 1 + test/integration/reset.cc | 210 ++++++++++++++++++++++++++++++++ test/plugins/MockSystem.cc | 1 + test/plugins/MockSystem.hh | 12 ++ test/worlds/reset.sdf | 39 ++++++ 5 files changed, 263 insertions(+) create mode 100644 test/integration/reset.cc create mode 100644 test/worlds/reset.sdf diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 6b712785a9..6ee746aee2 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -49,6 +49,7 @@ set(tests play_pause.cc pose_publisher_system.cc recreate_entities.cc + reset.cc save_world.cc scene_broadcaster_system.cc sdf_frame_semantics.cc diff --git a/test/integration/reset.cc b/test/integration/reset.cc new file mode 100644 index 0000000000..5f5879ccec --- /dev/null +++ b/test/integration/reset.cc @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2018 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 +#include + +#include +#include +#include + +#include +#include + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" +#include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/test_config.hh" + +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/World.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; +namespace components = ignition::gazebo::components; + +////////////////////////////////////////////////// +class ResetFixture: public InternalFixture> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + auto plugin = sm.LoadPlugin("libMockSystem.so", + "ignition::gazebo::MockSystem", + nullptr); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: ignition::gazebo::SystemPluginPtr systemPtr; + public: gazebo::MockSystem *mockSystem; + + private: gazebo::SystemLoader sm; +}; + +///////////////////////////////////////////////// +void worldReset() +{ + ignition::msgs::WorldControl req; + ignition::msgs::Boolean rep; + req.mutable_reset()->set_all(true); + transport::Node node; + + unsigned int timeout = 1000; + bool result; + bool executed = + node.Request("/world/default/control", req, timeout, rep, result); + + ASSERT_TRUE(executed); + ASSERT_TRUE(result); + ASSERT_TRUE(rep.data()); +} + +///////////////////////////////////////////////// +/// This test checks that that the sensors system handles cases where entities +/// are removed and then added back +TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) +{ + ignition::gazebo::ServerConfig serverConfig; + + const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/reset.sdf"; + + serverConfig.SetSdfFile(sdfFile); + + sdf::Root root; + root.Load(sdfFile); + gazebo::Server server(serverConfig); + + // A pointer to the ecm. This will be valid once we run the mock system + gazebo::EntityComponentManager *ecm = nullptr; + + this->mockSystem->configureCallback = + [&ecm](const Entity &, + const std::shared_ptr &, + EntityComponentManager &_ecm, + EventManager &) + { + ecm = &_ecm; + }; + + server.AddSystem(this->systemPtr); + // Verify initial conditions of the world + { + ASSERT_NE(nullptr, ecm); + auto entity = ecm->EntityByComponents(components::Name("box")); + ASSERT_NE(kNullEntity, entity); + auto poseComp = ecm->Component(entity); + ASSERT_NE(nullptr, poseComp); + EXPECT_FLOAT_EQ(poseComp->Data().Z(), 1.1f); + + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(0u, this->mockSystem->resetCallCount); + EXPECT_EQ(0u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(0u, this->mockSystem->updateCallCount); + EXPECT_EQ(0u, this->mockSystem->postUpdateCallCount); + } + + // Run so that things will happen in the world + // In this case, the box should fall some + server.Run(true, 100, false); + { + ASSERT_NE(nullptr, ecm); + auto entity = ecm->EntityByComponents(components::Name("box")); + ASSERT_NE(kNullEntity, entity); + auto poseComp = ecm->Component(entity); + ASSERT_NE(nullptr, poseComp); + EXPECT_LT(poseComp->Data().Z(), 1.1f); + + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(0u, this->mockSystem->resetCallCount); + EXPECT_EQ(100u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(100u, this->mockSystem->updateCallCount); + EXPECT_EQ(100u, this->mockSystem->postUpdateCallCount); + } + + // Validate update info in the reset + this->mockSystem->resetCallback = + [](const gazebo::UpdateInfo &_info, + gazebo::EntityComponentManager &) + { + EXPECT_EQ(0u, _info.iterations); + EXPECT_EQ(std::chrono::steady_clock::duration{0}, _info.simTime); + }; + + // Send command to reset to initial state + worldReset(); + + // It takes two iterations for this to propage, + // the first is for the message to be recieved and internal state setup + server.Run(true, 1, false); + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(0u, this->mockSystem->resetCallCount); + EXPECT_EQ(101u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(101u, this->mockSystem->updateCallCount); + EXPECT_EQ(101u, this->mockSystem->postUpdateCallCount); + + // The second iteration is where the reset actually occurs. + server.Run(true, 1, false); + { + ASSERT_NE(nullptr, ecm); + auto entity = ecm->EntityByComponents(components::Name("box")); + ASSERT_NE(kNullEntity, entity); + auto poseComp = ecm->Component(entity); + ASSERT_NE(nullptr, poseComp); + EXPECT_FLOAT_EQ(poseComp->Data().Z(), 1.1f); + + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(1u, this->mockSystem->resetCallCount); + + // These should not increment, because only reset is called + EXPECT_EQ(101u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(101u, this->mockSystem->updateCallCount); + EXPECT_EQ(101u, this->mockSystem->postUpdateCallCount); + } + + server.Run(true, 100, false); + { + ASSERT_NE(nullptr, ecm); + auto entity = ecm->EntityByComponents(components::Name("box")); + ASSERT_NE(kNullEntity, entity); + auto poseComp = ecm->Component(entity); + ASSERT_NE(nullptr, poseComp); + EXPECT_LT(poseComp->Data().Z(), 1.1f); + + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(1u, this->mockSystem->resetCallCount); + EXPECT_EQ(201u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(201u, this->mockSystem->updateCallCount); + EXPECT_EQ(201u, this->mockSystem->postUpdateCallCount); + } +} diff --git a/test/plugins/MockSystem.cc b/test/plugins/MockSystem.cc index d7602c5c38..e8c72e9c25 100644 --- a/test/plugins/MockSystem.cc +++ b/test/plugins/MockSystem.cc @@ -4,6 +4,7 @@ IGNITION_ADD_PLUGIN(ignition::gazebo::MockSystem, ignition::gazebo::System, ignition::gazebo::MockSystem::ISystemConfigure, + ignition::gazebo::MockSystem::ISystemReset, ignition::gazebo::MockSystem::ISystemPreUpdate, ignition::gazebo::MockSystem::ISystemUpdate, ignition::gazebo::MockSystem::ISystemPostUpdate) diff --git a/test/plugins/MockSystem.hh b/test/plugins/MockSystem.hh index b4fd804bb7..6705e7eee6 100644 --- a/test/plugins/MockSystem.hh +++ b/test/plugins/MockSystem.hh @@ -26,6 +26,7 @@ namespace ignition { class MockSystem : public gazebo::System, public gazebo::ISystemConfigure, + public gazebo::ISystemReset, public gazebo::ISystemPreUpdate, public gazebo::ISystemUpdate, public gazebo::ISystemPostUpdate @@ -33,6 +34,7 @@ namespace ignition { public: MockSystem() = default; public: ~MockSystem() = default; public: size_t configureCallCount {0}; + public: size_t resetCallCount {0}; public: size_t preUpdateCallCount {0}; public: size_t updateCallCount {0}; public: size_t postUpdateCallCount {0}; @@ -49,6 +51,8 @@ namespace ignition { EntityComponentManager &, EventManager &)> configureCallback; + + public: CallbackType resetCallback; public: CallbackType preUpdateCallback; public: CallbackType updateCallback; public: CallbackTypeConst postUpdateCallback; @@ -63,6 +67,14 @@ namespace ignition { this->configureCallback(_entity, _sdf, _ecm, _eventMgr); } + public: void Reset(const gazebo::UpdateInfo &_info, + gazebo::EntityComponentManager &_ecm) override final + { + ++this->resetCallCount; + if (this->resetCallback) + this->resetCallback(_info, _ecm); + } + public: void PreUpdate(const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) override final { diff --git a/test/worlds/reset.sdf b/test/worlds/reset.sdf new file mode 100644 index 0000000000..614afdeb23 --- /dev/null +++ b/test/worlds/reset.sdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + 0 0 1.1 0 0 0 + false + + + + 5 5 2.5 + + + + + 5 5 2.5 + + + 0 1 0 0.8 + 0 1 0 0.8 + 1 1 1 0.8 + + + + + + From e3368e5d28e843a34856fefb3e890371bd08369d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 15 Feb 2022 11:03:20 -0600 Subject: [PATCH 10/20] Fix diff test and add documentation Signed-off-by: Michael Carroll --- src/EntityComponentManager_TEST.cc | 8 ++++++-- src/systems/physics/Physics.cc | 12 ++++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 366c4e1202..7a27fdc030 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -3163,6 +3163,7 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(1, foundEntities); } +////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, CopyEcm) { Entity entity = manager.CreateEntity(); @@ -3184,7 +3185,8 @@ TEST_P(EntityComponentManagerFixture, CopyEcm) }); } -TEST_P(EntityComponentManagerFixture, ComputDiff) +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, ComputeDiff) { Entity entity1 = manager.CreateEntity(); math::Pose3d testPose{1, 2, 3, 0.1, 0.2, 0.3}; @@ -3228,10 +3230,11 @@ TEST_P(EntityComponentManagerFixture, ComputDiff) EntityComponentManagerDiff diff = managerCopy.RunComputeDiff(manager); EXPECT_EQ(1u, diff.RemovedEntities().size()); managerCopy.RunApplyDiff(manager, diff); - EXPECT_TRUE(managerCopy.HasEntitiesMarkedForRemoval()); + EXPECT_FALSE(managerCopy.HasEntitiesMarkedForRemoval()); } } +////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, ResetToWithDeletedEntity) { Entity entity1 = manager.CreateEntity(); @@ -3282,6 +3285,7 @@ TEST_P(EntityComponentManagerFixture, ResetToWithDeletedEntity) } } +////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, ResetToWithAddedEntity) { Entity entity1 = manager.CreateEntity(); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 83d6c7db0d..f40f111604 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -189,30 +189,42 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Create physics entities /// \param[in] _ecm Constant reference to ECM. + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system public: void CreatePhysicsEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create world entities /// \param[in] _ecm Constant reference to ECM. + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system public: void CreateWorldEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create model entities /// \param[in] _ecm Constant reference to ECM. + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system public: void CreateModelEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create link entities /// \param[in] _ecm Constant reference to ECM. + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system public: void CreateLinkEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create collision entities /// \param[in] _ecm Constant reference to ECM. + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system public: void CreateCollisionEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create joint entities /// \param[in] _ecm Constant reference to ECM. + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system public: void CreateJointEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); From 63ec622da87e907ecf46a6d7480755a7cc365778 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Feb 2022 16:43:39 -0600 Subject: [PATCH 11/20] Fix build and test errors after merge, some documentation and cleanup. Signed-off-by: Addisu Z. Taddese --- .../ignition/gazebo/EntityComponentManager.hh | 21 +++++++---- src/EntityComponentManager.cc | 36 ++++++++++--------- src/EntityComponentManager_TEST.cc | 14 ++++---- src/SimulationRunner.cc | 4 +-- 4 files changed, 43 insertions(+), 32 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 24b79d139d..e4c00bbaa6 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -72,7 +72,13 @@ namespace ignition /// \brief Destructor public: ~EntityComponentManager(); - public: void Copy(const EntityComponentManager &_fromEcm); + /// \brief Copies the contents of `_from` into this object. + /// \note This is a member function instead of a copy constructor so that + /// it can have additional parameters if the need arises in the future. + /// Additionally, not every data member is copied making its behavior + /// different from what would be expected from a copy constructor. + /// \param[in] _from Object to copy from + public: void CopyFrom(const EntityComponentManager &_fromEcm); /// \brief Creates a new Entity. /// \return An id for the Entity, or kNullEntity on failure. @@ -701,22 +707,23 @@ namespace ignition protected: void SetAllComponentsUnchanged(); /// Compute the diff between this EntityComponentManager and _other at the - /// entity level. + /// entity level. This does not compute the diff between components of an + /// entity. /// * If an entity is in `_other`, but not in `this`, insert the entity /// as an "added" entity. /// * If an entity is in `this`, but not in `other`, insert the entity /// as a "removed" entity. /// \return Data structure containing the added and removed entities - protected: EntityComponentManagerDiff ComputeDiff( + protected: EntityComponentManagerDiff ComputeEntityDiff( const EntityComponentManager &_other) const; - /// \brief Given a diff, apply it to this ECM. Note that for removed - /// entities, this would mark them for removal instead of actually + /// \brief Given an entity diff, apply it to this ECM. Note that for + /// removed entities, this would mark them for removal instead of actually /// removing the entities. /// \param[in] _other Original EntityComponentManager from which the diff /// was computed. - protected: void ApplyDiff(const EntityComponentManager &_other, - const EntityComponentManagerDiff &_diff); + protected: void ApplyEntityDiff(const EntityComponentManager &_other, + const EntityComponentManagerDiff &_diff); /// \brief Get whether an Entity exists and is new. /// diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 5575fc2b8f..d512f8c0e8 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -72,7 +72,13 @@ class ignition::gazebo::EntityComponentManagerPrivate /// `AddEntityToMessage`. public: void CalculateStateThreadLoad(); - public: void Copy(const EntityComponentManagerPrivate &_from); + /// \brief Copies the contents of `_from` into this object. + /// \note This is a member function instead of a copy constructor so that + /// it can have additional parameters if the need arises in the future. + /// Additionally, not every data member is copied making its behavior + /// different from what would be expected from a copy constructor. + /// \param[in] _from Object to copy from + public: void CopyFrom(const EntityComponentManagerPrivate &_from); /// \brief Create a message for the removed components /// \param[in] _entity Entity with the removed components @@ -290,12 +296,8 @@ EntityComponentManager::EntityComponentManager() ////////////////////////////////////////////////// EntityComponentManager::~EntityComponentManager() = default; -// ////////////////////////////////////////////////// -// EntityComponentManager::EntityComponentManager( -// EntityComponentManager &&_ecm) noexcept = default; - ////////////////////////////////////////////////// -void EntityComponentManagerPrivate::Copy( +void EntityComponentManagerPrivate::CopyFrom( const EntityComponentManagerPrivate &_from) { this->createdCompTypes = _from.createdCompTypes; @@ -2114,13 +2116,13 @@ void EntityComponentManager::UnpinAllEntities() } ///////////////////////////////////////////////// -void EntityComponentManager::Copy(const EntityComponentManager &_fromEcm) +void EntityComponentManager::CopyFrom(const EntityComponentManager &_fromEcm) { - this->dataPtr->Copy(*_fromEcm.dataPtr); + this->dataPtr->CopyFrom(*_fromEcm.dataPtr); } ///////////////////////////////////////////////// -EntityComponentManagerDiff EntityComponentManager::ComputeDiff( +EntityComponentManagerDiff EntityComponentManager::ComputeEntityDiff( const EntityComponentManager &_other) const { EntityComponentManagerDiff diff; @@ -2149,8 +2151,9 @@ EntityComponentManagerDiff EntityComponentManager::ComputeDiff( } ///////////////////////////////////////////////// -void EntityComponentManager::ApplyDiff(const EntityComponentManager &_other, - const EntityComponentManagerDiff &_diff) +void EntityComponentManager::ApplyEntityDiff( + const EntityComponentManager &_other, + const EntityComponentManagerDiff &_diff) { auto copyComponents = [&](Entity _entity) { @@ -2184,7 +2187,6 @@ void EntityComponentManager::ApplyDiff(const EntityComponentManager &_other, if (!this->HasEntity(entity)) { this->dataPtr->CreateEntityImplementation(entity); - this->RequestRemoveEntity(entity, false); // We want to set this entity as "removed", but // CreateEntityImplementation sets it as "newlyCreated", // so remove it from that list. @@ -2200,15 +2202,17 @@ void EntityComponentManager::ApplyDiff(const EntityComponentManager &_other, copyComponents(entity); this->SetParentEntity(entity, _other.ParentEntity(entity)); } + + this->RequestRemoveEntity(entity, false); } } ///////////////////////////////////////////////// void EntityComponentManager::ResetTo(const EntityComponentManager &_other) { - auto ecmDiff = this->ComputeDiff(_other); + auto ecmDiff = this->ComputeEntityDiff(_other); EntityComponentManager tmpCopy; - tmpCopy.Copy(_other); - tmpCopy.ApplyDiff(*this, ecmDiff); - this->Copy(tmpCopy); + tmpCopy.CopyFrom(_other); + tmpCopy.ApplyEntityDiff(*this, ecmDiff); + this->CopyFrom(tmpCopy); } diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 366c4e1202..4305803f48 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -111,13 +111,13 @@ class EntityCompMgrTest : public EntityComponentManager public: EntityComponentManagerDiff RunComputeDiff( const EntityComponentManager &_other) const { - return this->ComputeDiff(_other); + return this->ComputeEntityDiff(_other); } public: void RunApplyDiff(const EntityComponentManager &_other, const EntityComponentManagerDiff &_diff) { - this->ApplyDiff(_other, _diff); + this->ApplyEntityDiff(_other, _diff); } }; @@ -3170,7 +3170,7 @@ TEST_P(EntityComponentManagerFixture, CopyEcm) manager.CreateComponent(entity, components::Pose{testPose}); EntityCompMgrTest managerCopy; - managerCopy.Copy(manager); + managerCopy.CopyFrom(manager); EXPECT_EQ(manager.EntityCount(), managerCopy.EntityCount()); EXPECT_TRUE(managerCopy.HasEntity(entity)); EXPECT_TRUE( @@ -3184,14 +3184,14 @@ TEST_P(EntityComponentManagerFixture, CopyEcm) }); } -TEST_P(EntityComponentManagerFixture, ComputDiff) +TEST_P(EntityComponentManagerFixture, ComputeDiff) { Entity entity1 = manager.CreateEntity(); math::Pose3d testPose{1, 2, 3, 0.1, 0.2, 0.3}; manager.CreateComponent(entity1, components::Pose{testPose}); EntityCompMgrTest managerCopy; - managerCopy.Copy(manager); + managerCopy.CopyFrom(manager); Entity entity2 = manager.CreateEntity(); manager.CreateComponent(entity2, components::StringComponent{"Entity2"}); @@ -3254,7 +3254,7 @@ TEST_P(EntityComponentManagerFixture, ResetToWithDeletedEntity) } EntityCompMgrTest managerCopy; - managerCopy.Copy(manager); + managerCopy.CopyFrom(manager); manager.RequestRemoveEntity(entity1); @@ -3293,7 +3293,7 @@ TEST_P(EntityComponentManagerFixture, ResetToWithAddedEntity) manager.CreateComponent(entity2, components::Name{"entity2"}); EntityCompMgrTest managerCopy; - managerCopy.Copy(manager); + managerCopy.CopyFrom(manager); // Add entity3 after a copy has been made. Entity entity3 = manager.CreateEntity(); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 2f90442ce0..6edd7c63d3 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -170,7 +170,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, this->levelMgr->UpdateLevelsState(); // Store the initial state of the ECM; - this->initialEntityCompMgr.Copy(this->entityCompMgr); + this->initialEntityCompMgr.CopyFrom(this->entityCompMgr); // Load any additional plugins from the Server Configuration this->LoadServerPlugins(this->serverConfig.Plugins()); @@ -537,7 +537,7 @@ void SimulationRunner::UpdateSystems() if (this->resetInitiated) { IGN_PROFILE("Reset"); - for (auto &system : this->systemsReset) + for (auto &system : this->systemMgr->SystemsReset()) system->Reset(this->currentInfo, this->entityCompMgr); return; } From 859df328b6b02088ff0b4ba15e76fbc1588a1f3d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 29 Mar 2022 07:43:18 -0500 Subject: [PATCH 12/20] Bump year Signed-off-by: Michael Carroll --- examples/plugin/reset_plugin/joint_position_randomizer.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/plugin/reset_plugin/joint_position_randomizer.cc b/examples/plugin/reset_plugin/joint_position_randomizer.cc index 70a1fab8ec..0cf14a7ce9 100644 --- a/examples/plugin/reset_plugin/joint_position_randomizer.cc +++ b/examples/plugin/reset_plugin/joint_position_randomizer.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. From 749daca7be226a8d52f58d55531330a95a438178 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 29 Mar 2022 13:11:23 -0500 Subject: [PATCH 13/20] Fix merge from main Signed-off-by: Michael Carroll --- src/SystemManager.hh | 2 +- src/systems/physics/Physics.cc | 12 ++++++------ test/integration/reset.cc | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/SystemManager.hh b/src/SystemManager.hh index cbcb860b20..89a8f89d63 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -129,7 +129,7 @@ namespace ignition /// \brief Systems implementing Configure private: std::vector systemsConfigure; - /// \brief Systems implementing Reset + /// \brief Systems implementing Reset private: std::vector systemsReset; /// \brief Systems implementing PreUpdate diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 0fddf95041..0716cc302b 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -192,41 +192,41 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Create physics entities /// \param[in] _ecm Constant reference to ECM. /// \param[in] _warnIfEntityExists True to emit warnings if the same entity - /// already exists in the physics system + /// already exists in the physics system public: void CreatePhysicsEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create world entities /// \param[in] _ecm Constant reference to ECM. /// \param[in] _warnIfEntityExists True to emit warnings if the same entity - /// already exists in the physics system + /// already exists in the physics system public: void CreateWorldEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create model entities /// \param[in] _ecm Constant reference to ECM. /// \param[in] _warnIfEntityExists True to emit warnings if the same entity - /// already exists in the physics system + /// already exists in the physics system public: void CreateModelEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create link entities /// \param[in] _ecm Constant reference to ECM. /// \param[in] _warnIfEntityExists True to emit warnings if the same entity - /// already exists in the physics system + /// already exists in the physics system public: void CreateLinkEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create collision entities /// \param[in] _ecm Constant reference to ECM. /// \param[in] _warnIfEntityExists True to emit warnings if the same entity - /// already exists in the physics system + /// already exists in the physics system public: void CreateCollisionEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); /// \brief Create joint entities /// \param[in] _ecm Constant reference to ECM. /// \param[in] _warnIfEntityExists True to emit warnings if the same entity - /// already exists in the physics system + /// already exists in the physics system public: void CreateJointEntities(const EntityComponentManager &_ecm, bool _warnIfEntityExists = true); diff --git a/test/integration/reset.cc b/test/integration/reset.cc index 5f5879ccec..8469625569 100644 --- a/test/integration/reset.cc +++ b/test/integration/reset.cc @@ -164,7 +164,7 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) // Send command to reset to initial state worldReset(); - // It takes two iterations for this to propage, + // It takes two iterations for this to propage, // the first is for the message to be recieved and internal state setup server.Run(true, 1, false); EXPECT_EQ(1u, this->mockSystem->configureCallCount); From 371d8cec6225d2aca7111771b53e599d2c504388 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 29 Mar 2022 16:12:05 -0500 Subject: [PATCH 14/20] Fix CI platform Signed-off-by: Michael Carroll --- test/integration/reset.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/integration/reset.cc b/test/integration/reset.cc index 8469625569..2231fa6131 100644 --- a/test/integration/reset.cc +++ b/test/integration/reset.cc @@ -50,7 +50,7 @@ using namespace std::chrono_literals; namespace components = ignition::gazebo::components; ////////////////////////////////////////////////// -class ResetFixture: public InternalFixture> +class ResetFixture: public InternalFixture<::testing::Test> { protected: void SetUp() override { @@ -90,9 +90,9 @@ void worldReset() } ///////////////////////////////////////////////// -/// This test checks that that the sensors system handles cases where entities +/// This test checks that that the phycis system handles cases where entities /// are removed and then added back -TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) +TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) { ignition::gazebo::ServerConfig serverConfig; From d1fabbedc9f6e0e1a77114443d51c4f3eba3b790 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 30 Mar 2022 17:14:20 -0500 Subject: [PATCH 15/20] Update test/integration/reset.cc Signed-off-by: Michael Carroll Co-authored-by: Addisu Z. Taddese --- test/integration/reset.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/reset.cc b/test/integration/reset.cc index 2231fa6131..9546a65351 100644 --- a/test/integration/reset.cc +++ b/test/integration/reset.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. From c8ab4d540cd4d6751cf9a01948d1e31836d57125 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 18 Apr 2022 16:11:28 -0500 Subject: [PATCH 16/20] Address reviewer feedback Signed-off-by: Michael Carroll --- examples/plugin/reset_plugin/CMakeLists.txt | 2 +- ...ition_randomizer.cc => JointPositionRandomizer.cc} | 0 src/systems/physics/Physics.hh | 4 ++-- test/integration/reset.cc | 11 +++++------ test/worlds/reset.sdf | 4 ++++ 5 files changed, 12 insertions(+), 9 deletions(-) rename examples/plugin/reset_plugin/{joint_position_randomizer.cc => JointPositionRandomizer.cc} (100%) diff --git a/examples/plugin/reset_plugin/CMakeLists.txt b/examples/plugin/reset_plugin/CMakeLists.txt index 686a6dc780..7357da20f7 100644 --- a/examples/plugin/reset_plugin/CMakeLists.txt +++ b/examples/plugin/reset_plugin/CMakeLists.txt @@ -4,6 +4,6 @@ find_package(ignition-cmake2 REQUIRED) project(ResetPlugins) find_package(ignition-gazebo7 REQUIRED) -add_library(JointPositionRandomizer SHARED joint_position_randomizer.cc) +add_library(JointPositionRandomizer SHARED JointPositionRandomizer.cc) target_link_libraries(JointPositionRandomizer PRIVATE ignition-gazebo7::core) diff --git a/examples/plugin/reset_plugin/joint_position_randomizer.cc b/examples/plugin/reset_plugin/JointPositionRandomizer.cc similarity index 100% rename from examples/plugin/reset_plugin/joint_position_randomizer.cc rename to examples/plugin/reset_plugin/JointPositionRandomizer.cc diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index 54ac01518c..c43b7ecbd5 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -95,11 +95,11 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; - /// Documentation inherited + // Documentation inherited public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) final; - /// Documentation inherited + // Documentation inherited public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/test/integration/reset.cc b/test/integration/reset.cc index 9546a65351..df2d8e3fe4 100644 --- a/test/integration/reset.cc +++ b/test/integration/reset.cc @@ -90,14 +90,14 @@ void worldReset() } ///////////////////////////////////////////////// -/// This test checks that that the phycis system handles cases where entities +/// This test checks that that the physics system handles cases where entities /// are removed and then added back TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) { ignition::gazebo::ServerConfig serverConfig; - const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/reset.sdf"; + const std::string sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "reset.sdf"); serverConfig.SetSdfFile(sdfFile); @@ -138,7 +138,6 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) // In this case, the box should fall some server.Run(true, 100, false); { - ASSERT_NE(nullptr, ecm); auto entity = ecm->EntityByComponents(components::Name("box")); ASSERT_NE(kNullEntity, entity); auto poseComp = ecm->Component(entity); @@ -164,8 +163,8 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) // Send command to reset to initial state worldReset(); - // It takes two iterations for this to propage, - // the first is for the message to be recieved and internal state setup + // It takes two iterations for this to propagate, + // the first is for the message to be received and internal state setup server.Run(true, 1, false); EXPECT_EQ(1u, this->mockSystem->configureCallCount); EXPECT_EQ(0u, this->mockSystem->resetCallCount); diff --git a/test/worlds/reset.sdf b/test/worlds/reset.sdf index 614afdeb23..10c6c63426 100644 --- a/test/worlds/reset.sdf +++ b/test/worlds/reset.sdf @@ -14,6 +14,10 @@ name="ignition::gazebo::systems::UserCommands"> + + 0 + + 0 0 1.1 0 0 0 false From 3799ff3f5e4030e0f0351c56b05fa6a06295fd87 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 3 May 2022 11:31:58 -0500 Subject: [PATCH 17/20] Adjust randomized range to account for limits Signed-off-by: Michael Carroll --- .../reset_plugin/JointPositionRandomizer.cc | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/examples/plugin/reset_plugin/JointPositionRandomizer.cc b/examples/plugin/reset_plugin/JointPositionRandomizer.cc index 0cf14a7ce9..1f53bfac49 100644 --- a/examples/plugin/reset_plugin/JointPositionRandomizer.cc +++ b/examples/plugin/reset_plugin/JointPositionRandomizer.cc @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include @@ -42,18 +44,32 @@ class JointPositionRandomizer : public System, std::cout << "Reset joints\n"; auto joints = _ecm.EntitiesByComponents( components::ParentEntity(this->targetEntity), components::Joint()); + for (auto joint : joints) { - double pos = math::Rand::DblUniform(0, IGN_PI); - std::cout << "joint " << joint << " pos: " << pos << std::endl; + auto jointType = _ecm.Component(joint); + + double pos = 0.0; + + if (jointType->Data() == sdf::JointType::PRISMATIC) + { + pos = math::Rand::DblUniform(-0.8, 0.1); + std::cout << "prismatic joint (" << joint + << ") pos: (" << pos << " m)"<< std::endl; + } + else if (jointType->Data() == sdf::JointType::REVOLUTE) + { + pos = math::Rand::DblUniform(0, IGN_PI); + std::cout << "revolute joint (" << joint + << ") pos: (" << pos << " rad)"<< std::endl; + } _ecm.SetComponentData(joint, {pos}); } } private: Entity targetEntity; }; -} - +} // namespace reset_plugin IGNITION_ADD_PLUGIN(reset_plugin::JointPositionRandomizer, ignition::gazebo::System, From fbf6d0e438899a2d3c9023a0f504b15e06e078a1 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 3 May 2022 14:23:44 -0500 Subject: [PATCH 18/20] Fix header for new location Signed-off-by: Michael Carroll --- test/integration/reset.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/reset.cc b/test/integration/reset.cc index df2d8e3fe4..bc7f669f01 100644 --- a/test/integration/reset.cc +++ b/test/integration/reset.cc @@ -25,7 +25,7 @@ #include #include -#include +#include #include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/EntityComponentManager.hh" From a519149396ce1f8b5aea78e7e766b17c9f38a539 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 10 May 2022 08:50:19 -0500 Subject: [PATCH 19/20] Fixup CMake dependencies for reset_plugin Signed-off-by: Michael Carroll --- examples/plugin/reset_plugin/CMakeLists.txt | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/examples/plugin/reset_plugin/CMakeLists.txt b/examples/plugin/reset_plugin/CMakeLists.txt index 7357da20f7..c02c0dcfbc 100644 --- a/examples/plugin/reset_plugin/CMakeLists.txt +++ b/examples/plugin/reset_plugin/CMakeLists.txt @@ -1,9 +1,14 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) -find_package(ignition-cmake2 REQUIRED) - project(ResetPlugins) + +ign_find_package(ignition-plugin2 REQUIRED COMPONENTS register) +set(IGN_PLUGIN_VER ${ignition-plugin2_VERSION_MAJOR}) + find_package(ignition-gazebo7 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo7_VERSION_MAJOR}) + add_library(JointPositionRandomizer SHARED JointPositionRandomizer.cc) target_link_libraries(JointPositionRandomizer - PRIVATE ignition-gazebo7::core) + PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::core) From b5f6bc89f0130e66d2dce124c924e1df995f1f37 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 17 May 2022 09:06:42 -0500 Subject: [PATCH 20/20] Fix CMake Signed-off-by: Michael Carroll --- examples/plugin/reset_plugin/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/plugin/reset_plugin/CMakeLists.txt b/examples/plugin/reset_plugin/CMakeLists.txt index c02c0dcfbc..51412f097d 100644 --- a/examples/plugin/reset_plugin/CMakeLists.txt +++ b/examples/plugin/reset_plugin/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) project(ResetPlugins) -ign_find_package(ignition-plugin2 REQUIRED COMPONENTS register) +find_package(ignition-plugin2 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin2_VERSION_MAJOR}) find_package(ignition-gazebo7 REQUIRED)