From 11e029a0f47bbf04c584e5a7ce770b44b24b3c5b Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 15 Feb 2022 11:22:54 -0600 Subject: [PATCH 01/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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/54] 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 0c52ab3b91305ce732f90610ef3bde4b05b4158d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 22 Feb 2022 13:35:00 -0600 Subject: [PATCH 17/54] Reset sensors and scenebroadcaster implementation Signed-off-by: Michael Carroll --- src/ServerConfig.cc | 3 + src/SimulationRunner.cc | 3 +- src/SystemInternal.hh | 8 +- src/SystemManager.cc | 58 ++++- src/SystemManager.hh | 11 +- src/rendering/RenderUtil.cc | 113 +++++++++- .../scene_broadcaster/SceneBroadcaster.cc | 25 ++- .../scene_broadcaster/SceneBroadcaster.hh | 7 +- src/systems/sensors/Sensors.cc | 40 ++-- src/systems/sensors/Sensors.hh | 5 + test/integration/CMakeLists.txt | 1 + test/integration/reset.cc | 8 +- test/integration/reset_sensors.cc | 210 ++++++++++++++++++ test/worlds/reset.sdf | 6 +- test/worlds/reset_sensors.sdf | 130 +++++++++++ 15 files changed, 596 insertions(+), 32 deletions(-) create mode 100644 test/integration/reset_sensors.cc create mode 100644 test/worlds/reset_sensors.sdf diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 1024eb5d11..dbacc09f3a 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -202,6 +202,9 @@ class ignition::gazebo::ServerConfigPrivate this->timestamp = IGN_SYSTEM_TIME(); + igndbg << "Home: " << home << std::endl; + igndbg << "To ISO: " << common::timeToIso(this->timestamp) << std::endl; + // Set a default log record path this->logRecordPath = common::joinPaths(home, ".ignition", "gazebo", "log", common::timeToIso(this->timestamp)); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 0481a2df21..c5782f5cbf 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -538,8 +538,7 @@ void SimulationRunner::UpdateSystems() if (this->resetInitiated) { IGN_PROFILE("Reset"); - for (auto &system : this->systemMgr->SystemsReset()) - system->Reset(this->currentInfo, this->entityCompMgr); + this->systemMgr->Reset(this->currentInfo, this->entityCompMgr); return; } diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index cca49a965b..3162fa1ec7 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -105,15 +105,17 @@ namespace ignition /// system during the `Configure` call. public: Entity parentEntity = {kNullEntity}; + public: std::string fname = ""; + + public: std::string name = ""; + /// \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; + public: std::shared_ptr sdf = nullptr; /// \brief Vector of queries and callbacks public: std::vector updates; }; } - } // namespace gazebo -} // namespace ignition #endif // IGNITION_GAZEBO_SYSTEMINTERNAL_HH_ diff --git a/src/SystemManager.cc b/src/SystemManager.cc index bde885612c..cef4088b1a 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -45,7 +45,12 @@ void SystemManager::LoadPlugin(const Entity _entity, // System correctly loaded from library if (system) { - this->AddSystem(system.value(), _entity, _sdf); + SystemInternal ss(system.value()); + ss.entity = _entity; + ss.fname = _fname; + ss.name = _name; + ss.sdf = _sdf; + this->AddSystemImpl(ss, ss.entity, ss.sdf); igndbg << "Loaded system [" << _name << "] for entity [" << _entity << "]" << std::endl; } @@ -101,6 +106,57 @@ size_t SystemManager::ActivatePendingSystems() return count; } +////////////////////////////////////////////////// +void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) +{ + { + std::lock_guard lock(this->pendingSystemsMutex); + this->pendingSystems.clear(); + } + + // Clear all iterable collections of systems + this->systemsConfigure.clear(); + this->systemsReset.clear(); + this->systemsPreupdate.clear(); + this->systemsUpdate.clear(); + this->systemsPostupdate.clear(); + + this->entityCompMgr = &_ecm; + + for (auto& system: this->systems) + { + if (nullptr != system.reset) + { + // If implemented, call reset and add to pending systems. + system.reset->Reset(_info, *this->entityCompMgr); + + { + std::lock_guard lock(this->pendingSystemsMutex); + this->pendingSystems.push_back(system); + } + } + else + { + // Cannot reset systems that were created in memory rather than + // from a plugin, because there isn't access to the constructor. + if (nullptr != system.systemShared) + { + ignwarn << "Systems not created from plugins cannot be correctly " + << " reset without implementing ISystemReset interface.\n"; + continue; + } + + this->LoadPlugin(system.entity, + system.fname, + system.name, + system.sdf); + } + } + + this->systems.clear(); + this->ActivatePendingSystems(); +} + ////////////////////////////////////////////////// void SystemManager::AddSystem(const SystemPluginPtr &_system, Entity _entity, diff --git a/src/SystemManager.hh b/src/SystemManager.hh index f6f1e214a2..581b54b0d1 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -92,12 +92,17 @@ namespace ignition /// \return The number of newly-active systems public: size_t ActivatePendingSystems(); - /// \brief Get an vector of all active systems implementing "Configure" - /// \return Vector of systems's configure interfaces. + /// \brief Reset all managed systems + /// \param[in] _info Update information for the reset time + /// \param[in] _ecm ECM information for the reset time + public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm); + + /// \brief Get an vector of all systems implementing "Configure" + /// \return Vector of systems' configure interfaces. public: const std::vector& SystemsConfigure(); /// \brief Get an vector of all active systems implementing "Reset" - /// \return Vector of systems's reset interfaces. + /// \return Vector of systems' reset interfaces. public: const std::vector& SystemsReset(); /// \brief Get an vector of all active systems implementing "PreUpdate" diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 62522936ad..46d64cfbcd 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1645,6 +1645,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const std::string segmentationCameraSuffix{"/segmentation"}; const std::string wideAngleCameraSuffix{"/image"}; + int nNewScenes = 0; // Get all the new worlds // TODO(anyone) Only one scene is supported for now // extend the sensor system to support mutliple scenes in the future @@ -1653,13 +1654,14 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::World *, const components::Scene *_scene)->bool { + nNewScenes++; this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); return true; }); - + int nNewModels = 0; _ecm.Each( [&](const Entity &_entity, @@ -1668,6 +1670,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + nNewModels++; sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); @@ -1677,6 +1680,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( return true; }); + int nNewLinks = 0; _ecm.Each( [&](const Entity &_entity, @@ -1685,11 +1689,13 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + nNewLinks++; this->CreateLink(_ecm, _entity, _name, _pose, _parent); return true; }); // visuals + int nNewVisuals = 0; _ecm.Eachbool { + nNewVisuals++; this->CreateVisual(_ecm, _entity, _name, _pose, _geom, _castShadows, _transparency, _visibilityFlags, _parent); return true; }); // actors + int nNewActors = 0; _ecm.Each( [&](const Entity &_entity, const components::Actor *_actor, const components::Name *_name, const components::ParentEntity *_parent) -> bool { + nNewActors++; this->newActors.push_back(std::make_tuple(_entity, _actor->Data(), _name->Data(), _parent->Data())); @@ -1732,27 +1741,32 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( }); // lights + int nNewLights = 0; _ecm.Each( [&](const Entity &_entity, const components::Light *_light, const components::Name *_name, const components::ParentEntity *_parent) -> bool { + nNewLights++; this->CreateLight(_ecm, _entity, _light, _name, _parent); return true; }); // inertials + int nNewInertials = 0; _ecm.Each( [&](const Entity &_entity, const components::Inertial *_inrElement, const components::Pose *) -> bool { + nNewInertials++; this->entityInertials[_entity] = _inrElement->Data(); return true; }); // collisions + int nNewCollisions = 0; _ecm.Each( @@ -1764,12 +1778,14 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::CollisionElement *_collElement, const components::ParentEntity *_parent) -> bool { + nNewCollisions++; this->entityCollisions[_entity] = _collElement->Data(); this->linkToCollisionEntities[_parent->Data()].push_back(_entity); return true; }); // joints + int nNewJoints = 0; _ecm.Each( @@ -1782,6 +1798,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::ParentLinkName *_parentLinkName, const components::ChildLinkName *_childLinkName) -> bool { + nNewJoints++; sdf::Joint joint; joint.SetName(_name->Data()); joint.SetType(_jointType->Data()); @@ -1808,16 +1825,24 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( }); // particle emitters + int nNewEmitters = 0; _ecm.Each( [&](const Entity &_entity, const components::ParticleEmitter *_emitter, const components::ParentEntity *_parent) -> bool { + nNewEmitters++; this->newParticleEmitters.push_back( std::make_tuple(_entity, _emitter->Data(), _parent->Data())); return true; }); + int nNewCameras = 0; + int nNewDepthCameras = 0; + int nNewRgbdCameras = 0; + int nNewGpuLidars = 0; + int nNewThermalCameras = 0; + int nNewSegmentationCameras = 0; if (this->enableSensors) { // Create cameras @@ -1826,6 +1851,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::Camera *_camera, const components::ParentEntity *_parent)->bool { + nNewCameras++; this->AddNewSensor(_ecm, _entity, _camera->Data(), _parent->Data(), cameraSuffix); return true; @@ -1837,6 +1863,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::DepthCamera *_depthCamera, const components::ParentEntity *_parent)->bool { + nNewDepthCameras++; this->AddNewSensor(_ecm, _entity, _depthCamera->Data(), _parent->Data(), depthCameraSuffix); return true; @@ -1848,6 +1875,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::RgbdCamera *_rgbdCamera, const components::ParentEntity *_parent)->bool { + nNewRgbdCameras++; this->AddNewSensor(_ecm, _entity, _rgbdCamera->Data(), _parent->Data(), rgbdCameraSuffix); return true; @@ -1859,6 +1887,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::GpuLidar *_gpuLidar, const components::ParentEntity *_parent)->bool { + nNewGpuLidars++; this->AddNewSensor(_ecm, _entity, _gpuLidar->Data(), _parent->Data(), gpuLidarSuffix); return true; @@ -1870,6 +1899,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::ThermalCamera *_thermalCamera, const components::ParentEntity *_parent)->bool { + nNewThermalCameras++; this->AddNewSensor(_ecm, _entity, _thermalCamera->Data(), _parent->Data(), thermalCameraSuffix); return true; @@ -1881,6 +1911,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::SegmentationCamera *_segmentationCamera, const components::ParentEntity *_parent)->bool { + nNewSegmentationCameras++; this->AddNewSensor(_ecm, _entity, _segmentationCamera->Data(), _parent->Data(), segmentationCameraSuffix); return true; @@ -1897,6 +1928,30 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( return true; }); } + + if (nNewScenes || nNewModels || nNewLinks || nNewVisuals || nNewActors || + nNewLights || nNewInertials || nNewCollisions || nNewJoints || nNewEmitters || + nNewCameras || nNewDepthCameras || nNewRgbdCameras || nNewGpuLidars || + nNewThermalCameras || nNewSegmentationCameras) + { + igndbg << "CreateEntitiesFirstUpdate: \n"; + igndbg << "\tNew Scenes: " << nNewScenes << "\n"; + igndbg << "\tNew Models: " << nNewModels << "\n"; + igndbg << "\tNew Links: " << nNewLinks << "\n"; + igndbg << "\tNew Visuals: " << nNewVisuals << "\n"; + igndbg << "\tNew Actors: " << nNewActors << "\n"; + igndbg << "\tNew Lights: " << nNewLights << "\n"; + igndbg << "\tNew Inertials: " << nNewInertials << "\n"; + igndbg << "\tNew Collisions: " << nNewCollisions << "\n"; + igndbg << "\tNew Joints: " << nNewJoints << "\n"; + igndbg << "\tNew Emitters: " << nNewEmitters << "\n"; + igndbg << "\tNew Cameras: " << nNewCameras << "\n"; + igndbg << "\tNew DepthCameras: " << nNewDepthCameras << "\n"; + igndbg << "\tNew RgbdCameras: " << nNewRgbdCameras << "\n"; + igndbg << "\tNew GpuLidars: " << nNewGpuLidars<< "\n"; + igndbg << "\tNew ThermalCameras: " << nNewThermalCameras<< "\n"; + igndbg << "\tNew SegmentationCameras: " << nNewSegmentationCameras<< "\n"; + } } ////////////////////////////////////////////////// @@ -1914,17 +1969,20 @@ void RenderUtilPrivate::CreateEntitiesRuntime( // Get all the new worlds // TODO(anyone) Only one scene is supported for now // extend the sensor system to support mutliple scenes in the future + int nNewScenes = 0; _ecm.EachNew( [&](const Entity & _entity, const components::World *, const components::Scene *_scene)->bool { + nNewScenes++; this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); return true; }); + int nNewModels = 0; _ecm.EachNew( [&](const Entity &_entity, @@ -1933,6 +1991,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + nNewModels++; sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); @@ -1942,6 +2001,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( return true; }); + int nNewLinks = 0; _ecm.EachNew( [&](const Entity &_entity, @@ -1950,11 +2010,13 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { + nNewLinks++; this->CreateLink(_ecm, _entity, _name, _pose, _parent); return true; }); // visuals + int nNewVisuals = 0; _ecm.EachNewbool { + nNewVisuals++; this->CreateVisual(_ecm, _entity, _name, _pose, _geom, _castShadows, _transparency, _visibilityFlags, _parent); return true; }); // actors + int nNewActors = 0; _ecm.EachNew( [&](const Entity &_entity, const components::Actor *_actor, const components::Name *_name, const components::ParentEntity *_parent) -> bool { + nNewActors++; this->newActors.push_back( std::make_tuple(_entity, _actor->Data(), _name->Data(), _parent->Data())); @@ -1998,27 +2063,32 @@ void RenderUtilPrivate::CreateEntitiesRuntime( }); // lights + int nNewLights = 0; _ecm.EachNew( [&](const Entity &_entity, const components::Light *_light, const components::Name *_name, const components::ParentEntity *_parent) -> bool { + nNewLights++; this->CreateLight(_ecm, _entity, _light, _name, _parent); return true; }); // inertials + int nNewInertials = 0; _ecm.EachNew( [&](const Entity &_entity, const components::Inertial *_inrElement, const components::Pose *) -> bool { + nNewInertials++; this->entityInertials[_entity] = _inrElement->Data(); return true; }); // collisions + int nNewCollisions = 0; _ecm.EachNew( @@ -2030,12 +2100,14 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::CollisionElement *_collElement, const components::ParentEntity *_parent) -> bool { + nNewCollisions++; this->entityCollisions[_entity] = _collElement->Data(); this->linkToCollisionEntities[_parent->Data()].push_back(_entity); return true; }); // joints + int nNewJoints = 0; _ecm.EachNew( @@ -2048,6 +2120,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::ParentLinkName *_parentLinkName, const components::ChildLinkName *_childLinkName) -> bool { + nNewJoints++; sdf::Joint joint; joint.SetName(_name->Data()); joint.SetType(_jointType->Data()); @@ -2074,16 +2147,24 @@ void RenderUtilPrivate::CreateEntitiesRuntime( }); // particle emitters + int nNewEmitters = 0; _ecm.EachNew( [&](const Entity &_entity, const components::ParticleEmitter *_emitter, const components::ParentEntity *_parent) -> bool { + nNewEmitters++; this->newParticleEmitters.push_back( std::make_tuple(_entity, _emitter->Data(), _parent->Data())); return true; }); + int nNewCameras = 0; + int nNewDepthCameras = 0; + int nNewRgbdCameras = 0; + int nNewGpuLidars = 0; + int nNewThermalCameras = 0; + int nNewSegmentationCameras = 0; if (this->enableSensors) { // Create cameras @@ -2092,6 +2173,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Camera *_camera, const components::ParentEntity *_parent)->bool { + nNewCameras++; this->AddNewSensor(_ecm, _entity, _camera->Data(), _parent->Data(), cameraSuffix); return true; @@ -2103,6 +2185,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::DepthCamera *_depthCamera, const components::ParentEntity *_parent)->bool { + nNewDepthCameras++; this->AddNewSensor(_ecm, _entity, _depthCamera->Data(), _parent->Data(), depthCameraSuffix); return true; @@ -2114,6 +2197,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::RgbdCamera *_rgbdCamera, const components::ParentEntity *_parent)->bool { + nNewRgbdCameras++; this->AddNewSensor(_ecm, _entity, _rgbdCamera->Data(), _parent->Data(), rgbdCameraSuffix); return true; @@ -2125,6 +2209,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::GpuLidar *_gpuLidar, const components::ParentEntity *_parent)->bool { + nNewGpuLidars++; this->AddNewSensor(_ecm, _entity, _gpuLidar->Data(), _parent->Data(), gpuLidarSuffix); return true; @@ -2136,6 +2221,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::ThermalCamera *_thermalCamera, const components::ParentEntity *_parent)->bool { + nNewThermalCameras++; this->AddNewSensor(_ecm, _entity, _thermalCamera->Data(), _parent->Data(), thermalCameraSuffix); return true; @@ -2147,6 +2233,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::SegmentationCamera *_segmentationCamera, const components::ParentEntity *_parent)->bool { + nNewSegmentationCameras++; this->AddNewSensor(_ecm, _entity, _segmentationCamera->Data(), _parent->Data(), segmentationCameraSuffix); return true; @@ -2163,6 +2250,30 @@ void RenderUtilPrivate::CreateEntitiesRuntime( return true; }); } + + if (nNewScenes || nNewModels || nNewLinks || nNewVisuals || nNewActors || + nNewLights || nNewInertials || nNewCollisions || nNewJoints || nNewEmitters || + nNewCameras || nNewDepthCameras || nNewRgbdCameras || nNewGpuLidars || + nNewThermalCameras || nNewSegmentationCameras) + { + igndbg << "CreateEntitiesRuntime: \n"; + igndbg << "\tNew Scenes: " << nNewScenes << "\n"; + igndbg << "\tNew Models: " << nNewModels << "\n"; + igndbg << "\tNew Links: " << nNewLinks << "\n"; + igndbg << "\tNew Visuals: " << nNewVisuals << "\n"; + igndbg << "\tNew Actors: " << nNewActors << "\n"; + igndbg << "\tNew Lights: " << nNewLights << "\n"; + igndbg << "\tNew Inertials: " << nNewInertials << "\n"; + igndbg << "\tNew Collisions: " << nNewCollisions << "\n"; + igndbg << "\tNew Joints: " << nNewJoints << "\n"; + igndbg << "\tNew Emitters: " << nNewEmitters << "\n"; + igndbg << "\tNew Cameras: " << nNewCameras << "\n"; + igndbg << "\tNew DepthCameras: " << nNewDepthCameras << "\n"; + igndbg << "\tNew RgbdCameras: " << nNewRgbdCameras << "\n"; + igndbg << "\tNew GpuLidars: " << nNewGpuLidars<< "\n"; + igndbg << "\tNew ThermalCameras: " << nNewThermalCameras<< "\n"; + igndbg << "\tNew SegmentationCameras: " << nNewSegmentationCameras<< "\n"; + } } ////////////////////////////////////////////////// diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index ad168d2811..e5071d280e 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -377,6 +377,28 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, } } +////////////////////////////////////////////////// +void SceneBroadcaster::Reset(const UpdateInfo &_info, + EntityComponentManager &_manager) +{ + // Update scene graph with added entities before populating pose message + this->dataPtr->SceneGraphAddEntities(_manager); + + this->dataPtr->PoseUpdate(_info, _manager); + + // call SceneGraphRemoveEntities at the end of this update cycle so that + // removed entities are removed from the scene graph for the next update cycle + this->dataPtr->SceneGraphRemoveEntities(_manager); + + std::unique_lock lock(this->dataPtr->stateMutex); + this->dataPtr->stepMsg.Clear(); + + set(this->dataPtr->stepMsg.mutable_stats(), _info); + _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true); + this->dataPtr->statePub.Publish(this->dataPtr->stepMsg); + this->dataPtr->lastStatePubTime = std::chrono::system_clock::now(); +} + ////////////////////////////////////////////////// void SceneBroadcasterPrivate::PoseUpdate(const UpdateInfo &_info, const EntityComponentManager &_manager) @@ -1176,7 +1198,8 @@ void SceneBroadcasterPrivate::RemoveFromGraph(const Entity _entity, IGNITION_ADD_PLUGIN(SceneBroadcaster, ignition::gazebo::System, SceneBroadcaster::ISystemConfigure, - SceneBroadcaster::ISystemPostUpdate) + SceneBroadcaster::ISystemPostUpdate, + SceneBroadcaster::ISystemReset) // Add plugin alias so that we can refer to the plugin without the version // namespace diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.hh b/src/systems/scene_broadcaster/SceneBroadcaster.hh index faeedd0b44..39064c31ee 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.hh +++ b/src/systems/scene_broadcaster/SceneBroadcaster.hh @@ -40,7 +40,8 @@ namespace systems class SceneBroadcaster final: public System, public ISystemConfigure, - public ISystemPostUpdate + public ISystemPostUpdate, + public ISystemReset { /// \brief Constructor public: SceneBroadcaster(); @@ -58,6 +59,10 @@ namespace systems public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; + // Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 265fc49aa9..245fcf5855 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -356,10 +356,6 @@ void SensorsPrivate::RenderThread() this->RunOnce(); } - // clean up before exiting - for (const auto id : this->sensorIds) - this->sensorManager.Remove(id); - igndbg << "SensorsPrivate::RenderThread stopped" << std::endl; } @@ -508,13 +504,38 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->eventManager = &_eventMgr; - this->dataPtr->stopConn = _eventMgr.Connect( + this->dataPtr->stopConn = this->dataPtr->eventManager->Connect( std::bind(&SensorsPrivate::Stop, this->dataPtr.get())); // Kick off worker thread this->dataPtr->Run(); } +////////////////////////////////////////////////// +void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) +{ + IGN_PROFILE("Sensors::Reset"); + + if (this->dataPtr->running && this->dataPtr->initialized) + { + igndbg << "Resetting Sensors\n"; + + this->dataPtr->sensorMaskMutex.lock(); + this->dataPtr->sensorMask.clear(); + this->dataPtr->sensorMaskMutex.unlock(); + + auto time = math::durationToSecNsec(_info.simTime); + auto t = math::secNsecToDuration(time.first, time.second); + + for (auto id : this->dataPtr->sensorIds) + { + sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); + auto rs = dynamic_cast(s); + rs->SetNextDataUpdateTime(t); + } + } +} + ////////////////////////////////////////////////// void Sensors::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) @@ -533,14 +554,6 @@ void Sensors::PostUpdate(const UpdateInfo &_info, { IGN_PROFILE("Sensors::PostUpdate"); - // \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; - } - { std::unique_lock lock(this->dataPtr->renderMutex); if (!this->dataPtr->initialized && @@ -814,6 +827,7 @@ std::string Sensors::CreateSensor(const Entity &_entity, IGNITION_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, + Sensors::ISystemReset, Sensors::ISystemUpdate, Sensors::ISystemPostUpdate ) diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index 87e317bc6c..afbe42aa06 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -56,6 +56,7 @@ namespace systems class Sensors: public System, public ISystemConfigure, + public ISystemReset, public ISystemUpdate, public ISystemPostUpdate { @@ -71,6 +72,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; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 0631d4539d..5b58a3a267 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -82,6 +82,7 @@ set(tests_needing_display distortion_camera.cc gpu_lidar.cc optical_tactile_plugin.cc + reset_sensors.cc rgbd_camera.cc sensors_system.cc sensors_system_battery.cc diff --git a/test/integration/reset.cc b/test/integration/reset.cc index df2d8e3fe4..6e0624cc6a 100644 --- a/test/integration/reset.cc +++ b/test/integration/reset.cc @@ -125,7 +125,7 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) ASSERT_NE(kNullEntity, entity); auto poseComp = ecm->Component(entity); ASSERT_NE(nullptr, poseComp); - EXPECT_FLOAT_EQ(poseComp->Data().Z(), 1.1f); + EXPECT_FLOAT_EQ(poseComp->Data().Z(), 5.0f); EXPECT_EQ(1u, this->mockSystem->configureCallCount); EXPECT_EQ(0u, this->mockSystem->resetCallCount); @@ -142,7 +142,7 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) ASSERT_NE(kNullEntity, entity); auto poseComp = ecm->Component(entity); ASSERT_NE(nullptr, poseComp); - EXPECT_LT(poseComp->Data().Z(), 1.1f); + EXPECT_LT(poseComp->Data().Z(), 5.0f); EXPECT_EQ(1u, this->mockSystem->configureCallCount); EXPECT_EQ(0u, this->mockSystem->resetCallCount); @@ -180,7 +180,7 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) ASSERT_NE(kNullEntity, entity); auto poseComp = ecm->Component(entity); ASSERT_NE(nullptr, poseComp); - EXPECT_FLOAT_EQ(poseComp->Data().Z(), 1.1f); + EXPECT_FLOAT_EQ(poseComp->Data().Z(), 5.0f); EXPECT_EQ(1u, this->mockSystem->configureCallCount); EXPECT_EQ(1u, this->mockSystem->resetCallCount); @@ -198,7 +198,7 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) ASSERT_NE(kNullEntity, entity); auto poseComp = ecm->Component(entity); ASSERT_NE(nullptr, poseComp); - EXPECT_LT(poseComp->Data().Z(), 1.1f); + EXPECT_LT(poseComp->Data().Z(), 5.0f); EXPECT_EQ(1u, this->mockSystem->configureCallCount); EXPECT_EQ(1u, this->mockSystem->resetCallCount); diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc new file mode 100644 index 0000000000..9d06949398 --- /dev/null +++ b/test/integration/reset_sensors.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_sensors.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(), 5.0f); + + 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(), 5.0f); + + 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(), 5.0f); + + 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(), 5.0f); + + 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/worlds/reset.sdf b/test/worlds/reset.sdf index 10c6c63426..8a2894514e 100644 --- a/test/worlds/reset.sdf +++ b/test/worlds/reset.sdf @@ -19,17 +19,17 @@ - 0 0 1.1 0 0 0 + 0 0 5.0 0 0 0 false - 5 5 2.5 + 1 1 1 - 5 5 2.5 + 1 1 1 0 1 0 0.8 diff --git a/test/worlds/reset_sensors.sdf b/test/worlds/reset_sensors.sdf new file mode 100644 index 0000000000..bcc6baa338 --- /dev/null +++ b/test/worlds/reset_sensors.sdf @@ -0,0 +1,130 @@ + + + + + + + + + + + ogre2 + + + + + + 0 0 5.0 0 0 0 + false + + + + 1 1 1 + + + + + 1 1 1 + + + 0 1 0 0.8 + 0 1 0 0.8 + 1 1 1 0.8 + + + + + + + true + 10 0 1.0 0 0 -3.1415 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 0.1 0 0 0 0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 30 + camera + + + + + + true + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 1 + 30 + true + + 123 + + + 0 + 0 + + + + + + + + + From 825c280042aa2e634017259820d1f88e2e2d3f54 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 31 Mar 2022 16:07:13 -0500 Subject: [PATCH 18/54] Remove debug outputs Signed-off-by: Michael Carroll --- src/rendering/RenderUtil.cc | 112 ------------------------------------ 1 file changed, 112 deletions(-) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 46d64cfbcd..3dafd80ce1 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1645,7 +1645,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const std::string segmentationCameraSuffix{"/segmentation"}; const std::string wideAngleCameraSuffix{"/image"}; - int nNewScenes = 0; // Get all the new worlds // TODO(anyone) Only one scene is supported for now // extend the sensor system to support mutliple scenes in the future @@ -1654,14 +1653,12 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::World *, const components::Scene *_scene)->bool { - nNewScenes++; this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); return true; }); - int nNewModels = 0; _ecm.Each( [&](const Entity &_entity, @@ -1670,7 +1667,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { - nNewModels++; sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); @@ -1680,7 +1676,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( return true; }); - int nNewLinks = 0; _ecm.Each( [&](const Entity &_entity, @@ -1689,13 +1684,11 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { - nNewLinks++; this->CreateLink(_ecm, _entity, _name, _pose, _parent); return true; }); // visuals - int nNewVisuals = 0; _ecm.Eachbool { - nNewVisuals++; this->CreateVisual(_ecm, _entity, _name, _pose, _geom, _castShadows, _transparency, _visibilityFlags, _parent); return true; }); // actors - int nNewActors = 0; _ecm.Each( [&](const Entity &_entity, const components::Actor *_actor, const components::Name *_name, const components::ParentEntity *_parent) -> bool { - nNewActors++; this->newActors.push_back(std::make_tuple(_entity, _actor->Data(), _name->Data(), _parent->Data())); @@ -1741,32 +1731,27 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( }); // lights - int nNewLights = 0; _ecm.Each( [&](const Entity &_entity, const components::Light *_light, const components::Name *_name, const components::ParentEntity *_parent) -> bool { - nNewLights++; this->CreateLight(_ecm, _entity, _light, _name, _parent); return true; }); // inertials - int nNewInertials = 0; _ecm.Each( [&](const Entity &_entity, const components::Inertial *_inrElement, const components::Pose *) -> bool { - nNewInertials++; this->entityInertials[_entity] = _inrElement->Data(); return true; }); // collisions - int nNewCollisions = 0; _ecm.Each( @@ -1778,14 +1763,12 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::CollisionElement *_collElement, const components::ParentEntity *_parent) -> bool { - nNewCollisions++; this->entityCollisions[_entity] = _collElement->Data(); this->linkToCollisionEntities[_parent->Data()].push_back(_entity); return true; }); // joints - int nNewJoints = 0; _ecm.Each( @@ -1798,7 +1781,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::ParentLinkName *_parentLinkName, const components::ChildLinkName *_childLinkName) -> bool { - nNewJoints++; sdf::Joint joint; joint.SetName(_name->Data()); joint.SetType(_jointType->Data()); @@ -1825,24 +1807,16 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( }); // particle emitters - int nNewEmitters = 0; _ecm.Each( [&](const Entity &_entity, const components::ParticleEmitter *_emitter, const components::ParentEntity *_parent) -> bool { - nNewEmitters++; this->newParticleEmitters.push_back( std::make_tuple(_entity, _emitter->Data(), _parent->Data())); return true; }); - int nNewCameras = 0; - int nNewDepthCameras = 0; - int nNewRgbdCameras = 0; - int nNewGpuLidars = 0; - int nNewThermalCameras = 0; - int nNewSegmentationCameras = 0; if (this->enableSensors) { // Create cameras @@ -1851,7 +1825,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::Camera *_camera, const components::ParentEntity *_parent)->bool { - nNewCameras++; this->AddNewSensor(_ecm, _entity, _camera->Data(), _parent->Data(), cameraSuffix); return true; @@ -1863,7 +1836,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::DepthCamera *_depthCamera, const components::ParentEntity *_parent)->bool { - nNewDepthCameras++; this->AddNewSensor(_ecm, _entity, _depthCamera->Data(), _parent->Data(), depthCameraSuffix); return true; @@ -1875,7 +1847,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::RgbdCamera *_rgbdCamera, const components::ParentEntity *_parent)->bool { - nNewRgbdCameras++; this->AddNewSensor(_ecm, _entity, _rgbdCamera->Data(), _parent->Data(), rgbdCameraSuffix); return true; @@ -1887,7 +1858,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::GpuLidar *_gpuLidar, const components::ParentEntity *_parent)->bool { - nNewGpuLidars++; this->AddNewSensor(_ecm, _entity, _gpuLidar->Data(), _parent->Data(), gpuLidarSuffix); return true; @@ -1899,7 +1869,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::ThermalCamera *_thermalCamera, const components::ParentEntity *_parent)->bool { - nNewThermalCameras++; this->AddNewSensor(_ecm, _entity, _thermalCamera->Data(), _parent->Data(), thermalCameraSuffix); return true; @@ -1911,7 +1880,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const components::SegmentationCamera *_segmentationCamera, const components::ParentEntity *_parent)->bool { - nNewSegmentationCameras++; this->AddNewSensor(_ecm, _entity, _segmentationCamera->Data(), _parent->Data(), segmentationCameraSuffix); return true; @@ -1928,30 +1896,6 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( return true; }); } - - if (nNewScenes || nNewModels || nNewLinks || nNewVisuals || nNewActors || - nNewLights || nNewInertials || nNewCollisions || nNewJoints || nNewEmitters || - nNewCameras || nNewDepthCameras || nNewRgbdCameras || nNewGpuLidars || - nNewThermalCameras || nNewSegmentationCameras) - { - igndbg << "CreateEntitiesFirstUpdate: \n"; - igndbg << "\tNew Scenes: " << nNewScenes << "\n"; - igndbg << "\tNew Models: " << nNewModels << "\n"; - igndbg << "\tNew Links: " << nNewLinks << "\n"; - igndbg << "\tNew Visuals: " << nNewVisuals << "\n"; - igndbg << "\tNew Actors: " << nNewActors << "\n"; - igndbg << "\tNew Lights: " << nNewLights << "\n"; - igndbg << "\tNew Inertials: " << nNewInertials << "\n"; - igndbg << "\tNew Collisions: " << nNewCollisions << "\n"; - igndbg << "\tNew Joints: " << nNewJoints << "\n"; - igndbg << "\tNew Emitters: " << nNewEmitters << "\n"; - igndbg << "\tNew Cameras: " << nNewCameras << "\n"; - igndbg << "\tNew DepthCameras: " << nNewDepthCameras << "\n"; - igndbg << "\tNew RgbdCameras: " << nNewRgbdCameras << "\n"; - igndbg << "\tNew GpuLidars: " << nNewGpuLidars<< "\n"; - igndbg << "\tNew ThermalCameras: " << nNewThermalCameras<< "\n"; - igndbg << "\tNew SegmentationCameras: " << nNewSegmentationCameras<< "\n"; - } } ////////////////////////////////////////////////// @@ -1969,20 +1913,17 @@ void RenderUtilPrivate::CreateEntitiesRuntime( // Get all the new worlds // TODO(anyone) Only one scene is supported for now // extend the sensor system to support mutliple scenes in the future - int nNewScenes = 0; _ecm.EachNew( [&](const Entity & _entity, const components::World *, const components::Scene *_scene)->bool { - nNewScenes++; this->sceneManager.SetWorldId(_entity); const sdf::Scene &sceneSdf = _scene->Data(); this->newScenes.push_back(sceneSdf); return true; }); - int nNewModels = 0; _ecm.EachNew( [&](const Entity &_entity, @@ -1991,7 +1932,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { - nNewModels++; sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); @@ -2001,7 +1941,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( return true; }); - int nNewLinks = 0; _ecm.EachNew( [&](const Entity &_entity, @@ -2010,13 +1949,11 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Pose *_pose, const components::ParentEntity *_parent)->bool { - nNewLinks++; this->CreateLink(_ecm, _entity, _name, _pose, _parent); return true; }); // visuals - int nNewVisuals = 0; _ecm.EachNewbool { - nNewVisuals++; this->CreateVisual(_ecm, _entity, _name, _pose, _geom, _castShadows, _transparency, _visibilityFlags, _parent); return true; }); // actors - int nNewActors = 0; _ecm.EachNew( [&](const Entity &_entity, const components::Actor *_actor, const components::Name *_name, const components::ParentEntity *_parent) -> bool { - nNewActors++; this->newActors.push_back( std::make_tuple(_entity, _actor->Data(), _name->Data(), _parent->Data())); @@ -2063,32 +1997,27 @@ void RenderUtilPrivate::CreateEntitiesRuntime( }); // lights - int nNewLights = 0; _ecm.EachNew( [&](const Entity &_entity, const components::Light *_light, const components::Name *_name, const components::ParentEntity *_parent) -> bool { - nNewLights++; this->CreateLight(_ecm, _entity, _light, _name, _parent); return true; }); // inertials - int nNewInertials = 0; _ecm.EachNew( [&](const Entity &_entity, const components::Inertial *_inrElement, const components::Pose *) -> bool { - nNewInertials++; this->entityInertials[_entity] = _inrElement->Data(); return true; }); // collisions - int nNewCollisions = 0; _ecm.EachNew( @@ -2100,14 +2029,12 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::CollisionElement *_collElement, const components::ParentEntity *_parent) -> bool { - nNewCollisions++; this->entityCollisions[_entity] = _collElement->Data(); this->linkToCollisionEntities[_parent->Data()].push_back(_entity); return true; }); // joints - int nNewJoints = 0; _ecm.EachNew( @@ -2120,7 +2047,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::ParentLinkName *_parentLinkName, const components::ChildLinkName *_childLinkName) -> bool { - nNewJoints++; sdf::Joint joint; joint.SetName(_name->Data()); joint.SetType(_jointType->Data()); @@ -2147,24 +2073,16 @@ void RenderUtilPrivate::CreateEntitiesRuntime( }); // particle emitters - int nNewEmitters = 0; _ecm.EachNew( [&](const Entity &_entity, const components::ParticleEmitter *_emitter, const components::ParentEntity *_parent) -> bool { - nNewEmitters++; this->newParticleEmitters.push_back( std::make_tuple(_entity, _emitter->Data(), _parent->Data())); return true; }); - int nNewCameras = 0; - int nNewDepthCameras = 0; - int nNewRgbdCameras = 0; - int nNewGpuLidars = 0; - int nNewThermalCameras = 0; - int nNewSegmentationCameras = 0; if (this->enableSensors) { // Create cameras @@ -2173,7 +2091,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::Camera *_camera, const components::ParentEntity *_parent)->bool { - nNewCameras++; this->AddNewSensor(_ecm, _entity, _camera->Data(), _parent->Data(), cameraSuffix); return true; @@ -2185,7 +2102,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::DepthCamera *_depthCamera, const components::ParentEntity *_parent)->bool { - nNewDepthCameras++; this->AddNewSensor(_ecm, _entity, _depthCamera->Data(), _parent->Data(), depthCameraSuffix); return true; @@ -2197,7 +2113,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::RgbdCamera *_rgbdCamera, const components::ParentEntity *_parent)->bool { - nNewRgbdCameras++; this->AddNewSensor(_ecm, _entity, _rgbdCamera->Data(), _parent->Data(), rgbdCameraSuffix); return true; @@ -2209,7 +2124,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::GpuLidar *_gpuLidar, const components::ParentEntity *_parent)->bool { - nNewGpuLidars++; this->AddNewSensor(_ecm, _entity, _gpuLidar->Data(), _parent->Data(), gpuLidarSuffix); return true; @@ -2221,7 +2135,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::ThermalCamera *_thermalCamera, const components::ParentEntity *_parent)->bool { - nNewThermalCameras++; this->AddNewSensor(_ecm, _entity, _thermalCamera->Data(), _parent->Data(), thermalCameraSuffix); return true; @@ -2233,7 +2146,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const components::SegmentationCamera *_segmentationCamera, const components::ParentEntity *_parent)->bool { - nNewSegmentationCameras++; this->AddNewSensor(_ecm, _entity, _segmentationCamera->Data(), _parent->Data(), segmentationCameraSuffix); return true; @@ -2250,30 +2162,6 @@ void RenderUtilPrivate::CreateEntitiesRuntime( return true; }); } - - if (nNewScenes || nNewModels || nNewLinks || nNewVisuals || nNewActors || - nNewLights || nNewInertials || nNewCollisions || nNewJoints || nNewEmitters || - nNewCameras || nNewDepthCameras || nNewRgbdCameras || nNewGpuLidars || - nNewThermalCameras || nNewSegmentationCameras) - { - igndbg << "CreateEntitiesRuntime: \n"; - igndbg << "\tNew Scenes: " << nNewScenes << "\n"; - igndbg << "\tNew Models: " << nNewModels << "\n"; - igndbg << "\tNew Links: " << nNewLinks << "\n"; - igndbg << "\tNew Visuals: " << nNewVisuals << "\n"; - igndbg << "\tNew Actors: " << nNewActors << "\n"; - igndbg << "\tNew Lights: " << nNewLights << "\n"; - igndbg << "\tNew Inertials: " << nNewInertials << "\n"; - igndbg << "\tNew Collisions: " << nNewCollisions << "\n"; - igndbg << "\tNew Joints: " << nNewJoints << "\n"; - igndbg << "\tNew Emitters: " << nNewEmitters << "\n"; - igndbg << "\tNew Cameras: " << nNewCameras << "\n"; - igndbg << "\tNew DepthCameras: " << nNewDepthCameras << "\n"; - igndbg << "\tNew RgbdCameras: " << nNewRgbdCameras << "\n"; - igndbg << "\tNew GpuLidars: " << nNewGpuLidars<< "\n"; - igndbg << "\tNew ThermalCameras: " << nNewThermalCameras<< "\n"; - igndbg << "\tNew SegmentationCameras: " << nNewSegmentationCameras<< "\n"; - } } ////////////////////////////////////////////////// From 68f6f85524f7a83aaae11d82892ffde63bad7f61 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 31 Mar 2022 18:11:42 -0500 Subject: [PATCH 19/54] Address reviewer feedback Signed-off-by: Michael Carroll --- src/ServerConfig.cc | 3 --- src/SystemInternal.hh | 7 ++++++- src/SystemManager.cc | 19 +++++++++---------- src/SystemManager.hh | 15 ++++++++++++--- src/systems/sensors/Sensors.cc | 14 ++++++-------- test/integration/reset_sensors.cc | 10 +++++----- 6 files changed, 38 insertions(+), 30 deletions(-) diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index dbacc09f3a..1024eb5d11 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -202,9 +202,6 @@ class ignition::gazebo::ServerConfigPrivate this->timestamp = IGN_SYSTEM_TIME(); - igndbg << "Home: " << home << std::endl; - igndbg << "To ISO: " << common::timeToIso(this->timestamp) << std::endl; - // Set a default log record path this->logRecordPath = common::joinPaths(home, ".ignition", "gazebo", "log", common::timeToIso(this->timestamp)); diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 3162fa1ec7..caadeb1e36 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -105,13 +106,17 @@ namespace ignition /// system during the `Configure` call. public: Entity parentEntity = {kNullEntity}; + /// \brief Cached filename of the plugin used when system was loaded. + /// Used for reloading a system at runtime. public: std::string fname = ""; + /// \brief Cached plugin name of the plugin used when system was loaded. + /// Used for reloading a system at runtime. public: std::string name = ""; /// \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 sdf = nullptr; + 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 index cef4088b1a..14706d128a 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -49,8 +49,8 @@ void SystemManager::LoadPlugin(const Entity _entity, ss.entity = _entity; ss.fname = _fname; ss.name = _name; - ss.sdf = _sdf; - this->AddSystemImpl(ss, ss.entity, ss.sdf); + ss.configureSdf = _sdf; + this->AddSystemImpl(ss, ss.entity, ss.configureSdf); igndbg << "Loaded system [" << _name << "] for entity [" << _entity << "]" << std::endl; } @@ -121,14 +121,12 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) this->systemsUpdate.clear(); this->systemsPostupdate.clear(); - this->entityCompMgr = &_ecm; - - for (auto& system: this->systems) + for (auto& system : this->systems) { if (nullptr != system.reset) { // If implemented, call reset and add to pending systems. - system.reset->Reset(_info, *this->entityCompMgr); + system.reset->Reset(_info, _ecm); { std::lock_guard lock(this->pendingSystemsMutex); @@ -146,10 +144,11 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) continue; } - this->LoadPlugin(system.entity, - system.fname, - system.name, - system.sdf); + sdf::ElementPtr configureCopy(system.configureSdf->Clone()); + this->LoadPlugin(system.entity, + system.fname, + system.name, + configureCopy); } } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 581b54b0d1..d8b7d6b28d 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -92,9 +92,18 @@ namespace ignition /// \return The number of newly-active systems public: size_t ActivatePendingSystems(); - /// \brief Reset all managed systems - /// \param[in] _info Update information for the reset time - /// \param[in] _ecm ECM information for the reset time + /// \brief Perform a reset on all systems + /// + /// If a system implements the ISystemReset interface, it will be called. + // + /// Otherwise, if a system does not have the ISystemReset interface + /// implemented, and was created via loading a plugin, + /// that plugin will be reloaded. + /// + /// Otherwise, if a system is created from in-memory rather than a plugin, + /// that system will remain unaffected. + /// \param[in] _info Update info corresponding to the update time + /// \param[in] _ecm Version of the _ecm reset to an initial state public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm); /// \brief Get an vector of all systems implementing "Configure" diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 245fcf5855..cf2e0cdae0 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -512,7 +512,7 @@ void Sensors::Configure(const Entity &/*_id*/, } ////////////////////////////////////////////////// -void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) +void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) { IGN_PROFILE("Sensors::Reset"); @@ -520,18 +520,16 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) { igndbg << "Resetting Sensors\n"; - this->dataPtr->sensorMaskMutex.lock(); - this->dataPtr->sensorMask.clear(); - this->dataPtr->sensorMaskMutex.unlock(); - - auto time = math::durationToSecNsec(_info.simTime); - auto t = math::secNsecToDuration(time.first, time.second); + { + std::lock_guard lock(this->dataPtr->sensorMaskMutex); + this->dataPtr->sensorMask.clear(); + } for (auto id : this->dataPtr->sensorIds) { sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); auto rs = dynamic_cast(s); - rs->SetNextDataUpdateTime(t); + rs->SetNextDataUpdateTime(_info.simTime); } } } diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc index 9d06949398..27da0a6b52 100644 --- a/test/integration/reset_sensors.cc +++ b/test/integration/reset_sensors.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. @@ -92,12 +92,12 @@ void worldReset() ///////////////////////////////////////////////// /// 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)) +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_sensors.sdf"; + const std::string sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "reset_sensors.sdf"); serverConfig.SetSdfFile(sdfFile); @@ -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 3799ff3f5e4030e0f0351c56b05fa6a06295fd87 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 3 May 2022 11:31:58 -0500 Subject: [PATCH 20/54] 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 21/54] 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 22/54] 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 bf2ea81e5c3cbf35a41ac44c4c63344bb74ec84b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 13 May 2022 18:38:52 +0200 Subject: [PATCH 23/54] Download models in the background Signed-off-by: ahcorde --- examples/worlds/fuel.sdf | 98 +----------- include/ignition/gazebo/Server.hh | 14 ++ include/ignition/gazebo/ServerConfig.hh | 18 +++ src/LevelManager.cc | 6 + src/LevelManager.hh | 5 +- src/SdfEntityCreator.cc | 55 +++++-- src/Server.cc | 146 ++++++++++++++---- src/ServerConfig.cc | 36 ++++- src/ServerPrivate.cc | 4 + src/ServerPrivate.hh | 7 + src/SimulationRunner.cc | 52 +++++++ src/SimulationRunner.hh | 21 +++ src/ign.cc | 5 +- .../scene_broadcaster/SceneBroadcaster.cc | 17 ++ .../scene_broadcaster/SceneBroadcaster.hh | 8 +- test/worlds/models/building_L1/model.sdf | 14 +- 16 files changed, 357 insertions(+), 149 deletions(-) diff --git a/examples/worlds/fuel.sdf b/examples/worlds/fuel.sdf index 4d50302e1a..c4bcd34e91 100644 --- a/examples/worlds/fuel.sdf +++ b/examples/worlds/fuel.sdf @@ -17,50 +17,16 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + - - 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 - 0.8 0.8 0.8 1 - - - - + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Ground Plane + - Double pendulum -3 0 0 0 0 0 https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base @@ -73,15 +39,12 @@ - Gazebo (relative paths) 2 5 0 0 0 0 https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Gazebo - relative paths - Actor Test - 0 0 0 0 0 0 https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/actor - relative paths @@ -107,54 +70,5 @@ - - - true - - - - - false - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/dirt_diffusespecular.png - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/flat_normal.png - 10 - - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/grass_diffusespecular.png - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/flat_normal.png - 10 - - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/fungus_diffusespecular.png - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/flat_normal.png - 10 - - - 28 - 6 - - - 35 - 18 - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/fortress_heightmap.png - 257 257 50 - 0 0 -28 - - - - - - - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/fortress_heightmap.png - 257 257 50 - 0 0 -28 - - - - - - diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index 088fda99d8..5b98aafa09 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -120,6 +120,20 @@ namespace ignition /// \brief Destructor public: ~Server(); + /// \brief Construct the server using the parameters specified in a + /// ServerConfig and choose it should download models in the background + /// \param[in] _config Server configuration parameters. If this + /// parameter is omitted, then an empty world is loaded. + public: Server(bool _downloadInParallel, + const ServerConfig &_config = ServerConfig()); + + /// \brief Initialize server + private: void Init(); + + /// \brief Download models from the sdf + /// \return True if everything was fine False otherwise + private: bool DownloadModels(); + /// \brief Set the update period. The update period is the wall-clock time /// between ECS updates. /// Note that this is different from the simulation update rate. ECS diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 7311d092e8..93fecb22b6 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -213,6 +213,24 @@ namespace ignition /// an UpdateRate has not been set. public: std::optional UpdateRate() const; + /// \brief Set the run option + /// \return True if -r is setted or False otherwise + public: void SetRunOption(bool _run); + + /// \brief Get the run option + /// \return True if -r is setted or False otherwise + public: bool RunOption() const; + + /// \brief True if the models are downloaded in a thread false otherwise + /// \param _downloadInParallel True if the models are downloaded in a + /// thread false otherwise + public: void SetDownloadInParallel(bool _downloadInParallel); + + /// \brief Return True if the models are downloaded in a thread false + /// otherwise + /// \return True if the models are downloaded in a thread false otherwise + public: bool DownloadInParallel() const; + /// \brief Get whether the server is using the level system /// \return True if the server is set to use the level system public: bool UseLevels() const; diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 81fa242484..b1b01b9947 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -514,6 +514,12 @@ void LevelManager::ConfigureDefaultLevel() this->entityCreator->SetParent(defaultLevel, this->worldEntity); } +///////////////////////////////////////////////// +void LevelManager::AddWorld(const sdf::World *_world) +{ + this->entityCreator->CreateEntities(_world); +} + ///////////////////////////////////////////////// void LevelManager::CreatePerformers() { diff --git a/src/LevelManager.hh b/src/LevelManager.hh index 556ad87d29..5be36beaed 100644 --- a/src/LevelManager.hh +++ b/src/LevelManager.hh @@ -88,6 +88,10 @@ namespace ignition /// every update cycle public: void UpdateLevelsState(); + /// \brief This will add a world to the scene + /// \param[in] _world SDF world to add to the scene + public: void AddWorld(const sdf::World *_world); + /// \brief Load entities that have been marked for loading. /// \param[in] _namesToLoad List of of entity names to load private: void LoadActiveEntities( @@ -184,4 +188,3 @@ namespace ignition } // IGNITION_GAZEBO_LEVELMANAGER_HH #endif - diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d77dd33c36..e4022f43db 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -220,19 +220,52 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) { IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::World)"); - // World entity - Entity worldEntity = this->dataPtr->ecm->CreateEntity(); - - // World components - this->dataPtr->ecm->CreateComponent(worldEntity, components::World()); - this->dataPtr->ecm->CreateComponent(worldEntity, - components::Name(_world->Name())); - - // scene - if (_world->Scene()) + // In the update loop, you can for example get the name of the world and set + // it as a property that can be read from the QML. + Entity worldEntity; + this->dataPtr->ecm->Each( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Name *_name, + const ignition::gazebo::components::World *_w)->bool { + std::cerr << "/worldEntity " << worldEntity << '\n'; + worldEntity = _entity; + // worldComp = _w; + std::cerr << "/_name " << _name->Data() << '\n'; + // if (_name == "empty_world") + // { + // _name = _world->Name(); + // } + // std::cerr << "/_name " << _name << '\n'; + + return false; + }); + + components::World *worldComp; + components::Name *nameComp; + + worldComp = this->dataPtr->ecm->Component(worldEntity); + nameComp = this->dataPtr->ecm->Component(worldEntity); + // worldComp->Data()->SetName(_world->Name()); + nameComp->Data() = _world->Name(); + + if (!worldComp) + { + // World entity + worldEntity = this->dataPtr->ecm->CreateEntity(); + + // World components + this->dataPtr->ecm->CreateComponent(worldEntity, components::World()); this->dataPtr->ecm->CreateComponent(worldEntity, - components::Scene(*_world->Scene())); + components::Name(_world->Name())); + + // scene + if (_world->Scene()) + { + this->dataPtr->ecm->CreateComponent(worldEntity, + components::Scene(*_world->Scene())); + } } // atmosphere diff --git a/src/Server.cc b/src/Server.cc index 87381ee516..845b63daa7 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -43,7 +43,7 @@ struct DefaultWorld { static std::string world = std::string("" "" - "") + + "") + "" ""; @@ -56,29 +56,30 @@ Server::Server(const ServerConfig &_config) : dataPtr(new ServerPrivate) { this->dataPtr->config = _config; + this->Init(); +} - // Configure the fuel client - fuel_tools::ClientConfig config; - if (!_config.ResourceCache().empty()) - config.SetCacheLocation(_config.ResourceCache()); - this->dataPtr->fuelClient = std::make_unique(config); - - // Configure SDF to fetch assets from ignition fuel. - sdf::setFindCallback(std::bind(&ServerPrivate::FetchResource, - this->dataPtr.get(), std::placeholders::_1)); - common::addFindFileURICallback(std::bind(&ServerPrivate::FetchResourceUri, - this->dataPtr.get(), std::placeholders::_1)); - - addResourcePaths(); +///////////////////////////////////////////////// +Server::Server(bool _downloadInParallel, const ServerConfig &_config) + : dataPtr(new ServerPrivate) +{ + this->dataPtr->config = _config; + this->dataPtr->downloadInParallel = _downloadInParallel; + this->Init(); +} +///////////////////////////////////////////////// +bool Server::DownloadModels() +{ sdf::Errors errors; - - switch (_config.Source()) + std::cerr << "this->dataPtr->config.Source() " + << static_cast(this->dataPtr->config.Source()) << '\n'; + switch (this->dataPtr->config.Source()) { // Load a world if specified. Check SDF string first, then SDF file case ServerConfig::SourceType::kSdfRoot: { - this->dataPtr->sdfRoot = _config.SdfRoot()->Clone(); + this->dataPtr->sdfRoot = this->dataPtr->config.SdfRoot()->Clone(); ignmsg << "Loading SDF world from SDF DOM.\n"; break; } @@ -86,40 +87,69 @@ Server::Server(const ServerConfig &_config) case ServerConfig::SourceType::kSdfString: { std::string msg = "Loading SDF string. "; - if (_config.SdfFile().empty()) + if (this->dataPtr->config.SdfFile().empty()) { msg += "File path not available.\n"; } else { - msg += "File path [" + _config.SdfFile() + "].\n"; + msg += "File path [" + this->dataPtr->config.SdfFile() + "].\n"; } ignmsg << msg; - errors = this->dataPtr->sdfRoot.LoadSdfString(_config.SdfString()); + errors = this->dataPtr->sdfRoot.LoadSdfString( + this->dataPtr->config.SdfString()); break; } case ServerConfig::SourceType::kSdfFile: { - std::string filePath = resolveSdfWorldFile(_config.SdfFile(), - _config.ResourceCache()); + std::string filePath = resolveSdfWorldFile(this->dataPtr->config.SdfFile(), + this->dataPtr->config.ResourceCache()); if (filePath.empty()) { - ignerr << "Failed to find world [" << _config.SdfFile() << "]" + ignerr << "Failed to find world [" + << this->dataPtr->config.SdfFile() << "]" << std::endl; - return; + return false; } ignmsg << "Loading SDF world file[" << filePath << "].\n"; - // \todo(nkoenig) Async resource download. - // This call can block for a long period of time while - // resources are downloaded. Blocking here causes the GUI to block with - // a black screen (search for "Async resource download" in - // 'src/gui_main.cc'. + while (this->dataPtr->downloadInParallel && + this->dataPtr->simRunners.size() == 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + errors = this->dataPtr->sdfRoot.Load(filePath); - break; + + if (!this->dataPtr->downloadInParallel) + return true; + + if (this->dataPtr->sdfRoot.WorldCount() == 0) + { + ignerr << "There is no world available" << "\n"; + return false; + } + + while (this->dataPtr->simRunners.size() == 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + for (std::unique_ptr &runner : + this->dataPtr->simRunners) + { + for (size_t j = 0; j < this->dataPtr->sdfRoot.WorldCount(); ++j) + { + runner->AddWorld(this->dataPtr->sdfRoot.WorldByIndex(j)); + } + runner->SetFetchedAllIncludes(true); + } + ignmsg << "Download models in parallel has finished. " + << "Now you can start the simulation" << std::endl; + break; } case ServerConfig::SourceType::kNone: @@ -133,6 +163,54 @@ Server::Server(const ServerConfig &_config) } } + if (!errors.empty()) + { + for (auto &err : errors) + ignerr << err << "\n"; + return false; + } + return true; +} + +///////////////////////////////////////////////// +void Server::Init() +{ + // Configure the fuel client + fuel_tools::ClientConfig config; + if (!this->dataPtr->config.ResourceCache().empty()) + config.SetCacheLocation(this->dataPtr->config.ResourceCache()); + this->dataPtr->fuelClient = std::make_unique(config); + + // Configure SDF to fetch assets from ignition fuel. + sdf::setFindCallback(std::bind(&ServerPrivate::FetchResource, + this->dataPtr.get(), std::placeholders::_1)); + common::addFindFileURICallback(std::bind(&ServerPrivate::FetchResourceUri, + this->dataPtr.get(), std::placeholders::_1)); + + addResourcePaths(); + + sdf::Errors errors; + + if (this->dataPtr->downloadInParallel) + { + this->dataPtr->downloadModelsThread = std::thread([&]() + { + this->DownloadModels(); + }); + + ignmsg << "Loading default world.\n"; + // Load an empty world. + /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. + errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World()); + } + else + { + if (!this->DownloadModels()) + { + return; + } + } + if (!errors.empty()) { for (auto &err : errors) @@ -141,18 +219,18 @@ Server::Server(const ServerConfig &_config) } // Add record plugin - if (_config.UseLogRecord()) + if (this->dataPtr->config.UseLogRecord()) { - this->dataPtr->AddRecordPlugin(_config); + this->dataPtr->AddRecordPlugin(this->dataPtr->config); } this->dataPtr->CreateEntities(); // Set the desired update period, this will override the desired RTF given in // the world file which was parsed by CreateEntities. - if (_config.UpdatePeriod()) + if (this->dataPtr->config.UpdatePeriod()) { - this->SetUpdatePeriod(_config.UpdatePeriod().value()); + this->SetUpdatePeriod(this->dataPtr->config.UpdatePeriod().value()); } // Establish publishers and subscribers. diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 1024eb5d11..37529e4050 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -232,12 +232,15 @@ class ignition::gazebo::ServerConfigPrivate physicsEngine(_cfg->physicsEngine), renderEngineServer(_cfg->renderEngineServer), renderEngineGui(_cfg->renderEngineGui), + running(_cfg->running), + downloadInParallel(_cfg->downloadInParallel), plugins(_cfg->plugins), networkRole(_cfg->networkRole), networkSecondaries(_cfg->networkSecondaries), seed(_cfg->seed), logRecordTopics(_cfg->logRecordTopics), - isHeadlessRendering(_cfg->isHeadlessRendering) { } + isHeadlessRendering(_cfg->isHeadlessRendering), + source(_cfg->source) { } // \brief The SDF file that the server should load public: std::string sdfFile = ""; @@ -281,6 +284,13 @@ class ignition::gazebo::ServerConfigPrivate /// will be used. public: std::string renderEngineGui = ""; + /// \brief This is used to indicate that Run has been called, and the + /// server is in the run state. + public: bool running{false}; + + /// \brief True if models are downloaded in the background, false otherwise + public: bool downloadInParallel{false}; + /// \brief List of plugins to load. public: std::list plugins; @@ -383,6 +393,30 @@ std::optional return std::nullopt; } +///////////////////////////////////////////////// +bool ServerConfig::RunOption() const +{ + return this->dataPtr->running; +} + +///////////////////////////////////////////////// +void ServerConfig::SetRunOption(bool _run) +{ + this->dataPtr->running = _run; +} + +///////////////////////////////////////////////// +void ServerConfig::SetDownloadInParallel(bool _downloadInParallel) +{ + this->dataPtr->downloadInParallel = _downloadInParallel; +} + +///////////////////////////////////////////////// +bool ServerConfig::DownloadInParallel() const +{ + return this->dataPtr->downloadInParallel; +} + ///////////////////////////////////////////////// bool ServerConfig::UseLevels() const { diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 7bd36065ce..a2b68e7245 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -102,6 +102,10 @@ ServerPrivate::~ServerPrivate() { this->stopThread->join(); } + if (this->downloadModelsThread.joinable()) + { + this->downloadModelsThread.join(); + } } ////////////////////////////////////////////////// diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index b7f23c26d9..62e18c360c 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -164,6 +164,13 @@ namespace ignition /// Server. It is used in the SDFormat world generator when saving worlds public: std::unordered_map fuelUriMap; + /// \brief Thread to download the models + public: std::thread downloadModelsThread; + + /// \brief True if models are downloaded in the background, false + /// otherwise + public: bool downloadInParallel{false}; + /// \brief List of names for all worlds loaded in this server. private: std::vector worldNames; diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 0481a2df21..ef7953e954 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -721,6 +721,9 @@ bool SimulationRunner::Run(const uint64_t _iterations) // Keep number of iterations requested by caller uint64_t processedIterations{0}; + bool isInitialRunOfSimulationSet = false; + bool initialize = false; + // Execute all the systems until we are told to stop, or the number of // iterations is reached. while (this->running && (_iterations == 0 || @@ -728,6 +731,36 @@ bool SimulationRunner::Run(const uint64_t _iterations) { IGN_PROFILE("SimulationRunner::Run - Iteration"); + std::lock_guard lock(this->mutexDownloadParallel); + + // If the models are still being downloaded, it doesn't allow to start + // the simulation + if (this->serverConfig.DownloadInParallel() && !this->FetchedAllIncludes()) + { + this->SetPaused(true); + } + else + { + // when the models are downloaded we should set which is the run option + // used by the user. + if (!isInitialRunOfSimulationSet) + { + isInitialRunOfSimulationSet = true; + // this->SetPaused(!this->serverConfig.RunOption()); + + this->initialEntityCompMgr.CopyFrom(this->entityCompMgr); + + WorldControl control; + control.rewind = true; + control.pause = !this->serverConfig.RunOption(); + { + std::lock_guard lockBuffer(this->msgBufferMutex); + this->worldControls.push_back(control); + } + // this->requestedRewind = true; + } + } + // Update the step size and desired rtf this->UpdatePhysicsParams(); @@ -1487,3 +1520,22 @@ void SimulationRunner::SetNextStepAsBlockingPaused(const bool value) { this->blockingPausedStepPending = value; } + +////////////////////////////////////////////////// +bool SimulationRunner::FetchedAllIncludes() const +{ + return downloadedAllModels; +} + +////////////////////////////////////////////////// +void SimulationRunner::SetFetchedAllIncludes(bool _downloadedAllModels) +{ + this->downloadedAllModels = _downloadedAllModels; +} + +////////////////////////////////////////////////// +void SimulationRunner::AddWorld(const sdf::World *_world) +{ + std::lock_guard lock(this->mutexDownloadParallel); + levelMgr->AddWorld(_world); +} diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 1b35c8c3d3..9079d87ede 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -95,6 +95,20 @@ namespace ignition /// \return True if the operation completed successfully. public: bool Run(const uint64_t _iterations); + /// \brief This will add a world to the scene + /// \param[in] _light SDF world to add to the scene + public: void AddWorld(const sdf::World *_world); + + /// \brief Check if there is any model being downloaded in the backgound. + /// \return False if there is any model being downloaded in the + /// background, True otherwise + public: bool FetchedAllIncludes() const; + + /// \brief Set if there is any model being downloaded in the backgound. + /// \param[in] _downloadedAllModels False if there is any model being + /// downloaded in the background, true otherwise. + public: void SetFetchedAllIncludes(bool _downloadedAllModels); + /// \brief Perform a simulation step: /// * Publish stats and process control messages /// * Update levels and systems @@ -537,6 +551,13 @@ namespace ignition private: std::unique_ptr newWorldControlState; private: bool resetInitiated{false}; + + /// \brief Mutex to protect the runner when a new model is inserted + private: std::mutex mutexDownloadParallel; + + /// \brief True if all model are downloaded, false otherwise + private: std::atomic downloadedAllModels{false}; + friend class LevelManager; }; } diff --git a/src/ign.cc b/src/ign.cc index 9a384281d8..03d415f352 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -129,6 +129,8 @@ extern "C" int runServer(const char *_sdfString, // Path for logs std::string recordPathMod = serverConfig.LogRecordPath(); + serverConfig.SetRunOption(_run); + // Path for compressed log, used to check for duplicates std::string cmpPath = std::string(recordPathMod); if (!std::string(1, cmpPath.back()).compare(ignition::common::separator(""))) @@ -348,7 +350,8 @@ extern "C" int runServer(const char *_sdfString, } // Create the Gazebo server - ignition::gazebo::Server server(serverConfig); + serverConfig.SetDownloadInParallel(true); + ignition::gazebo::Server server(true, serverConfig); // Run the server server.Run(true, _iterations, _run == 0); diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index ad168d2811..a431aad35d 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -256,8 +256,17 @@ void SceneBroadcaster::Configure( } this->dataPtr->worldEntity = _entity; + + if (!this->dataPtr->worldName.empty() && this->dataPtr->node) + { + std::cerr << "RESETING node" << '\n'; + this->dataPtr->node.reset(); + } + this->dataPtr->worldName = name->Data(); + std::cerr << "SceneBroadcaster::Configure " << this->dataPtr->worldName << '\n'; + auto readHertz = _sdf->Get("dynamic_pose_hertz", 60); this->dataPtr->dyPoseHertz = readHertz.first; @@ -377,6 +386,13 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, } } +////////////////////////////////////////////////// +void SceneBroadcaster::Reset(const UpdateInfo &_info, + EntityComponentManager &_manager) +{ + std::cerr << "SceneBroadcaster reset" << this->dataPtr->worldName << '\n'; +} + ////////////////////////////////////////////////// void SceneBroadcasterPrivate::PoseUpdate(const UpdateInfo &_info, const EntityComponentManager &_manager) @@ -496,6 +512,7 @@ void SceneBroadcasterPrivate::PoseUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) { + this->worldName = _worldName; auto ns = transport::TopicUtils::AsValidTopic("/world/" + _worldName); if (ns.empty()) { diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.hh b/src/systems/scene_broadcaster/SceneBroadcaster.hh index faeedd0b44..a565af7072 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.hh +++ b/src/systems/scene_broadcaster/SceneBroadcaster.hh @@ -40,7 +40,8 @@ namespace systems class SceneBroadcaster final: public System, public ISystemConfigure, - public ISystemPostUpdate + public ISystemPostUpdate, + public ISystemReset { /// \brief Constructor public: SceneBroadcaster(); @@ -54,6 +55,10 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; + // Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + // Documentation inherited public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; @@ -66,4 +71,3 @@ namespace systems } } #endif - diff --git a/test/worlds/models/building_L1/model.sdf b/test/worlds/models/building_L1/model.sdf index a238c0dfc2..8e0c5fff05 100644 --- a/test/worlds/models/building_L1/model.sdf +++ b/test/worlds/models/building_L1/model.sdf @@ -6,7 +6,7 @@ - model://building_L1/meshes/floor_1.obj + meshes/floor_1.obj @@ -15,7 +15,7 @@ 0.0 - model://building_L1/meshes/blue_linoleum.png + meshes/blue_linoleum.png @@ -23,7 +23,7 @@ - model://building_L1/meshes/floor_1.obj + meshes/floor_1.obj @@ -37,7 +37,7 @@ - model://building_L1/meshes/wall_1.obj + meshes/wall_1.obj @@ -46,15 +46,15 @@ 0.0 - model://building_L1/meshes/default.png + meshes/default.png - + - model://building_L1/meshes/wall_1.obj + meshes/wall_1.obj From e1acbb0c7a891a0059bfb6cc07e9d6ca064a074d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 17 May 2022 15:06:46 -0500 Subject: [PATCH 24/54] Address reviewer feedback Signed-off-by: Michael Carroll --- src/SystemManager.cc | 34 ++++++++++++++++++---- src/systems/sensors/Sensors.cc | 52 ++++++++++++++++++---------------- 2 files changed, 56 insertions(+), 30 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 14706d128a..afa7e40148 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -106,6 +106,19 @@ size_t SystemManager::ActivatePendingSystems() return count; } +////////////////////////////////////////////////// +/// \brief Structure to temporarily store plugin information for reset +struct PluginInfo { + /// \brief Entity plugin is attached to + Entity entity; + /// \brief Filename of the plugin library + std::string fname; + /// \brief Name of the plugin + std::string name; + /// \brief SDF element (content of the plugin tag) + sdf::ElementPtr sdf; +} + ////////////////////////////////////////////////// void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) { @@ -121,6 +134,8 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) this->systemsUpdate.clear(); this->systemsPostupdate.clear(); + std::vector pluginsToBeLoaded; + for (auto& system : this->systems) { if (nullptr != system.reset) @@ -144,15 +159,24 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) continue; } - sdf::ElementPtr configureCopy(system.configureSdf->Clone()); - this->LoadPlugin(system.entity, - system.fname, - system.name, - configureCopy); + + PluginInfo info = { + system.entity, system.fname, system.name, + system.configureSdf->Clone() + }; + + pluginsToBeLoaded.push_back(info); } } this->systems.clear(); + + // Load plugins which do not implement reset after clearing this->systems + // to ensure the previous instance is destroyed before the new one is created + // and configured. + for (const auto &pluginInfo : pluginsToBeLoaded) { + this->LoadPlugin(pluginInfo); + } this->ActivatePendingSystems(); } diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index cf2e0cdae0..7757d42a80 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -288,7 +288,6 @@ void SensorsPrivate::RunOnce() this->sensorStateChanged.clear(); } - this->sensorMaskMutex.lock(); // Check the active sensors against masked sensors. // // The internal state of a rendering sensor is not updated until the @@ -298,14 +297,16 @@ void SensorsPrivate::RunOnce() // To prevent this, add sensors that are currently being rendered to // a mask. Sensors are removed from the mask when 90% of the update // delta has passed, which will allow rendering to proceed. - for (const auto & sensor : this->activeSensors) { - // 90% of update delta (1/UpdateRate()); - auto delta = std::chrono::duration_cast< std::chrono::milliseconds>( - std::chrono::duration< double >(0.9 / sensor->UpdateRate())); - this->sensorMask[sensor->Id()] = this->updateTime + delta; + std::unique_lock lockMask(this->sensorMaskMutex); + for (const auto & sensor : this->activeSensors) + { + // 90% of update delta (1/UpdateRate()); + auto delta = std::chrono::duration_cast< std::chrono::milliseconds>( + std::chrono::duration< double >(0.9 / sensor->UpdateRate())); + this->sensorMask[sensor->Id()] = this->updateTime + delta; + } } - this->sensorMaskMutex.unlock(); { IGN_PROFILE("PreRender"); @@ -521,7 +522,7 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) igndbg << "Resetting Sensors\n"; { - std::lock_guard lock(this->dataPtr->sensorMaskMutex); + std::unique_lock lock(this->dataPtr->sensorMaskMutex); this->dataPtr->sensorMask.clear(); } @@ -578,31 +579,32 @@ void Sensors::PostUpdate(const UpdateInfo &_info, std::vector activeSensors; - this->dataPtr->sensorMaskMutex.lock(); - for (auto id : this->dataPtr->sensorIds) { - sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); - auto rs = dynamic_cast(s); - - auto it = this->dataPtr->sensorMask.find(id); - if (it != this->dataPtr->sensorMask.end()) + std::unique_lock lk(this->dataPtr->sensorMaskMutex); + for (auto id : this->dataPtr->sensorIds) { - if (it->second <= t) + sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); + auto rs = dynamic_cast(s); + + auto it = this->dataPtr->sensorMask.find(id); + if (it != this->dataPtr->sensorMask.end()) { - this->dataPtr->sensorMask.erase(it); + if (it->second <= t) + { + this->dataPtr->sensorMask.erase(it); + } + else + { + continue; + } } - else + + if (rs && rs->NextDataUpdateTime() <= t) { - continue; + activeSensors.push_back(rs); } } - - if (rs && rs->NextDataUpdateTime() <= t) - { - activeSensors.push_back(rs); - } } - this->dataPtr->sensorMaskMutex.unlock(); if (!activeSensors.empty() || this->dataPtr->renderUtil.PendingSensors() > 0) From f91188d244fbcf36f5209bce2af777229a4b7217 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 20 May 2022 21:01:43 +0200 Subject: [PATCH 25/54] improve implementation Signed-off-by: ahcorde --- src/SdfEntityCreator.cc | 54 ++++------ src/Server.cc | 45 ++++++-- src/SimulationRunner.cc | 4 +- src/SystemInternal.hh | 9 +- src/SystemManager.cc | 102 +++++++++++++++++- src/SystemManager.hh | 16 ++- .../scene_broadcaster/SceneBroadcaster.cc | 38 +++++-- 7 files changed, 212 insertions(+), 56 deletions(-) diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index dfcacb2563..bad1828942 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -220,52 +220,36 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) { IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::World)"); - // In the update loop, you can for example get the name of the world and set - // it as a property that can be read from the QML. - Entity worldEntity; + Entity worldEntity = kNullEntity; this->dataPtr->ecm->Each( [&](const ignition::gazebo::Entity &_entity, const ignition::gazebo::components::Name *_name, - const ignition::gazebo::components::World *_w)->bool + const ignition::gazebo::components::World *)->bool { - std::cerr << "/worldEntity " << worldEntity << '\n'; - worldEntity = _entity; - // worldComp = _w; - std::cerr << "/_name " << _name->Data() << '\n'; - // if (_name == "empty_world") - // { - // _name = _world->Name(); - // } - // std::cerr << "/_name " << _name << '\n'; - - return false; + if (_world->Name() == _name->Data()) + { + worldEntity = _entity; + return false; + } + return true; }); - components::World *worldComp; - components::Name *nameComp; - - worldComp = this->dataPtr->ecm->Component(worldEntity); - nameComp = this->dataPtr->ecm->Component(worldEntity); - // worldComp->Data()->SetName(_world->Name()); - nameComp->Data() = _world->Name(); - - if (!worldComp) + if (worldEntity == kNullEntity) { - // World entity worldEntity = this->dataPtr->ecm->CreateEntity(); + } - // World components - this->dataPtr->ecm->CreateComponent(worldEntity, components::World()); - this->dataPtr->ecm->CreateComponent(worldEntity, - components::Name(_world->Name())); + // World components + this->dataPtr->ecm->CreateComponent(worldEntity, components::World()); + this->dataPtr->ecm->CreateComponent(worldEntity, + components::Name(_world->Name())); - // scene - if (_world->Scene()) - { - this->dataPtr->ecm->CreateComponent(worldEntity, - components::Scene(*_world->Scene())); - } + // scene + if (_world->Scene()) + { + this->dataPtr->ecm->CreateComponent(worldEntity, + components::Scene(*_world->Scene())); } // atmosphere diff --git a/src/Server.cc b/src/Server.cc index 1c26f32137..428176bbc0 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -39,11 +39,11 @@ struct DefaultWorld /// \brief Get the default world as a string. /// Plugins will be loaded from the server.config file. /// \return An SDF string that contains the default world. - public: static std::string &World() + public: static std::string &World(std::string _worldName="default") { static std::string world = std::string("" "" - "") + + "") + "" ""; @@ -71,6 +71,7 @@ Server::Server(bool _downloadInParallel, const ServerConfig &_config) ///////////////////////////////////////////////// bool Server::DownloadModels() { + sdf::Root root; sdf::Errors errors; std::cerr << "this->dataPtr->config.Source() " << static_cast(this->dataPtr->config.Source()) << '\n'; @@ -96,8 +97,9 @@ bool Server::DownloadModels() msg += "File path [" + this->dataPtr->config.SdfFile() + "].\n"; } ignmsg << msg; - errors = this->dataPtr->sdfRoot.LoadSdfString( + errors = root.LoadSdfString( this->dataPtr->config.SdfString()); + this->dataPtr->sdfRoot = root.Clone(); break; } @@ -122,7 +124,8 @@ bool Server::DownloadModels() std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - errors = this->dataPtr->sdfRoot.Load(filePath); + errors = root.Load(filePath); + this->dataPtr->sdfRoot = root.Clone(); if (!this->dataPtr->downloadInParallel) return true; @@ -149,7 +152,7 @@ bool Server::DownloadModels() } ignmsg << "Download models in parallel has finished. " << "Now you can start the simulation" << std::endl; - break; + break; } case ServerConfig::SourceType::kNone: @@ -158,7 +161,8 @@ bool Server::DownloadModels() ignmsg << "Loading default world.\n"; // Load an empty world. /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. - errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World()); + errors = root.LoadSdfString(DefaultWorld::World()); + this->dataPtr->sdfRoot = root.Clone(); break; } } @@ -201,7 +205,34 @@ void Server::Init() ignmsg << "Loading default world.\n"; // Load an empty world. /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. - errors = this->dataPtr->sdfRoot.LoadSdfString(DefaultWorld::World()); + // std::string filePath = resolveSdfWorldFile(this->dataPtr->config.SdfFile(), + // this->dataPtr->config.ResourceCache()); + // + // if (filePath.empty()) + // { + // ignerr << "Failed to find world [" + // << this->dataPtr->config.SdfFile() << "]" + // << std::endl; + // return; + // } + + common::SystemPaths systemPaths; + + // Worlds from environment variable + systemPaths.SetFilePathEnv(kResourcePathEnv); + + // Worlds installed with ign-gazebo + systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + + std::string filePath = systemPaths.FindFile(this->dataPtr->config.SdfFile()); + + ignerr << "filePath " << filePath << std::endl; + + std::string worldName; + auto errors2 = this->dataPtr->sdfRoot.GetWorldName(filePath, worldName); + + errors = this->dataPtr->sdfRoot.LoadSdfString( + DefaultWorld::World(worldName)); } else { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index aa26a434b2..5b3a43e2b6 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -538,8 +538,7 @@ void SimulationRunner::UpdateSystems() if (this->resetInitiated) { IGN_PROFILE("Reset"); - for (auto &system : this->systemMgr->SystemsReset()) - system->Reset(this->currentInfo, this->entityCompMgr); + // this->systemMgr->Reset(this->currentInfo, this->entityCompMgr); return; } @@ -722,7 +721,6 @@ bool SimulationRunner::Run(const uint64_t _iterations) uint64_t processedIterations{0}; bool isInitialRunOfSimulationSet = false; - bool initialize = false; // Execute all the systems until we are told to stop, or the number of // iterations is reached. diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 01b1dfb6fc..3bd90f48b7 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -105,6 +105,14 @@ namespace ignition /// system during the `Configure` call. public: Entity parentEntity = {kNullEntity}; + /// \brief Cached plugin name of the plugin used when system was loaded. + /// Used for reloading a system at runtime. + public: std::string name = ""; + + /// \brief Cached filename of the plugin used when system was loaded. + /// Used for reloading a system at runtime. + public: std::string fname = ""; + /// \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; @@ -116,4 +124,3 @@ namespace ignition } // namespace gazebo } // namespace ignition #endif // IGNITION_GAZEBO_SYSTEMINTERNAL_HH_ - diff --git a/src/SystemManager.cc b/src/SystemManager.cc index bde885612c..e8e52b8e6d 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -20,6 +20,21 @@ using namespace ignition; using namespace gazebo; +////////////////////////////////////////////////// +/// \brief Structure to temporarily store plugin information for reset +struct PluginInfo { + /// \brief Entity plugin is attached to + Entity entity; + /// \brief Filename of the plugin library + std::string fname; + /// \brief Name of the plugin + std::string name; + /// \brief SDF element (content of the plugin tag) + sdf::ElementPtr sdf; +}; + +std::vector loadedPlugins; + ////////////////////////////////////////////////// SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, EntityComponentManager *_entityCompMgr, @@ -36,6 +51,26 @@ void SystemManager::LoadPlugin(const Entity _entity, const std::string &_name, const sdf::ElementPtr &_sdf) { + for (const auto pluginInfo: loadedPlugins) + { + if (_entity == pluginInfo.entity && + _fname == pluginInfo.fname && + _name == pluginInfo.name) + { + // Plugin already loaded + igndbg << "This system Plugin [" << _name + << "] is already loaded in this entity [" << _entity + << "]" << std::endl; + return; + } + } + + PluginInfo info; + info.entity = _entity; + info.name = _name; + info.fname = _fname; + loadedPlugins.push_back(info); + std::optional system; { std::lock_guard lock(this->systemLoaderMutex); @@ -45,7 +80,12 @@ void SystemManager::LoadPlugin(const Entity _entity, // System correctly loaded from library if (system) { - this->AddSystem(system.value(), _entity, _sdf); + // this->AddSystem(system.value(), _entity, _sdf); + SystemInternal ss(system.value(), _entity); + ss.fname = _fname; + ss.name = _name; + ss.configureSdf = _sdf; + this->AddSystemImpl(ss, ss.configureSdf); igndbg << "Loaded system [" << _name << "] for entity [" << _entity << "]" << std::endl; } @@ -101,6 +141,66 @@ size_t SystemManager::ActivatePendingSystems() return count; } +////////////////////////////////////////////////// +void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) +{ + { + std::lock_guard lock(this->pendingSystemsMutex); + this->pendingSystems.clear(); + } + + // Clear all iterable collections of systems + this->systemsConfigure.clear(); + this->systemsReset.clear(); + this->systemsPreupdate.clear(); + this->systemsUpdate.clear(); + this->systemsPostupdate.clear(); + + std::vector pluginsToBeLoaded; + + for (auto& system : this->systems) + { + if (nullptr != system.reset) + { + // If implemented, call reset and add to pending systems. + system.reset->Reset(_info, _ecm); + + { + std::lock_guard lock(this->pendingSystemsMutex); + this->pendingSystems.push_back(system); + } + } + else + { + // Cannot reset systems that were created in memory rather than + // from a plugin, because there isn't access to the constructor. + if (nullptr != system.systemShared) + { + ignwarn << "Systems not created from plugins cannot be correctly " + << " reset without implementing ISystemReset interface.\n"; + continue; + } + + PluginInfo info = { + system.parentEntity, system.fname, system.name, + system.configureSdf->Clone() + }; + + pluginsToBeLoaded.push_back(info); + } + } + + this->systems.clear(); + + // Load plugins which do not implement reset after clearing this->systems + // to ensure the previous instance is destroyed before the new one is created + // and configured. + for (const auto &pluginInfo : pluginsToBeLoaded) { + this->LoadPlugin(pluginInfo.entity, pluginInfo.fname, pluginInfo.name, pluginInfo.sdf); + } + this->ActivatePendingSystems(); +} + ////////////////////////////////////////////////// void SystemManager::AddSystem(const SystemPluginPtr &_system, Entity _entity, diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 1a6071ab0e..8b6a956dfa 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -92,12 +92,26 @@ namespace ignition /// \return The number of newly-active systems public: size_t ActivatePendingSystems(); + /// \brief Perform a reset on all systems + /// + /// If a system implements the ISystemReset interface, it will be called. + // + /// Otherwise, if a system does not have the ISystemReset interface + /// implemented, and was created via loading a plugin, + /// that plugin will be reloaded. + /// + /// Otherwise, if a system is created from in-memory rather than a plugin, + /// that system will remain unaffected. + /// \param[in] _info Update info corresponding to the update time + /// \param[in] _ecm Version of the _ecm reset to an initial state + public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm); + /// \brief Get an vector of all active systems implementing "Configure" /// \return Vector of systems's configure interfaces. public: const std::vector& SystemsConfigure(); /// \brief Get an vector of all active systems implementing "Reset" - /// \return Vector of systems's reset interfaces. + /// \return Vector of systems' reset interfaces. public: const std::vector& SystemsReset(); /// \brief Get an vector of all active systems implementing "PreUpdate" diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 6a0f762c45..df30bdeae7 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -257,12 +257,6 @@ void SceneBroadcaster::Configure( this->dataPtr->worldEntity = _entity; - if (!this->dataPtr->worldName.empty() && this->dataPtr->node) - { - std::cerr << "RESETING node" << '\n'; - this->dataPtr->node.reset(); - } - this->dataPtr->worldName = name->Data(); std::cerr << "SceneBroadcaster::Configure " << this->dataPtr->worldName << '\n'; @@ -390,7 +384,34 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, void SceneBroadcaster::Reset(const UpdateInfo &_info, EntityComponentManager &_manager) { - std::cerr << "SceneBroadcaster reset" << this->dataPtr->worldName << '\n'; + const components::Name *name = + _manager.Component(this->dataPtr->worldEntity); + if (name == nullptr) + { + ignerr << "World with id: " << this->dataPtr->worldEntity + << " has no name. SceneBroadcaster cannot create transport topics\n"; + return; + } + this->dataPtr->worldName = name->Data(); + + ignerr << "SceneBroadcaster::Reset " << this->dataPtr->worldName << std::endl; + this->dataPtr->node.reset(); + // Update scene graph with added entities before populating pose message + this->dataPtr->SceneGraphAddEntities(_manager); + + this->dataPtr->PoseUpdate(_info, _manager); + + // call SceneGraphRemoveEntities at the end of this update cycle so that + // removed entities are removed from the scene graph for the next update cycle + this->dataPtr->SceneGraphRemoveEntities(_manager); + + std::unique_lock lock(this->dataPtr->stateMutex); + this->dataPtr->stepMsg.Clear(); + + set(this->dataPtr->stepMsg.mutable_stats(), _info); + _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true); + this->dataPtr->statePub.Publish(this->dataPtr->stepMsg); + this->dataPtr->lastStatePubTime = std::chrono::system_clock::now(); } ////////////////////////////////////////////////// @@ -1193,7 +1214,8 @@ void SceneBroadcasterPrivate::RemoveFromGraph(const Entity _entity, IGNITION_ADD_PLUGIN(SceneBroadcaster, ignition::gazebo::System, SceneBroadcaster::ISystemConfigure, - SceneBroadcaster::ISystemPostUpdate) + SceneBroadcaster::ISystemPostUpdate, + SceneBroadcaster::ISystemReset) // Add plugin alias so that we can refer to the plugin without the version // namespace From 110c6a8739e9e7f5b39e21ccb276e5290a0c51c8 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 23 May 2022 17:31:47 +0200 Subject: [PATCH 26/54] cleanup Signed-off-by: ahcorde --- src/Server.cc | 24 +++-------- src/SimulationRunner.cc | 2 - src/SystemInternal.hh | 1 + src/SystemManager.cc | 7 +++- .../scene_broadcaster/SceneBroadcaster.cc | 40 +------------------ .../scene_broadcaster/SceneBroadcaster.hh | 7 +--- 6 files changed, 14 insertions(+), 67 deletions(-) diff --git a/src/Server.cc b/src/Server.cc index 428176bbc0..ff6b9250ae 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -39,7 +39,7 @@ struct DefaultWorld /// \brief Get the default world as a string. /// Plugins will be loaded from the server.config file. /// \return An SDF string that contains the default world. - public: static std::string &World(std::string _worldName="default") + public: static std::string &World(const std::string &_worldName = "default") { static std::string world = std::string("" "" @@ -105,8 +105,9 @@ bool Server::DownloadModels() case ServerConfig::SourceType::kSdfFile: { - std::string filePath = resolveSdfWorldFile(this->dataPtr->config.SdfFile(), - this->dataPtr->config.ResourceCache()); + std::string filePath = resolveSdfWorldFile( + this->dataPtr->config.SdfFile(), + this->dataPtr->config.ResourceCache()); if (filePath.empty()) { @@ -203,18 +204,6 @@ void Server::Init() }); ignmsg << "Loading default world.\n"; - // Load an empty world. - /// \todo(nkoenig) Add a "AddWorld" function to sdf::Root. - // std::string filePath = resolveSdfWorldFile(this->dataPtr->config.SdfFile(), - // this->dataPtr->config.ResourceCache()); - // - // if (filePath.empty()) - // { - // ignerr << "Failed to find world [" - // << this->dataPtr->config.SdfFile() << "]" - // << std::endl; - // return; - // } common::SystemPaths systemPaths; @@ -224,9 +213,8 @@ void Server::Init() // Worlds installed with ign-gazebo systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); - std::string filePath = systemPaths.FindFile(this->dataPtr->config.SdfFile()); - - ignerr << "filePath " << filePath << std::endl; + std::string filePath = systemPaths.FindFile( + this->dataPtr->config.SdfFile()); std::string worldName; auto errors2 = this->dataPtr->sdfRoot.GetWorldName(filePath, worldName); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 5b3a43e2b6..75b7334ae6 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -744,7 +744,6 @@ bool SimulationRunner::Run(const uint64_t _iterations) if (!isInitialRunOfSimulationSet) { isInitialRunOfSimulationSet = true; - // this->SetPaused(!this->serverConfig.RunOption()); this->initialEntityCompMgr.CopyFrom(this->entityCompMgr); @@ -755,7 +754,6 @@ bool SimulationRunner::Run(const uint64_t _iterations) std::lock_guard lockBuffer(this->msgBufferMutex); this->worldControls.push_back(control); } - // this->requestedRewind = true; } } diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 3bd90f48b7..a9d9d435cf 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/src/SystemManager.cc b/src/SystemManager.cc index e8e52b8e6d..f2c4aca17b 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -51,7 +51,7 @@ void SystemManager::LoadPlugin(const Entity _entity, const std::string &_name, const sdf::ElementPtr &_sdf) { - for (const auto pluginInfo: loadedPlugins) + for (const auto pluginInfo : loadedPlugins) { if (_entity == pluginInfo.entity && _fname == pluginInfo.fname && @@ -196,7 +196,10 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) // to ensure the previous instance is destroyed before the new one is created // and configured. for (const auto &pluginInfo : pluginsToBeLoaded) { - this->LoadPlugin(pluginInfo.entity, pluginInfo.fname, pluginInfo.name, pluginInfo.sdf); + this->LoadPlugin(pluginInfo.entity, + pluginInfo.fname, + pluginInfo.name, + pluginInfo.sdf); } this->ActivatePendingSystems(); } diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index df30bdeae7..5b6c3a86d0 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -259,8 +259,6 @@ void SceneBroadcaster::Configure( this->dataPtr->worldName = name->Data(); - std::cerr << "SceneBroadcaster::Configure " << this->dataPtr->worldName << '\n'; - auto readHertz = _sdf->Get("dynamic_pose_hertz", 60); this->dataPtr->dyPoseHertz = readHertz.first; @@ -380,40 +378,6 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, } } -////////////////////////////////////////////////// -void SceneBroadcaster::Reset(const UpdateInfo &_info, - EntityComponentManager &_manager) -{ - const components::Name *name = - _manager.Component(this->dataPtr->worldEntity); - if (name == nullptr) - { - ignerr << "World with id: " << this->dataPtr->worldEntity - << " has no name. SceneBroadcaster cannot create transport topics\n"; - return; - } - this->dataPtr->worldName = name->Data(); - - ignerr << "SceneBroadcaster::Reset " << this->dataPtr->worldName << std::endl; - this->dataPtr->node.reset(); - // Update scene graph with added entities before populating pose message - this->dataPtr->SceneGraphAddEntities(_manager); - - this->dataPtr->PoseUpdate(_info, _manager); - - // call SceneGraphRemoveEntities at the end of this update cycle so that - // removed entities are removed from the scene graph for the next update cycle - this->dataPtr->SceneGraphRemoveEntities(_manager); - - std::unique_lock lock(this->dataPtr->stateMutex); - this->dataPtr->stepMsg.Clear(); - - set(this->dataPtr->stepMsg.mutable_stats(), _info); - _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true); - this->dataPtr->statePub.Publish(this->dataPtr->stepMsg); - this->dataPtr->lastStatePubTime = std::chrono::system_clock::now(); -} - ////////////////////////////////////////////////// void SceneBroadcasterPrivate::PoseUpdate(const UpdateInfo &_info, const EntityComponentManager &_manager) @@ -533,7 +497,6 @@ void SceneBroadcasterPrivate::PoseUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) { - this->worldName = _worldName; auto ns = transport::TopicUtils::AsValidTopic("/world/" + _worldName); if (ns.empty()) { @@ -1214,8 +1177,7 @@ void SceneBroadcasterPrivate::RemoveFromGraph(const Entity _entity, IGNITION_ADD_PLUGIN(SceneBroadcaster, ignition::gazebo::System, SceneBroadcaster::ISystemConfigure, - SceneBroadcaster::ISystemPostUpdate, - SceneBroadcaster::ISystemReset) + SceneBroadcaster::ISystemPostUpdate) // Add plugin alias so that we can refer to the plugin without the version // namespace diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.hh b/src/systems/scene_broadcaster/SceneBroadcaster.hh index 08fdae39c4..0438ced7a6 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.hh +++ b/src/systems/scene_broadcaster/SceneBroadcaster.hh @@ -40,8 +40,7 @@ namespace systems class SceneBroadcaster final: public System, public ISystemConfigure, - public ISystemPostUpdate, - public ISystemReset + public ISystemPostUpdate { /// \brief Constructor public: SceneBroadcaster(); @@ -55,10 +54,6 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; - // Documentation inherited - public: void Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; - // Documentation inherited public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; From 443ac22b7eda842220f4928bc16bcb639a994134 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 26 May 2022 13:53:17 +0200 Subject: [PATCH 27/54] updated method name Signed-off-by: ahcorde --- src/Server.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Server.cc b/src/Server.cc index ff6b9250ae..c75f52df66 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -217,7 +217,7 @@ void Server::Init() this->dataPtr->config.SdfFile()); std::string worldName; - auto errors2 = this->dataPtr->sdfRoot.GetWorldName(filePath, worldName); + auto errors2 = this->dataPtr->sdfRoot.WorldName(filePath, worldName); errors = this->dataPtr->sdfRoot.LoadSdfString( DefaultWorld::World(worldName)); From 4770dc7fa5d1205dbfeff84a9c5d3912ae52c44b Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 31 May 2022 12:28:28 -0500 Subject: [PATCH 28/54] Updates for main and add topic test Signed-off-by: Michael Carroll --- src/SystemManager.cc | 12 ++-- test/integration/reset_sensors.cc | 95 ++++++++++++++++++++----------- test/worlds/reset_sensors.sdf | 59 +++++-------------- 3 files changed, 82 insertions(+), 84 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 93194f2695..96af59932d 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -45,12 +45,11 @@ void SystemManager::LoadPlugin(const Entity _entity, // System correctly loaded from library if (system) { - SystemInternal ss(system.value()); - ss.entity = _entity; + SystemInternal ss(system.value(), _entity); ss.fname = _fname; ss.name = _name; ss.configureSdf = _sdf; - this->AddSystemImpl(ss, ss.entity, ss.configureSdf); + this->AddSystemImpl(ss, ss.configureSdf); gzdbg << "Loaded system [" << _name << "] for entity [" << _entity << "]" << std::endl; } @@ -117,7 +116,7 @@ struct PluginInfo { std::string name; /// \brief SDF element (content of the plugin tag) sdf::ElementPtr sdf; -} +}; ////////////////////////////////////////////////// void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) @@ -161,7 +160,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) PluginInfo info = { - system.entity, system.fname, system.name, + system.parentEntity, system.fname, system.name, system.configureSdf->Clone() }; @@ -175,7 +174,8 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) // to ensure the previous instance is destroyed before the new one is created // and configured. for (const auto &pluginInfo : pluginsToBeLoaded) { - this->LoadPlugin(pluginInfo); + this->LoadPlugin(pluginInfo.entity, pluginInfo.fname, pluginInfo.name, + pluginInfo.sdf); } this->ActivatePendingSystems(); } diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc index 27da0a6b52..ad89cece36 100644 --- a/test/integration/reset_sensors.cc +++ b/test/integration/reset_sensors.cc @@ -24,30 +24,30 @@ #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 +#include + +#include "gz/sim/Entity.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" +#include "gz/sim/SdfEntityCreator.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/Types.hh" +#include "gz/sim/test_config.hh" + +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/World.hh" #include "plugins/MockSystem.hh" -#include "../helpers/EnvTestFixture.hh" +#include "helpers/EnvTestFixture.hh" -using namespace ignition; -using namespace gazebo; +using namespace gz; +using namespace sim; using namespace std::chrono_literals; -namespace components = ignition::gazebo::components; +namespace components = gz::sim::components; ////////////////////////////////////////////////// class ResetFixture: public InternalFixture> @@ -57,25 +57,25 @@ class ResetFixture: public InternalFixture> InternalFixture::SetUp(); auto plugin = sm.LoadPlugin("libMockSystem.so", - "ignition::gazebo::MockSystem", + "gz::sim::MockSystem", nullptr); EXPECT_TRUE(plugin.has_value()); this->systemPtr = plugin.value(); - this->mockSystem = static_cast( - systemPtr->QueryInterface()); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); } - public: ignition::gazebo::SystemPluginPtr systemPtr; - public: gazebo::MockSystem *mockSystem; + public: gz::sim::SystemPluginPtr systemPtr; + public: sim::MockSystem *mockSystem; - private: gazebo::SystemLoader sm; + private: sim::SystemLoader sm; }; ///////////////////////////////////////////////// void worldReset() { - ignition::msgs::WorldControl req; - ignition::msgs::Boolean rep; + gz::msgs::WorldControl req; + gz::msgs::Boolean rep; req.mutable_reset()->set_all(true); transport::Node node; @@ -94,7 +94,7 @@ void worldReset() /// are removed and then added back TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) { - ignition::gazebo::ServerConfig serverConfig; + gz::sim::ServerConfig serverConfig; const std::string sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "reset_sensors.sdf"); @@ -103,10 +103,29 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) sdf::Root root; root.Load(sdfFile); - gazebo::Server server(serverConfig); + sim::Server server(serverConfig); + + const std::string sensorName = "air_pressure_sensor"; + auto topic = "world/default/model/box/link/link/" + "sensor/air_pressure_sensor/air_pressure"; + + // Subscribe to air_pressure topic + bool received{false}; + msgs::FluidPressure msg; + msg.Clear(); + std::function cb = + [&received, &msg](const msgs::FluidPressure &_msg) + { + // Only need one message + if (received) + return; + + msg = _msg; + received = true; + }; // A pointer to the ecm. This will be valid once we run the mock system - gazebo::EntityComponentManager *ecm = nullptr; + sim::EntityComponentManager *ecm = nullptr; this->mockSystem->configureCallback = [&ecm](const Entity &, @@ -134,6 +153,9 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) EXPECT_EQ(0u, this->mockSystem->postUpdateCallCount); } + transport::Node node; + node.Subscribe(topic, cb); + // Run so that things will happen in the world // In this case, the box should fall some server.Run(true, 100, false); @@ -150,12 +172,16 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) EXPECT_EQ(100u, this->mockSystem->preUpdateCallCount); EXPECT_EQ(100u, this->mockSystem->updateCallCount); EXPECT_EQ(100u, this->mockSystem->postUpdateCallCount); + EXPECT_TRUE(received); } + node.Unsubscribe(topic); + received = false; + // Validate update info in the reset this->mockSystem->resetCallback = - [](const gazebo::UpdateInfo &_info, - gazebo::EntityComponentManager &) + [](const sim::UpdateInfo &_info, + sim::EntityComponentManager &) { EXPECT_EQ(0u, _info.iterations); EXPECT_EQ(std::chrono::steady_clock::duration{0}, _info.simTime); @@ -192,6 +218,8 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) EXPECT_EQ(101u, this->mockSystem->postUpdateCallCount); } + node.Subscribe(topic, cb); + server.Run(true, 100, false); { ASSERT_NE(nullptr, ecm); @@ -206,5 +234,6 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) EXPECT_EQ(201u, this->mockSystem->preUpdateCallCount); EXPECT_EQ(201u, this->mockSystem->updateCallCount); EXPECT_EQ(201u, this->mockSystem->postUpdateCallCount); + EXPECT_TRUE(received); } } diff --git a/test/worlds/reset_sensors.sdf b/test/worlds/reset_sensors.sdf index bcc6baa338..f7cb653884 100644 --- a/test/worlds/reset_sensors.sdf +++ b/test/worlds/reset_sensors.sdf @@ -42,6 +42,20 @@ 1 1 1 0.8 + + 1 + 30 + true + + 0 + + + 0 + 0 + + + + @@ -81,50 +95,5 @@ - - - true - 4 0 3.0 0 0.0 3.14 - - 0.05 0.05 0.05 0 0 0 - - 0.1 - - 0.000166667 - 0.000166667 - 0.000166667 - - - - - - 0.1 0.1 0.1 - - - - - - - 0.1 0.1 0.1 - - - - - 1 - 30 - true - - 123 - - - 0 - 0 - - - - - - - From 2286ec5bb4811ad16907ba142047718ad403f87e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 31 May 2022 13:55:13 -0500 Subject: [PATCH 29/54] Fix interaction with log playback static variable Signed-off-by: Michael Carroll --- src/systems/log/LogPlayback.cc | 8 ++++++++ src/systems/log/LogPlayback.hh | 5 +++++ 2 files changed, 13 insertions(+) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 38da93369e..c1aebf07a5 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -453,6 +453,13 @@ bool LogPlaybackPrivate::ExtractStateAndResources() } } +////////////////////////////////////////////////// +void LogPlayback::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) +{ + LogPlaybackPrivate::started = false; + this->dataPtr->Start(_ecm); +} + ////////////////////////////////////////////////// void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { @@ -619,6 +626,7 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) IGNITION_ADD_PLUGIN(gz::sim::systems::LogPlayback, gz::sim::System, LogPlayback::ISystemConfigure, + LogPlayback::ISystemReset, LogPlayback::ISystemUpdate) IGNITION_ADD_PLUGIN_ALIAS(LogPlayback, diff --git a/src/systems/log/LogPlayback.hh b/src/systems/log/LogPlayback.hh index 3d49f6195c..bc8fd28110 100644 --- a/src/systems/log/LogPlayback.hh +++ b/src/systems/log/LogPlayback.hh @@ -39,6 +39,7 @@ namespace systems class LogPlayback: public System, public ISystemConfigure, + public ISystemReset, public ISystemUpdate { /// \brief Constructor @@ -53,6 +54,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 7e9245af9ef543b9b87dcc5e57fa6e1ba1f9d5e2 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 1 Jun 2022 11:45:24 -0500 Subject: [PATCH 30/54] Also disable on Mac where OGRE2 doesn't load Signed-off-by: Michael Carroll --- test/integration/reset_sensors.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc index ad89cece36..35af912be1 100644 --- a/test/integration/reset_sensors.cc +++ b/test/integration/reset_sensors.cc @@ -92,7 +92,7 @@ void worldReset() ///////////////////////////////////////////////// /// 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_WIN32(HandleReset)) +TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) { gz::sim::ServerConfig serverConfig; From 0c4a4c5642582ca216fd5e61dc48570cb9f49521 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 7 Jun 2022 09:37:02 -0500 Subject: [PATCH 31/54] Address reviewer feedback. Signed-off-by: Michael Carroll --- src/SystemManager.cc | 5 ++--- src/SystemManager.hh | 2 +- src/systems/log/LogPlayback.cc | 2 +- src/systems/sensors/Sensors.cc | 33 ++++++++++++++++++++++++++------- 4 files changed, 30 insertions(+), 12 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 96af59932d..f0805aa18f 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -110,7 +110,7 @@ size_t SystemManager::ActivatePendingSystems() struct PluginInfo { /// \brief Entity plugin is attached to Entity entity; - /// \brief Filename of the plugin library + /// \brief Filename of the plugin library std::string fname; /// \brief Name of the plugin std::string name; @@ -154,11 +154,10 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) if (nullptr != system.systemShared) { ignwarn << "Systems not created from plugins cannot be correctly " - << " reset without implementing ISystemReset interface.\n"; + << " reset without implementing ISystemReset interface.\n"; continue; } - PluginInfo info = { system.parentEntity, system.fname, system.name, system.configureSdf->Clone() diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 8b0e7aed06..1717c9f1a7 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -103,7 +103,7 @@ namespace gz /// Otherwise, if a system is created from in-memory rather than a plugin, /// that system will remain unaffected. /// \param[in] _info Update info corresponding to the update time - /// \param[in] _ecm Version of the _ecm reset to an initial state + /// \param[in] _ecm Version of the ECM reset to an initial state public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm); /// \brief Get an vector of all systems implementing "Configure" diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index c1aebf07a5..30c839d669 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -454,7 +454,7 @@ bool LogPlaybackPrivate::ExtractStateAndResources() } ////////////////////////////////////////////////// -void LogPlayback::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) +void LogPlayback::Reset(const UpdateInfo &, EntityComponentManager &_ecm) { LogPlaybackPrivate::started = false; this->dataPtr->Start(_ecm); diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 2783baa1da..443605844b 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -533,7 +533,19 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) for (auto id : this->dataPtr->sensorIds) { sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); + + if (nullptr == s) + { + continue; + } + auto rs = dynamic_cast(s); + + if (nullptr == rs) + { + continue; + } + rs->SetNextDataUpdateTime(_info.simTime); } } @@ -578,9 +590,6 @@ void Sensors::PostUpdate(const UpdateInfo &_info, { this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - auto time = math::durationToSecNsec(_info.simTime); - auto t = math::secNsecToDuration(time.first, time.second); - std::vector activeSensors; { @@ -588,12 +597,23 @@ void Sensors::PostUpdate(const UpdateInfo &_info, for (auto id : this->dataPtr->sensorIds) { sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); + + if (nullptr == s) + { + continue; + } + auto rs = dynamic_cast(s); + if (nullptr == rs) + { + continue; + } + auto it = this->dataPtr->sensorMask.find(id); if (it != this->dataPtr->sensorMask.end()) { - if (it->second <= t) + if (it->second <= _info.simTime) { this->dataPtr->sensorMask.erase(it); } @@ -603,7 +623,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, } } - if (rs && rs->NextDataUpdateTime() <= t) + if (rs && rs->NextDataUpdateTime() <= _info.simTime) { activeSensors.push_back(rs); } @@ -626,14 +646,13 @@ void Sensors::PostUpdate(const UpdateInfo &_info, } this->dataPtr->activeSensors = std::move(activeSensors); - this->dataPtr->updateTime = t; + this->dataPtr->updateTime = _info.simTime; this->dataPtr->updateAvailable = true; this->dataPtr->renderCv.notify_one(); } } } - ////////////////////////////////////////////////// void SensorsPrivate::UpdateBatteryState(const EntityComponentManager &_ecm) { From ae3d0762c4f067e649b81bba3e38b59fb990b934 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 14 Jun 2022 12:14:44 -0500 Subject: [PATCH 32/54] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michael Carroll Co-authored-by: Louise Poubel Co-authored-by: Alejandro Hernández Cordero --- src/SystemManager.cc | 8 ++++---- src/SystemManager.hh | 2 +- test/integration/reset_sensors.cc | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 57281b09ea..5cd4938d77 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -137,7 +137,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) std::vector pluginsToBeLoaded; - for (auto& system : this->systems) + for (auto &system : this->systems) { if (nullptr != system.reset) { @@ -157,9 +157,8 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) { ignwarn << "Systems not created from plugins cannot be correctly " << " reset without implementing ISystemReset interface.\n"; - continue; + continue; } - PluginInfo info = { system.parentEntity, system.fname, system.name, system.configureSdf->Clone() @@ -174,7 +173,8 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) // Load plugins which do not implement reset after clearing this->systems // to ensure the previous instance is destroyed before the new one is created // and configured. - for (const auto &pluginInfo : pluginsToBeLoaded) { + for (const auto &pluginInfo : pluginsToBeLoaded) + { this->LoadPlugin(pluginInfo.entity, pluginInfo.fname, pluginInfo.name, pluginInfo.sdf); } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 49d624af5b..47d66ab4a0 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -106,7 +106,7 @@ namespace gz /// \param[in] _ecm Version of the ECM reset to an initial state public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm); - /// \brief Get an vector of all systems implementing "Configure" + /// \brief Get a vector of all systems implementing "Configure" /// \return Vector of systems' configure interfaces. public: const std::vector& SystemsConfigure(); diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc index 35af912be1..7878c4b4f0 100644 --- a/test/integration/reset_sensors.cc +++ b/test/integration/reset_sensors.cc @@ -191,7 +191,7 @@ TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) worldReset(); // It takes two iterations for this to propage, - // the first is for the message to be recieved and internal state setup + // 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); From df6cc360ef6c286a532463df9c2ce6f9410ceb44 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 14 Jun 2022 12:21:54 -0500 Subject: [PATCH 33/54] Better logging for reset in-memory systems Signed-off-by: Michael Carroll --- src/SystemManager.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 5cd4938d77..e13ae45a7b 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -155,8 +155,8 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) // from a plugin, because there isn't access to the constructor. if (nullptr != system.systemShared) { - ignwarn << "Systems not created from plugins cannot be correctly " - << " reset without implementing ISystemReset interface.\n"; + ignwarn << "System created in-memory without ISystemReset detected: " + << system.name << " reset may not work correctly\n"; continue; } PluginInfo info = { From 768aee2e4b23eda449aefbbb9ebf91105653e1dc Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 14 Jun 2022 12:24:08 -0500 Subject: [PATCH 34/54] Impove sensor reset logging and drop cast --- src/systems/sensors/Sensors.cc | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 99b558b390..05a76f25fe 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -565,17 +565,11 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) if (nullptr == s) { + ignwarn << "Sensor removed before reset: " << id << "\n"; continue; } - auto rs = dynamic_cast(s); - - if (nullptr == rs) - { - continue; - } - - rs->SetNextDataUpdateTime(_info.simTime); + s->SetNextDataUpdateTime(_info.simTime); } } } From 26d7396f5a9367f153ad5f0d46bfd64952de5d7d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 14 Jun 2022 15:06:03 -0500 Subject: [PATCH 35/54] More informative warning on systems not implementing reset Signed-off-by: Michael Carroll --- src/SystemManager.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index e13ae45a7b..0d51b18304 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -155,8 +155,10 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) // from a plugin, because there isn't access to the constructor. if (nullptr != system.systemShared) { - ignwarn << "System created in-memory without ISystemReset detected: " - << system.name << " reset may not work correctly\n"; + ignwarn << "In-memory without ISystemReset detected: [" + << system.name << "]\n" + << "Systems created without plugins that do not implement Reset" + << " will not be reloaded. Reset may not work correctly\n"; continue; } PluginInfo info = { From b211196e074a8c32d91967af6bc2a58f12ecf9ac Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 14 Jun 2022 15:06:31 -0500 Subject: [PATCH 36/54] Prune headers in reset_sensors test Signed-off-by: Michael Carroll --- test/integration/reset_sensors.cc | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc index 7878c4b4f0..daff0fb2e7 100644 --- a/test/integration/reset_sensors.cc +++ b/test/integration/reset_sensors.cc @@ -20,26 +20,17 @@ #include #include -#include +#include #include -#include - #include #include -#include "gz/sim/Entity.hh" -#include "gz/sim/EntityComponentManager.hh" -#include "gz/sim/EventManager.hh" -#include "gz/sim/SdfEntityCreator.hh" #include "gz/sim/Server.hh" #include "gz/sim/SystemLoader.hh" -#include "gz/sim/Types.hh" -#include "gz/sim/test_config.hh" +#include "gz/sim/SystemPluginPtr.hh" -#include "gz/sim/components/Model.hh" -#include "gz/sim/components/Name.hh" -#include "gz/sim/components/Pose.hh" -#include "gz/sim/components/World.hh" +#include +#include #include "plugins/MockSystem.hh" #include "helpers/EnvTestFixture.hh" @@ -47,7 +38,6 @@ using namespace gz; using namespace sim; using namespace std::chrono_literals; -namespace components = gz::sim::components; ////////////////////////////////////////////////// class ResetFixture: public InternalFixture> From 101654aa38d575e9d248ee6b23ef713a1223d061 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 14 Jun 2022 15:06:39 -0500 Subject: [PATCH 37/54] Update LogPlayback reset functionality Signed-off-by: Michael Carroll --- src/systems/log/LogPlayback.cc | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 30c839d669..40cb18d060 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -454,10 +454,13 @@ bool LogPlaybackPrivate::ExtractStateAndResources() } ////////////////////////////////////////////////// -void LogPlayback::Reset(const UpdateInfo &, EntityComponentManager &_ecm) +void LogPlayback::Reset(const UpdateInfo &, EntityComponentManager &) { - LogPlaybackPrivate::started = false; - this->dataPtr->Start(_ecm); + // In this case, Reset is a noop + // LogPlayback already has handling for jumps in time as part of the + // Update method. + // Leaving this function implemented but empty prevents the SystemManager + // from trying to destroy and recreate the plugin. } ////////////////////////////////////////////////// From 23ccdf3792430e820ee655649546ab51ca3deb5e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 17 Jun 2022 18:24:06 +0200 Subject: [PATCH 38/54] Fixed merge Signed-off-by: ahcorde --- src/SdfEntityCreator.cc | 10 +++++----- src/Server.cc | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 4a5601f5b1..5562d2fb86 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -221,11 +221,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::World)"); Entity worldEntity = kNullEntity; - this->dataPtr->ecm->Each( - [&](const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Name *_name, - const ignition::gazebo::components::World *)->bool + this->dataPtr->ecm->Each( + [&](const gz::sim::Entity &_entity, + const gz::sim::components::Name *_name, + const gz::sim::components::World *)->bool { if (_world->Name() == _name->Data()) { diff --git a/src/Server.cc b/src/Server.cc index 49de476772..534b9b9ec2 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -211,7 +211,7 @@ void Server::Init() systemPaths.SetFilePathEnv(kResourcePathEnv); // Worlds installed with ign-gazebo - systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + systemPaths.AddFilePaths(GZ_SIM_WORLD_INSTALL_DIR); std::string filePath = systemPaths.FindFile( this->dataPtr->config.SdfFile()); From b3e88e958326f070a2e5775949a58079b01c24c2 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 24 Jun 2022 21:15:01 +0200 Subject: [PATCH 39/54] Improved implmentation Signed-off-by: ahcorde --- src/SdfEntityCreator.cc | 9 +- src/SimulationRunner.cc | 3 +- src/SystemManager.cc | 16 +- .../resource_spawner/ResourceSpawner.cc | 73 +-- .../apply_joint_force/ApplyJointForce.cc | 17 +- .../apply_joint_force/ApplyJointForce.hh | 5 + src/systems/imu/Imu.cc | 7 + src/systems/imu/Imu.hh | 4 + .../JointStatePublisher.cc | 7 + .../JointStatePublisher.hh | 5 + src/systems/lift_drag/LiftDrag.cc | 13 +- src/systems/lift_drag/LiftDrag.hh | 8 +- .../scene_broadcaster/SceneBroadcaster.cc | 25 +- .../scene_broadcaster/SceneBroadcaster.hh | 7 +- src/systems/user_commands/UserCommands.cc | 448 +++++++++--------- src/systems/user_commands/UserCommands.hh | 7 +- 16 files changed, 379 insertions(+), 275 deletions(-) diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index a5615d5f1c..99241e14a8 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -238,13 +238,12 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) if (worldEntity == kNullEntity) { worldEntity = this->dataPtr->ecm->CreateEntity(); + // World components + this->dataPtr->ecm->CreateComponent(worldEntity, components::World()); + this->dataPtr->ecm->CreateComponent(worldEntity, + components::Name(_world->Name())); } - // World components - this->dataPtr->ecm->CreateComponent(worldEntity, components::World()); - this->dataPtr->ecm->CreateComponent(worldEntity, - components::Name(_world->Name())); - // scene if (_world->Scene()) { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index fbb0fb1dae..ebf90566a3 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -538,8 +538,7 @@ void SimulationRunner::UpdateSystems() if (this->resetInitiated) { GZ_PROFILE("Reset"); - for (auto &system : this->systemMgr->SystemsReset()) - system->Reset(this->currentInfo, this->entityCompMgr); + this->systemMgr->Reset(this->currentInfo, this->entityCompMgr); return; } diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 5316bef5ce..b5ee3b281a 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -59,11 +59,16 @@ void SystemManager::LoadPlugin(const Entity _entity, _fname == pluginInfo.fname && _name == pluginInfo.name) { - // Plugin already loaded - igndbg << "This system Plugin [" << _name - << "] is already loaded in this entity [" << _entity - << "]" << std::endl; - return; + if (_name == "gz::sim::systems::UserCommands" || + _name == "gz::sim::systems::SceneBroadcaster" || + _name == "gz::sim::systems::Physics") + { + // Plugin already loaded + igndbg << "This system Plugin [" << _name + << "] is already loaded in this entity [" << _entity + << "]" << std::endl; + return; + } } } @@ -157,6 +162,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) this->systemsPreupdate.clear(); this->systemsUpdate.clear(); this->systemsPostupdate.clear(); + loadedPlugins.clear(); std::vector pluginsToBeLoaded; diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index 7f0b52fd53..8bd462a1e4 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -476,48 +476,51 @@ void ResourceSpawner::OnPathClicked(const QString &_path) void ResourceSpawner::OnDownloadFuelResource(const QString &_path, const QString &_name, const QString &_owner, int index) { - Resource modelResource; - std::string localPath; - - // Set the waiting cursor while the resource downloads - QGuiApplication::setOverrideCursor(Qt::WaitCursor); - if (this->dataPtr->fuelClient->DownloadModel( - gz::common::URI(_path.toStdString()), localPath)) + std::thread t([&, _path, _name, _owner, index]() { - // Successful download, set thumbnail - std::string thumbnailPath = common::joinPaths(localPath, "thumbnails"); - this->SetThumbnail(thumbnailPath, modelResource); - modelResource.isDownloaded = true; - modelResource.sdfPath = common::joinPaths(localPath, "model.sdf"); - modelResource.isFuel = true; - // Update the current grid of resources - this->dataPtr->resourceModel.UpdateResourceModel(index, modelResource); - - // Update the ground truth ownerModelMap - if (this->dataPtr->ownerModelMap.find(_owner.toStdString()) != - this->dataPtr->ownerModelMap.end()) + Resource modelResource; + std::string localPath; + // Set the waiting cursor while the resource downloads + QGuiApplication::setOverrideCursor(Qt::WaitCursor); + if (this->dataPtr->fuelClient->DownloadModel( + gz::common::URI(_path.toStdString()), localPath)) { - std::vector fuelResources = - this->dataPtr->ownerModelMap[_owner.toStdString()]; - for (auto &resource : fuelResources) + // Successful download, set thumbnail + std::string thumbnailPath = common::joinPaths(localPath, "thumbnails"); + this->SetThumbnail(thumbnailPath, modelResource); + modelResource.isDownloaded = true; + modelResource.sdfPath = common::joinPaths(localPath, "model.sdf"); + modelResource.isFuel = true; + // Update the current grid of resources + this->dataPtr->resourceModel.UpdateResourceModel(index, modelResource); + + // Update the ground truth ownerModelMap + if (this->dataPtr->ownerModelMap.find(_owner.toStdString()) != + this->dataPtr->ownerModelMap.end()) { - if (resource.name == _name.toStdString()) + std::vector fuelResources = + this->dataPtr->ownerModelMap[_owner.toStdString()]; + for (auto &resource : fuelResources) { - resource.isDownloaded = modelResource.isDownloaded; - resource.isFuel = modelResource.isFuel; - resource.sdfPath = modelResource.sdfPath; - this->SetThumbnail(thumbnailPath, resource); - this->dataPtr->ownerModelMap[_owner.toStdString()] = fuelResources; - break; + if (resource.name == _name.toStdString()) + { + resource.isDownloaded = modelResource.isDownloaded; + resource.isFuel = modelResource.isFuel; + resource.sdfPath = modelResource.sdfPath; + this->SetThumbnail(thumbnailPath, resource); + this->dataPtr->ownerModelMap[_owner.toStdString()] = fuelResources; + break; + } } } } - } - else - { - gzwarn << "Download failed. Try again." << std::endl; - } - QGuiApplication::restoreOverrideCursor(); + else + { + gzwarn << "Download failed. Try again." << std::endl; + } + QGuiApplication::restoreOverrideCursor(); + }); + t.detach(); } ///////////////////////////////////////////////// diff --git a/src/systems/apply_joint_force/ApplyJointForce.cc b/src/systems/apply_joint_force/ApplyJointForce.cc index c95908d563..cfe10b50e7 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.cc +++ b/src/systems/apply_joint_force/ApplyJointForce.cc @@ -62,6 +62,12 @@ ApplyJointForce::ApplyJointForce() { } +////////////////////////////////////////////////// +void ApplyJointForce::Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ +} + ////////////////////////////////////////////////// void ApplyJointForce::Configure(const Entity &_entity, const std::shared_ptr &_sdf, @@ -164,13 +170,14 @@ void ApplyJointForcePrivate::OnCmdForce(const msgs::Double &_msg) } GZ_ADD_PLUGIN(ApplyJointForce, - gz::sim::System, - ApplyJointForce::ISystemConfigure, - ApplyJointForce::ISystemPreUpdate) + gz::sim::System, + ApplyJointForce::ISystemConfigure, + ApplyJointForce::ISystemReset, + ApplyJointForce::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(ApplyJointForce, - "gz::sim::systems::ApplyJointForce") + "gz::sim::systems::ApplyJointForce") // TODO(CH3): Deprecated, remove on version 8 GZ_ADD_PLUGIN_ALIAS(ApplyJointForce, - "ignition::gazebo::systems::ApplyJointForce") + "ignition::gazebo::systems::ApplyJointForce") diff --git a/src/systems/apply_joint_force/ApplyJointForce.hh b/src/systems/apply_joint_force/ApplyJointForce.hh index 0c93a28c20..4050bcb56a 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.hh +++ b/src/systems/apply_joint_force/ApplyJointForce.hh @@ -35,6 +35,7 @@ namespace systems class ApplyJointForce : public System, public ISystemConfigure, + public ISystemReset, public ISystemPreUpdate { /// \brief Constructor @@ -43,6 +44,10 @@ namespace systems /// \brief Destructor public: ~ApplyJointForce() override = default; + /// Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + // Documentation inherited public: void Configure(const Entity &_entity, const std::shared_ptr &_sdf, diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index b8866adeaa..4c18e73f13 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -104,6 +104,12 @@ Imu::Imu() : System(), dataPtr(std::make_unique()) ////////////////////////////////////////////////// Imu::~Imu() = default; +////////////////////////////////////////////////// +void Imu::Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ +} + ////////////////////////////////////////////////// void Imu::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) @@ -360,6 +366,7 @@ void ImuPrivate::RemoveImuEntities( GZ_ADD_PLUGIN(Imu, System, Imu::ISystemPreUpdate, + Imu::ISystemReset, Imu::ISystemPostUpdate ) diff --git a/src/systems/imu/Imu.hh b/src/systems/imu/Imu.hh index 828aa31e74..5e06389d65 100644 --- a/src/systems/imu/Imu.hh +++ b/src/systems/imu/Imu.hh @@ -39,6 +39,7 @@ namespace systems class Imu: public System, public ISystemPreUpdate, + public ISystemReset, public ISystemPostUpdate { /// \brief Constructor @@ -47,6 +48,9 @@ namespace systems /// \brief Destructor public: ~Imu() override; + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index ca37844bcf..5274327795 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/src/systems/joint_state_publisher/JointStatePublisher.cc @@ -46,6 +46,12 @@ JointStatePublisher::JointStatePublisher() { } +////////////////////////////////////////////////// +void JointStatePublisher::Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ +} + ////////////////////////////////////////////////// void JointStatePublisher::Configure( const Entity &_entity, const std::shared_ptr &_sdf, @@ -315,6 +321,7 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, GZ_ADD_PLUGIN(JointStatePublisher, gz::sim::System, JointStatePublisher::ISystemConfigure, + JointStatePublisher::ISystemReset, JointStatePublisher::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(JointStatePublisher, diff --git a/src/systems/joint_state_publisher/JointStatePublisher.hh b/src/systems/joint_state_publisher/JointStatePublisher.hh index a6358da4b3..776b588fd8 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.hh +++ b/src/systems/joint_state_publisher/JointStatePublisher.hh @@ -49,6 +49,7 @@ namespace systems class JointStatePublisher : public System, public ISystemConfigure, + public ISystemReset, public ISystemPostUpdate { /// \brief Constructor @@ -66,6 +67,10 @@ namespace systems public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; + /// Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + /// \brief Create components for a joint. /// \param[in] _ecm The EntityComponentManager. /// \param[in] _joint The joint entity to create component for. diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index e318cbcc0c..ef1a1bff51 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -510,6 +510,12 @@ void LiftDrag::Configure(const Entity &_entity, this->dataPtr->sdfConfig = _sdf->Clone(); } +////////////////////////////////////////////////// +void LiftDrag::Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ +} + ////////////////////////////////////////////////// void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { @@ -558,9 +564,10 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) } GZ_ADD_PLUGIN(LiftDrag, - gz::sim::System, - LiftDrag::ISystemConfigure, - LiftDrag::ISystemPreUpdate) + gz::sim::System, + LiftDrag::ISystemConfigure, + LiftDrag::ISystemPreUpdate, + LiftDrag::ISystemReset) GZ_ADD_PLUGIN_ALIAS(LiftDrag, "gz::sim::systems::LiftDrag") diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index efbed40502..d7eab2c07e 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -66,7 +66,8 @@ namespace systems class LiftDrag : public System, public ISystemConfigure, - public ISystemPreUpdate + public ISystemPreUpdate, + public ISystemReset { /// \brief Constructor public: LiftDrag(); @@ -84,6 +85,10 @@ namespace systems public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; + /// Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; @@ -93,4 +98,3 @@ namespace systems } #endif - diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index c460a6864f..db7d24d79d 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -400,6 +400,28 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, } } +////////////////////////////////////////////////// +void SceneBroadcaster::Reset(const UpdateInfo &_info, + EntityComponentManager &_manager) +{ + // Update scene graph with added entities before populating pose message + this->dataPtr->SceneGraphAddEntities(_manager); + + this->dataPtr->PoseUpdate(_info, _manager); + + // call SceneGraphRemoveEntities at the end of this update cycle so that + // removed entities are removed from the scene graph for the next update cycle + this->dataPtr->SceneGraphRemoveEntities(_manager); + + std::unique_lock lock(this->dataPtr->stateMutex); + this->dataPtr->stepMsg.Clear(); + + set(this->dataPtr->stepMsg.mutable_stats(), _info); + _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true); + this->dataPtr->statePub.Publish(this->dataPtr->stepMsg); + this->dataPtr->lastStatePubTime = std::chrono::system_clock::now(); +} + ////////////////////////////////////////////////// void SceneBroadcasterPrivate::PoseUpdate(const UpdateInfo &_info, const EntityComponentManager &_manager) @@ -1202,7 +1224,8 @@ void SceneBroadcasterPrivate::RemoveFromGraph(const Entity _entity, GZ_ADD_PLUGIN(SceneBroadcaster, gz::sim::System, SceneBroadcaster::ISystemConfigure, - SceneBroadcaster::ISystemPostUpdate) + SceneBroadcaster::ISystemPostUpdate, + SceneBroadcaster::ISystemReset) // Add plugin alias so that we can refer to the plugin without the version // namespace diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.hh b/src/systems/scene_broadcaster/SceneBroadcaster.hh index 2ffd62b16c..4448725592 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.hh +++ b/src/systems/scene_broadcaster/SceneBroadcaster.hh @@ -40,7 +40,8 @@ namespace systems class SceneBroadcaster final: public System, public ISystemConfigure, - public ISystemPostUpdate + public ISystemPostUpdate, + public ISystemReset { /// \brief Constructor public: SceneBroadcaster(); @@ -58,6 +59,10 @@ namespace systems public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; + // Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index fbd38b20d9..6e1f5eeb70 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -194,8 +194,12 @@ class CreateCommand : public UserCommandBase public: CreateCommand(msgs::EntityFactory *_msg, std::shared_ptr &_iface); + public: ~CreateCommand(); + // Documentation inherited public: bool Execute() final; + + private: std::thread threadDownloadModel; }; /// \brief Command to remove an entity from simulation. @@ -760,6 +764,11 @@ void UserCommands::Configure(const Entity &_entity, gzmsg << "Material service on [" << wheelSlipService << "]" << std::endl; } +void UserCommands::Reset(const UpdateInfo &/*_info*/, + EntityComponentManager &/*_ecm*/) +{ +} + ////////////////////////////////////////////////// void UserCommands::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &) @@ -1055,258 +1064,266 @@ CreateCommand::CreateCommand(msgs::EntityFactory *_msg, { } +CreateCommand::~CreateCommand() +{ + threadDownloadModel.join(); +} + ////////////////////////////////////////////////// bool CreateCommand::Execute() { - auto createMsg = dynamic_cast(this->msg); - if (nullptr == createMsg) - { - gzerr << "Internal error, null create message" << std::endl; - return false; - } - - // Load SDF - sdf::Root root; - sdf::Light lightSdf; - sdf::Errors errors; - switch (createMsg->from_case()) + threadDownloadModel = std::thread([&]() { - case msgs::EntityFactory::kSdf: + auto createMsg = dynamic_cast(this->msg); + if (nullptr == createMsg) { - errors = root.LoadSdfString(createMsg->sdf()); - break; - } - case msgs::EntityFactory::kSdfFilename: - { - errors = root.Load(createMsg->sdf_filename()); - break; - } - case msgs::EntityFactory::kModel: - { - // TODO(louise) Support model msg - gzerr << "model field not yet supported." << std::endl; + gzerr << "Internal error, null create message" << std::endl; return false; } - case msgs::EntityFactory::kLight: - { - lightSdf = convert(createMsg->light()); - break; - } - case msgs::EntityFactory::kCloneName: + + // Load SDF + sdf::Root root; + sdf::Light lightSdf; + sdf::Errors errors; + switch (createMsg->from_case()) { - auto validClone = false; - auto clonedEntity = kNullEntity; - auto entityToClone = this->iface->ecm->EntityByComponents( - components::Name(createMsg->clone_name())); - if (kNullEntity != entityToClone) + case msgs::EntityFactory::kSdf: + { + errors = root.LoadSdfString(createMsg->sdf()); + break; + } + case msgs::EntityFactory::kSdfFilename: { - auto parentComp = - this->iface->ecm->Component(entityToClone); + errors = root.Load(createMsg->sdf_filename()); + break; + } + case msgs::EntityFactory::kModel: + { + // TODO(louise) Support model msg + gzerr << "model field not yet supported." << std::endl; + return false; + } + case msgs::EntityFactory::kLight: + { + lightSdf = convert(createMsg->light()); + break; + } + case msgs::EntityFactory::kCloneName: + { + auto validClone = false; + auto clonedEntity = kNullEntity; + auto entityToClone = this->iface->ecm->EntityByComponents( + components::Name(createMsg->clone_name())); + if (kNullEntity != entityToClone) + { + auto parentComp = + this->iface->ecm->Component(entityToClone); + + // TODO(anyone) add better support for creating non-top level entities. + // For now, we will only clone top level entities + if (parentComp && parentComp->Data() == this->iface->worldEntity) + { + auto parentEntity = parentComp->Data(); + clonedEntity = this->iface->ecm->Clone(entityToClone, + parentEntity, createMsg->name(), createMsg->allow_renaming()); + validClone = kNullEntity != clonedEntity; + } + } - // TODO(anyone) add better support for creating non-top level entities. - // For now, we will only clone top level entities - if (parentComp && parentComp->Data() == this->iface->worldEntity) + if (!validClone) { - auto parentEntity = parentComp->Data(); - clonedEntity = this->iface->ecm->Clone(entityToClone, - parentEntity, createMsg->name(), createMsg->allow_renaming()); - validClone = kNullEntity != clonedEntity; + gzerr << "Request to clone an entity named [" + << createMsg->clone_name() << "] failed." << std::endl; + return false; } - } - if (!validClone) + if (createMsg->has_pose()) + { + // TODO(anyone) handle if relative_to is filled + auto pose = sim::convert(createMsg->pose()); + this->iface->ecm->SetComponentData(clonedEntity, + pose); + } + return true; + } + default: { - gzerr << "Request to clone an entity named [" - << createMsg->clone_name() << "] failed." << std::endl; + gzerr << "Missing [from] field in create message." << std::endl; return false; } + } - if (createMsg->has_pose()) - { - // TODO(anyone) handle if relative_to is filled - auto pose = sim::convert(createMsg->pose()); - this->iface->ecm->SetComponentData(clonedEntity, - pose); - } - return true; + if (!errors.empty()) + { + for (auto &err : errors) + gzerr << err << std::endl; + return false; + } + + bool isModel{false}; + bool isLight{false}; + bool isActor{false}; + bool isRoot{false}; + if (nullptr != root.Model()) + { + isRoot = true; + isModel = true; + } + else if (nullptr != root.Light()) + { + isRoot = true; + isLight = true; + } + else if (nullptr != root.Actor()) + { + isRoot = true; + isActor = true; } - default: + else if (!lightSdf.Name().empty()) { - gzerr << "Missing [from] field in create message." << std::endl; + isLight = true; + } + else + { + gzerr << "Expected exactly one top-level , or on" + << " SDF." << std::endl; return false; } - } - if (!errors.empty()) - { - for (auto &err : errors) - gzerr << err << std::endl; - return false; - } + if ((isModel && isLight) || (isModel && isActor) || (isLight && isActor)) + { + gzwarn << "Expected exactly one top-level , or , " + << "but found more. Only the 1st will be spawned." << std::endl; + } - bool isModel{false}; - bool isLight{false}; - bool isActor{false}; - bool isRoot{false}; - if (nullptr != root.Model()) - { - isRoot = true; - isModel = true; - } - else if (nullptr != root.Light()) - { - isRoot = true; - isLight = true; - } - else if (nullptr != root.Actor()) - { - isRoot = true; - isActor = true; - } - else if (!lightSdf.Name().empty()) - { - isLight = true; - } - else - { - gzerr << "Expected exactly one top-level , or on" - << " SDF." << std::endl; - return false; - } + // Check the name of the entity being spawned + std::string desiredName; + if (!createMsg->name().empty()) + { + desiredName = createMsg->name(); + } + else if (isModel) + { + desiredName = root.Model()->Name(); + } + else if (isLight && isRoot) + { + desiredName = root.Light()->Name(); + } + else if (isLight) + { + desiredName = lightSdf.Name(); + } + else if (isActor) + { + desiredName = root.Actor()->Name(); + } - if ((isModel && isLight) || (isModel && isActor) || (isLight && isActor)) - { - gzwarn << "Expected exactly one top-level , or , " - << "but found more. Only the 1st will be spawned." << std::endl; - } + // Check if there's already a top-level entity with the given name + if (kNullEntity != this->iface->ecm->EntityByComponents( + components::Name(desiredName), + components::ParentEntity(this->iface->worldEntity))) + { + if (!createMsg->allow_renaming()) + { + gzwarn << "Entity named [" << desiredName << "] already exists and " + << "[allow_renaming] is false. Entity not spawned." + << std::endl; + return false; + } - // Check the name of the entity being spawned - std::string desiredName; - if (!createMsg->name().empty()) - { - desiredName = createMsg->name(); - } - else if (isModel) - { - desiredName = root.Model()->Name(); - } - else if (isLight && isRoot) - { - desiredName = root.Light()->Name(); - } - else if (isLight) - { - desiredName = lightSdf.Name(); - } - else if (isActor) - { - desiredName = root.Actor()->Name(); - } + // Generate unique name + std::string newName = desiredName; + int i = 0; + while (kNullEntity != this->iface->ecm->EntityByComponents( + components::Name(newName), + components::ParentEntity(this->iface->worldEntity))) + { + newName = desiredName + "_" + std::to_string(i++); + } + desiredName = newName; + } - // Check if there's already a top-level entity with the given name - if (kNullEntity != this->iface->ecm->EntityByComponents( - components::Name(desiredName), - components::ParentEntity(this->iface->worldEntity))) - { - if (!createMsg->allow_renaming()) + // Create entities + Entity entity{kNullEntity}; + if (isModel) { - gzwarn << "Entity named [" << desiredName << "] already exists and " - << "[allow_renaming] is false. Entity not spawned." - << std::endl; - return false; + auto model = *root.Model(); + model.SetName(desiredName); + entity = this->iface->creator->CreateEntities(&model); } - - // Generate unique name - std::string newName = desiredName; - int i = 0; - while (kNullEntity != this->iface->ecm->EntityByComponents( - components::Name(newName), - components::ParentEntity(this->iface->worldEntity))) + else if (isLight && isRoot) { - newName = desiredName + "_" + std::to_string(i++); + auto light = *root.Light(); + light.SetName(desiredName); + entity = this->iface->creator->CreateEntities(&light); + } + else if (isLight) + { + lightSdf.SetName(desiredName); + entity = this->iface->creator->CreateEntities(&lightSdf); + } + else if (isActor) + { + auto actor = *root.Actor(); + actor.SetName(desiredName); + entity = this->iface->creator->CreateEntities(&actor); } - desiredName = newName; - } - - // Create entities - Entity entity{kNullEntity}; - if (isModel) - { - auto model = *root.Model(); - model.SetName(desiredName); - entity = this->iface->creator->CreateEntities(&model); - } - else if (isLight && isRoot) - { - auto light = *root.Light(); - light.SetName(desiredName); - entity = this->iface->creator->CreateEntities(&light); - } - else if (isLight) - { - lightSdf.SetName(desiredName); - entity = this->iface->creator->CreateEntities(&lightSdf); - } - else if (isActor) - { - auto actor = *root.Actor(); - actor.SetName(desiredName); - entity = this->iface->creator->CreateEntities(&actor); - } - - this->iface->creator->SetParent(entity, this->iface->worldEntity); - // Pose - std::optional createPose; - if (createMsg->has_pose()) - { - createPose = msgs::Convert(createMsg->pose()); - } + this->iface->creator->SetParent(entity, this->iface->worldEntity); - // Spherical coordinates - if (createMsg->has_spherical_coordinates()) - { - auto scComp = this->iface->ecm->Component( - this->iface->worldEntity); - if (nullptr == scComp) + // Pose + std::optional createPose; + if (createMsg->has_pose()) { - gzwarn << "Trying to create entity [" << desiredName - << "] with spherical coordinates, but world's spherical " - << "coordinates aren't set. Entity will be created at the world " - << "origin." << std::endl; + createPose = msgs::Convert(createMsg->pose()); } - else + + // Spherical coordinates + if (createMsg->has_spherical_coordinates()) { - // deg to rad - math::Vector3d latLonEle{ - GZ_DTOR(createMsg->spherical_coordinates().latitude_deg()), - GZ_DTOR(createMsg->spherical_coordinates().longitude_deg()), - createMsg->spherical_coordinates().elevation()}; - - auto pos = scComp->Data().PositionTransform(latLonEle, - math::SphericalCoordinates::SPHERICAL, - math::SphericalCoordinates::LOCAL2); - - // Override pos and add to yaw - if (!createPose.has_value()) - createPose = math::Pose3d::Zero; - createPose.value().SetX(pos.X()); - createPose.value().SetY(pos.Y()); - createPose.value().SetZ(pos.Z()); - createPose.value().Rot() = math::Quaterniond(0, 0, - GZ_DTOR(createMsg->spherical_coordinates().heading_deg())) * - createPose.value().Rot(); + auto scComp = this->iface->ecm->Component( + this->iface->worldEntity); + if (nullptr == scComp) + { + gzwarn << "Trying to create entity [" << desiredName + << "] with spherical coordinates, but world's spherical " + << "coordinates aren't set. Entity will be created at the world " + << "origin." << std::endl; + } + else + { + // deg to rad + math::Vector3d latLonEle{ + GZ_DTOR(createMsg->spherical_coordinates().latitude_deg()), + GZ_DTOR(createMsg->spherical_coordinates().longitude_deg()), + createMsg->spherical_coordinates().elevation()}; + + auto pos = scComp->Data().PositionTransform(latLonEle, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::LOCAL2); + + // Override pos and add to yaw + if (!createPose.has_value()) + createPose = math::Pose3d::Zero; + createPose.value().SetX(pos.X()); + createPose.value().SetY(pos.Y()); + createPose.value().SetZ(pos.Z()); + createPose.value().Rot() = math::Quaterniond(0, 0, + GZ_DTOR(createMsg->spherical_coordinates().heading_deg())) * + createPose.value().Rot(); + } } - } - if (createPose.has_value()) - { - auto poseComp = this->iface->ecm->Component(entity); - *poseComp = components::Pose(createPose.value()); - } + if (createPose.has_value()) + { + auto poseComp = this->iface->ecm->Component(entity); + *poseComp = components::Pose(createPose.value()); + } - gzdbg << "Created entity [" << entity << "] named [" << desiredName << "]" - << std::endl; + gzdbg << "Created entity [" << entity << "] named [" << desiredName << "]" + << std::endl; + }); return true; } @@ -1922,7 +1939,8 @@ bool WheelSlipCommand::Execute() GZ_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, - UserCommands::ISystemPreUpdate + UserCommands::ISystemPreUpdate, + UserCommands::ISystemReset ) GZ_ADD_PLUGIN_ALIAS(UserCommands, diff --git a/src/systems/user_commands/UserCommands.hh b/src/systems/user_commands/UserCommands.hh index f035c684a4..084e6cc3be 100644 --- a/src/systems/user_commands/UserCommands.hh +++ b/src/systems/user_commands/UserCommands.hh @@ -73,7 +73,8 @@ namespace systems class UserCommands final: public System, public ISystemConfigure, - public ISystemPreUpdate + public ISystemPreUpdate, + public ISystemReset { /// \brief Constructor public: explicit UserCommands(); @@ -95,6 +96,10 @@ namespace systems public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; + /// Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + /// \brief Private data pointer. private: std::unique_ptr dataPtr; }; From ec890ef412ed74f9c5990fb042d3eb6751e5bf83 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 27 Jun 2022 17:46:41 -0700 Subject: [PATCH 40/54] Fixed SystemPlugin loader Signed-off-by: ahcorde --- src/SystemLoader.cc | 2 -- src/systems/sensors/Sensors.cc | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index c4f82a7f5c..f7206c9983 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -136,7 +136,6 @@ class gz::sim::SystemLoaderPrivate return false; } - this->systemPluginsAdded.insert(_plugin); return true; } @@ -151,7 +150,6 @@ class gz::sim::SystemLoaderPrivate public: std::unordered_set systemPluginPaths; /// \brief System plugins that have instances loaded via the manager. - public: std::unordered_set systemPluginsAdded; }; ////////////////////////////////////////////////// diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index c46aea662c..d0f2228ea4 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -570,7 +570,7 @@ void Sensors::Configure(const Entity &/*_id*/, ////////////////////////////////////////////////// void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) { - IGN_PROFILE("Sensors::Reset"); + GZ_PROFILE("Sensors::Reset"); if (this->dataPtr->running && this->dataPtr->initialized) { From 5d3cdced9f658ccf28d455730997be0f8619d8d3 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 28 Jun 2022 10:22:35 -0700 Subject: [PATCH 41/54] Clean code Signed-off-by: ahcorde --- src/Server.cc | 4 ---- src/SystemLoader.cc | 4 ---- src/SystemManager.cc | 2 +- src/systems/apply_joint_force/ApplyJointForce.cc | 7 ------- src/systems/apply_joint_force/ApplyJointForce.hh | 5 ----- src/systems/imu/Imu.cc | 11 ++--------- src/systems/imu/Imu.hh | 5 ----- .../joint_state_publisher/JointStatePublisher.cc | 7 ------- .../joint_state_publisher/JointStatePublisher.hh | 5 ----- src/systems/lift_drag/LiftDrag.cc | 9 +-------- src/systems/lift_drag/LiftDrag.hh | 7 +------ src/systems/user_commands/UserCommands.cc | 14 +++++--------- src/systems/user_commands/UserCommands.hh | 7 +------ 13 files changed, 11 insertions(+), 76 deletions(-) diff --git a/src/Server.cc b/src/Server.cc index 534b9b9ec2..471083a1eb 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -73,8 +73,6 @@ bool Server::DownloadModels() { sdf::Root root; sdf::Errors errors; - std::cerr << "this->dataPtr->config.Source() " - << static_cast(this->dataPtr->config.Source()) << '\n'; switch (this->dataPtr->config.Source()) { // Load a world if specified. Check SDF string first, then SDF file @@ -203,8 +201,6 @@ void Server::Init() this->DownloadModels(); }); - gzerr << "Loading default world.\n"; - common::SystemPaths systemPaths; // Worlds from environment variable diff --git a/src/SystemLoader.cc b/src/SystemLoader.cc index c4f82a7f5c..426e9fb5aa 100644 --- a/src/SystemLoader.cc +++ b/src/SystemLoader.cc @@ -136,7 +136,6 @@ class gz::sim::SystemLoaderPrivate return false; } - this->systemPluginsAdded.insert(_plugin); return true; } @@ -149,9 +148,6 @@ class gz::sim::SystemLoaderPrivate /// \brief Paths to search for system plugins. public: std::unordered_set systemPluginPaths; - - /// \brief System plugins that have instances loaded via the manager. - public: std::unordered_set systemPluginsAdded; }; ////////////////////////////////////////////////// diff --git a/src/SystemManager.cc b/src/SystemManager.cc index b5ee3b281a..e5b349d830 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -53,7 +53,7 @@ void SystemManager::LoadPlugin(const Entity _entity, const std::string &_name, const sdf::ElementPtr &_sdf) { - for (const auto pluginInfo : loadedPlugins) + for (const auto & pluginInfo : loadedPlugins) { if (_entity == pluginInfo.entity && _fname == pluginInfo.fname && diff --git a/src/systems/apply_joint_force/ApplyJointForce.cc b/src/systems/apply_joint_force/ApplyJointForce.cc index cfe10b50e7..f993ab7897 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.cc +++ b/src/systems/apply_joint_force/ApplyJointForce.cc @@ -62,12 +62,6 @@ ApplyJointForce::ApplyJointForce() { } -////////////////////////////////////////////////// -void ApplyJointForce::Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) -{ -} - ////////////////////////////////////////////////// void ApplyJointForce::Configure(const Entity &_entity, const std::shared_ptr &_sdf, @@ -172,7 +166,6 @@ void ApplyJointForcePrivate::OnCmdForce(const msgs::Double &_msg) GZ_ADD_PLUGIN(ApplyJointForce, gz::sim::System, ApplyJointForce::ISystemConfigure, - ApplyJointForce::ISystemReset, ApplyJointForce::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(ApplyJointForce, diff --git a/src/systems/apply_joint_force/ApplyJointForce.hh b/src/systems/apply_joint_force/ApplyJointForce.hh index 4050bcb56a..0c93a28c20 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.hh +++ b/src/systems/apply_joint_force/ApplyJointForce.hh @@ -35,7 +35,6 @@ namespace systems class ApplyJointForce : public System, public ISystemConfigure, - public ISystemReset, public ISystemPreUpdate { /// \brief Constructor @@ -44,10 +43,6 @@ namespace systems /// \brief Destructor public: ~ApplyJointForce() override = default; - /// Documentation inherited - public: void Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; - // Documentation inherited public: void Configure(const Entity &_entity, const std::shared_ptr &_sdf, diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 4c18e73f13..66e457f90f 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -104,12 +104,6 @@ Imu::Imu() : System(), dataPtr(std::make_unique()) ////////////////////////////////////////////////// Imu::~Imu() = default; -////////////////////////////////////////////////// -void Imu::Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) -{ -} - ////////////////////////////////////////////////// void Imu::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) @@ -365,9 +359,8 @@ void ImuPrivate::RemoveImuEntities( } GZ_ADD_PLUGIN(Imu, System, - Imu::ISystemPreUpdate, - Imu::ISystemReset, - Imu::ISystemPostUpdate + Imu::ISystemPreUpdate, + Imu::ISystemPostUpdate ) GZ_ADD_PLUGIN_ALIAS(Imu, "gz::sim::systems::Imu") diff --git a/src/systems/imu/Imu.hh b/src/systems/imu/Imu.hh index 5e06389d65..f51eeb24fd 100644 --- a/src/systems/imu/Imu.hh +++ b/src/systems/imu/Imu.hh @@ -39,7 +39,6 @@ namespace systems class Imu: public System, public ISystemPreUpdate, - public ISystemReset, public ISystemPostUpdate { /// \brief Constructor @@ -47,10 +46,6 @@ namespace systems /// \brief Destructor public: ~Imu() override; - - public: void Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; - /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index 5274327795..ca37844bcf 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/src/systems/joint_state_publisher/JointStatePublisher.cc @@ -46,12 +46,6 @@ JointStatePublisher::JointStatePublisher() { } -////////////////////////////////////////////////// -void JointStatePublisher::Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) -{ -} - ////////////////////////////////////////////////// void JointStatePublisher::Configure( const Entity &_entity, const std::shared_ptr &_sdf, @@ -321,7 +315,6 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, GZ_ADD_PLUGIN(JointStatePublisher, gz::sim::System, JointStatePublisher::ISystemConfigure, - JointStatePublisher::ISystemReset, JointStatePublisher::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(JointStatePublisher, diff --git a/src/systems/joint_state_publisher/JointStatePublisher.hh b/src/systems/joint_state_publisher/JointStatePublisher.hh index 776b588fd8..a6358da4b3 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.hh +++ b/src/systems/joint_state_publisher/JointStatePublisher.hh @@ -49,7 +49,6 @@ namespace systems class JointStatePublisher : public System, public ISystemConfigure, - public ISystemReset, public ISystemPostUpdate { /// \brief Constructor @@ -67,10 +66,6 @@ namespace systems public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; - /// Documentation inherited - public: void Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; - /// \brief Create components for a joint. /// \param[in] _ecm The EntityComponentManager. /// \param[in] _joint The joint entity to create component for. diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index ef1a1bff51..bffe225c25 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -510,12 +510,6 @@ void LiftDrag::Configure(const Entity &_entity, this->dataPtr->sdfConfig = _sdf->Clone(); } -////////////////////////////////////////////////// -void LiftDrag::Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) -{ -} - ////////////////////////////////////////////////// void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { @@ -566,8 +560,7 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) GZ_ADD_PLUGIN(LiftDrag, gz::sim::System, LiftDrag::ISystemConfigure, - LiftDrag::ISystemPreUpdate, - LiftDrag::ISystemReset) + LiftDrag::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(LiftDrag, "gz::sim::systems::LiftDrag") diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index d7eab2c07e..8c7cba996d 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -66,8 +66,7 @@ namespace systems class LiftDrag : public System, public ISystemConfigure, - public ISystemPreUpdate, - public ISystemReset + public ISystemPreUpdate { /// \brief Constructor public: LiftDrag(); @@ -85,10 +84,6 @@ namespace systems public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; - /// Documentation inherited - public: void Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; - /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 6e1f5eeb70..52e280672f 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -764,11 +764,6 @@ void UserCommands::Configure(const Entity &_entity, gzmsg << "Material service on [" << wheelSlipService << "]" << std::endl; } -void UserCommands::Reset(const UpdateInfo &/*_info*/, - EntityComponentManager &/*_ecm*/) -{ -} - ////////////////////////////////////////////////// void UserCommands::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &) @@ -1323,6 +1318,7 @@ bool CreateCommand::Execute() gzdbg << "Created entity [" << entity << "] named [" << desiredName << "]" << std::endl; + return true; }); return true; @@ -1937,10 +1933,10 @@ bool WheelSlipCommand::Execute() return false; } -GZ_ADD_PLUGIN(UserCommands, System, - UserCommands::ISystemConfigure, - UserCommands::ISystemPreUpdate, - UserCommands::ISystemReset +GZ_ADD_PLUGIN(UserCommands, + System, + UserCommands::ISystemConfigure, + UserCommands::ISystemPreUpdate ) GZ_ADD_PLUGIN_ALIAS(UserCommands, diff --git a/src/systems/user_commands/UserCommands.hh b/src/systems/user_commands/UserCommands.hh index 084e6cc3be..f035c684a4 100644 --- a/src/systems/user_commands/UserCommands.hh +++ b/src/systems/user_commands/UserCommands.hh @@ -73,8 +73,7 @@ namespace systems class UserCommands final: public System, public ISystemConfigure, - public ISystemPreUpdate, - public ISystemReset + public ISystemPreUpdate { /// \brief Constructor public: explicit UserCommands(); @@ -96,10 +95,6 @@ namespace systems public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; - /// Documentation inherited - public: void Reset(const UpdateInfo &_info, - EntityComponentManager &_ecm) final; - /// \brief Private data pointer. private: std::unique_ptr dataPtr; }; From 7db60c543196c2e59ba1cb3f8eeea6cfd03d18e5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 28 Jun 2022 10:51:17 -0700 Subject: [PATCH 42/54] fix merge Signed-off-by: ahcorde --- src/SystemManager.cc | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index b05783d5e0..6f333138b9 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -147,19 +147,6 @@ size_t SystemManager::ActivatePendingSystems() return count; } -////////////////////////////////////////////////// -/// \brief Structure to temporarily store plugin information for reset -struct PluginInfo { - /// \brief Entity plugin is attached to - Entity entity; - /// \brief Filename of the plugin library - std::string fname; - /// \brief Name of the plugin - std::string name; - /// \brief SDF element (content of the plugin tag) - sdf::ElementPtr sdf; -}; - ////////////////////////////////////////////////// void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) { From 4248294413a2be7bf19dfd2c973eb4c997225093 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 28 Jun 2022 11:54:32 -0700 Subject: [PATCH 43/54] Fix Signed-off-by: ahcorde --- src/SystemManager.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 6f333138b9..956da6278a 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -56,7 +56,6 @@ void SystemManager::LoadPlugin(const Entity _entity, for (const auto & pluginInfo : loadedPlugins) { if (_entity == pluginInfo.entity && - _fname == pluginInfo.fname && _name == pluginInfo.name) { if (_name == "gz::sim::systems::UserCommands" || From 5cb7630fa1af693019e621640ab55a79ad2cb499 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 28 Jun 2022 12:47:39 -0700 Subject: [PATCH 44/54] make linters happy Signed-off-by: ahcorde --- src/systems/log/LogPlayback.cc | 2 +- src/systems/user_commands/UserCommands.cc | 15 ++++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 0241917a28..9f6fe1c379 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -459,7 +459,7 @@ void LogPlayback::Reset(const UpdateInfo &, EntityComponentManager &) // In this case, Reset is a noop // LogPlayback already has handling for jumps in time as part of the // Update method. - // Leaving this function implemented but empty prevents the SystemManager + // Leaving this function implemented but empty prevents the SystemManager // from trying to destroy and recreate the plugin. } diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 52e280672f..5e0b20d4a6 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1112,10 +1112,11 @@ bool CreateCommand::Execute() if (kNullEntity != entityToClone) { auto parentComp = - this->iface->ecm->Component(entityToClone); + this->iface->ecm->Component( + entityToClone); - // TODO(anyone) add better support for creating non-top level entities. - // For now, we will only clone top level entities + // TODO(anyone) add better support for creating non-top level. + // entities For now, we will only clone top level entities if (parentComp && parentComp->Data() == this->iface->worldEntity) { auto parentEntity = parentComp->Data(); @@ -1277,14 +1278,14 @@ bool CreateCommand::Execute() // Spherical coordinates if (createMsg->has_spherical_coordinates()) { - auto scComp = this->iface->ecm->Component( - this->iface->worldEntity); + auto scComp = this->iface->ecm->Component< + components::SphericalCoordinates>(this->iface->worldEntity); if (nullptr == scComp) { gzwarn << "Trying to create entity [" << desiredName << "] with spherical coordinates, but world's spherical " - << "coordinates aren't set. Entity will be created at the world " - << "origin." << std::endl; + << "coordinates aren't set. Entity will be created at the " + << "world origin." << std::endl; } else { From 5465f2724a880b1a663187d37ef82256c7620c8c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 28 Jun 2022 12:55:53 -0700 Subject: [PATCH 45/54] make linters happy Signed-off-by: ahcorde --- src/systems/log/LogPlayback.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 9f6fe1c379..ef68d01346 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -457,7 +457,7 @@ bool LogPlaybackPrivate::ExtractStateAndResources() void LogPlayback::Reset(const UpdateInfo &, EntityComponentManager &) { // In this case, Reset is a noop - // LogPlayback already has handling for jumps in time as part of the + // LogPlayback already has handling for jumps in time as part of the // Update method. // Leaving this function implemented but empty prevents the SystemManager // from trying to destroy and recreate the plugin. From 33a15780703108bcbb2b1e0fb15a69582844da84 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 29 Jun 2022 11:28:03 -0700 Subject: [PATCH 46/54] update worldname method Signed-off-by: ahcorde --- src/Server.cc | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/Server.cc b/src/Server.cc index 471083a1eb..3b8a5bb33e 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -212,11 +212,20 @@ void Server::Init() std::string filePath = systemPaths.FindFile( this->dataPtr->config.SdfFile()); - std::string worldName; - auto errors2 = this->dataPtr->sdfRoot.WorldName(filePath, worldName); + std::vector worldNames; + auto errors2 = this->dataPtr->sdfRoot.WorldNameFromFile( + filePath, worldNames); - errors = this->dataPtr->sdfRoot.LoadSdfString( - DefaultWorld::World(worldName)); + if (worldNames.size() > 0) + { + // we only support one world for now + errors = this->dataPtr->sdfRoot.LoadSdfString( + DefaultWorld::World(worldNames[0])); + } + else + { + return; + } } else { From 557d2b7315a47c5c6f1148ccc5de3cb1a6e51151 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 29 Jun 2022 12:05:41 -0700 Subject: [PATCH 47/54] Added feedback Signed-off-by: ahcorde --- include/gz/sim/Server.hh | 7 ------- include/gz/sim/ServerConfig.hh | 5 +++-- src/Server.cc | 15 +++------------ src/ServerPrivate.hh | 4 ---- src/gz.cc | 2 +- 5 files changed, 7 insertions(+), 26 deletions(-) diff --git a/include/gz/sim/Server.hh b/include/gz/sim/Server.hh index 4792d19b30..23715e15b6 100644 --- a/include/gz/sim/Server.hh +++ b/include/gz/sim/Server.hh @@ -120,13 +120,6 @@ namespace gz /// \brief Destructor public: ~Server(); - /// \brief Construct the server using the parameters specified in a - /// ServerConfig and choose it should download models in the background - /// \param[in] _config Server configuration parameters. If this - /// parameter is omitted, then an empty world is loaded. - public: Server(bool _downloadInParallel, - const ServerConfig &_config = ServerConfig()); - /// \brief Initialize server private: void Init(); diff --git a/include/gz/sim/ServerConfig.hh b/include/gz/sim/ServerConfig.hh index 4694dd2d08..d25e6f2f3c 100644 --- a/include/gz/sim/ServerConfig.hh +++ b/include/gz/sim/ServerConfig.hh @@ -215,7 +215,8 @@ namespace gz public: std::optional UpdateRate() const; /// \brief Set the run option - /// \return True if -r is setted or False otherwise + /// \param[in] _run True if the server should run on start, + /// false otherwise public: void SetRunOption(bool _run); /// \brief Get the run option @@ -223,7 +224,7 @@ namespace gz public: bool RunOption() const; /// \brief True if the models are downloaded in a thread false otherwise - /// \param _downloadInParallel True if the models are downloaded in a + /// \param[in] _downloadInParallel True if the models are downloaded in a /// thread false otherwise public: void SetDownloadInParallel(bool _downloadInParallel); diff --git a/src/Server.cc b/src/Server.cc index 3b8a5bb33e..0b73269169 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -59,15 +59,6 @@ Server::Server(const ServerConfig &_config) this->Init(); } -///////////////////////////////////////////////// -Server::Server(bool _downloadInParallel, const ServerConfig &_config) - : dataPtr(new ServerPrivate) -{ - this->dataPtr->config = _config; - this->dataPtr->downloadInParallel = _downloadInParallel; - this->Init(); -} - ///////////////////////////////////////////////// bool Server::DownloadModels() { @@ -117,7 +108,7 @@ bool Server::DownloadModels() gzmsg << "Loading SDF world file[" << filePath << "].\n"; - while (this->dataPtr->downloadInParallel && + while (this->dataPtr->config.DownloadInParallel() && this->dataPtr->simRunners.size() == 0) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -126,7 +117,7 @@ bool Server::DownloadModels() errors = root.Load(filePath); this->dataPtr->sdfRoot = root.Clone(); - if (!this->dataPtr->downloadInParallel) + if (!this->dataPtr->config.DownloadInParallel()) return true; if (this->dataPtr->sdfRoot.WorldCount() == 0) @@ -194,7 +185,7 @@ void Server::Init() sdf::Errors errors; - if (this->dataPtr->downloadInParallel) + if (this->dataPtr->config.DownloadInParallel()) { this->dataPtr->downloadModelsThread = std::thread([&]() { diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index 4f4be91c87..7b1d02fb85 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -189,10 +189,6 @@ namespace gz /// \brief Thread to download the models public: std::thread downloadModelsThread; - /// \brief True if models are downloaded in the background, false - /// otherwise - public: bool downloadInParallel{false}; - /// \brief List of names for all worlds loaded in this server. private: std::vector worldNames; diff --git a/src/gz.cc b/src/gz.cc index 6564886751..e963912d83 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -351,7 +351,7 @@ extern "C" int runServer(const char *_sdfString, // Create the Gazebo server serverConfig.SetDownloadInParallel(true); - gz::sim::Server server(true, serverConfig); + gz::sim::Server server(serverConfig); // Run the server server.Run(true, _iterations, _run == 0); From 45c64c6d80414bb9506bc4d664af3b7eacaaffad Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 30 Jun 2022 09:37:31 -0700 Subject: [PATCH 48/54] Reduce diff Signed-off-by: ahcorde --- src/systems/scene_broadcaster/SceneBroadcaster.cc | 1 - test/worlds/models/building_L1/model.sdf | 14 +++++++------- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 32eb6b51e7..bddf6d854a 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -265,7 +265,6 @@ void SceneBroadcaster::Configure( } this->dataPtr->worldEntity = _entity; - this->dataPtr->worldName = name->Data(); auto readHertz = _sdf->Get("dynamic_pose_hertz", 60); diff --git a/test/worlds/models/building_L1/model.sdf b/test/worlds/models/building_L1/model.sdf index 8e0c5fff05..a238c0dfc2 100644 --- a/test/worlds/models/building_L1/model.sdf +++ b/test/worlds/models/building_L1/model.sdf @@ -6,7 +6,7 @@ - meshes/floor_1.obj + model://building_L1/meshes/floor_1.obj @@ -15,7 +15,7 @@ 0.0 - meshes/blue_linoleum.png + model://building_L1/meshes/blue_linoleum.png @@ -23,7 +23,7 @@ - meshes/floor_1.obj + model://building_L1/meshes/floor_1.obj @@ -37,7 +37,7 @@ - meshes/wall_1.obj + model://building_L1/meshes/wall_1.obj @@ -46,15 +46,15 @@ 0.0 - meshes/default.png + model://building_L1/meshes/default.png - + - meshes/wall_1.obj + model://building_L1/meshes/wall_1.obj From b9b015b08d807dbf3cd4310e1ed32bf1e32a4d14 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 30 Jun 2022 09:38:52 -0700 Subject: [PATCH 49/54] Reduce diff Signed-off-by: ahcorde --- 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 12a2c7e3d8..f2ec766259 100644 --- a/examples/plugin/reset_plugin/CMakeLists.txt +++ b/examples/plugin/reset_plugin/CMakeLists.txt @@ -9,6 +9,6 @@ find_package(gz-sim7 REQUIRED) set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) add_library(JointPositionRandomizer SHARED JointPositionRandomizer.cc) -target_link_libraries(JointPositionRandomizer +target_link_libraries(JointPositionRandomizer PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} PRIVATE gz-sim${GZ_SIM_VER}::core) From 2ca8584dbf797b65ae468dc3092e5323c5d863d4 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 30 Jun 2022 09:47:36 -0700 Subject: [PATCH 50/54] Reduce diff Signed-off-by: ahcorde --- src/systems/apply_joint_force/ApplyJointForce.cc | 10 +++++----- src/systems/imu/Imu.cc | 4 ++-- src/systems/imu/Imu.hh | 1 + src/systems/lift_drag/LiftDrag.cc | 6 +++--- src/systems/lift_drag/LiftDrag.hh | 1 + src/systems/scene_broadcaster/SceneBroadcaster.hh | 1 + src/systems/sensors/Sensors.cc | 2 +- 7 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/systems/apply_joint_force/ApplyJointForce.cc b/src/systems/apply_joint_force/ApplyJointForce.cc index f993ab7897..c95908d563 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.cc +++ b/src/systems/apply_joint_force/ApplyJointForce.cc @@ -164,13 +164,13 @@ void ApplyJointForcePrivate::OnCmdForce(const msgs::Double &_msg) } GZ_ADD_PLUGIN(ApplyJointForce, - gz::sim::System, - ApplyJointForce::ISystemConfigure, - ApplyJointForce::ISystemPreUpdate) + gz::sim::System, + ApplyJointForce::ISystemConfigure, + ApplyJointForce::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(ApplyJointForce, - "gz::sim::systems::ApplyJointForce") + "gz::sim::systems::ApplyJointForce") // TODO(CH3): Deprecated, remove on version 8 GZ_ADD_PLUGIN_ALIAS(ApplyJointForce, - "ignition::gazebo::systems::ApplyJointForce") + "ignition::gazebo::systems::ApplyJointForce") diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index f6531c7269..5c0ad2ed1d 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -359,8 +359,8 @@ void ImuPrivate::RemoveImuEntities( } GZ_ADD_PLUGIN(Imu, System, - Imu::ISystemPreUpdate, - Imu::ISystemPostUpdate + Imu::ISystemPreUpdate, + Imu::ISystemPostUpdate ) GZ_ADD_PLUGIN_ALIAS(Imu, "gz::sim::systems::Imu") diff --git a/src/systems/imu/Imu.hh b/src/systems/imu/Imu.hh index f51eeb24fd..828aa31e74 100644 --- a/src/systems/imu/Imu.hh +++ b/src/systems/imu/Imu.hh @@ -46,6 +46,7 @@ namespace systems /// \brief Destructor public: ~Imu() override; + /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index bffe225c25..e318cbcc0c 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -558,9 +558,9 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) } GZ_ADD_PLUGIN(LiftDrag, - gz::sim::System, - LiftDrag::ISystemConfigure, - LiftDrag::ISystemPreUpdate) + gz::sim::System, + LiftDrag::ISystemConfigure, + LiftDrag::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(LiftDrag, "gz::sim::systems::LiftDrag") diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index 8c7cba996d..efbed40502 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -93,3 +93,4 @@ namespace systems } #endif + diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.hh b/src/systems/scene_broadcaster/SceneBroadcaster.hh index 4448725592..5e95c87882 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.hh +++ b/src/systems/scene_broadcaster/SceneBroadcaster.hh @@ -71,3 +71,4 @@ namespace systems } } #endif + diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 7d3db868cc..b746f13402 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -317,8 +317,8 @@ void SensorsPrivate::RunOnce() for (const auto & sensor : this->activeSensors) { // 90% of update delta (1/UpdateRate()); - std::chrono::duration< double >(0.9 / sensor->UpdateRate())); auto delta = std::chrono::duration_cast( + std::chrono::duration< double >(0.9 / sensor->UpdateRate())); this->sensorMask[sensor->Id()] = this->updateTime + delta; } } From 836cd8243b806147686c74a71728bcae9b8a2b2e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 7 Jul 2022 14:33:44 +0200 Subject: [PATCH 51/54] Added logic to remove system plugins Signed-off-by: ahcorde --- src/SimulationRunner.cc | 2 + src/SystemManager.cc | 111 +++++++++++++++++++++++++++++++--------- src/SystemManager.hh | 11 ++++ 3 files changed, 100 insertions(+), 24 deletions(-) diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 6dcbc62c9b..e8771316b9 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -747,6 +747,8 @@ bool SimulationRunner::Run(const uint64_t _iterations) this->initialEntityCompMgr.CopyFrom(this->entityCompMgr); + this->systemMgr->SetRemoveInitialSystemPlugins(); + WorldControl control; control.rewind = true; control.pause = !this->serverConfig.RunOption(); diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 956da6278a..9be5aab097 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -35,8 +35,6 @@ struct PluginInfo { sdf::ElementPtr sdf; }; -std::vector loadedPlugins; - ////////////////////////////////////////////////// SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, EntityComponentManager *_entityCompMgr, @@ -48,34 +46,90 @@ SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, } ////////////////////////////////////////////////// -void SystemManager::LoadPlugin(const Entity _entity, - const std::string &_fname, - const std::string &_name, - const sdf::ElementPtr &_sdf) +void SystemManager::SetRemoveInitialSystemPlugins() { - for (const auto & pluginInfo : loadedPlugins) + std::set removePendingSystems; + /// Found systems already loaded + if (this->systems.size() >= 3) { - if (_entity == pluginInfo.entity && - _name == pluginInfo.name) + for (unsigned int i = 0; i < 3; ++i) { - if (_name == "gz::sim::systems::UserCommands" || - _name == "gz::sim::systems::SceneBroadcaster" || - _name == "gz::sim::systems::Physics") + bool foundSystem = false; + for (unsigned int j = 3; j < this->systems.size(); ++j) { - // Plugin already loaded - igndbg << "This system Plugin [" << _name - << "] is already loaded in this entity [" << _entity - << "]" << std::endl; - return; + if (this->systems[i].name == this->systems[j].name) + { + foundSystem = true; + removeSystems.insert(j); + break; + } + } + + for (unsigned int j = 0; j < this->pendingSystems.size(); ++j) + { + if (this->systems[i].name == this->systems[j].name) + { + foundSystem = true; + removePendingSystems.insert(j); + break; + } + } + if (!foundSystem) + { + removeSystems.insert(i); + } + } + } + else + { + /// Check if there are system plugin repeated + if (this->pendingSystems.size() >= 3) + { + for (unsigned int i = 0; i < 3; ++i) + { + bool foundSystem = false; + for (unsigned int j = 3; j < this->pendingSystems.size(); ++j) + { + if (this->pendingSystems[i].name == this->pendingSystems[j].name) + { + removePendingSystems.insert(j); + foundSystem = true; + break; + } + } + if (!foundSystem && this->pendingSystems.size() > 3) + { + removePendingSystems.insert(i); + } } } } + int counter = 0; + for (auto const & index : removePendingSystems) + { + this->pendingSystems.erase(this->pendingSystems.begin() + index + counter); + counter--; + } + + counter = 0; + for (auto const & index : removeSystems) + { + this->systems.erase(this->systems.begin() + index + counter); + counter--; + } +} + +////////////////////////////////////////////////// +void SystemManager::LoadPlugin(const Entity _entity, + const std::string &_fname, + const std::string &_name, + const sdf::ElementPtr &_sdf) +{ PluginInfo info; info.entity = _entity; info.name = _name; info.fname = _fname; - loadedPlugins.push_back(info); std::optional system; { @@ -160,10 +214,11 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) this->systemsPreupdate.clear(); this->systemsUpdate.clear(); this->systemsPostupdate.clear(); - loadedPlugins.clear(); std::vector pluginsToBeLoaded; + unsigned int index = 0; + for (auto& system : this->systems) { if (nullptr != system.reset) @@ -171,6 +226,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) // If implemented, call reset and add to pending systems. system.reset->Reset(_info, _ecm); + if (this->removeSystems.find(index) == this->removeSystems.end()) { std::lock_guard lock(this->pendingSystemsMutex); this->pendingSystems.push_back(system); @@ -186,17 +242,24 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) << system.name << "]\n" << "Systems created without plugins that do not implement Reset" << " will not be reloaded. Reset may not work correctly\n"; + index++; continue; } - PluginInfo info = { - system.parentEntity, system.fname, system.name, - system.configureSdf->Clone() - }; - pluginsToBeLoaded.push_back(info); + if (this->removeSystems.find(index) == this->removeSystems.end()) + { + PluginInfo info = { + system.parentEntity, system.fname, system.name, + system.configureSdf->Clone() + }; + + pluginsToBeLoaded.push_back(info); + } } + index++; } + this->removeSystems.clear(); this->systems.clear(); // Load plugins which do not implement reset after clearing this->systems diff --git a/src/SystemManager.hh b/src/SystemManager.hh index e4d9ee4e0a..a69e8ac8e7 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -106,6 +106,14 @@ namespace gz /// \param[in] _ecm Version of the ECM reset to an initial state public: void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm); + /// \brief When loading the model in the background we inserted an empty + /// world that contains 3 system plugins (SceneBroadcaster, UserCommand + /// and Physics). This three system plugins need to be checked when the + /// models are downloaded. If a new system is added that was already added + /// then this new one is removed, or if a model is not found then is + /// removed because this plugin was not specify in the world + public: void SetRemoveInitialSystemPlugins(); + /// \brief Get a vector of all systems implementing "Configure" /// \return Vector of systems' configure interfaces. public: const std::vector& SystemsConfigure(); @@ -180,6 +188,9 @@ namespace gz /// \brief Pointer to associated event manager private: EventManager *eventMgr; + + /// \brief Systems to remove when reset is called + private: std::set removeSystems; }; } } // namespace sim From d0ae28505c31e5b8e90c750bf2495a6dc44acc0b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 7 Jul 2022 15:15:22 +0200 Subject: [PATCH 52/54] make linters happy Signed-off-by: ahcorde --- src/SystemManager.cc | 2 ++ src/SystemManager.hh | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 9be5aab097..82dcfe9fb9 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -19,6 +19,8 @@ #include "gz/sim/Conversions.hh" #include "SystemManager.hh" +#include + using namespace gz; using namespace sim; diff --git a/src/SystemManager.hh b/src/SystemManager.hh index a69e8ac8e7..1880ef4810 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -18,6 +18,7 @@ #define GZ_SIM_SYSTEMMANAGER_HH_ #include +#include #include #include @@ -111,7 +112,7 @@ namespace gz /// and Physics). This three system plugins need to be checked when the /// models are downloaded. If a new system is added that was already added /// then this new one is removed, or if a model is not found then is - /// removed because this plugin was not specify in the world + /// removed because this plugin was not specify in the world public: void SetRemoveInitialSystemPlugins(); /// \brief Get a vector of all systems implementing "Configure" From 29e0a8a1f34613855cf278b6b40f2e8f88b63a90 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 8 Jul 2022 21:36:16 +0200 Subject: [PATCH 53/54] update method name Signed-off-by: ahcorde --- src/Server.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Server.cc b/src/Server.cc index 0b73269169..d4b21d8db5 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -204,7 +204,7 @@ void Server::Init() this->dataPtr->config.SdfFile()); std::vector worldNames; - auto errors2 = this->dataPtr->sdfRoot.WorldNameFromFile( + auto errors2 = this->dataPtr->sdfRoot.WorldNamesFromFile( filePath, worldNames); if (worldNames.size() > 0) From 78dd5bc0a454fd23bb14dd80e076c4cc74b42ea5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 18 Jul 2022 17:23:37 +0200 Subject: [PATCH 54/54] Added feedback Signed-off-by: ahcorde --- examples/worlds/fuel.sdf | 65 +++++++++++++++++++++++++++++----- include/gz/sim/ServerConfig.hh | 2 +- src/SdfEntityCreator.cc | 9 +++-- src/Server.cc | 7 ++-- src/SimulationRunner.hh | 2 +- src/SystemManager.cc | 4 +-- 6 files changed, 69 insertions(+), 20 deletions(-) diff --git a/examples/worlds/fuel.sdf b/examples/worlds/fuel.sdf index 098923c9ae..b8a88380f4 100644 --- a/examples/worlds/fuel.sdf +++ b/examples/worlds/fuel.sdf @@ -18,34 +18,34 @@ - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Ground Plane + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane -3 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Double pendulum with base 3 -1 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack 2 5 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Gazebo - relative paths + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Gazebo - relative paths - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/actor - relative paths + https://fuel.gazebosim.org/1.0/OpenRobotics/models/actor - relative paths @@ -56,19 +56,68 @@ - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + + + true + + + + + false + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/dirt_diffusespecular.png + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/flat_normal.png + 10 + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/grass_diffusespecular.png + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/flat_normal.png + 10 + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/fungus_diffusespecular.png + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/flat_normal.png + 10 + + + 28 + 6 + + + 35 + 18 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/fortress_heightmap.png + 257 257 50 + 0 0 -28 + + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap/tip/files/materials/textures/fortress_heightmap.png + 257 257 50 + 0 0 -28 + + + + + + diff --git a/include/gz/sim/ServerConfig.hh b/include/gz/sim/ServerConfig.hh index d25e6f2f3c..8f5db694f0 100644 --- a/include/gz/sim/ServerConfig.hh +++ b/include/gz/sim/ServerConfig.hh @@ -220,7 +220,7 @@ namespace gz public: void SetRunOption(bool _run); /// \brief Get the run option - /// \return True if -r is setted or False otherwise + /// \return True if the server should run on start, false otherwise public: bool RunOption() const; /// \brief True if the models are downloaded in a thread false otherwise diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 99241e14a8..d13c72ab46 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -221,11 +221,10 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) GZ_PROFILE("SdfEntityCreator::CreateEntities(sdf::World)"); Entity worldEntity = kNullEntity; - this->dataPtr->ecm->Each( - [&](const gz::sim::Entity &_entity, - const gz::sim::components::Name *_name, - const gz::sim::components::World *)->bool + this->dataPtr->ecm->Each( + [&](const Entity &_entity, + const components::Name *_name, + const components::World *)->bool { if (_world->Name() == _name->Data()) { diff --git a/src/Server.cc b/src/Server.cc index d4b21d8db5..54940f8ac0 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -175,7 +175,7 @@ void Server::Init() config.SetCacheLocation(this->dataPtr->config.ResourceCache()); this->dataPtr->fuelClient = std::make_unique(config); - // Configure SDF to fetch assets from ignition fuel. + // Configure SDF to fetch assets from gazebo fuel. sdf::setFindCallback(std::bind(&ServerPrivate::FetchResource, this->dataPtr.get(), std::placeholders::_1)); common::addFindFileURICallback(std::bind(&ServerPrivate::FetchResourceUri, @@ -197,7 +197,7 @@ void Server::Init() // Worlds from environment variable systemPaths.SetFilePathEnv(kResourcePathEnv); - // Worlds installed with ign-gazebo + // Worlds installed with gz-sim systemPaths.AddFilePaths(GZ_SIM_WORLD_INSTALL_DIR); std::string filePath = systemPaths.FindFile( @@ -209,7 +209,8 @@ void Server::Init() if (worldNames.size() > 0) { - // we only support one world for now + // TODO(ahcorde): Add support for more worlds, for now we only support, + // we only support one world. errors = this->dataPtr->sdfRoot.LoadSdfString( DefaultWorld::World(worldNames[0])); } diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 4611ee30f1..c632b469df 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -96,7 +96,7 @@ namespace gz public: bool Run(const uint64_t _iterations); /// \brief This will add a world to the scene - /// \param[in] _light SDF world to add to the scene + /// \param[in] _world SDF world to add to the scene public: void AddWorld(const sdf::World *_world); /// \brief Check if there is any model being downloaded in the backgound. diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 82dcfe9fb9..f02d971010 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -244,7 +244,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) << system.name << "]\n" << "Systems created without plugins that do not implement Reset" << " will not be reloaded. Reset may not work correctly\n"; - index++; + ++index; continue; } @@ -258,7 +258,7 @@ void SystemManager::Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) pluginsToBeLoaded.push_back(info); } } - index++; + ++index; } this->removeSystems.clear();