Rcl timer with ros time

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(
-    timer->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, allocator);
+  if (NULL == timer->impl) {
+    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);
-  uint64_t period = rcl_atomic_load_uint64_t(&timer->impl->period);
+  int64_t next_call_time = rcl_atomic_load_int64_t(&timer->impl->next_call_time);
+  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