optimize timeout judgement according to different condition (#187)

*  optimize timeout judgement according to different condition

Under the condition that "hasToWait && wait_timeout", there is no need to re-calc hasData to judge timeout
because predicate already include the logic. We only need do it if hasToWait && !wait_timeout. This commit
optimize timeout judgement which slightly improve the performance in some conditions

Signed-off-by: jwang <jing.j.wang@intel.com>

* replace cv->wait(lock) with cv->wait(lock, predicate) and simplify timeout logic

Signed-off-by: jwang <jing.j.wang@intel.com>

* Improve logic to return TIMEOUT when not hasData && timeout->sec/nsec==0

Signed-off-by: jwang <jing.j.wang@intel.com>

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];