From 33ea3049bd7b7ce7ab286a3673fee515d72bc113 Mon Sep 17 00:00:00 2001
From: Chen Lihui <lihui.chen@sony.com>
Date: Sun, 25 Apr 2021 14:03:46 +0800
Subject: [PATCH 1/2] update guard condition by managing multiple mutexs and
 condition_variables

Signed-off-by: Chen Lihui <lihui.chen@sony.com>
---
 rmw_fastrtps_shared_cpp/src/rmw_wait.cpp      |  2 +-
 .../src/types/guard_condition.hpp             | 40 ++++++++++---------
 2 files changed, 22 insertions(+), 20 deletions(-)

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp
index 4275e4ffc..0effb311b 100644
--- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp
+++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp
@@ -239,7 +239,7 @@ __rmw_wait(
     for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) {
       void * data = guard_conditions->guard_conditions[i];
       auto guard_condition = static_cast<GuardCondition *>(data);
-      guard_condition->detachCondition();
+      guard_condition->detachCondition(conditionMutex, conditionVariable);
       if (!guard_condition->getHasTriggered()) {
         guard_conditions->guard_conditions[i] = 0;
       }
diff --git a/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp b/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp
index ab5076522..2bb11aecf 100644
--- a/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp
+++ b/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp
@@ -15,10 +15,12 @@
 #ifndef TYPES__GUARD_CONDITION_HPP_
 #define TYPES__GUARD_CONDITION_HPP_
 
+#include <algorithm>
 #include <array>
 #include <atomic>
 #include <cassert>
 #include <condition_variable>
+#include <list>
 #include <mutex>
 #include <utility>
 
@@ -28,24 +30,18 @@ class GuardCondition
 {
 public:
   GuardCondition()
-  : hasTriggered_(false),
-    conditionMutex_(nullptr), conditionVariable_(nullptr) {}
+  : hasTriggered_(false) {}
 
   void
   trigger()
   {
     std::lock_guard<std::mutex> lock(internalMutex_);
-
-    if (conditionMutex_ != nullptr) {
-      std::unique_lock<std::mutex> clock(*conditionMutex_);
-      // the change to hasTriggered_ needs to be mutually exclusive with
-      // rmw_wait() which checks hasTriggered() and decides if wait() needs to
-      // be called
-      hasTriggered_ = true;
+    hasTriggered_ = true;
+    for (auto & cond : conditions_) {
+      // TODO(iuhilnehc-ynos): conditionMutex is not used, remove it
+      std::unique_lock<std::mutex> clock(*cond.first);
       clock.unlock();
-      conditionVariable_->notify_one();
-    } else {
-      hasTriggered_ = true;
+      cond.second->notify_one();
     }
   }
 
@@ -53,16 +49,22 @@ class GuardCondition
   attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable)
   {
     std::lock_guard<std::mutex> lock(internalMutex_);
-    conditionMutex_ = conditionMutex;
-    conditionVariable_ = conditionVariable;
+    conditions_.push_back({conditionMutex, conditionVariable});
   }
 
   void
-  detachCondition()
+  detachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable)
   {
     std::lock_guard<std::mutex> lock(internalMutex_);
-    conditionMutex_ = nullptr;
-    conditionVariable_ = nullptr;
+    auto it = std::find_if(
+      conditions_.begin(),
+      conditions_.end(),
+      [conditionMutex, conditionVariable](const auto & cond) {
+        return cond.first == conditionMutex && cond.second == conditionVariable;
+      });
+    if (it != conditions_.end()) {
+      conditions_.erase(it);
+    }
   }
 
   bool
@@ -80,8 +82,8 @@ class GuardCondition
 private:
   std::mutex internalMutex_;
   std::atomic_bool hasTriggered_;
-  std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_);
-  std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_);
+  std::list<std::pair<std::mutex *, std::condition_variable *>> conditions_
+    RCPPUTILS_TSA_GUARDED_BY(internalMutex_);
 };
 
 #endif  // TYPES__GUARD_CONDITION_HPP_

From 5540a34798d57a123053b1696627cd18b596c488 Mon Sep 17 00:00:00 2001
From: Chen Lihui <lihui.chen@sony.com>
Date: Tue, 27 Apr 2021 10:00:57 +0800
Subject: [PATCH 2/2] use vector instead of list

Co-authored-by: William Woodall <william@osrfoundation.org>
Signed-off-by: Chen Lihui <lihui.chen@sony.com>
---
 rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp b/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp
index 2bb11aecf..4ae5e937b 100644
--- a/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp
+++ b/rmw_fastrtps_shared_cpp/src/types/guard_condition.hpp
@@ -20,9 +20,9 @@
 #include <atomic>
 #include <cassert>
 #include <condition_variable>
-#include <list>
 #include <mutex>
 #include <utility>
+#include <vector>
 
 #include "rcpputils/thread_safety_annotations.hpp"
 
@@ -82,7 +82,7 @@ class GuardCondition
 private:
   std::mutex internalMutex_;
   std::atomic_bool hasTriggered_;
-  std::list<std::pair<std::mutex *, std::condition_variable *>> conditions_
+  std::vector<std::pair<std::mutex *, std::condition_variable *>> conditions_
     RCPPUTILS_TSA_GUARDED_BY(internalMutex_);
 };