Rclpy wait for service

rclpy/CMakeLists.txt

@@ -110,7 +110,7 @@ if(BUILD_TESTING)
       WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
       PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}"
       APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}
-      TIMEOUT 90
+      TIMEOUT 120
     )
   endif()
 endif()

rclpy/rclpy/client.py

@@ -14,6 +14,7 @@
 
 import threading
 
+from rclpy.graph_listener import GraphEventSubscription
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 import rclpy.utilities
 
@@ -77,3 +78,39 @@ def wait_for_future(self):
         thread1 = ResponseThread(self)
         thread1.start()
         thread1.join()
+
+    def service_is_ready(self):
+        return _rclpy.rclpy_service_server_is_available(self.node_handle, self.client_handle)
+
+    def wait_for_service(self, timeout_sec=None):
+        """
+        Block until the service is available.
+
+        :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
+        :type timeout_sec: float or None
+        :rtype: bool
+        :returns: true if the service is available
+        """
+        timeout_nsec = rclpy.utilities.timeout_sec_to_nsec(timeout_sec)
+        result = self.service_is_ready()
+        if result or timeout_sec == 0:
+            return result
+
+        event = threading.Event()
+
+        def on_graph_event():
+            nonlocal self
+            nonlocal event
+            nonlocal result
+            result = self.service_is_ready()
+            if result:
+                event.set()
+
+        def on_timeout():
+            nonlocal event
+            event.set()
+
+        with GraphEventSubscription(self.node_handle, on_graph_event, timeout_nsec, on_timeout):
+            event.wait()
+
+        return result

rclpy/rclpy/executors.py

@@ -12,26 +12,16 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from concurrent.futures import ThreadPoolExecutor as _ThreadPoolExecutor
+from concurrent.futures import ThreadPoolExecutor
 import multiprocessing
-from threading import Condition as _Condition
-from threading import Lock as _Lock
+from threading import Condition
+from threading import Lock
 
-from rclpy.constants import S_TO_NS
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
-from rclpy.timer import WallTimer as _WallTimer
+from rclpy.timer import WallTimer
 from rclpy.utilities import ok
-
-
-class _WaitSet:
-    """Make sure the wait set gets destroyed when a generator exits."""
-
-    def __enter__(self):
-        self.wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
-        return self.wait_set
-
-    def __exit__(self, t, v, tb):
-        _rclpy.rclpy_destroy_wait_set(self.wait_set)
+from rclpy.utilities import timeout_sec_to_nsec
+from rclpy.wait_set import WaitSet
 
 
 class _WorkTracker:
@@ -40,7 +30,7 @@ class _WorkTracker:
     def __init__(self):
         # Number of tasks that are being executed
         self._num_work_executing = 0
-        self._work_condition = _Condition()
+        self._work_condition = Condition()
 
     def __enter__(self):
         """Increment the amount of executing work by 1."""
@@ -57,16 +47,18 @@ def wait(self, timeout_sec=None):
         """
         Wait until all work completes.
 
-        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
         :type timeout_sec: float or None
-        :rtype: bool True if all work completed
+        :rtype: bool
+        :returns: True if all work completed
         """
+        if timeout_sec is not None and timeout_sec < 0:
+            timeout_sec = None
         # Wait for all work to complete
-        if timeout_sec is None or timeout_sec >= 0:
-            with self._work_condition:
-                if not self._work_condition.wait_for(
-                        lambda: self._num_work_executing == 0, timeout_sec):
-                    return False
+        with self._work_condition:
+            if not self._work_condition.wait_for(
+                    lambda: self._num_work_executing == 0, timeout_sec):
+                return False
         return True
 
 
@@ -85,7 +77,7 @@ class Executor:
     def __init__(self):
         super().__init__()
         self._nodes = set()
-        self._nodes_lock = _Lock()
+        self._nodes_lock = Lock()
         # This is triggered when wait_for_ready_callbacks should rebuild the wait list
         gc, gc_handle = _rclpy.rclpy_create_guard_condition()
         self._guard_condition = gc
@@ -100,7 +92,7 @@ def shutdown(self, timeout_sec=None):
 
         Return true if all outstanding callbacks finished executing.
 
