Trigger guard condition when timer is reset (#589)

Signed-off-by: Donghee Ye <[email protected]> Signed-off-by: Jorge Perez <[email protected]>
ros2 · Apr 9, 2020 · d6d9f27 · d6d9f27
1 parent 76b5ffe
commit d6d9f27
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 14 deletions.
diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c
@@ -33,7 +33,8 @@ typedef struct rcl_timer_impl_t
   rcl_clock_t * clock;
   // The associated context.
   rcl_context_t * context;
-  // A guard condition used to wake a wait set if using ROSTime, else zero initialized.
+  // A guard condition used to wake the associated wait set, either when
+  // ROSTime causes the timer to expire or when the timer is reset.
   rcl_guard_condition_t guard_condition;
   // The user supplied callback.
   atomic_uintptr_t callback;
@@ -153,12 +154,12 @@ rcl_timer_init(
   impl.clock = clock;
   impl.context = context;
   impl.guard_condition = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options();
+  rcl_ret_t ret = rcl_guard_condition_init(&(impl.guard_condition), context, options);
+  if (RCL_RET_OK != ret) {
+    return ret;
+  }
   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), context, options);
-    if (RCL_RET_OK != ret) {
-      return ret;
-    }
     rcl_jump_threshold_t threshold;
     threshold.on_clock_change = true;
     threshold.min_forward.nanoseconds = 1;
@@ -403,6 +404,10 @@ rcl_timer_reset(rcl_timer_t * timer)
   int64_t period = rcutils_atomic_load_uint64_t(&timer->impl->period);
   rcutils_atomic_store(&timer->impl->next_call_time, now + period);
   rcutils_atomic_store(&timer->impl->canceled, false);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&timer->impl->guard_condition);
+  if (ret != RCL_RET_OK) {
+    RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to trigger timer guard condition");
+  }
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset");
   return RCL_RET_OK;
 }

diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c
@@ -547,6 +547,14 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
       if (!wait_set->timers[i]) {
         continue;  // Skip NULL timers.
       }
+      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);
+      }
       bool is_canceled = false;
       rcl_ret_t ret = rcl_timer_is_canceled(wait_set->timers[i], &is_canceled);
       if (ret != RCL_RET_OK) {
@@ -556,14 +564,6 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
         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);
-      }
       // use timer time to to set the rmw_wait timeout
       // TODO(sloretz) fix spurious wake-ups on ROS_TIME timers with ROS_TIME enabled
       int64_t timer_timeout = INT64_MAX;