From 920bb14b55239615a309b8177b870faf39500ff9 Mon Sep 17 00:00:00 2001
From: Teo Koon Peng <koonpeng@openrobotics.org>
Date: Fri, 19 Aug 2022 16:11:57 +0800
Subject: [PATCH] fix timers in task

Signed-off-by: Teo Koon Peng <koonpeng@openrobotics.org>
---
 rclpy/CMakeLists.txt             |  1 +
 rclpy/rclpy/executors.py         | 33 +++++++++++++---------
 rclpy/test/test_timer_in_task.py | 48 ++++++++++++++++++++++++++++++++
 3 files changed, 69 insertions(+), 13 deletions(-)
 create mode 100644 rclpy/test/test_timer_in_task.py

diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt
index a9966c040..41091576e 100644
--- a/rclpy/CMakeLists.txt
+++ b/rclpy/CMakeLists.txt
@@ -186,6 +186,7 @@ if(BUILD_TESTING)
       test/test_time_source.py
       test/test_time.py
       test/test_timer.py
+      test/test_timer_in_task.py
       test/test_topic_or_service_is_hidden.py
       test/test_topic_endpoint_info.py
       test/test_type_support.py
diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index dbaba882f..22431e4be 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -19,7 +19,6 @@
 from threading import Condition
 from threading import Lock
 from threading import RLock
-import time
 from typing import Any
 from typing import Callable
 from typing import Coroutine
@@ -169,6 +168,8 @@ def __init__(self, *, context: Context = None) -> None:
         self._clock = Clock(clock_type=ClockType.STEADY_TIME)
         self._sigint_gc = SignalHandlerGuardCondition(context)
         self._context.on_shutdown(self.wake)
+        self._spin_until_future_complete_timer: Optional[Timer] = None
+        self._spin_until_future_complete_timeout = False
 
     @property
     def context(self) -> Context:
@@ -287,18 +288,16 @@ def spin_until_future_complete(self, future: Future, timeout_sec: float = None)
             while self._context.ok() and not future.done() and not self._is_shutdown:
                 self.spin_once_until_future_complete(future, timeout_sec)
         else:
-            start = time.monotonic()
-            end = start + timeout_sec
-            timeout_left = timeout_sec
-
-            while self._context.ok() and not future.done() and not self._is_shutdown:
-                self.spin_once_until_future_complete(future, timeout_left)
-                now = time.monotonic()
-
-                if now >= end:
-                    return
-
-                timeout_left = end - now
+            if self._spin_until_future_complete_timer is not None:
+                # this should not happen
+                raise RuntimeError('Executor already spinning')
+            self._spin_until_future_complete_timer = Timer(
+                None, None, timeout_sec_to_nsec(timeout_sec),
+                self._clock, context=self._context)
+            self._spin_until_future_complete_timeout = False
+            while not future.done() and not self._spin_until_future_complete_timeout:
+                self.spin_once()
+            self._spin_until_future_complete_timeout = None
 
     def spin_once(self, timeout_sec: float = None) -> None:
         """
@@ -502,6 +501,8 @@ def _wait_for_ready_callbacks(
                     guards.append(gc)
             if timeout_timer is not None:
                 timers.append(timeout_timer)
+            if self._spin_until_future_complete_timer is not None:
+                timers.append(self._spin_until_future_complete_timer)
 
             guards.append(self._guard)
             guards.append(self._sigint_gc)
@@ -664,6 +665,12 @@ def _wait_for_ready_callbacks(
                 (timeout_timer is not None and timeout_timer.handle.pointer in timers_ready)
             ):
                 raise TimeoutException()
+            if (
+                self._spin_until_future_complete_timer is not None and
+                self._spin_until_future_complete_timer.handle.pointer in timers_ready
+            ):
+                self._spin_until_future_complete_timeout = True
+                raise TimeoutException()
         if self._is_shutdown:
             raise ShutdownException()
         if condition():
diff --git a/rclpy/test/test_timer_in_task.py b/rclpy/test/test_timer_in_task.py
new file mode 100644
index 000000000..dc98e6941
--- /dev/null
+++ b/rclpy/test/test_timer_in_task.py
@@ -0,0 +1,48 @@
+# Copyright 2022 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import unittest
+
+import rclpy
+import rclpy.executors
+
+
+class TestTimerInTask(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        cls.context = rclpy.context.Context()
+        rclpy.init(context=cls.context)
+        cls.node = rclpy.create_node('TestTimerInTask', context=cls.context)
+        cls.executor = rclpy.executors.SingleThreadedExecutor(context=cls.context)
+        cls.executor.add_node(cls.node)
+
+    @classmethod
+    def tearDownClass(cls):
+        cls.node.destroy_node()
+        rclpy.shutdown(context=cls.context)
+
+    def test_timer_in_task(self):
+        fut = rclpy.Future()
+
+        async def work():
+            self.node.create_timer(0.1, lambda: fut.set_result(None))
+            await fut
+
+        task = self.executor.create_task(work())
+        self.executor.spin_until_future_complete(task, 1)
+        self.assertTrue(task.done())
+
+
+if __name__ == '__main__':
+    unittest.main()