fix listener locking

rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_client_info.hpp

@@ -76,12 +76,16 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener
           if (conditionMutex_ != NULL) {
             std::unique_lock<std::mutex> clock(*conditionMutex_);
             list.push_back(response);
+            // the change to list_has_data_ needs to be mutually exclusive with
+            // rmw_wait() which checks hasData() and decides if wait() needs to
+            // be called
+            list_has_data_.store(true);
             clock.unlock();
             conditionVariable_->notify_one();
           } else {
             list.push_back(response);
+            list_has_data_.store(true);
           }
-          list_has_data_.store(true);
         }
       }
     }

rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_service_info.hpp

@@ -78,12 +78,16 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener
         if (conditionMutex_ != NULL) {
           std::unique_lock<std::mutex> clock(*conditionMutex_);
           list.push_back(request);
+          // the change to list_has_data_ needs to be mutually exclusive with
+          // rmw_wait() which checks hasData() and decides if wait() needs to
+          // be called
+          list_has_data_.store(true);
           clock.unlock();
           conditionVariable_->notify_one();
         } else {
           list.push_back(request);
+          list_has_data_.store(true);
         }
-        list_has_data_.store(true);
       }
     }
   }

rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/custom_subscriber_info.hpp

@@ -15,6 +15,7 @@
 #ifndef RMW_FASTRTPS_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_
 #define RMW_FASTRTPS_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_
 
+#include <atomic>
 #include <condition_variable>
 #include <mutex>
 #include <utility>
@@ -59,6 +60,8 @@ class SubListener : public eprosima::fastrtps::SubscriberListener
 
     if (conditionMutex_ != NULL) {
       std::unique_lock<std::mutex> clock(*conditionMutex_);
+      // the change to data_ needs to be mutually exclusive with rmw_wait()
+      // which checks hasData() and decides if wait() needs to be called
       ++data_;
       clock.unlock();
       conditionVariable_->notify_one();
@@ -104,7 +107,7 @@ class SubListener : public eprosima::fastrtps::SubscriberListener
 
 private:
   std::mutex internalMutex_;
-  uint32_t data_;
+  std::atomic_size_t data_;
   std::mutex * conditionMutex_;
   std::condition_variable * conditionVariable_;
 };

rmw_fastrtps_cpp/src/rmw_wait.cpp

@@ -124,6 +124,10 @@ rmw_wait(
     }
   }
 
+  // This mutex prevents any of the listeners
+  // to change the internal state and notify the condition
+  // between the call to hasData() / hasTriggered() and wait()
+  // otherwise the decision to wait might be incorrect
   std::unique_lock<std::mutex> lock(*conditionMutex);
 
   // First check variables.
@@ -182,49 +186,48 @@ rmw_wait(
     }
   }
 
+  // Unlock the condition variable mutex to prevent deadlocks that can occur if
+  // a listener triggers while the condition variable is being detached.
+  // Listeners will no longer be prevented from changing their internal state,
+  // but that should not cause issues (if a listener has data / has triggered
+  // after we check, it will be caught on the next call to this function).
+  lock.unlock();
+
   for (size_t i = 0; i < subscriptions->subscriber_count; ++i) {
     void * data = subscriptions->subscribers[i];
     CustomSubscriberInfo * custom_subscriber_info = static_cast<CustomSubscriberInfo *>(data);
+    custom_subscriber_info->listener_->detachCondition();
     if (!custom_subscriber_info->listener_->hasData()) {
       subscriptions->subscribers[i] = 0;
     }
-    lock.unlock();
-    custom_subscriber_info->listener_->detachCondition();
-    lock.lock();
   }
 
   for (size_t i = 0; i < clients->client_count; ++i) {
     void * data = clients->clients[i];
     CustomClientInfo * custom_client_info = static_cast<CustomClientInfo *>(data);
+    custom_client_info->listener_->detachCondition();
     if (!custom_client_info->listener_->hasData()) {
       clients->clients[i] = 0;
     }
-    lock.unlock();
-    custom_client_info->listener_->detachCondition();
-    lock.lock();
   }
 
   for (size_t i = 0; i < services->service_count; ++i) {
     void * data = services->services[i];
     CustomServiceInfo * custom_service_info = static_cast<CustomServiceInfo *>(data);
+    custom_service_info->listener_->detachCondition();
     if (!custom_service_info->listener_->hasData()) {
       services->services[i] = 0;
     }
-    lock.unlock();
-    custom_service_info->listener_->detachCondition();
-    lock.lock();
   }
 
   if (guard_conditions) {
     for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) {
       void * data = guard_conditions->guard_conditions[i];
       GuardCondition * guard_condition = static_cast<GuardCondition *>(data);
+      guard_condition->detachCondition();
       if (!guard_condition->getHasTriggered()) {
         guard_conditions->guard_conditions[i] = 0;
       }
-      lock.unlock();
-      guard_condition->detachCondition();
-      lock.lock();
     }
   }
   // Make timeout behavior consistent with rcl expectations for zero timeout value

rmw_fastrtps_cpp/src/types/guard_condition.hpp

@@ -16,6 +16,7 @@
 #define TYPES__GUARD_CONDITION_HPP_
 
 #include <array>
+#include <atomic>
 #include <cassert>
 #include <condition_variable>
 #include <mutex>
@@ -35,6 +36,9 @@ class GuardCondition
 
     if (conditionMutex_ != NULL) {
       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;
       clock.unlock();
       conditionVariable_->notify_one();
@@ -75,7 +79,7 @@ class GuardCondition
 
 private:
   std::mutex internalMutex_;
-  bool hasTriggered_;
+  std::atomic_bool hasTriggered_;
   std::mutex * conditionMutex_;
   std::condition_variable * conditionVariable_;
 };