-        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
         :type timeout_sec: float or None
         :rtype: bool
         """
@@ -152,11 +144,11 @@ def spin_once(self, timeout_sec=None):
 
         A custom executor should use :func:`Executor.wait_for_ready_callbacks` to get work.
 
-        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
         :type timeout_sec: float or None
         :rtype: None
         """
-        raise NotImplementedError
+        raise NotImplementedError()
 
     def _take_timer(self, tmr):
         _rclpy.rclpy_call_timer(tmr.timer_handle)
@@ -260,22 +252,16 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
         """
         Yield callbacks that are ready to be performed.
 
-        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
         :type timeout_sec: float or None
         :param nodes: A list of nodes to wait on. Wait on all nodes if None.
         :type nodes: list or None
         :rtype: Generator[(callable, entity, :class:`rclpy.node.Node`)]
         """
         timeout_timer = None
-        # Get timeout in nanoseconds. 0 = don't wait. < 0 means block forever
-        timeout_nsec = None
-        if timeout_sec is None:
-            timeout_nsec = -1
-        elif timeout_sec <= 0:
-            timeout_nsec = 0
-        else:
-            timeout_nsec = int(float(timeout_sec) * S_TO_NS)
-            timeout_timer = _WallTimer(None, None, timeout_nsec)
+        timeout_nsec = timeout_sec_to_nsec(timeout_sec)
+        if timeout_nsec > 0:
+            timeout_timer = WallTimer(None, None, timeout_nsec)
 
         if nodes is None:
             nodes = self.get_nodes()
@@ -304,94 +290,75 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                 timers.append(timeout_timer)
 
             # Construct a wait set
-            with _WaitSet() as wait_set:
-                _rclpy.rclpy_wait_set_init(
-                    wait_set,
-                    len(subscriptions),
-                    len(guards) + 2,
-                    len(timers),
-                    len(clients),
-                    len(services))
-
-                entities = {
-                    'subscription': (subscriptions, 'subscription_handle'),
-                    'guard_condition': (guards, 'guard_handle'),
-                    'client': (clients, 'client_handle'),
-                    'service': (services, 'service_handle'),
-                    'timer': (timers, 'timer_handle'),
-                }
-                for entity, (handles, handle_name) in entities.items():
-                    _rclpy.rclpy_wait_set_clear_entities(entity, wait_set)
-                    for h in handles:
-                        _rclpy.rclpy_wait_set_add_entity(
-                            entity, wait_set, h.__getattribute__(handle_name)
-                        )
-                _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, sigint_gc)
-                _rclpy.rclpy_wait_set_add_entity(
-                    'guard_condition', wait_set, self._guard_condition)
+            with WaitSet() as wait_set:
+                wait_set.add_subscriptions(subscriptions)
+                wait_set.add_clients(clients)
+                wait_set.add_services(services)
+                wait_set.add_timers(timers)
+                wait_set.add_guard_conditions(guards)
+                wait_set.add_guard_condition(sigint_gc, sigint_gc_handle)
+                wait_set.add_guard_condition(self._guard_condition, self._guard_condition_handle)
 
                 # Wait for something to become ready
-                _rclpy.rclpy_wait(wait_set, timeout_nsec)
-
-                # get ready entities
-                subs_ready = _rclpy.rclpy_get_ready_entities('subscription', wait_set)
-                guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set)
-                timers_ready = _rclpy.rclpy_get_ready_entities('timer', wait_set)
-                clients_ready = _rclpy.rclpy_get_ready_entities('client', wait_set)
-                services_ready = _rclpy.rclpy_get_ready_entities('service', wait_set)
+                wait_set.wait(timeout_nsec)
+
+                # Check sigint guard condition
+                if wait_set.is_ready(sigint_gc_handle):
+                    raise KeyboardInterrupt()
+                _rclpy.rclpy_destroy_entity(sigint_gc)
+
+                # Mark all guards as triggered before yielding since they're auto-taken
+                for gc in [g for g in guards if wait_set.is_ready(g.guard_pointer)]:
+                    gc._executor_triggered = True
+
+                # Process ready entities one node at a time
+                for node in nodes:
+                    for tmr in [t for t in timers if wait_set.is_ready(t.timer_pointer)]:
+                        # Check that a timer is ready to workaround rcl issue with cancelled timers
+                        if _rclpy.rclpy_is_timer_ready(tmr.timer_handle):
+                            if tmr == timeout_timer:
+                                continue
+                            elif tmr.callback_group.can_execute(tmr):
+                                handler = self._make_handler(
+                                    tmr, self._take_timer, self._execute_timer)
+                                yielded_work = True
+                                yield handler, tmr, node
+
+                    for sub in [s for s in subscriptions if wait_set.is_ready(
+                            s.subscription_pointer)]:
+                        if sub.callback_group.can_execute(sub):
+                            handler = self._make_handler(
+                                sub, self._take_subscription, self._execute_subscription)
+                            yielded_work = True
+                            yield handler, sub, node
 
