diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 4ed931276..7b9c78548 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -321,14 +321,24 @@ def spin_until_future_complete( future.add_done_callback(lambda x: self.wake()) if timeout_sec is None or timeout_sec < 0: - while self._context.ok() and not future.done() and not self._is_shutdown: + while ( + self._context.ok() and + not future.done() and + not future.cancelled() + and not self._is_shutdown + ): self._spin_once_until_future_complete(future, timeout_sec) else: start = time.monotonic() end = start + timeout_sec timeout_left = TimeoutObject(timeout_sec) - while self._context.ok() and not future.done() and not self._is_shutdown: + while ( + self._context.ok() and + not future.done() and + not future.cancelled() + and not self._is_shutdown + ): self._spin_once_until_future_complete(future, timeout_left) now = time.monotonic() @@ -610,6 +620,8 @@ def _wait_for_ready_callbacks( with self._tasks_lock: # Get rid of any tasks that are done self._tasks = list(filter(lambda t_e_n: not t_e_n[0].done(), self._tasks)) + # Get rid of any tasks that are cancelled + self._tasks = list(filter(lambda t_e_n: not t_e_n[0].cancelled(), self._tasks)) # Gather entities that can be waited on subscriptions: List[Subscription] = [] diff --git a/rclpy/rclpy/task.py b/rclpy/rclpy/task.py index 81a56ab5b..c75d3395e 100644 --- a/rclpy/rclpy/task.py +++ b/rclpy/rclpy/task.py @@ -61,7 +61,7 @@ def __del__(self) -> None: def __await__(self) -> Generator[None, None, Optional[T]]: # Yield if the task is not finished - while not self._done: + while not self._done and not self._cancelled: yield return self.result() @@ -239,7 +239,12 @@ def __call__(self) -> None: The return value of the handler is stored as the task result. """ - if self._done or self._executing or not self._task_lock.acquire(blocking=False): + if ( + self._done or + self._cancelled or + self._executing or + not self._task_lock.acquire(blocking=False) + ): return try: if self._done: diff --git a/rclpy/test/test_executor.py b/rclpy/test/test_executor.py index 7b7027486..e69e4ed30 100644 --- a/rclpy/test/test_executor.py +++ b/rclpy/test/test_executor.py @@ -273,6 +273,27 @@ async def coroutine(): self.assertTrue(future.done()) self.assertEqual('Sentinel Result', future.result()) + def test_create_task_coroutine_cancel(self) -> None: + self.assertIsNotNone(self.node.handle) + executor = SingleThreadedExecutor(context=self.context) + executor.add_node(self.node) + + async def coroutine(): + await asyncio.sleep(1) + return 'Sentinel Result' + + future = executor.create_task(coroutine) + self.assertFalse(future.done()) + self.assertFalse(future.cancelled()) + + future.cancel() + self.assertTrue(future.cancelled()) + + executor.spin_until_future_complete(future) + self.assertFalse(future.done()) + self.assertTrue(future.cancelled()) + self.assertEqual(None, future.result()) + def test_create_task_normal_function(self) -> None: self.assertIsNotNone(self.node.handle) executor = SingleThreadedExecutor(context=self.context)