From f48085bc5d72b312f4ed0d1721a13565d4f13eb7 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Tue, 31 Aug 2021 23:17:38 -0400 Subject: [PATCH] Lock views during system PostUpdates (#1001) Signed-off-by: Ashton Larkin --- .../ignition/gazebo/EntityComponentManager.hh | 24 +++++++-- .../gazebo/detail/EntityComponentManager.hh | 21 +++++++- src/EntityComponentManager.cc | 52 ++++++++++++++----- src/SimulationRunner.cc | 2 + 4 files changed, 81 insertions(+), 18 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 3dce1196dc..2c8fe2fb06 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -657,9 +657,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 FindView( const std::vector &_types) const; /// \brief Add a new view to the set of stored views. @@ -697,6 +699,22 @@ namespace ignition const std::unordered_set &_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. diff --git a/include/ignition/gazebo/detail/EntityComponentManager.hh b/include/ignition/gazebo/detail/EntityComponentManager.hh index 05c5c34492..b92beb81c8 100644 --- a/include/ignition/gazebo/detail/EntityComponentManager.hh +++ b/include/ignition/gazebo/detail/EntityComponentManager.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -488,11 +489,29 @@ detail::View *EntityComponentManager::FindView() const { auto viewKey = std::vector{ComponentTypeTs::typeId...}; - auto baseViewPtr = this->FindView(viewKey); + auto baseViewMutexPair = this->FindView(viewKey); + auto baseViewPtr = baseViewMutexPair.first; if (nullptr != baseViewPtr) { auto view = static_cast*>(baseViewPtr); + std::unique_ptr> 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>(*mutexPtr); + } + // add any new entities to the view before using it for (const auto &[entity, isNew] : view->ToAddEntities()) { diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 906ad24166..659e6132f7 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -133,8 +133,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::ComponentTypeHasher> views; + std::pair, + std::unique_ptr>, 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 @@ -271,7 +278,7 @@ void EntityComponentManager::ClearNewlyCreatedEntities() for (auto &view : this->dataPtr->views) { - view.second->ResetNewEntityState(); + view.second.first->ResetNewEntityState(); } } @@ -319,7 +326,7 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, { for (auto &view : this->dataPtr->views) { - view.second->MarkEntityToRemove(removedEntity); + view.second.first->MarkEntityToRemove(removedEntity); } } } @@ -377,7 +384,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. @@ -420,7 +427,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); @@ -656,7 +663,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)); } @@ -687,7 +694,7 @@ bool EntityComponentManager::CreateComponentImplementation( for (auto &viewPair : this->dataPtr->views) { - viewPair.second->NotifyComponentAddition(_entity, + viewPair.second.first->NotifyComponentAddition(_entity, this->IsNewEntity(_entity), _componentTypeId); } } @@ -795,14 +802,18 @@ const EntityGraph &EntityComponentManager::Entities() const } ////////////////////////////////////////////////// -detail::BaseView *EntityComponentManager::FindView( +std::pair EntityComponentManager::FindView( const std::vector &_types) const { std::lock_guard lockViews(this->dataPtr->viewsMutex); + std::pair 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; } ////////////////////////////////////////////////// @@ -813,9 +824,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 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()))).first; + return iter->second.first.get(); } ////////////////////////////////////////////////// @@ -824,7 +836,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 @@ -1554,6 +1566,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) { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 844d997795..250d0f2fb2 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -572,6 +572,7 @@ 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) @@ -579,6 +580,7 @@ void SimulationRunner::UpdateSystems() this->postUpdateStartBarrier->Wait(); this->postUpdateStopBarrier->Wait(); } + this->entityCompMgr.LockAddingEntitiesToViews(false); } }