From c547e3fd937c709b7b0101ad4802fd58a233b4aa Mon Sep 17 00:00:00 2001
From: Brian Chen <brian.chen@openrobotics.org>
Date: Fri, 26 Aug 2022 02:00:02 -0700
Subject: [PATCH 1/2] add executor.spin_some

Signed-off-by: Brian Chen <brian.chen@openrobotics.org>
---
 rclpy/rclpy/executors.py | 74 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 74 insertions(+)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 24fb380cf..aabb93c74 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -326,6 +326,13 @@ 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 +734,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 +812,40 @@ 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

From e76004d24cfd208f2661d62db17198570ce6d2bd Mon Sep 17 00:00:00 2001
From: Brian Chen <brian.chen@openrobotics.org>
Date: Fri, 26 Aug 2022 02:30:56 -0700
Subject: [PATCH 2/2] lint

Signed-off-by: Brian Chen <brian.chen@openrobotics.org>
---
 rclpy/rclpy/executors.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index aabb93c74..916879153 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -327,7 +327,8 @@ 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.
+        """
+        Execute all currently avaliable work.
 
         :param timeout_sec: Seconds to wait. Waits forever if ``None`` or negative.
         """
@@ -828,7 +829,6 @@ def _poll_ready_callbacks(*args, **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: