Support world reset

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,14 @@ namespace ignition
       /// \brief Destructor
       public: ~EntityComponentManager();
 
+      /// \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.
       public: Entity CreateEntity();
@@ -669,6 +678,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 +706,25 @@ namespace ignition
       /// \brief Mark all components as not changed.
       protected: void SetAllComponentsUnchanged();
 
+      /// Compute the diff between this EntityComponentManager and _other at the
+      /// 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 ComputeEntityDiff(
+                     const EntityComponentManager &_other) const;
+
+      /// \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 ApplyEntityDiff(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

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 {

include/ignition/gazebo/detail/BaseView.hh

@@ -18,6 +18,7 @@
 #define IGNITION_GAZEBO_DETAIL_BASEVIEW_HH_
 
 #include <cstddef>
+#include <memory>
 #include <set>
 #include <unordered_map>
 #include <vector>
@@ -187,6 +188,8 @@ class IGNITION_GAZEBO_VISIBLE BaseView
   /// \sa ToAddEntities
   public: void ClearToAddEntities();
 
+  public: virtual std::unique_ptr<BaseView> 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

include/ignition/gazebo/detail/View.hh

@@ -17,6 +17,7 @@
 #ifndef IGNITION_GAZEBO_DETAIL_VIEW_HH_
 #define IGNITION_GAZEBO_DETAIL_VIEW_HH_
 
+#include <memory>
 #include <tuple>
 #include <unordered_map>
 #include <unordered_set>
@@ -105,6 +106,8 @@ class View : public BaseView
   /// \brief Documentation inherited
   public: void Reset() override;
 
+  public: std::unique_ptr<BaseView> 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<ComponentTypeTs...>::Reset()
   this->invalidConstData.clear();
   this->missingCompTracker.clear();
 }
+
+//////////////////////////////////////////////////
+template<typename ...ComponentTypeTs>
+std::unique_ptr<BaseView> View<ComponentTypeTs...>::Clone() const
+{
+  return std::make_unique<View<ComponentTypeTs...>>(*this);
+}
 }  // namespace detail
 }  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
 }  // namespace gazebo

src/CMakeLists.txt

@@ -49,6 +49,7 @@ set (sources
   BaseView.cc
   Conversions.cc
   EntityComponentManager.cc
+  EntityComponentManagerDiff.cc
   LevelManager.cc
   Link.cc
   Model.cc

src/EntityComponentManager.cc

@@ -16,6 +16,7 @@
 */
 
 #include "ignition/gazebo/EntityComponentManager.hh"
+#include "EntityComponentManagerDiff.hh"
 
 #include <map>
 #include <memory>
@@ -71,6 +72,14 @@ class ignition::gazebo::EntityComponentManagerPrivate
   /// `AddEntityToMessage`.
   public: void CalculateStateThreadLoad();
 
+  /// \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
   /// \param[in, out] _msg Entity message
@@ -287,6 +296,43 @@ EntityComponentManager::EntityComponentManager()
 //////////////////////////////////////////////////
 EntityComponentManager::~EntityComponentManager() = default;
 
+//////////////////////////////////////////////////
+void EntityComponentManagerPrivate::CopyFrom(
+    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 +2114,105 @@ void EntityComponentManager::UnpinAllEntities()
 {
   this->dataPtr->pinnedEntities.clear();
 }
+
+/////////////////////////////////////////////////
+void EntityComponentManager::CopyFrom(const EntityComponentManager &_fromEcm)
+{
+  this->dataPtr->CopyFrom(*_fromEcm.dataPtr);
+}
+
+/////////////////////////////////////////////////
+EntityComponentManagerDiff EntityComponentManager::ComputeEntityDiff(
+    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::ApplyEntityDiff(
+    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);
+      // We want to set this entity as "removed", but
+      // CreateEntityImplementation sets it as "newlyCreated",
+      // so remove it from that list.
+      {
+        std::lock_guard<std::mutex> 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));
+    }
+
+    this->RequestRemoveEntity(entity, false);
+  }
+}
+
+/////////////////////////////////////////////////
+void EntityComponentManager::ResetTo(const EntityComponentManager &_other)
+{
+  auto ecmDiff = this->ComputeEntityDiff(_other);
+  EntityComponentManager tmpCopy;
+  tmpCopy.CopyFrom(_other);
+  tmpCopy.ApplyEntityDiff(*this, ecmDiff);
+  this->CopyFrom(tmpCopy);
+}

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<Entity> &EntityComponentManagerDiff::AddedEntities() const
+{
+  return this->addedEntities;
+}
+
+//////////////////////////////////////////////////
+const std::vector<Entity> &EntityComponentManagerDiff::RemovedEntities() const
+{
+  return this->removedEntities;
+}
+
+//////////////////////////////////////////////////
+void EntityComponentManagerDiff::ClearAddedEntities()
+{
+  this->addedEntities.clear();
+}
+
+//////////////////////////////////////////////////
+void EntityComponentManagerDiff::ClearRemovedEntities()
+{
+  this->removedEntities.clear();
+}