From 2bf7223f560e24bf615345f4c12f75943cfc45c1 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 19 Mar 2020 18:10:47 +0100 Subject: [PATCH] Simplify ECMSingleton and ECMProvider - The ECM pointer is the same even if multiple worlds are part of the simulation - Removed old synchronization logic --- .../plugins/ECMProvider/ECMProvider.cpp | 36 ++--- .../plugins/ECMProvider/ECMSingleton.cpp | 143 +++--------------- .../scenario/plugins/gazebo/ECMProvider.h | 9 +- .../scenario/plugins/gazebo/ECMSingleton.h | 24 ++- 4 files changed, 41 insertions(+), 171 deletions(-) diff --git a/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp b/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp index 196a26dbc..e74b4cefd 100644 --- a/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp +++ b/cpp/scenario/plugins/ECMProvider/ECMProvider.cpp @@ -26,12 +26,9 @@ #include "scenario/plugins/gazebo/ECMProvider.h" #include "scenario/gazebo/Log.h" -#include "scenario/gazebo/components/ECMMutex.h" #include "scenario/plugins/gazebo/ECMSingleton.h" #include -#include -#include #include #include @@ -43,13 +40,11 @@ using namespace scenario::plugins::gazebo; class ECMProvider::Impl { public: - std::string worldName; - ignition::gazebo::Entity worldEntity; }; ECMProvider::ECMProvider() : System() - , pImpl{new Impl()} + , pImpl{std::make_unique()} {} ECMProvider::~ECMProvider() @@ -57,35 +52,22 @@ ECMProvider::~ECMProvider() gymppDebug << "Destroying the ECMProvider" << std::endl; }; -void ECMProvider::Configure(const ignition::gazebo::Entity& /*entity*/, +void ECMProvider::Configure(const ignition::gazebo::Entity& entity, const std::shared_ptr& /*sdf*/, ignition::gazebo::EntityComponentManager& ecm, ignition::gazebo::EventManager& eventMgr) { - auto worldEntities = ecm.EntitiesByComponents(ignition::gazebo::components::World()); - - if (worldEntities.size() == 0) { - gymppError << "Didn't find any world in the context of the ECMProvider plugin" << std::endl; - assert(false); + if (ECMSingleton::get().valid()) { + gymppWarning << "The ECM singleton has been already configured" << std::endl; return; } - assert(worldEntities.size() == 1); - pImpl->worldEntity = worldEntities[0]; - - auto nameComponent = ecm.Component(pImpl->worldEntity); - pImpl->worldName = nameComponent->Data(); - assert(!pImpl->worldName.empty()); + gymppDebug << "Storing ECM resources in the singleton" << std::endl; - // Register the EntityComponentManager and the EventManager in the singleton - if (!ECMSingleton::get().valid(pImpl->worldName)) { - gymppDebug << "Inserting ECM for world '" << pImpl->worldName << "'" << std::endl; - - if (!ECMSingleton::get().storePtrs(pImpl->worldName, &ecm, &eventMgr)) { - gymppError << "Failed to store ECM in the singleton for world '" << pImpl->worldName - << "'" << std::endl; - return; - } + if (!ECMSingleton::get().storePtrs(&ecm, &eventMgr)) { + gymppError << "Failed to store ECM in the singleton for entity [" << entity << "]" + << std::endl; + return; } } diff --git a/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp b/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp index 1f21a3c2a..a08e1d5b2 100644 --- a/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp +++ b/cpp/scenario/plugins/ECMProvider/ECMSingleton.cpp @@ -27,35 +27,16 @@ #include "scenario/plugins/gazebo/ECMSingleton.h" #include "scenario/gazebo/Log.h" -#include +#include #include -#include using namespace scenario::plugins::gazebo; -using RobotName = std::string; - -struct Pointers -{ - std::atomic eventMgr = nullptr; - std::atomic ecm = nullptr; -}; - -struct PreUpdateSynchronizationData -{ - bool isPreUpdate; - std::mutex preUpdateMutex; - std::mutex processingMutex; - std::condition_variable executorsCV; - std::condition_variable preUpdateCV; - std::atomic nrExecutorsWaiting; -}; class ECMSingleton::Impl { public: - using WorldName = std::string; - std::unordered_map resources; - std::unordered_map synchronizationData; + std::atomic eventMgr = nullptr; + std::atomic ecm = nullptr; }; ECMSingleton::ECMSingleton() @@ -68,135 +49,49 @@ ECMSingleton& ECMSingleton::get() return instance; } -bool ECMSingleton::valid(const std::string& worldName) const +bool ECMSingleton::valid() const { - return exist(worldName) && pImpl->resources.at(worldName).ecm - && pImpl->resources.at(worldName).eventMgr; + return pImpl->ecm && pImpl->eventMgr; } -bool ECMSingleton::exist(const std::string& worldName) const +ignition::gazebo::EventManager* ECMSingleton::getEventManager() const { - if (pImpl->resources.find(worldName) != pImpl->resources.end()) { - return true; - } - else { - return false; - } -} - -ignition::gazebo::EventManager* ECMSingleton::getEventManager(const std::string& worldName) const -{ - if (!exist(worldName)) { - gymppError << "The event manager was never stored" << std::endl; - return nullptr; - } - - if (!valid(worldName)) { + if (!this->valid()) { gymppError << "The pointers are not valid" << std::endl; return nullptr; } - return pImpl->resources.at(worldName).eventMgr; + return pImpl->eventMgr; } -bool ECMSingleton::storePtrs(const std::string& worldName, - ignition::gazebo::EntityComponentManager* ecm, +bool ECMSingleton::storePtrs(ignition::gazebo::EntityComponentManager* ecm, ignition::gazebo::EventManager* eventMgr) { if (!ecm || !eventMgr) { - gymppError << "The pointer to the ECM or EventManager is null" << std::endl; + gymppError << "The pointer to the ECM or EventManager is not valid" + << std::endl; return false; } - if (exist(worldName)) { - gymppWarning << "The pointers for world '" << worldName << "' have already been stored." - << " This method will do nothing" << std::endl; - return true; - } - - gymppDebug << "Storing the ECM and the EventManager in the singleton" << std::endl; - pImpl->resources[worldName].ecm = ecm; - pImpl->resources[worldName].eventMgr = eventMgr; + pImpl->ecm = ecm; + pImpl->eventMgr = eventMgr; return true; } -void ECMSingleton::notifyExecutorFinished(const std::string& worldName) -{ - pImpl->synchronizationData[worldName].executorsCV.notify_all(); -} - -void ECMSingleton::notifyAndWaitPreUpdate(const std::string& worldName) +ignition::gazebo::EntityComponentManager* ECMSingleton::getECM() const { - auto& syncroData = pImpl->synchronizationData[worldName]; - - // Temporarily lock the executors mutex (see the waitPreUpdate method). - std::unique_lock processingLock(syncroData.processingMutex); - - // Notify to all executors that the simulation reached the PreUpdate step - syncroData.isPreUpdate = true; - syncroData.preUpdateCV.notify_all(); - - // If the are pending executors, they are waiting in their condition variable lambda. - // Unlock the executors mutex and wait that all executors are processed. - syncroData.executorsCV.wait(processingLock, [&] { return syncroData.nrExecutorsWaiting == 0; }); - syncroData.isPreUpdate = false; -} - -std::unique_lock ECMSingleton::waitPreUpdate(const std::string& worldName) -{ - // Let's define 'executor' every function that calls this method. - // Executors want to wait to reach the PreUpdate step of the simulation, block its execution, - // and run some custom code (e.g. adding entities in the ECM). - // Executors run syncronously, one after the other. The simulation starts again when all - // executors finished. - - auto& syncroData = pImpl->synchronizationData[worldName]; - - // Initialize the lock returned to the executor to wait for their processing - std::unique_lock processingLock(syncroData.processingMutex, std::defer_lock); - - // Executors can arrive here in parallel. When this variable is back to 0, the simulation - // starts again. - syncroData.nrExecutorsWaiting++; - - // Process one executor at time and wait to reach the PreUpdate step - std::unique_lock lock(syncroData.preUpdateMutex); - syncroData.preUpdateCV.wait(lock, [&] { - if (syncroData.isPreUpdate) { - // When the simulation reaches the PreUpdate step, this lambda is called. - // One executors acquires the lock and continues. The others are blocked here. - processingLock.lock(); - return true; - } - return false; - }); - - // Decrease the counter - syncroData.nrExecutorsWaiting--; - - // Return the lock. When it goes out of executor scope, the next executors (if any) will get - // unlocked from the condition variable lambda. - return processingLock; -} - -ignition::gazebo::EntityComponentManager* ECMSingleton::getECM(const std::string& worldName) const -{ - if (!exist(worldName)) { - gymppError << "The ECM of world '" << worldName << "' was never stored" << std::endl; - return nullptr; - } - if (!valid(worldName)) { + if (!this->valid()) { gymppError << "The pointers are not valid" << std::endl; return nullptr; } - return pImpl->resources.at(worldName).ecm; + return pImpl->ecm; } -void ECMSingleton::clean(const std::string& worldName) +void ECMSingleton::clean() { - pImpl->resources.erase(worldName); - pImpl->synchronizationData.erase(worldName); + pImpl->ecm = nullptr; + pImpl->eventMgr = nullptr; } diff --git a/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMProvider.h b/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMProvider.h index 42fb52270..a05cd75b3 100644 --- a/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMProvider.h +++ b/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMProvider.h @@ -34,7 +34,6 @@ #include #include -#include namespace scenario { namespace plugins { @@ -48,10 +47,6 @@ class scenario::plugins::gazebo::ECMProvider final : public ignition::gazebo::System , public ignition::gazebo::ISystemConfigure { -private: - class Impl; - std::unique_ptr pImpl = nullptr; - public: ECMProvider(); ~ECMProvider() override; @@ -60,6 +55,10 @@ class scenario::plugins::gazebo::ECMProvider final const std::shared_ptr& sdf, ignition::gazebo::EntityComponentManager& ecm, ignition::gazebo::EventManager& eventMgr) override; + +private: + class Impl; + std::unique_ptr pImpl = nullptr; }; #endif // SCENARIO_PLUGINS_GAZEBO_ECMPROVIDER diff --git a/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMSingleton.h b/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMSingleton.h index 738e50572..831c6a1e6 100644 --- a/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMSingleton.h +++ b/cpp/scenario/plugins/ECMProvider/include/scenario/plugins/gazebo/ECMSingleton.h @@ -43,33 +43,27 @@ namespace scenario { class scenario::plugins::gazebo::ECMSingleton { -private: - class Impl; - std::unique_ptr pImpl; - public: ECMSingleton(); ~ECMSingleton() = default; + ECMSingleton(ECMSingleton&) = delete; void operator=(const ECMSingleton&) = delete; static ECMSingleton& get(); - void clean(const std::string& worldName); - bool valid(const std::string& worldName) const; + void clean(); + bool valid() const; - bool exist(const std::string& worldName) const; + ignition::gazebo::EventManager* getEventManager() const; + ignition::gazebo::EntityComponentManager* getECM() const; - ignition::gazebo::EventManager* getEventManager(const std::string& worldName) const; - ignition::gazebo::EntityComponentManager* getECM(const std::string& worldName) const; - - bool storePtrs(const std::string& worldName, - ignition::gazebo::EntityComponentManager* ecm, + bool storePtrs(ignition::gazebo::EntityComponentManager* ecm, ignition::gazebo::EventManager* eventMgr); - void notifyExecutorFinished(const std::string& worldName); - void notifyAndWaitPreUpdate(const std::string& worldName); - std::unique_lock waitPreUpdate(const std::string& worldName); +private: + class Impl; + std::unique_ptr pImpl; }; #endif // SCENARIO_PLUGINS_GAZEBO_ECMSINGLETON_H