Rcl timer with ros time (#286)

* rcl_wait() on timers with ROS clock

Timers handle time jump callbacks
rcl_wait() wakes when ros time gets a time after timer's next call time

* Set timer impl to NULL after fini

rcl/include/rcl/timer.h

@@ -23,6 +23,7 @@ extern "C"
 #include <stdbool.h>
 
 #include "rcl/allocator.h"
+#include "rcl/guard_condition.h"
 #include "rcl/macros.h"
 #include "rcl/time.h"
 #include "rcl/types.h"
@@ -564,6 +565,25 @@ rcl_timer_reset(rcl_timer_t * timer);
 const rcl_allocator_t *
 rcl_timer_get_allocator(const rcl_timer_t * timer);
 
+/// Retrieve a guard condition used by the timer to wake the waitset when using ROSTime.
+/**
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] timer the timer to be queried
+ * \return `NULL` if the timer is invalid or does not have a guard condition, or
+ * \return a guard condition pointer.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_guard_condition_t *
+rcl_timer_get_guard_condition(const rcl_timer_t * timer);
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/timer.c

@@ -30,14 +30,18 @@ typedef struct rcl_timer_impl_t
 {
   // The clock providing time.
   rcl_clock_t * clock;
+  // A guard condition used to wake a wait set if using ROSTime, else zero initialized.
+  rcl_guard_condition_t guard_condition;
   // The user supplied callback.
   atomic_uintptr_t callback;
   // This is a duration in nanoseconds.
   atomic_uint_least64_t period;
   // This is a time in nanoseconds since an unspecified time.
-  atomic_uint_least64_t last_call_time;
+  atomic_int_least64_t last_call_time;
   // This is a time in nanoseconds since an unspecified time.
-  atomic_uint_least64_t next_call_time;
+  atomic_int_least64_t next_call_time;
+  // Credit for time elapsed before ROS time is activated or deactivated.
+  atomic_int_least64_t time_credit;
   // A flag which indicates if the timer is canceled.
   atomic_bool canceled;
   // The user supplied allocator.
@@ -51,6 +55,70 @@ rcl_get_zero_initialized_timer()
   return null_timer;
 }
 
+void _rcl_timer_time_jump(
+  const struct rcl_time_jump_t * time_jump,
+  bool before_jump,
+  void * user_data)
+{
+  rcl_timer_t * timer = (rcl_timer_t *)user_data;
+
+  if (before_jump) {
+    if (RCL_ROS_TIME_ACTIVATED == time_jump->clock_change ||
+      RCL_ROS_TIME_DEACTIVATED == time_jump->clock_change)
+    {
+      rcl_time_point_value_t now;
+      if (RCL_RET_OK != rcl_clock_get_now(timer->impl->clock, &now)) {
+        RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to get current time in jump callback");
+        return;
+      }
+      // Source of time is changing, but the timer has ellapsed some portion of its period
+      // Save ellapsed duration pre jump so the timer only waits the remainder in the new epoch
+      if (0 == now) {
+        // No time credit if clock is uninitialized
+        return;
+      }
+      const int64_t next_call_time = rcl_atomic_load_int64_t(&timer->impl->next_call_time);
+      rcl_atomic_store(&timer->impl->time_credit, next_call_time - now);
+    }
+  } else {
+    rcl_time_point_value_t now;
+    if (RCL_RET_OK != rcl_clock_get_now(timer->impl->clock, &now)) {
+      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to get current time in jump callback");
+      return;
+    }
+    const int64_t last_call_time = rcl_atomic_load_int64_t(&timer->impl->last_call_time);
+    const int64_t next_call_time = rcl_atomic_load_int64_t(&timer->impl->next_call_time);
+    const int64_t period = rcl_atomic_load_uint64_t(&timer->impl->period);
+    if (RCL_ROS_TIME_ACTIVATED == time_jump->clock_change ||
+      RCL_ROS_TIME_DEACTIVATED == time_jump->clock_change)
+    {
+      // ROS time activated or deactivated
+      if (0 == now) {
+        // Can't apply time credit if clock is uninitialized
+        return;
+      }
+      int64_t time_credit = rcl_atomic_exchange_int64_t(&timer->impl->time_credit, 0);
+      if (time_credit) {
+        // set times in new epoch so timer only waits the remainder of the period
+        rcl_atomic_store(&timer->impl->next_call_time, now - time_credit + period);
+        rcl_atomic_store(&timer->impl->last_call_time, now - time_credit);
+      }
+    } else if (next_call_time <= now) {
+      // Post Forward jump and timer is ready
+      if (RCL_RET_OK != rcl_trigger_guard_condition(&timer->impl->guard_condition)) {
+        RCUTILS_LOG_ERROR_NAMED(
+          ROS_PACKAGE_NAME, "Failed to get trigger guard condition in jump callback");
+      }
+    } else if (now < last_call_time) {
+      // Post backwards time jump that went further back than 1 period
+      // next callback should happen after 1 period
+      rcl_atomic_store(&timer->impl->next_call_time, now + period);
+      rcl_atomic_store(&timer->impl->last_call_time, now);
+      return;
+    }
+  }
+}
+
 rcl_ret_t
 rcl_timer_init(
   rcl_timer_t * timer,
@@ -78,15 +146,48 @@ rcl_timer_init(
   }
   rcl_timer_impl_t impl;
   impl.clock = clock;
+  impl.guard_condition = rcl_get_zero_initialized_guard_condition();
+  if (RCL_ROS_TIME == impl.clock->type) {
+    rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options();
+    rcl_ret_t ret = rcl_guard_condition_init(&(impl.guard_condition), options);
+    if (RCL_RET_OK != ret) {
+      return ret;
+    }
+    rcl_jump_threshold_t threshold;
+    threshold.on_clock_change = true;
+    threshold.min_forward.nanoseconds = 1;
+    threshold.min_backward.nanoseconds = -1;
+    ret = rcl_clock_add_jump_callback(clock, threshold, _rcl_timer_time_jump, timer);
+    if (RCL_RET_OK != ret) {
+      if (RCL_RET_OK != rcl_guard_condition_fini(&(impl.guard_condition))) {
+        // Should be impossible
+        RCUTILS_LOG_ERROR_NAMED(
+          ROS_PACKAGE_NAME, "Failed to fini guard condition after failing to add jump callback");
+      }
+      return ret;
+    }
+  }
   atomic_init(&impl.callback, (uintptr_t)callback);
   atomic_init(&impl.period, period);
+  atomic_init(&impl.time_credit, 0);
   atomic_init(&impl.last_call_time, now);
   atomic_init(&impl.next_call_time, now + period);
   atomic_init(&impl.canceled, false);
   impl.allocator = allocator;
   timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
+  if (NULL == timer->impl) {
-    timer->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, allocator);
+    if (RCL_RET_OK != rcl_guard_condition_fini(&(impl.guard_condition))) {
+      // Should be impossible
+      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini guard condition after bad alloc");
+    }
+    if (RCL_RET_OK != rcl_clock_remove_jump_callback(clock, _rcl_timer_time_jump, timer)) {
+      // Should be impossible
+      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to remove callback after bad alloc");
+    }
+
+    RCL_SET_ERROR_MSG("allocating memory failed", allocator);
+    return RCL_RET_BAD_ALLOC;
+  }
   *timer->impl = impl;
   return RCL_RET_OK;
 }
@@ -100,7 +201,18 @@ rcl_timer_fini(rcl_timer_t * timer)
   // Will return either RCL_RET_OK or RCL_RET_ERROR since the timer is valid.
   rcl_ret_t result = rcl_timer_cancel(timer);
   rcl_allocator_t allocator = timer->impl->allocator;
+  rcl_ret_t fail_ret = rcl_guard_condition_fini(&(timer->impl->guard_condition));
+  if (RCL_RET_OK != fail_ret) {
+    RCL_SET_ERROR_MSG("Failure to fini guard condition", allocator);
+  }
+  if (RCL_ROS_TIME == timer->impl->clock->type) {
+    fail_ret = rcl_clock_remove_jump_callback(timer->impl->clock, _rcl_timer_time_jump, timer);
+    if (RCL_RET_OK != fail_ret) {
+      RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to remove timer jump callback");
+    }
+  }
   allocator.deallocate(timer->impl, allocator.state);
+  timer->impl = NULL;
   return result;
 }
 
@@ -138,28 +250,27 @@ rcl_timer_call(rcl_timer_t * timer)
     RCL_SET_ERROR_MSG("clock now returned negative time point value", *allocator);
     return RCL_RET_ERROR;
   }
-  uint64_t unow = (uint64_t)now;
   rcl_time_point_value_t previous_ns =
-    rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, unow);
+    rcl_atomic_exchange_int64_t(&timer->impl->last_call_time, now);
   rcl_timer_callback_t typed_callback =
     (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
 
-  uint64_t next_call_time = rcl_atomic_load_uint64_t(&timer->impl->next_call_time);
+  int64_t next_call_time = rcl_atomic_load_int64_t(&timer->impl->next_call_time);
-  uint64_t period = rcl_atomic_load_uint64_t(&timer->impl->period);
+  int64_t period = rcl_atomic_load_uint64_t(&timer->impl->period);
   // always move the next call time by exactly period forward
   // don't use now as the base to avoid extending each cycle by the time
   // between the timer being ready and the callback being triggered
   next_call_time += period;
   // in case the timer has missed at least once cycle
-  if (next_call_time < unow) {
+  if (next_call_time < now) {
     if (0 == period) {
       // a timer with a period of zero is considered always ready
-      next_call_time = unow;
+      next_call_time = now;
     } else {
       // move the next call time forward by as many periods as necessary
-      uint64_t now_ahead = unow - next_call_time;
+      int64_t now_ahead = now - next_call_time;
       // rounding up without overflow
-      uint64_t periods_ahead = 1 + (now_ahead - 1) / period;
+      int64_t periods_ahead = 1 + (now_ahead - 1) / period;
       next_call_time += periods_ahead * period;
     }
   }
@@ -200,12 +311,12 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt
   }
   RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT, *allocator);
   rcl_time_point_value_t now;
-  rcl_ret_t ret = rcutils_steady_time_now(&now);
+  rcl_ret_t ret = rcl_clock_get_now(timer->impl->clock, &now);
   if (ret != RCL_RET_OK) {
     return ret;  // rcl error state should already be set.
   }
   *time_until_next_call =
-    rcl_atomic_load_uint64_t(&timer->impl->next_call_time) - now;
+    rcl_atomic_load_int64_t(&timer->impl->next_call_time) - now;
   return RCL_RET_OK;
 }
 
@@ -221,12 +332,12 @@ rcl_timer_get_time_since_last_call(
   }
   RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT, *allocator);
   rcl_time_point_value_t now;
-  rcl_ret_t ret = rcutils_steady_time_now(&now);
+  rcl_ret_t ret = rcl_clock_get_now(timer->impl->clock, &now);
   if (ret != RCL_RET_OK) {
     return ret;  // rcl error state should already be set.
   }
   *time_since_last_call =
-    now - rcl_atomic_load_uint64_t(&timer->impl->last_call_time);
+    now - rcl_atomic_load_int64_t(&timer->impl->last_call_time);
   return RCL_RET_OK;
 }
 
@@ -310,7 +421,7 @@ rcl_timer_reset(rcl_timer_t * timer)
   RCL_CHECK_FOR_NULL_WITH_MSG(
     timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID, rcl_get_default_allocator());
   rcl_time_point_value_t now;
-  rcl_ret_t now_ret = rcutils_steady_time_now(&now);
+  rcl_ret_t now_ret = rcl_clock_get_now(timer->impl->clock, &now);
   if (now_ret != RCL_RET_OK) {
     return now_ret;  // rcl error state should already be set.
   }
@@ -330,6 +441,15 @@ rcl_timer_get_allocator(const rcl_timer_t * timer)
   return &timer->impl->allocator;
 }
 
+rcl_guard_condition_t *
+rcl_timer_get_guard_condition(const rcl_timer_t * timer)
+{
+  if (NULL == timer || NULL == timer->impl || NULL == timer->impl->guard_condition.impl) {
+    return NULL;
+  }
+  return &timer->impl->guard_condition;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/wait.c

@@ -354,16 +354,39 @@ rcl_wait_set_resize(
     SET_RESIZE_RMW_REALLOC(
       subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count)
   );
-  SET_RESIZE(
+  // Guard condition RCL size is the resize amount given
-    guard_condition,
+  SET_RESIZE(guard_condition,;,;);  // NOLINT
-    SET_RESIZE_RMW_DEALLOC(
+
-      rmw_guard_conditions.guard_conditions,
+  // Guard condition RMW size needs to be guard conditions + timers
-      rmw_guard_conditions.guard_condition_count),
+  rmw_guard_conditions_t * rmw_gcs = &(wait_set->impl->rmw_guard_conditions);
-    SET_RESIZE_RMW_REALLOC(
+  const size_t num_rmw_gc = guard_conditions_size + timers_size;
-      guard_condition,
+  // Clear added guard conditions
-      rmw_guard_conditions.guard_conditions,
+  rmw_gcs->guard_condition_count = 0u;
-      rmw_guard_conditions.guard_condition_count)
+  if (0u == num_rmw_gc) {
-  );
+    if (rmw_gcs->guard_conditions) {
+      wait_set->impl->allocator.deallocate(
+        (void *)rmw_gcs->guard_conditions, wait_set->impl->allocator.state);
+      rmw_gcs->guard_conditions = NULL;
+    }
+  } else {
+    rmw_gcs->guard_conditions = (void **)wait_set->impl->allocator.reallocate(
+      rmw_gcs->guard_conditions, sizeof(void *) * num_rmw_gc, wait_set->impl->allocator.state);
+    if (!rmw_gcs->guard_conditions) {
+      // Deallocate rcl arrays to match unallocated rmw guard conditions
+      wait_set->impl->allocator.deallocate(
+        (void *)wait_set->guard_conditions, wait_set->impl->allocator.state);
+      wait_set->size_of_guard_conditions = 0u;
+      wait_set->guard_conditions = NULL;
+      wait_set->impl->allocator.deallocate(
+        (void *)wait_set->timers, wait_set->impl->allocator.state);
+      wait_set->size_of_timers = 0u;
+      wait_set->timers = NULL;
+      RCL_SET_ERROR_MSG("allocating memory failed", wait_set->impl->allocator);
+      return RCL_RET_BAD_ALLOC;
+    }
+    memset(rmw_gcs->guard_conditions, 0, sizeof(void *) * num_rmw_gc);
+  }
+
   SET_RESIZE(timer,;,;);  // NOLINT
   SET_RESIZE(client,
     SET_RESIZE_RMW_DEALLOC(
@@ -397,6 +420,16 @@ rcl_wait_set_add_timer(
   const rcl_timer_t * timer)
 {
   SET_ADD(timer)
+  // Add timer guard conditions to end of rmw guard condtion set.
+  rcl_guard_condition_t * guard_condition = rcl_timer_get_guard_condition(timer);
+  if (NULL != guard_condition) {
+    // rcl_wait() will take care of moving these backwards and setting guard_condition_count.
+    const size_t index = wait_set->size_of_guard_conditions + (wait_set->impl->timer_index - 1);
+    rmw_guard_condition_t * rmw_handle = rcl_guard_condition_get_rmw_handle(guard_condition);
+    RCL_CHECK_FOR_NULL_WITH_MSG(
+      rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR, wait_set->impl->allocator);
+    wait_set->impl->rmw_guard_conditions.guard_conditions[index] = rmw_handle->data;
+  }
   return RCL_RET_OK;
 }
 
@@ -443,6 +476,8 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
   rmw_time_t * timeout_argument = NULL;
   rmw_time_t temporary_timeout_storage;
 
+  bool is_timer_timeout = false;
+  int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX;
   // calculate the number of valid (non-NULL and non-canceled) timers
   size_t number_of_valid_timers = wait_set->size_of_timers;
   {  // scope to prevent i from colliding below
@@ -460,38 +495,36 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
       if (is_canceled) {
         number_of_valid_timers--;
         wait_set->timers[i] = NULL;
+        continue;
+      }
+      rmw_guard_conditions_t * rmw_gcs = &(wait_set->impl->rmw_guard_conditions);
+      size_t gc_idx = wait_set->size_of_guard_conditions + i;
+      if (NULL != rmw_gcs->guard_conditions[gc_idx]) {
+        // This timer has a guard condition, so move it to make a legal wait set.
+        rmw_gcs->guard_conditions[rmw_gcs->guard_condition_count] =
+          rmw_gcs->guard_conditions[gc_idx];
+        ++(rmw_gcs->guard_condition_count);
+      } else {
+        // No guard condition, instead use to set the rmw_wait timeout
+        int64_t timer_timeout = INT64_MAX;
+        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;
+        }
       }
     }
   }
 
-  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;
-      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;

rcl/test/rcl/test_timer.cpp

@@ -13,6 +13,8 @@
 // limitations under the License.
 
 #include <gtest/gtest.h>
+#include <chrono>
+#include <thread>
 
 #include "rcl/timer.h"
 
@@ -246,3 +248,223 @@ TEST_F(TestTimerFixture, test_canceled_timer) {
   ret = rcl_clock_fini(&clock);
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
 }
+
+TEST_F(TestTimerFixture, test_rostime_time_until_next_call) {
+  rcl_ret_t ret;
+  const int64_t sec_5 = RCL_S_TO_NS(5);
+  int64_t time_until = 0;
+
+  rcl_clock_t clock;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
+  });
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
+
+  rcl_timer_t timer = rcl_get_zero_initialized_timer();
+  ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
+  });
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string_safe();
+  ret = rcl_timer_get_time_until_next_call(&timer, &time_until);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  EXPECT_EQ(sec_5 - 1, time_until);
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5)) << rcl_get_error_string_safe();
+  ret = rcl_timer_get_time_until_next_call(&timer, &time_until);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  EXPECT_EQ(0, time_until);
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5 + 1)) <<
+    rcl_get_error_string_safe();
+  ret = rcl_timer_get_time_until_next_call(&timer, &time_until);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  EXPECT_EQ(-1, time_until);
+}
+
+TEST_F(TestTimerFixture, test_system_time_to_ros_time) {
+  rcl_ret_t ret;
+  const int64_t sec_5 = RCL_S_TO_NS(5);
+
+  rcl_clock_t clock;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
+  });
+
+  rcl_timer_t timer = rcl_get_zero_initialized_timer();
+  ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
+  });
+
+  int64_t time_until_pre = 0;
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until_pre)) <<
+    rcl_get_error_string_safe();
+  ASSERT_LT(0, time_until_pre);
+  ASSERT_GT(sec_5, time_until_pre);
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
+
+  int64_t time_until = 0;
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) <<
+    rcl_get_error_string_safe();
+  // Because of time credit the time until next call should be less than before
+  EXPECT_GT(time_until_pre, time_until);
+  EXPECT_LT(0, time_until);
+}
+
+TEST_F(TestTimerFixture, test_ros_time_to_system_time) {
+  rcl_ret_t ret;
+  const int64_t sec_5 = RCL_S_TO_NS(5);
+  const int64_t sec_1 = RCL_S_TO_NS(1);
+
+  rcl_clock_t clock;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
+  });
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
+
+  rcl_timer_t timer = rcl_get_zero_initialized_timer();
+  ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
+  });
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
+
+  int64_t time_until_pre = 0;
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until_pre)) <<
+    rcl_get_error_string_safe();
+  ASSERT_EQ(sec_5 - (sec_1 - 1), time_until_pre);
+
+  ASSERT_EQ(RCL_RET_OK, rcl_disable_ros_time_override(&clock)) << rcl_get_error_string_safe();
+
+  int64_t time_until = 0;
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) <<
+    rcl_get_error_string_safe();
+  // Because of time credit the time until next call should be less than before
+  EXPECT_GT(time_until_pre, time_until);
+  EXPECT_LT(0, time_until);
+}
+
+TEST_F(TestTimerFixture, test_ros_time_backwards_jump) {
+  rcl_ret_t ret;
+  const int64_t sec_5 = RCL_S_TO_NS(5);
+  const int64_t sec_3 = RCL_S_TO_NS(3);
+  const int64_t sec_2 = RCL_S_TO_NS(2);
+  const int64_t sec_1 = RCL_S_TO_NS(1);
+
+  rcl_clock_t clock;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
+  });
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_2)) << rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
+
+  rcl_timer_t timer = rcl_get_zero_initialized_timer();
+  ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
+  });
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_3)) << rcl_get_error_string_safe();
+  {
+    // Moved forward a little bit, timer should be closer to being ready
+    int64_t time_until = 0;
+    ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) <<
+      rcl_get_error_string_safe();
+    EXPECT_EQ(sec_5 - (sec_3 - sec_2), time_until);
+  }
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string_safe();
+  {
+    // Jumped back before timer was created, so last_call_time should be 1 period
+    int64_t time_until = 0;
+    ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) <<
+      rcl_get_error_string_safe();
+    EXPECT_EQ(sec_5, time_until);
+  }
+}
+
+TEST_F(TestTimerFixture, test_ros_time_wakes_wait) {
+  const int64_t sec_5 = RCL_S_TO_NS(5);
+  const int64_t sec_1 = RCL_S_TO_NS(1);
+  const int64_t sec_1_5 = RCL_S_TO_NS(3) / 2;
+
+  rcl_ret_t ret;
+  rcl_clock_t clock;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  ASSERT_EQ(RCL_RET_OK, rcl_clock_init(RCL_ROS_TIME, &clock, &allocator)) <<
+    rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
+  });
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string_safe();
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
+
+  rcl_timer_t timer = rcl_get_zero_initialized_timer();
+  ret = rcl_timer_init(&timer, &clock, sec_1, nullptr, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+    EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
+  });
+
+  bool timer_was_ready = false;
+
+  std::thread wait_thr([&](void) {
+      rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+      ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator());
+      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
+      ASSERT_EQ(RCL_RET_OK, rcl_wait_set_add_timer(&wait_set, &timer)) <<
+        rcl_get_error_string_safe();
+      // *INDENT-OFF* (Uncrustify wants strange un-indentation here)
+      OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
+        EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) <<
+          rcl_get_error_string_safe();
+      });
+      // *INDENT-ON*
+
+      ret = rcl_wait(&wait_set, sec_5);
+      // set some flag indicating wait was exited
+      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+      if (wait_set.timers[0] != NULL) {
+        timer_was_ready = true;
+      }
+    });
+
+  // Timer not exceeded, should not wake
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1_5)) <<
+    rcl_get_error_string_safe();
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  EXPECT_FALSE(timer_was_ready);
+
+  // Timer exceeded, should wake
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5)) << rcl_get_error_string_safe();
+  auto start = std::chrono::steady_clock::now();
+  wait_thr.join();
+  auto finish = std::chrono::steady_clock::now();
+  EXPECT_TRUE(timer_was_ready);
+  EXPECT_LT(finish - start, std::chrono::milliseconds(100));
+}