Merge branch 'main' into adlarkin/clone_entities

Author: adlarkin

include/ignition/gazebo/EntityComponentManager.hh

@@ -712,9 +712,11 @@ namespace ignition
       /// \brief Find a view based on the provided component type ids.
       /// \param[in] _types The component type ids that serve as a key into
       /// a map of views.
-      /// \return A pointer to the view. nullptr is returned if the view wasn't
-      /// found.
-      private: detail::BaseView *FindView(
+      /// \return A pair containing a the view itself and a mutex that can be
+      /// used for locking the view while entities are being added to it.
+      /// If a view defined by _types does not exist, the pair will contain
+      /// nullptrs.
+      private: std::pair<detail::BaseView *, std::mutex *> FindView(
                    const std::vector<ComponentTypeId> &_types) const;
 
       /// \brief Add a new view to the set of stored views.
@@ -752,6 +754,22 @@ namespace ignition
           const std::unordered_set<ComponentTypeId> &_types = {},
           bool _full = false) const;
 
+      /// \brief Set whether views should be locked when entities are being
+      /// added to them. This can be used to prevent race conditions in
+      /// system PostUpdates, since these are run in parallel (entities are
+      /// added to views when the view is used, so if two systems try to access
+      /// the same view in PostUpdate, we run the risk of multiple threads
+      /// reading/writing from the same data).
+      /// \param[in] _lock Whether the views should lock while entities are
+      /// being added to them (true) or not (false).
+      private: void LockAddingEntitiesToViews(bool _lock);
+
+      /// \brief Get whether views should be locked when entities are being
+      /// added to them.
+      /// \return True if views should be locked during entitiy addition, false
+      /// otherwise.
+      private: bool LockAddingEntitiesToViews() const;
+
       // Make runners friends so that they can manage entity creation and
       // removal. This should be safe since runners are internal
       // to Gazebo.

include/ignition/gazebo/detail/EntityComponentManager.hh

@@ -20,6 +20,7 @@
 #include <cstring>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <optional>
 #include <set>
 #include <tuple>
@@ -488,11 +489,29 @@ detail::View<ComponentTypeTs...> *EntityComponentManager::FindView() const
 {
   auto viewKey = std::vector<ComponentTypeId>{ComponentTypeTs::typeId...};
 
-  auto baseViewPtr = this->FindView(viewKey);
+  auto baseViewMutexPair = this->FindView(viewKey);
+  auto baseViewPtr = baseViewMutexPair.first;
   if (nullptr != baseViewPtr)
   {
     auto view = static_cast<detail::View<ComponentTypeTs...>*>(baseViewPtr);
 
+    std::unique_ptr<std::lock_guard<std::mutex>> viewLock;
+    if (this->LockAddingEntitiesToViews())
+    {
+      // lock the mutex unique to this view in order to prevent multiple threads
+      // from concurrently reading/modifying the view's toAddEntities data
+      // (for example, this is useful in system PostUpdates since they are run
+      // in parallel)
+      auto mutexPtr = baseViewMutexPair.second;
+      if (nullptr == mutexPtr)
+      {
+        ignerr << "Internal error: requested to lock a view, but no mutex "
+          << "exists for this view. This should never happen!" << std::endl;
+        return view;
+      }
+      viewLock = std::make_unique<std::lock_guard<std::mutex>>(*mutexPtr);
+    }
+
     // add any new entities to the view before using it
     for (const auto &[entity, isNew] : view->ToAddEntities())
     {

src/EntityComponentManager.cc

@@ -138,8 +138,15 @@ class ignition::gazebo::EntityComponentManagerPrivate
   public: mutable std::mutex removedComponentsMutex;
 
   /// \brief The set of all views.
+  /// The value is a pair of the view itself and a mutex that can be used for
+  /// locking the view to ensure thread safety when adding entities to the view.
   public: mutable std::unordered_map<detail::ComponentTypeKey,
-          std::unique_ptr<detail::BaseView>, detail::ComponentTypeHasher> views;
+          std::pair<std::unique_ptr<detail::BaseView>,
+            std::unique_ptr<std::mutex>>, detail::ComponentTypeHasher> views;
+
+  /// \brief A flag that indicates whether views should be locked while adding
+  /// new entities to them or not.
+  public: bool lockAddEntitiesToViews{false};
 
   /// \brief Cache of previously queried descendants. The key is the parent
   /// entity for which descendants were queried, and the value are all its
@@ -417,7 +424,7 @@ void EntityComponentManager::ClearNewlyCreatedEntities()
 
   for (auto &view : this->dataPtr->views)
   {
-    view.second->ResetNewEntityState();
+    view.second.first->ResetNewEntityState();
   }
 }
 
@@ -465,7 +472,7 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity,
   {
     for (auto &view : this->dataPtr->views)
     {
-      view.second->MarkEntityToRemove(removedEntity);
+      view.second.first->MarkEntityToRemove(removedEntity);
     }
   }
 }
@@ -523,7 +530,7 @@ void EntityComponentManager::ProcessRemoveEntityRequests()
       // Remove the entity from views.
       for (auto &view : this->dataPtr->views)
       {
-        view.second->RemoveEntity(entity);
+        view.second.first->RemoveEntity(entity);
       }
     }
     // Clear the set of entities to remove.
@@ -566,7 +573,7 @@ bool EntityComponentManager::RemoveComponent(
 
     // update views to reflect the component removal
     for (auto &viewPair : this->dataPtr->views)
-      viewPair.second->NotifyComponentRemoval(_entity, _typeId);
+      viewPair.second.first->NotifyComponentRemoval(_entity, _typeId);
   }
 
   this->dataPtr->AddModifiedComponent(_entity);
@@ -802,7 +809,7 @@ bool EntityComponentManager::CreateComponentImplementation(
     updateData = false;
     for (auto &viewPair : this->dataPtr->views)
     {
-      auto &view = viewPair.second;
+      auto &view = viewPair.second.first;
       if (this->EntityMatches(_entity, view->ComponentTypes()))
         view->MarkEntityToAdd(_entity, this->IsNewEntity(_entity));
     }
@@ -833,7 +840,7 @@ bool EntityComponentManager::CreateComponentImplementation(
 
       for (auto &viewPair : this->dataPtr->views)
       {
-        viewPair.second->NotifyComponentAddition(_entity,
+        viewPair.second.first->NotifyComponentAddition(_entity,
             this->IsNewEntity(_entity), _componentTypeId);
       }
     }
@@ -941,14 +948,18 @@ const EntityGraph &EntityComponentManager::Entities() const
 }
 
 //////////////////////////////////////////////////
-detail::BaseView *EntityComponentManager::FindView(
+std::pair<detail::BaseView *, std::mutex *> EntityComponentManager::FindView(
     const std::vector<ComponentTypeId> &_types) const
 {
   std::lock_guard<std::mutex> lockViews(this->dataPtr->viewsMutex);
+  std::pair<detail::BaseView *, std::mutex *> viewMutexPair(nullptr, nullptr);
   auto iter = this->dataPtr->views.find(_types);
   if (iter != this->dataPtr->views.end())
-    return iter->second.get();
-  return nullptr;
+  {
+    viewMutexPair.first = iter->second.first.get();
+    viewMutexPair.second = iter->second.second.get();
+  }
+  return viewMutexPair;
 }
 
 //////////////////////////////////////////////////
@@ -959,9 +970,10 @@ detail::BaseView *EntityComponentManager::AddView(
   // If the view already exists, then the map will return the iterator to
   // the location that prevented the insertion.
   std::lock_guard<std::mutex> lockViews(this->dataPtr->viewsMutex);
-  auto iter = this->dataPtr->views.insert(
-      std::make_pair(_types, std::move(_view))).first;
-  return iter->second.get();
+  auto iter = this->dataPtr->views.insert(std::make_pair(_types,
+        std::make_pair(std::move(_view),
+          std::make_unique<std::mutex>()))).first;
+  return iter->second.first.get();
 }
 
 //////////////////////////////////////////////////
@@ -970,7 +982,7 @@ void EntityComponentManager::RebuildViews()
   IGN_PROFILE("EntityComponentManager::RebuildViews");
   for (auto &viewPair : this->dataPtr->views)
   {
-    auto &view = viewPair.second;
+    auto &view = viewPair.second.first;
     view->Reset();
 
     // Add all the entities that match the component types to the
@@ -1700,6 +1712,18 @@ void EntityComponentManager::SetEntityCreateOffset(uint64_t _offset)
   this->dataPtr->entityCount = _offset;
 }
 
+/////////////////////////////////////////////////
+void EntityComponentManager::LockAddingEntitiesToViews(bool _lock)
+{
+  this->dataPtr->lockAddEntitiesToViews = _lock;
+}
+
+/////////////////////////////////////////////////
+bool EntityComponentManager::LockAddingEntitiesToViews() const
+{
+  return this->dataPtr->lockAddEntitiesToViews;
+}
+
 /////////////////////////////////////////////////
 void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity)
 {

src/SimulationRunner.cc

@@ -572,13 +572,15 @@ void SimulationRunner::UpdateSystems()
 
   {
     IGN_PROFILE("PostUpdate");
+    this->entityCompMgr.LockAddingEntitiesToViews(true);
     // If no systems implementing PostUpdate have been added, then
     // the barriers will be uninitialized, so guard against that condition.
     if (this->postUpdateStartBarrier && this->postUpdateStopBarrier)
     {
       this->postUpdateStartBarrier->Wait();
       this->postUpdateStopBarrier->Wait();
     }
+    this->entityCompMgr.LockAddingEntitiesToViews(false);
   }
 }