-            # Check sigint guard condition
-            if sigint_gc_handle in guards_ready:
-                raise KeyboardInterrupt
-            _rclpy.rclpy_destroy_entity(sigint_gc)
+                    for gc in [g for g in node.guards if g._executor_triggered]:
+                        if gc.callback_group.can_execute(gc):
+                            handler = self._make_handler(
+                                gc, self._take_guard_condition, self._execute_guard_condition)
+                            yielded_work = True
+                            yield handler, gc, node
 
-            # Mark all guards as triggered before yielding any handlers since they're auto-taken
-            for gc in [g for g in guards if g.guard_pointer in guards_ready]:
-                gc._executor_triggered = True
+                    for client in [c for c in clients if wait_set.is_ready(c.client_pointer)]:
+                        if client.callback_group.can_execute(client):
+                            handler = self._make_handler(
+                                client, self._take_client, self._execute_client)
+                            yielded_work = True
+                            yield handler, client, node
 
-            # Process ready entities one node at a time
-            for node in nodes:
-                for tmr in [t for t in node.timers if t.timer_pointer in timers_ready]:
-                    # Check that a timer is ready to workaround rcl issue with cancelled timers
-                    if _rclpy.rclpy_is_timer_ready(tmr.timer_handle):
-                        if tmr.callback_group.can_execute(tmr):
+                    for srv in [s for s in services if wait_set.is_ready(s.service_pointer)]:
+                        if srv.callback_group.can_execute(srv):
                             handler = self._make_handler(
-                                tmr, self._take_timer, self._execute_timer)
+                                srv, self._take_service, self._execute_service)
                             yielded_work = True
-                            yield handler, tmr, node
-
-                for sub in [s for s in node.subscriptions if s.subscription_pointer in subs_ready]:
-                    if sub.callback_group.can_execute(sub):
-                        handler = self._make_handler(
-                            sub, self._take_subscription, self._execute_subscription)
-                        yielded_work = True
-                        yield handler, sub, node
-
-                for gc in [g for g in node.guards if g._executor_triggered]:
-                    if gc.callback_group.can_execute(gc):
-                        handler = self._make_handler(
-                            gc, self._take_guard_condition, self._execute_guard_condition)
-                        yielded_work = True
-                        yield handler, gc, node
-
-                for client in [c for c in node.clients if c.client_pointer in clients_ready]:
-                    if client.callback_group.can_execute(client):
-                        handler = self._make_handler(
-                            client, self._take_client, self._execute_client)
-                        yielded_work = True
-                        yield handler, client, node
-
-                for srv in [s for s in node.services if s.service_pointer in services_ready]:
-                    if srv.callback_group.can_execute(srv):
-                        handler = self._make_handler(
-                            srv, self._take_service, self._execute_service)
-                        yielded_work = True
-                        yield handler, srv, node
-
-            # Check timeout timer
-            if (timeout_nsec == 0 or
-                    (timeout_timer is not None and timeout_timer.timer_pointer in timers_ready)):
-                break
+                            yield handler, srv, node
+
+                # Check timeout timer
+                if (
+                    timeout_nsec == 0 or
+                    (timeout_timer is not None and wait_set.is_ready(timeout_timer.timer_pointer))
+                ):
+                    break
 
 
 class SingleThreadedExecutor(Executor):
@@ -426,7 +393,7 @@ def __init__(self, num_threads=None):
                 num_threads = multiprocessing.cpu_count()
             except NotImplementedError:
                 num_threads = 1
-        self._executor = _ThreadPoolExecutor(num_threads)
+        self._executor = ThreadPoolExecutor(num_threads)
 
     def spin_once(self, timeout_sec=None):
         try: