Timer tests (#163)

* dont return timeout errcode if we didnt hit user specified timeout

* update axisting test accordingly

* add timer test

* Revert "dont return timeout errcode if we didnt hit user specified timeout"

This reverts commit f44a93b8528e87ce46594af0d1184c116ade0cb4.

* alternative fix to not return timeout when timer is ready

this also fixes a bug where the wait would always return when all the
timers were canceled

fixes rclcpp#319

* fixups

* more tests

* another one just for fun

* remove new tests

* clearer variable name

* update comment

rcl/src/rcl/wait.c

@@ -528,18 +528,13 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
   rmw_time_t * timeout_argument = NULL;
   rmw_time_t temporary_timeout_storage;
 
-  if (timeout == 0) {
+  // calculate the number of valid (non-NULL and non-canceled) timers
-    // Then it is non-blocking, so set the temporary storage to 0, 0 and pass it.
+  size_t number_of_valid_timers = wait_set->size_of_timers;
-    temporary_timeout_storage.sec = 0;
+  {  // scope to prevent i from colliding below
-    temporary_timeout_storage.nsec = 0;
-    timeout_argument = &temporary_timeout_storage;
-  } else if (timeout > 0 || wait_set->size_of_timers > 0) {
-    int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX;
-    // Compare the timeout to the time until next callback for each timer.
-    // Take the lowest and use that for the wait timeout.
     uint64_t i = 0;
     for (i = 0; i < wait_set->impl->timer_index; ++i) {
       if (!wait_set->timers[i]) {
+        number_of_valid_timers--;
         continue;  // Skip NULL timers.
       }
       bool is_canceled = false;
@@ -548,18 +543,40 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
         return ret;  // The rcl error state should already be set.
       }
       if (is_canceled) {
-        continue;  // Skip canceled timers.
+        number_of_valid_timers--;
+        wait_set->timers[i] = NULL;
       }
+    }
+  }
+
+  bool is_timer_timeout = false;
+  if (timeout == 0) {
+    // Then it is non-blocking, so set the temporary storage to 0, 0 and pass it.
+    temporary_timeout_storage.sec = 0;
+    temporary_timeout_storage.nsec = 0;
+    timeout_argument = &temporary_timeout_storage;
+  } else if (timeout > 0 || number_of_valid_timers > 0) {
+    int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX;
+    // Compare the timeout to the time until next callback for each timer.
+    // Take the lowest and use that for the wait timeout.
+    uint64_t i = 0;
+    for (i = 0; i < wait_set->impl->timer_index; ++i) {
+      if (!wait_set->timers[i]) {
+        continue;  // Skip NULL timers.
+      }
+      // at this point we know any non-NULL timers are also not canceled
 
       int64_t timer_timeout = INT64_MAX;
-      ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout);
+      rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout);
       if (ret != RCL_RET_OK) {
         return ret;  // The rcl error state should already be set.
       }
       if (timer_timeout < min_timeout) {
+        is_timer_timeout = true;
         min_timeout = timer_timeout;
       }
     }
+
     // If min_timeout was negative, we need to wake up immediately.
     if (min_timeout < 0) {
       min_timeout = 0;
@@ -577,7 +594,23 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     &wait_set->impl->rmw_clients,
     wait_set->impl->rmw_waitset,
     timeout_argument);
-  // Check for timeout.
+  // Check for ready timers next
+  // and set not ready timers (which includes canceled timers) to NULL.
+  size_t i;
+  for (i = 0; i < wait_set->impl->timer_index; ++i) {
+    if (!wait_set->timers[i]) {
+      continue;
+    }
+    bool is_ready = false;
+    rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready);
+    if (ret != RCL_RET_OK) {
+      return ret;  // The rcl error state should already be set.
+    }
+    if (!is_ready) {
+      wait_set->timers[i] = NULL;
+    }
+  }
+  // Check for timeout, return RCL_RET_TIMEOUT only if it wasn't a timer.
   if (ret == RMW_RET_TIMEOUT) {
     // Assume none were set (because timeout was reached first), and clear all.
     rcl_ret_t rcl_ret;
@@ -591,25 +624,13 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     assert(rcl_ret == RCL_RET_OK);  // Defensive, shouldn't fail with valid wait_set.
     rcl_ret = rcl_wait_set_clear_clients(wait_set);
     assert(rcl_ret == RCL_RET_OK);  // Defensive, shouldn't fail with valid wait_set.
-    return RCL_RET_TIMEOUT;
+    if (!is_timer_timeout) {
-  }
+      return RCL_RET_TIMEOUT;
-  // Check for error.
+    }
-  if (ret != RMW_RET_OK) {
+  } else if (ret != RMW_RET_OK) {
     RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), wait_set->impl->allocator);
     return RCL_RET_ERROR;
   }
-  // Check for ready timers next, and set not ready timers to NULL.
-  size_t i;
-  for (i = 0; i < wait_set->impl->timer_index; ++i) {
-    bool is_ready = false;
-    rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready);
-    if (ret != RCL_RET_OK) {
-      return ret;  // The rcl error state should already be set.
-    }
-    if (!is_ready) {
-      wait_set->timers[i] = NULL;
-    }
-  }
   // Set corresponding rcl subscription handles NULL.
   for (i = 0; i < wait_set->size_of_subscriptions; ++i) {
     if (!wait_set->impl->rmw_subscriptions.subscribers[i]) {

rcl/test/rcl/test_wait.cpp

@@ -123,7 +123,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) {
   ret = rcl_wait(&wait_set, timeout);
   std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now();
   // We expect a timeout here (timer value reached)
-  ASSERT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
   // Check time
   int64_t diff = std::chrono::duration_cast<std::chrono::nanoseconds>(after_sc - before_sc).count();
   EXPECT_LE(diff, RCL_MS_TO_NS(10) + TOLERANCE);