From c7ba43ac2530124859c03e6a377396a1dc8fc258 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 8 Feb 2022 11:58:52 -0600 Subject: [PATCH 1/5] 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 ed8ea050325bdbf5a8088ab74c8a48457199aa73 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 8 Feb 2022 12:01:26 -0600 Subject: [PATCH 2/5] 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 351dfe6fa0..1b35c8c3d3 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -404,6 +404,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; @@ -532,6 +536,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 9c38505922..cbcb860b20 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 90738b73ef4ebd5718b376436c60adfca0a51463 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 24 Feb 2022 16:43:39 -0600 Subject: [PATCH 3/5] 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 596982e4af1587db3505b63947bc5ac12224646d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 8 Mar 2022 14:35:21 -0600 Subject: [PATCH 4/5] lint Signed-off-by: Michael Carroll --- src/SystemManager.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 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 From 81eabc6d30f26d783d88783940dee5408ae54383 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 8 Mar 2022 16:33:17 -0600 Subject: [PATCH 5/5] Step one additional time for reset Signed-off-by: Michael Carroll --- test/integration/log_system.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 975def19dd..e63b4e8f04 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -918,7 +918,7 @@ TEST_F(LogSystemTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LogControl)) latestSpherePose = spherePose; } - // Rewind + // Rewind to zero req.Clear(); req.set_rewind(true); @@ -926,7 +926,7 @@ TEST_F(LogSystemTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LogControl)) EXPECT_TRUE(result); EXPECT_TRUE(res.data()); - server.Run(true, 2, false); + server.Run(true, 3, false); EXPECT_TRUE(sphereFound); sphereFound = false;