diff --git a/rmw_fastrtps_cpp/src/rmw_wait.cpp b/rmw_fastrtps_cpp/src/rmw_wait.cpp
index 2b7560255..30f53250a 100644
--- a/rmw_fastrtps_cpp/src/rmw_wait.cpp
+++ b/rmw_fastrtps_cpp/src/rmw_wait.cpp
@@ -143,26 +143,22 @@ rmw_wait(
   // otherwise the decision to wait might be incorrect
   std::unique_lock<std::mutex> lock(*conditionMutex);
 
-  // First check variables.
-  // If wait_timeout is null, wait indefinitely (so we have to wait)
-  // If wait_timeout is not null and either of its fields are nonzero, we have to wait
-  bool hasToWait = (wait_timeout && (wait_timeout->sec > 0 || wait_timeout->nsec > 0)) ||
-    !wait_timeout;
-  hasToWait &= !check_wait_set_for_data(subscriptions, guard_conditions, services, clients);
+  bool hasData = check_wait_set_for_data(subscriptions, guard_conditions, services, clients);
+  auto predicate = [subscriptions, guard_conditions, services, clients]() {
+      return check_wait_set_for_data(subscriptions, guard_conditions, services, clients);
+    };
 
   bool timeout = false;
-
-  if (hasToWait) {
+  if (!hasData) {
     if (!wait_timeout) {
-      conditionVariable->wait(lock);
-    } else {
-      auto predicate = [subscriptions, guard_conditions, services, clients]() {
-          return check_wait_set_for_data(subscriptions, guard_conditions, services, clients);
-        };
+      conditionVariable->wait(lock, predicate);
+    } else if (wait_timeout->sec > 0 || wait_timeout->nsec > 0) {
       auto n = std::chrono::duration_cast<std::chrono::nanoseconds>(
         std::chrono::seconds(wait_timeout->sec));
       n += std::chrono::nanoseconds(wait_timeout->nsec);
       timeout = !conditionVariable->wait_for(lock, n, predicate);
+    } else {
+      timeout = true;
     }
   }
 
@@ -173,14 +169,6 @@ rmw_wait(
   // after we check, it will be caught on the next call to this function).
   lock.unlock();
 
-  // Even if this was a non-blocking wait, signal a timeout if there's no data.
-  // This makes the return behavior consistent with rcl expectations for zero timeout value.
-  // Do this before detaching the listeners because the data gets cleared for guard conditions.
-  bool hasData = check_wait_set_for_data(subscriptions, guard_conditions, services, clients);
-  if (!hasData && wait_timeout && wait_timeout->sec == 0 && wait_timeout->nsec == 0) {
-    timeout = true;
-  }
-
   if (subscriptions) {
     for (size_t i = 0; i < subscriptions->subscriber_count; ++i) {
       void * data = subscriptions->subscribers[i];