Simplify ECMSingleton and ECMProvider

- The ECM pointer is the same even if multiple worlds are part of the simulation - Removed old synchronization logic
robotology-legacy · Mar 19, 2020 · 2bf7223 · 2bf7223
1 parent 26df8ce
commit 2bf7223
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 171 deletions.
diff --git 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 <ignition/gazebo/Entity.hh>
-#include <ignition/gazebo/components/Name.hh>
-#include <ignition/gazebo/components/World.hh>
 #include <ignition/plugin/Register.hh>
 
 #include <cassert>
@@ -43,49 +40,34 @@ 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<Impl>()}
 {}
 
 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<const sdf::Element>& /*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<ignition::gazebo::components::Name>(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
@@ -27,35 +27,16 @@
 #include "scenario/plugins/gazebo/ECMSingleton.h"
 #include "scenario/gazebo/Log.h"
 
-#include <mutex>
+#include <atomic>
 #include <ostream>
-#include <unordered_map>
 
 using namespace scenario::plugins::gazebo;
-using RobotName = std::string;
-
-struct Pointers
-{
-    std::atomic<ignition::gazebo::EventManager*> eventMgr = nullptr;
-    std::atomic<ignition::gazebo::EntityComponentManager*> ecm = nullptr;
-};
-
-struct PreUpdateSynchronizationData
-{
-    bool isPreUpdate;
-    std::mutex preUpdateMutex;
-    std::mutex processingMutex;
-    std::condition_variable executorsCV;
-    std::condition_variable preUpdateCV;
-    std::atomic<size_t> nrExecutorsWaiting;
-};
 
 class ECMSingleton::Impl
 {
 public:
-    using WorldName = std::string;
-    std::unordered_map<WorldName, Pointers> resources;
-    std::unordered_map<WorldName, PreUpdateSynchronizationData> synchronizationData;
+    std::atomic<ignition::gazebo::EventManager*> eventMgr = nullptr;
+    std::atomic<ignition::gazebo::EntityComponentManager*> 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<std::mutex> 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
@@ -34,7 +34,6 @@
 #include <sdf/Element.hh>
 
 #include <memory>
-#include <optional>
 
 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<Impl> pImpl = nullptr;
-
 public:
     ECMProvider();
     ~ECMProvider() override;
@@ -60,6 +55,10 @@ class scenario::plugins::gazebo::ECMProvider final
                    const std::shared_ptr<const sdf::Element>& sdf,
                    ignition::gazebo::EntityComponentManager& ecm,
                    ignition::gazebo::EventManager& eventMgr) override;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> 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
@@ -43,33 +43,27 @@ namespace scenario {
 
 class scenario::plugins::gazebo::ECMSingleton
 {
-private:
-    class Impl;
-    std::unique_ptr<Impl> 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<std::mutex> waitPreUpdate(const std::string& worldName);
+private:
+    class Impl;
+    std::unique_ptr<Impl> pImpl;
 };
 
 #endif // SCENARIO_PLUGINS_GAZEBO_ECMSINGLETON_H