diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 24fb380cf..916879153 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -326,6 +326,14 @@ def spin_once_until_future_complete(self, future: Future, timeout_sec: float = N """ raise NotImplementedError() + def spin_some(self, timeout_sec: Optional[float] = None): + """ + Execute all currently avaliable work. + + :param timeout_sec: Seconds to wait. Waits forever if ``None`` or negative. + """ + raise NotImplementedError() + def _take_timer(self, tmr): with tmr.handle: tmr.handle.call_timer() @@ -727,6 +735,36 @@ def spin_once(self, timeout_sec: float = None) -> None: def spin_once_until_future_complete(self, future: Future, timeout_sec: float = None) -> None: self.spin_once(timeout_sec) + def spin_some(self, timeout_sec: Optional[float] = None): + timeout_sec = float('inf') if timeout_sec is None or timeout_sec < 0 else timeout_sec + + def _poll_ready_callbacks(*args, **kwargs): + timeout_sec = kwargs['timeout_sec'] + while True: + t1 = time.time() + t2 = t1 + if self._cb_iter is None or self._last_args != args or self._last_kwargs != kwargs: + # Create a new generator + self._last_args = args + self._last_kwargs = kwargs + self._cb_iter = self._wait_for_ready_callbacks(*args, **kwargs) + try: + handler, _, _ = next(self._cb_iter) + handler() + if handler.exception() is not None: + raise handler.exception() + except StopIteration: + # Generator ran out of work + self._cb_iter = None + t2 = time.time() + timeout_sec -= t2 - t1 + try: + _poll_ready_callbacks(timeout_sec=timeout_sec) + except ShutdownException: + pass + except TimeoutException: + pass + class MultiThreadedExecutor(Executor): """ @@ -775,3 +813,39 @@ def spin_once(self, timeout_sec: float = None) -> None: def spin_once_until_future_complete(self, future: Future, timeout_sec: float = None) -> None: self._spin_once_impl(timeout_sec, future.done) + + def spin_some(self, timeout_sec: Optional[float] = None): + timeout_sec = float('inf') if timeout_sec is None or timeout_sec < 0 else timeout_sec + + def _poll_ready_callbacks(*args, **kwargs): + timeout_sec = kwargs['timeout_sec'] + while True: + t1 = time.time() + t2 = t1 + if self._cb_iter is None or self._last_args != args or self._last_kwargs != kwargs: + # Create a new generator + self._last_args = args + self._last_kwargs = kwargs + self._cb_iter = self._wait_for_ready_callbacks(*args, **kwargs) + try: + handler, _, _ = next(self._cb_iter) + def handler_wrapper(handler): + handler() + if handler.exception() is not None: + raise handler.exception() + self._executor.submit(handler_wrapper(handler)) + except StopIteration: + # Generator ran out of work + self._cb_iter = None + t2 = time.time() + timeout_sec -= t2 - t1 + try: + _poll_ready_callbacks(timeout_sec=timeout_sec) + except ExternalShutdownException: + pass + except ShutdownException: + pass + except TimeoutException: + pass + except ConditionReachedException: + pass