Move the hasData checks for non-blocking wait 'timeout' higher (#158)

Avoids the data having been already cleared for guard conditions

rmw_fastrtps_cpp/src/rmw_wait.cpp

@@ -194,6 +194,14 @@ 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_waitset_for_data(subscriptions, guard_conditions, services, clients);
+  if (!hasData && wait_timeout && wait_timeout->sec == 0 && wait_timeout->nsec == 0) {
+    timeout = true;
+  }
+
   for (size_t i = 0; i < subscriptions->subscriber_count; ++i) {
     void * data = subscriptions->subscribers[i];
     CustomSubscriberInfo * custom_subscriber_info = static_cast<CustomSubscriberInfo *>(data);
@@ -231,11 +239,6 @@ rmw_wait(
       }
     }
   }
-  // Make timeout behavior consistent with rcl expectations for zero timeout value
-  bool hasData = check_waitset_for_data(subscriptions, guard_conditions, services, clients);
-  if (!hasData && wait_timeout && wait_timeout->sec == 0 && wait_timeout->nsec == 0) {
-    return RMW_RET_TIMEOUT;
-  }
 
   return timeout ? RMW_RET_TIMEOUT : RMW_RET_OK;
 }