From ab7220797610ac3ce7600d33714195417eb4ae70 Mon Sep 17 00:00:00 2001
From: Janosch Machowinski <j.machowinski@nospam.org>
Date: Thu, 7 Mar 2024 20:45:25 +0100
Subject: [PATCH] fix(rcl_wait): Improved timeout computation in case of many
 timers

This commit changes the computation of the timer timeout, to be more
precise, in the case, of many registered timers.

Signed-off-by: Janosch Machowinski <J.Machowinski@cellumation.com>
---
 rcl/src/rcl/wait.c         | 44 +++++++++++++++++++++++++++++++-------
 rcl/test/rcl/test_wait.cpp |  2 --
 2 files changed, 36 insertions(+), 10 deletions(-)

diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c
index 8f46ee8f5..33d1c373f 100644
--- a/rcl/src/rcl/wait.c
+++ b/rcl/src/rcl/wait.c
@@ -543,7 +543,14 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
   rmw_time_t temporary_timeout_storage;
 
   bool is_timer_timeout = false;
-  int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX;
+
+  int64_t min_next_call_time[RCL_STEADY_TIME + 1];
+  rcl_clock_t * clocks[RCL_STEADY_TIME + 1] = {0, 0, 0, 0};
+
+  min_next_call_time[RCL_ROS_TIME] = INT64_MAX;
+  min_next_call_time[RCL_SYSTEM_TIME] = INT64_MAX;
+  min_next_call_time[RCL_STEADY_TIME] = INT64_MAX;
+
   {  // scope to prevent i from colliding below
     uint64_t i = 0;
     for (i = 0; i < wait_set->impl->timer_index; ++i) {
@@ -561,14 +568,14 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
 
       rcl_clock_t * clock;
       if (rcl_timer_clock(wait_set->timers[i], &clock) != RCL_RET_OK) {
-        //should never happen
+        // should never happen
         return RCL_RET_ERROR;
       }
 
       if (clock->type == RCL_ROS_TIME) {
         bool timer_override_active = false;
         if (rcl_is_enabled_ros_time_override(clock, &timer_override_active) != RCL_RET_OK) {
-          //should never happen
+          // should never happen
           return RCL_RET_ERROR;
         }
 
@@ -580,9 +587,11 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
         }
       }
 
-      // use timer time to 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);
+      clocks[clock->type] = clock;
+
+      // get the time of the next call to the timer
+      int64_t next_call_time = INT64_MAX;
+      rcl_ret_t ret = rcl_timer_get_next_call_time(wait_set->timers[i], &next_call_time);
       if (ret == RCL_RET_TIMER_CANCELED) {
         wait_set->timers[i] = NULL;
         continue;
@@ -590,9 +599,9 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
       if (ret != RCL_RET_OK) {
         return ret;  // The rcl error state should already be set.
       }
-      if (timer_timeout < min_timeout) {
+      if (next_call_time < min_next_call_time[clock->type]) {
         is_timer_timeout = true;
-        min_timeout = timer_timeout;
+        min_next_call_time[clock->type] = next_call_time;
       }
     }
   }
@@ -603,6 +612,25 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     temporary_timeout_storage.nsec = 0;
     timeout_argument = &temporary_timeout_storage;
   } else if (timeout > 0 || is_timer_timeout) {
+    int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX;
+    for (size_t i = 0; i <= RCL_STEADY_TIME; i++) {
+      if (clocks[i] == 0) {
+        continue;
+      }
+
+      int64_t cur_time;
+      rmw_ret_t ret = rcl_clock_get_now(clocks[i], &cur_time);
+      if (ret != RCL_RET_OK) {
+        return ret;  // The rcl error state should already be set.
+      }
+
+      int64_t timeout = min_next_call_time[i] - cur_time;
+
+      if (min_timeout > timeout) {
+        min_timeout = timeout;
+      }
+    }
+
     // If min_timeout was negative, we need to wake up immediately.
     if (min_timeout < 0) {
       min_timeout = 0;
diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp
index b69c89415..fe5015f9d 100644
--- a/rcl/test/rcl/test_wait.cpp
+++ b/rcl/test/rcl/test_wait.cpp
@@ -323,7 +323,6 @@ TEST_F(WaitSetTestFixture, zero_timeout_overrun_timer) {
 
 // Test rcl_wait with a timeout value and an overrun timer
 TEST_F(WaitSetTestFixture, no_wakeup_on_override_timer) {
-
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret =
     rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator());
@@ -371,7 +370,6 @@ TEST_F(WaitSetTestFixture, no_wakeup_on_override_timer) {
 
 // Test rcl_wait with a timeout value and an overrun timer
 TEST_F(WaitSetTestFixture, wakeup_on_override_timer) {
-
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret =
     rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator());