fix calculation of next timer call (ros2#291)

* fix calculation of next timer call

* fix logic for period zero

Author: dirk-thomas

rcl/src/rcl/timer.c

@@ -36,6 +36,8 @@ typedef struct rcl_timer_impl_t
   atomic_uint_least64_t period;
   // This is a time in nanoseconds since an unspecified time.
   atomic_uint_least64_t last_call_time;
+  // This is a time in nanoseconds since an unspecified time.
+  atomic_uint_least64_t next_call_time;
   // A flag which indicates if the timer is canceled.
   atomic_bool canceled;
   // The user supplied allocator.
@@ -75,6 +77,7 @@ rcl_timer_init(
   atomic_init(&impl.callback, (uintptr_t)callback);
   atomic_init(&impl.period, period);
   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);
@@ -127,11 +130,37 @@ rcl_timer_call(rcl_timer_t * timer)
   if (now_ret != RCL_RET_OK) {
     return now_ret;  // rcl error state should already be set.
   }
+  if (now < 0) {
+    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, now);
+    rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, unow);
   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);
+  // 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 (0 == period) {
+      // a timer with a period of zero is considered always ready
+      next_call_time = unow;
+    } else {
+      // move the next call time forward by as many periods as necessary
+      uint64_t now_ahead = unow - next_call_time;
+      // rounding up without overflow
+      uint64_t periods_ahead = 1 + (now_ahead - 1) / period;
+      next_call_time += periods_ahead * period;
+    }
+  }
+  rcl_atomic_store(&timer->impl->next_call_time, next_call_time);
+
   if (typed_callback != NULL) {
     int64_t since_last_call = now - previous_ns;
     typed_callback(timer, since_last_call);
@@ -171,9 +200,8 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt
   if (ret != RCL_RET_OK) {
     return ret;  // rcl error state should already be set.
   }
-  int64_t period = rcl_atomic_load_uint64_t(&timer->impl->period);
   *time_until_next_call =
-    (rcl_atomic_load_uint64_t(&timer->impl->last_call_time) + period) - now;
+    rcl_atomic_load_uint64_t(&timer->impl->next_call_time) - now;
   return RCL_RET_OK;
 }
 
@@ -282,7 +310,8 @@ rcl_timer_reset(rcl_timer_t * timer)
   if (now_ret != RCL_RET_OK) {
     return now_ret;  // rcl error state should already be set.
   }
-  rcl_atomic_store(&timer->impl->last_call_time, now);
+  int64_t period = rcl_atomic_load_uint64_t(&timer->impl->period);
+  rcl_atomic_store(&timer->impl->next_call_time, now + period);
   rcl_atomic_store(&timer->impl->canceled, false);
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset")
   return RCL_RET_OK;