Make timer callback optional and fix timer logic

Make timer callback optional and fix wait bugs

rcl/include/rcl/timer.h

@@ -77,7 +77,12 @@ rcl_get_zero_initialized_timer(void);
  * The period is a duration (rather an absolute time in the future).
  * If the period is 0 then it will always be ready.
  *
- * The callback must be a function which returns void and takes two arguments,
+ * The callback is an optional argument.
+ * Valid inputs are either a pointer to the function callback, or NULL to
+ * indicate that no callback will be stored in rcl.
+ * If the callback is null, the caller client library is responsible for
+ * firing the timer callback.
+ * Else, it must be a function which returns void and takes two arguments,
  * the first being a pointer to the associated timer, and the second a uint64_t
  * which is the time since the previous call, or since the timer was created
  * if it is the first call to the callback.
@@ -148,6 +153,10 @@ rcl_timer_fini(rcl_timer_t * timer);
  * the timer's period has not yet elapsed.
  * It is up to the calling code to make sure the period has elapsed by first
  * calling rcl_timer_is_ready().
+ * If the callback pointer is null (either set in init or exchanged after
+ * initialized), no callback is fired.
+ * However, this function should still be called by the client library to
+ * update the state of the timer.
  * The order of operations in this command are as follows:
  *
  *  - Ensure the timer has not been canceled.
@@ -326,7 +335,9 @@ rcl_timer_get_callback(const rcl_timer_t * timer);
 /* This function can fail, and therefore return NULL, if:
  *   - timer is NULL
  *   - timer has not been initialized (the implementation is invalid)
- *   - the new_callback argument is NULL
+ *
+ * This function can set callback to null, in which case the callback is
+ * ignored when rcl_timer_call is called.
  *
  * This function is thread-safe.
  * This function is lock-free so long as the C11's stdatomic.h function

rcl/include/rcl/wait.h

@@ -186,7 +186,7 @@ rcl_wait_set_add_subscription(
 /// Remove (sets to NULL) the subscriptions in the wait set.
 /* This function should be used after passing using rcl_wait, but before
  * adding new subscriptions to the set.
- * Sets all of the entries in the underlying rmw array to null, and sets the 
+ * Sets all of the entries in the underlying rmw array to null, and sets the
  * count in the rmw array to 0.
  *
  * Calling this on an uninitialized (zero initialized) wait set will fail.

rcl/src/rcl/timer.c

@@ -51,7 +51,6 @@ rcl_timer_init(
   rcl_allocator_t allocator)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT);
   if (timer->impl) {
     RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized");
     return RCL_RET_ALREADY_INIT;
@@ -106,10 +105,13 @@ rcl_timer_call(rcl_timer_t * timer)
   }
   rcl_time_point_value_t previous_ns =
     rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady);
-  uint64_t since_last_call = now_steady - previous_ns;
   rcl_timer_callback_t typed_callback =
     (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
-  typed_callback(timer, since_last_call);
+
+  if (typed_callback != NULL) {
+    uint64_t since_last_call = now_steady - previous_ns;
+    typed_callback(timer, since_last_call);
+  }
   return RCL_RET_OK;
 }
 
@@ -195,7 +197,6 @@ rcl_timer_callback_t
 rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback)
 {
   RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
-  RCL_CHECK_ARGUMENT_FOR_NULL(new_callback, NULL);
   RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
   return (rcl_timer_callback_t)rcl_atomic_exchange_uintptr_t(
     &timer->impl->callback, (uintptr_t)new_callback);

rcl/src/rcl/wait.c

@@ -512,44 +512,49 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     return RCL_RET_WAIT_SET_EMPTY;
   }
   // Calculate the timeout argument.
-  rmw_time_t * timeout_argument;
+  // By default, set the timer to block indefinitely if none of the below conditions are met.
+  rmw_time_t * timeout_argument = NULL;
   rmw_time_t temporary_timeout_storage;
-  if (timeout < 0) {
+
-    // Pass NULL to wait to indicate block indefinitely.
-    timeout_argument = NULL;
-  }
-  if (timeout > 0) {
-    // Determine the nearest timeout (given or a timer).
-    uint64_t min_timeout = timeout;
-    // If min_timeout is > 0, then compare it to the time until each timer.
-    // Take the lowest and use that for the wait timeout.
-    if (min_timeout > 0) {
-      size_t i;
-      for (i = 0; i < wait_set->size_of_timers; ++i) {
-        if (!wait_set->timers[i]) {
-          continue;  // Skip NULL timers.
-        }
-        int64_t 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 ((uint64_t)timer_timeout < min_timeout) {
-          min_timeout = timer_timeout;
-        }
-      }
-    }
-    // Set that in the temporary storage and point to that for the argument.
-    temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout);
-    temporary_timeout_storage.nsec = min_timeout % 1000000000;
-    timeout_argument = &temporary_timeout_storage;
-  }
   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 {
+    int64_t min_timeout = INT64_MAX;
+    if (timeout > 0) {
+      // Compare the timeout to the time until next callback for each timer.
+      min_timeout = timeout;
+    }
+    // Take the lowest and use that for the wait timeout.
+    uint64_t i = 0;
+    for (i = 0; i < wait_set->size_of_timers; ++i) {
+      if (!wait_set->timers[i]) {
+        continue;  // Skip NULL timers.
+      }
+      int64_t 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) {
+        min_timeout = timer_timeout;
+      }
+    }
+    if (min_timeout == INT64_MAX) {
+      timeout_argument = NULL;
+    } else {
+      // If min_timeout was negative, we need to wake up immediately.
+      if (min_timeout < 0) {
+        min_timeout = 0;
+      }
+      temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout);
+      temporary_timeout_storage.nsec = min_timeout % 1000000000;
+      timeout_argument = &temporary_timeout_storage;
+    }
   }
+
   // Wait.
   rmw_ret_t ret = rmw_wait(
     &wait_set->impl->rmw_subscriptions,