From 372c1a9cfb02719096d9e027db8a0954d461316e Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 10 Oct 2017 13:30:51 -0700
Subject: [PATCH 01/49] Dirty: First proof of concept for wait_for_service
 implementation

---
 rclpy/rclpy/client.py         |  17 ++++
 rclpy/rclpy/executors.py      |  12 +--
 rclpy/rclpy/graph_listener.py | 181 ++++++++++++++++++++++++++++++++++
 rclpy/rclpy/wait_set.py       |  26 +++++
 rclpy/src/rclpy/_rclpy.c      |  87 ++++++++++++++++
 5 files changed, 312 insertions(+), 11 deletions(-)
 create mode 100644 rclpy/rclpy/graph_listener.py
 create mode 100644 rclpy/rclpy/wait_set.py

diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py
index 15bd45151..3b3deaeae 100644
--- a/rclpy/rclpy/client.py
+++ b/rclpy/rclpy/client.py
@@ -14,6 +14,7 @@
 
 import threading
 
+from rclpy.graph_listener import GraphEventSubscription as _GraphEventSubscription
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 import rclpy.utilities
 
@@ -77,3 +78,19 @@ 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.
+
+        Return true if the service is available.
+
+        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :type timeout_sec: float or None
+        :rtype: bool
+        """
+        with _GraphEventSubscription(self.node_handle) as event_sub:
+            return event_sub.wait_for(self.service_is_ready, timeout_sec)
diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 7825ac8aa..607e1b2b9 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -21,17 +21,7 @@
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.timer import WallTimer as _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.wait_set import WaitSet as _WaitSet
 
 
 class _WorkTracker:
diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
new file mode 100644
index 000000000..2ecbc905e
--- /dev/null
+++ b/rclpy/rclpy/graph_listener.py
@@ -0,0 +1,181 @@
+# Copyright 2017 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 threading
+
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.wait_set import WaitSet as _WaitSet
+
+
+class GraphListenerSingleton:
+    """
+    Manage a thread to listen for graph events.
+
+    This class manages a single thread to listen for graph event updates.
+    Every node has a graph event guard condition associated with it.
+    It is triggered when a publisher, service, etc either becomes or stops being available.
+    These guard conditions could be waited on by the executor, but that would mean deadlock if
+    all executor threads are waiting for graph events (e.g. by calling wait_for_service() in a
+    subscriber callback).
+    This listener enables executor threads to be blocked for graph events without deadlock.
+    """
+
+    def __new__(cls, *args, **kwargs):
+        if not hasattr(cls, "__singleton"):
+            setattr(cls, "__singleton", super().__new__(cls, *args, **kwargs))
+        return getattr(cls, "__singleton")
+
+    def __init__(self):
+        # Maps node_handle to a list of subscriber threading.Condition
+        self._subscriptions = {}
+        self._gc, self._gc_handle = _rclpy.rclpy_create_guard_condition()
+        self._thread = None
+        self._lock = threading.Lock()
+
+    def __del__(self):
+        with self._lock:
+            assert not self._subscriptions
+            th = self._thread
+        if th:
+            th.join()
+        _rclpy.rclpy_destroy_entity('guard_condition', self._gc)
+
+    def add_node(self, node_handle):
+        """
+        Listen for graph updates on the given node.
+
+        :param node_handle: rclpy node handle
+        :type node_handle: PyCapsule
+        :rtype: threading.Condition
+        """
+        with self._lock:
+            if self._thread is None:
+                self._thread = threading.Thread(target=self._runner)
+                self._thread.daemon = True
+                self._thread.start()
+
+            condition = threading.Condition()
+
+            if node_handle not in self._subscriptions:
+                self._subscriptions[node_handle] = [condition]
+                # signal thread to rebuild wait list
+                _rclpy.rclpy_trigger_guard_condition(self._gc)
+            else:
+                self._subscriptions[node_handle].append(condition)
+
+            return condition
+
+    def remove_condition(self, node_handle, condition):
+        """
+        Stop listening for graph updates for the given node and condition.
+
+        :param node_handle: rclpy node handle
+        :type node_handle: PyCapsule
+        :param condition: the condition object used by the subscriber
+        :type condition: threading.Condition
+        """
+        with self._lock:
+            assert node_handle in self._subscriptions
+
+            condition_list = self._subscriptions[node_handle]
+            condition_list.remove(condition)
+
+            if not condition_list:
+                # last subscriber for this node, remove the node
+                del self._subscriptions[node_handle]
+
+                # tell the thread to rebuild the wait set
+                _rclpy.rclpy_trigger_guard_condition(self._gc)
+
+    def _runner(self):
+        while True:
+            # Map guard condition handles to node_handle
+            gc_handle_to_node_handle = {self._gc_handle: None}
+            guard_conditions = [self._gc]
+            with self._lock:
+                if not self._subscriptions:
+                    # nothing to wait on, kill the thread
+                    self._thread = None
+                    return
+                for node_handle in self._subscriptions.keys():
+                    gc, gc_handle = _rclpy.rclpy_get_graph_guard_condition(node_handle)
+                    gc_handle_to_node_handle[gc_handle] = node_handle
+                    guard_conditions.append(gc)
+
+            # Build a wait set
+            guards_ready = []
+            with _WaitSet() as wait_set:
+                _rclpy.rclpy_wait_set_init(
+                    wait_set,
+                    0,
+                    len(guard_conditions),
+                    0,
+                    0,
+                    0)
+                for gc in guard_conditions:
+                    _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, gc)
+
+                # Wait forever
+                _rclpy.rclpy_wait(wait_set, -1)
+                guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set)
+
+            # notify graph event subscribers
+            with self._lock:
+                for gc_handle in guards_ready:
+                    node_handle = gc_handle_to_node_handle[gc_handle]
+                    if node_handle in self._subscriptions:
+                        for condition in self._subscriptions[node_handle]:
+                            condition.acquire()
+                            try:
+                                condition.notify_all()
+                            finally:
+                                condition.release()
+
+
+class GraphEventSubscription:
+    """Manage a subscription to graph event updates."""
+
+    def __init__(self, node_handle):
+        self._listener = GraphListenerSingleton()
+        self._node_handle = node_handle
+        self._condition = None
+
+    def __enter__(self):
+        self._condition = self._listener.add_node(self._node_handle)
+        return self
+
+    def __exit__(self, t, v, tb):
+        self._listener.remove_condition(self._node_handle, self._condition)
+
+    def wait_for(self, predicate, timeout_sec=None):
+        """
+        Wait for either the predicate to return true, or timeout.
+
+        The predicate is called at the beginning, and every time the graph is updated.
+
+        :param predicate: a callable that returns a boolean
+        :type predicate: callable
+        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :type timeout_sec: float or None
+        :rtype: the returned value of the last call to the predicate
+        """
+        result = predicate()
+        if result or (timeout_sec is not None and timeout_sec <= 0):
+            return result
+
+        self._condition.acquire()
+        try:
+            return self._condition.wait_for(predicate, timeout=timeout_sec)
+        finally:
+            self._condition.release()
diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py
new file mode 100644
index 000000000..2b294353a
--- /dev/null
+++ b/rclpy/rclpy/wait_set.py
@@ -0,0 +1,26 @@
+# Copyright 2017 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.
+
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+
+
+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)
diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index d6a4e4a75..bf120b51f 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -44,6 +44,43 @@ static void catch_function(int signo)
   }
 }
 
+/// Get a guard condition for node graph events
+/**
+ * Raises TypeError if the provided argument is not a PyCapsule.
+ *
+ * A successful call will return a list with two elements:
+ *
+ * - a Capsule with the pointer of the retrieved rcl_guard_condition_t * structure
+ * - an integer representing the memory address of the rcl_guard_condition_t
+ *
+ * \param[in] A capsule containing rcl_node_t *
+ * \return a list with the capsule and memory location, or
+ * \return NULL on failure
+ */
+static PyObject *
+rclpy_get_graph_guard_condition(PyObject * Py_UNUSED(self), PyObject * args)
+{
+  PyObject * pynode;
+
+  if (!PyArg_ParseTuple(args, "O", &pynode)) {
+    return NULL;
+  }
+  if (!PyCapsule_IsValid(pynode, NULL)) {
+    PyErr_Format(PyExc_TypeError, "Expected a PyCapsule as an argument");
+    return NULL;
+  }
+
+  rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, NULL);
+  rcl_guard_condition_t * guard_condition =
+    (rcl_guard_condition_t *)rcl_node_get_graph_guard_condition(node);
+
+  PyObject * pylist = PyList_New(2);
+  PyList_SET_ITEM(pylist, 0, PyCapsule_New(guard_condition, NULL, NULL));
+  PyList_SET_ITEM(pylist, 1, PyLong_FromUnsignedLongLong((uint64_t)&guard_condition->impl));
+
+  return pylist;
+}
+
 /// Create a sigint guard condition
 /**
  * A successful call will return a list with two elements:
@@ -1525,6 +1562,47 @@ rclpy_send_response(PyObject * Py_UNUSED(self), PyObject * args)
   Py_RETURN_NONE;
 }
 
+/// Check if a service server is available
+/**
+ * Raises ValueError if the arguments are not capsules
+ *
+ * \param[in] pynode Capsule pointing to the node the entity belongs to
+ * \param[in] pyclient Capsule pointing to the client
+ * \return True if the service server is available
+ */
+static PyObject *
+rclpy_service_server_is_available(PyObject * Py_UNUSED(self), PyObject * args)
+{
+  PyObject * pynode;
+  PyObject * pyclient;
+
+  if (!PyArg_ParseTuple(args, "OO", &pynode, &pyclient)) {
+    return NULL;
+  }
+  if (!PyCapsule_CheckExact(pynode) || !PyCapsule_CheckExact(pyclient)) {
+    PyErr_Format(PyExc_ValueError, "Expected two PyCapsule as arugments");
+    return NULL;
+  }
+
+  rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, NULL);
+  rcl_client_t * client = (rcl_client_t *)PyCapsule_GetPointer(pyclient, NULL);
+
+  bool is_ready;
+  rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready);
+
+  if (ret != RCL_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError,
+      "Failed to check service availability: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+    return NULL;
+  }
+
+  if (is_ready) {
+    Py_RETURN_TRUE;
+  }
+  Py_RETURN_FALSE;
+}
+
 /// Destroy an entity attached to a node
 /**
  * Entity type must be one of ["subscription", "publisher", "client", "service"].
@@ -2608,6 +2686,15 @@ static PyMethodDef rclpy_methods[] = {
     "Create a Timer."
   },
 
+  {
+    "rclpy_service_server_is_available", rclpy_service_server_is_available, METH_VARARGS,
+    "Return true if the service server is available"
+  },
+
+  {
+    "rclpy_get_graph_guard_condition", rclpy_get_graph_guard_condition, METH_VARARGS,
+    "Get a guard condition that is triggered when the node graph updates."
+  },
   {
     "rclpy_get_sigint_guard_condition", rclpy_get_sigint_guard_condition, METH_NOARGS,
     "Create a guard_condition triggered when sigint is received."

From 817990a32828e921b2128722ab6f5e911e4b6c73 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 09:03:45 -0700
Subject: [PATCH 02/49] Moved wait set code to its own class for code reuse

---
 rclpy/rclpy/executors.py |  65 ++++++-----------
 rclpy/rclpy/wait_set.py  | 148 +++++++++++++++++++++++++++++++++++++--
 2 files changed, 164 insertions(+), 49 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 607e1b2b9..2b0da22f2 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -294,63 +294,41 @@ 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)
-
-                # 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 = _WaitSet()
+            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
+            wait_set.wait(timeout_nsec)
 
             # Check sigint guard condition
-            if sigint_gc_handle in guards_ready:
+            if wait_set.is_ready(sigint_gc_handle):
                 raise KeyboardInterrupt
             _rclpy.rclpy_destroy_entity(sigint_gc)
 
             # 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]:
+            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 node.timers if t.timer_pointer in timers_ready]:
+                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.callback_group.can_execute(tmr):
+                        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 node.subscriptions if s.subscription_pointer in subs_ready]:
+                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)
@@ -364,14 +342,14 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                         yielded_work = True
                         yield handler, gc, node
 
-                for client in [c for c in node.clients if c.client_pointer in clients_ready]:
+                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
 
-                for srv in [s for s in node.services if s.service_pointer in services_ready]:
+                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(
                             srv, self._take_service, self._execute_service)
@@ -380,7 +358,8 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
 
             # Check timeout timer
             if (timeout_nsec == 0 or
-                    (timeout_timer is not None and timeout_timer.timer_pointer in timers_ready)):
+                    (timeout_timer is not None and wait_set.is_ready(
+                        timeout_timer.timer_pointer))):
                 break
 
 
diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py
index 2b294353a..1d852b645 100644
--- a/rclpy/rclpy/wait_set.py
+++ b/rclpy/rclpy/wait_set.py
@@ -12,15 +12,151 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import threading
+
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 
 
 class WaitSet:
-    """Make sure the wait set gets destroyed when a generator exits."""
+    """Provide conveneint methods and destroy the wait set when garbage collected."""
+
+    def __init__(self):
+        # maps entity pointers (the python integer, not the PyCapsule) to a bool
+        self._ready_pointers = {}
+
+        # maps pointers (integers) to handles (PyCapsule)
+        self._subscriptions = {}
+        self._guard_conditions = {}
+        self._timers = {}
+        self._clients = {}
+        self._services = {}
+
+        # Set when the wait set needs to be built or rebuilt
+        self._needs_building = True
+        self._wait_set = None
+        # rcl_wait is not thread safe, so prevent multiple wait calls at once
+        self._wait_lock = threading.Lock()
+
+    def __del__(self):
+        if self._wait_set is not None:
+            _rclpy.rclpy_destroy_wait_set(self._wait_set)
+
+    def add_subscription(self, subscription_handle, subscription_pointer):
+        self._subscriptions[subscription_pointer] = subscription_handle
+        self._ready_pointers[subscription_pointer] = False
+        self._needs_building = True
+
+    def remove_subscription(self, subscription_pointer):
+        del self._subscriptions[subscription_pointer]
+        del self._ready_pointers[subscription_pointer]
+        self._needs_building = True
+
+    def add_subscriptions(self, subscriptions):
+        for sub in subscriptions:
+            self.add_subscription(sub.subscription_handle, sub.subscription_pointer)
+
+    def add_guard_condition(self, gc_handle, gc_pointer):
+        self._guard_conditions[gc_pointer] = gc_handle
+        self._ready_pointers[gc_pointer] = False
+        self._needs_building = True
+
+    def add_guard_conditions(self, guards):
+        for gc in guards:
+            self.add_guard_condition(gc.guard_handle, gc.guard_pointer)
+
+    def remove_guard_condition(self, gc_pointer):
+        del self._guard_conditions[gc_pointer]
+        del self._ready_pointers[gc_pointer]
+        self._needs_building = True
+
+    def add_timer(self, timer_handle, timer_pointer):
+        self._timers[timer_pointer] = timer_handle
+        self._ready_pointers[timer_pointer] = False
+        self._needs_building = True
+
+    def add_timers(self, timers):
+        for tmr in timers:
+            self.add_timer(tmr.timer_handle, tmr.timer_pointer)
+
+    def remove_timer(self, timer_pointer):
+        del self._timers[timer_pointer]
+        del self._ready_pointers[timer_pointer]
+        self._needs_building = True
+
+    def add_client(self, client_handle, client_pointer):
+        self._clients[client_pointer] = client_handle
+        self._ready_pointers[client_pointer] = False
+        self._needs_building = True
+
+    def add_clients(self, clients):
+        for cli in clients:
+            self.add_client(cli.client_handle, cli.client_pointer)
+
+    def remove_client(self, client_pointer):
+        del self._clients[client_pointer]
+        del self._ready_pointers[client_pointer]
+        self._needs_building = True
+
+    def add_service(self, service_handle, service_pointer):
+        self._services[service_pointer] = service_handle
+        self._ready_pointers[service_pointer] = False
+        self._needs_building = True
+
+    def add_services(self, services):
+        for srv in services:
+            self.add_service(srv.service_handle, srv.service_pointer)
+
+    def remove_service(self, service_pointer):
+        del self._services[service_pointer]
+        del self._ready_pointers[service_pointer]
+        self._needs_building = True
+
+    def wait(self, timeout_nsec):
+        with self._wait_lock:
+            if self._needs_building:
+                if self._wait_set is not None:
+                    _rclpy.rclpy_destroy_wait_set(self._wait_set)
+
+                self._wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
+                _rclpy.rclpy_wait_set_init(
+                    self._wait_set,
+                    len(self._subscriptions),
+                    len(self._guard_conditions),
+                    len(self._timers),
+                    len(self._clients),
+                    len(self._services))
+
+                _rclpy.rclpy_wait_set_clear_entities('subscription', self._wait_set)
+                _rclpy.rclpy_wait_set_clear_entities('guard_condition', self._wait_set)
+                _rclpy.rclpy_wait_set_clear_entities('timer', self._wait_set)
+                _rclpy.rclpy_wait_set_clear_entities('client', self._wait_set)
+                _rclpy.rclpy_wait_set_clear_entities('service', self._wait_set)
+
+                for handle in self._subscriptions.values():
+                    _rclpy.rclpy_wait_set_add_entity('subscription', self._wait_set, handle)
+                for handle in self._guard_conditions.values():
+                    _rclpy.rclpy_wait_set_add_entity('guard_condition', self._wait_set, handle)
+                for handle in self._timers.values():
+                    _rclpy.rclpy_wait_set_add_entity('timer', self._wait_set, handle)
+                for handle in self._clients.values():
+                    _rclpy.rclpy_wait_set_add_entity('client', self._wait_set, handle)
+                for handle in self._services.values():
+                    _rclpy.rclpy_wait_set_add_entity('service', self._wait_set, handle)
+
+            _rclpy.rclpy_wait(self._wait_set, timeout_nsec)
+
+            ws = self._wait_set
+            sub = [(e, True) for e in _rclpy.rclpy_get_ready_entities('subscription', ws)]
+            grd = [(e, True) for e in _rclpy.rclpy_get_ready_entities('guard_condition', ws)]
+            tmr = [(e, True) for e in _rclpy.rclpy_get_ready_entities('timer', ws)]
+            cli = [(e, True) for e in _rclpy.rclpy_get_ready_entities('client', ws)]
+            srv = [(e, True) for e in _rclpy.rclpy_get_ready_entities('service', ws)]
 
-    def __enter__(self):
-        self.wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
-        return self.wait_set
+        self._ready_pointers.update(sub)
+        self._ready_pointers.update(grd)
+        self._ready_pointers.update(tmr)
+        self._ready_pointers.update(cli)
+        self._ready_pointers.update(srv)
 
-    def __exit__(self, t, v, tb):
-        _rclpy.rclpy_destroy_wait_set(self.wait_set)
+    def is_ready(self, entity_pointer):
+        return self._ready_pointers[entity_pointer]

From 9b91a62478827137bcf43739ae5f4c4cd0d34db3 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 09:04:21 -0700
Subject: [PATCH 03/49] Added timeout_sec_to_nsec()

---
 rclpy/rclpy/executors.py | 11 +++--------
 rclpy/rclpy/utilities.py | 14 ++++++++++++++
 2 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 2b0da22f2..659b86c48 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -17,10 +17,10 @@
 from threading import Condition as _Condition
 from threading import Lock as _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.utilities import ok
+from rclpy.utilities import timeout_sec_to_nsec
 from rclpy.wait_set import WaitSet as _WaitSet
 
 
@@ -258,13 +258,8 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
         """
         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_nsec = timeout_sec_to_nsec(timeout_sec)
+        if timeout_nsec > 0:
             timeout_timer = _WallTimer(None, None, timeout_nsec)
 
         if nodes is None:
diff --git a/rclpy/rclpy/utilities.py b/rclpy/rclpy/utilities.py
index 00de43212..eb5597b94 100644
--- a/rclpy/rclpy/utilities.py
+++ b/rclpy/rclpy/utilities.py
@@ -14,6 +14,7 @@
 
 import threading
 
+from rclpy.constants import S_TO_NS
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 
 g_shutdown_lock = threading.Lock()
@@ -38,3 +39,16 @@ def try_shutdown():
 
 def get_rmw_implementation_identifier():
     return _rclpy.rclpy_get_rmw_implementation_identifier()
+
+
+def timeout_sec_to_nsec(timeout_sec):
+    """Convert timeout in seconds to rcl compatible timeout in nanoseconds."""
+    if timeout_sec is None:
+        # Block forever
+        return -1
+    elif timeout_sec <= 0:
+        # Return immediately
+        return 0
+    else:
+        # wait for given time
+        return int(float(timeout_sec) * S_TO_NS)

From 371c9179a3ddd6a11b9a71ab9cd09db30fc4a255 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 09:04:57 -0700
Subject: [PATCH 04/49] wait_for_service() implemented with timers

---
 rclpy/rclpy/client.py         |  29 +++-
 rclpy/rclpy/graph_listener.py | 242 ++++++++++++++++++----------------
 2 files changed, 153 insertions(+), 118 deletions(-)

diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py
index 3b3deaeae..5e8f9784c 100644
--- a/rclpy/rclpy/client.py
+++ b/rclpy/rclpy/client.py
@@ -86,11 +86,30 @@ def wait_for_service(self, timeout_sec=None):
         """
         Block until the service is available.
 
-        Return true if the service is available.
-
         :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
         :type timeout_sec: float or None
-        :rtype: bool
+        :rtype: bool true if the service is available
         """
-        with _GraphEventSubscription(self.node_handle) as event_sub:
-            return event_sub.wait_for(self.service_is_ready, timeout_sec)
+        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
diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index 2ecbc905e..c813e4724 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -13,169 +13,185 @@
 # limitations under the License.
 
 import threading
+import traceback
 
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.timer import WallTimer as _WallTimer
 from rclpy.wait_set import WaitSet as _WaitSet
 
 
 class GraphListenerSingleton:
-    """
-    Manage a thread to listen for graph events.
-
-    This class manages a single thread to listen for graph event updates.
-    Every node has a graph event guard condition associated with it.
-    It is triggered when a publisher, service, etc either becomes or stops being available.
-    These guard conditions could be waited on by the executor, but that would mean deadlock if
-    all executor threads are waiting for graph events (e.g. by calling wait_for_service() in a
-    subscriber callback).
-    This listener enables executor threads to be blocked for graph events without deadlock.
-    """
+    """Manage a thread to listen for graph events."""
 
     def __new__(cls, *args, **kwargs):
-        if not hasattr(cls, "__singleton"):
-            setattr(cls, "__singleton", super().__new__(cls, *args, **kwargs))
-        return getattr(cls, "__singleton")
+        if not hasattr(cls, '__singleton'):
+            setattr(cls, '__singleton', super().__new__(cls, *args, **kwargs))
+        return getattr(cls, '__singleton')
 
     def __init__(self):
-        # Maps node_handle to a list of subscriber threading.Condition
-        self._subscriptions = {}
-        self._gc, self._gc_handle = _rclpy.rclpy_create_guard_condition()
+        # Maps guard_condition pointers to a list of subscriber callbacks
+        self._callbacks = {}
+        # Maps timer instances to timer callbacks
+        self._timers = {}
+        self._gc_handle, self._gc_pointer = _rclpy.rclpy_create_guard_condition()
         self._thread = None
-        self._lock = threading.Lock()
+        self._lock = threading.RLock()
+        self._wait_set = _WaitSet()
+        self._wait_set.add_guard_condition(self._gc_handle, self._gc_pointer)
 
     def __del__(self):
         with self._lock:
-            assert not self._subscriptions
             th = self._thread
+            self._thread = None
         if th:
             th.join()
-        _rclpy.rclpy_destroy_entity('guard_condition', self._gc)
+        _rclpy.rclpy_destroy_entity('guard_condition', self._gc_handle)
 
-    def add_node(self, node_handle):
+    def _try_start_thread(self):
+        # Assumes lock is already held
+        if self._thread is None:
+            self._thread = threading.Thread(target=self._runner)
+            self._thread.daemon = True
+            self._thread.start()
+
+    def add_timer(self, timer_period_ns, callback):
+        """
+        Call callback when timer triggers.
+
+        :param timer_period_ns: time until timer triggers in nanoseconds
+        :type timer_period_ns: integer
+        :param callback: called when the graph updates
+        :type callback: callable
+        :rtype: rclpy.timer.WallTimer instance
+        """
+        with self._lock:
+            tmr = _WallTimer(callback, None, timer_period_ns)
+            self._timers[tmr] = callback
+            self._wait_set.add_timer(tmr.timer_handle, tmr.timer_pointer)
+
+            self._try_start_thread()
+            return tmr
+
+    def remove_timer(self, timer):
         """
-        Listen for graph updates on the given node.
+        Remove a timer from the wait set.
+
+        :param timer: A timer returned from add_timer()
+        :type timer: rclpy.timer.WallTimer instance
+        """
+        with self._lock:
+            if timer in self._timers:
+                del self._timers[timer]
+                self._wait_set.remove_timer(timer.timer_handle, timer.timer_pointer)
+
+    def add_callback(self, node_handle, callback):
+        """
+        Call callback when node graph gets updates.
 
         :param node_handle: rclpy node handle
         :type node_handle: PyCapsule
-        :rtype: threading.Condition
+        :param callback: called when the graph updates
+        :type callback: callable
         """
         with self._lock:
+            gc_handle, gc_pointer = _rclpy.rclpy_get_graph_guard_condition(node_handle)
+            if gc_pointer not in self._callbacks:
+                # new node, rebuild wait set
+                self._callbacks[gc_pointer] = []
+                self._wait_set.add_guard_condition(gc_handle, gc_pointer)
+                _rclpy.rclpy_trigger_guard_condition(self._gc_handle)
+
+            # Add a callback
+            self._callbacks[gc_pointer].append(callback)
+
+            self._try_start_thread()
+            # start the thread if necessary
             if self._thread is None:
                 self._thread = threading.Thread(target=self._runner)
                 self._thread.daemon = True
                 self._thread.start()
 
-            condition = threading.Condition()
-
-            if node_handle not in self._subscriptions:
-                self._subscriptions[node_handle] = [condition]
-                # signal thread to rebuild wait list
-                _rclpy.rclpy_trigger_guard_condition(self._gc)
-            else:
-                self._subscriptions[node_handle].append(condition)
-
-            return condition
-
-    def remove_condition(self, node_handle, condition):
+    def remove_callback(self, node_handle, callback):
         """
-        Stop listening for graph updates for the given node and condition.
+        Stop listening for graph updates for the given node and callback.
 
         :param node_handle: rclpy node handle
         :type node_handle: PyCapsule
-        :param condition: the condition object used by the subscriber
-        :type condition: threading.Condition
+        :param callback: called when the graph updates
+        :type callback: callable
         """
         with self._lock:
-            assert node_handle in self._subscriptions
-
-            condition_list = self._subscriptions[node_handle]
-            condition_list.remove(condition)
+            gc_handle, gc_pointer = _rclpy.rclpy_get_graph_guard_condition(node_handle)
+            if gc_pointer in self._callbacks:
+                # Remove the callback
+                self._callbacks[gc_pointer].remove(callback)
 
-            if not condition_list:
-                # last subscriber for this node, remove the node
-                del self._subscriptions[node_handle]
-
-                # tell the thread to rebuild the wait set
-                _rclpy.rclpy_trigger_guard_condition(self._gc)
+                if not self._callbacks[gc_pointer]:
+                    # last subscriber for this node, remove the node and rebuild the wait set
+                    del self._callbacks[gc_pointer]
+                    self._wait_set.remove_guard_condition(gc_pointer)
+                    _rclpy.rclpy_trigger_guard_condition(self._gc_handle)
 
     def _runner(self):
         while True:
-            # Map guard condition handles to node_handle
-            gc_handle_to_node_handle = {self._gc_handle: None}
-            guard_conditions = [self._gc]
-            with self._lock:
-                if not self._subscriptions:
-                    # nothing to wait on, kill the thread
-                    self._thread = None
-                    return
-                for node_handle in self._subscriptions.keys():
-                    gc, gc_handle = _rclpy.rclpy_get_graph_guard_condition(node_handle)
-                    gc_handle_to_node_handle[gc_handle] = node_handle
-                    guard_conditions.append(gc)
-
-            # Build a wait set
-            guards_ready = []
-            with _WaitSet() as wait_set:
-                _rclpy.rclpy_wait_set_init(
-                    wait_set,
-                    0,
-                    len(guard_conditions),
-                    0,
-                    0,
-                    0)
-                for gc in guard_conditions:
-                    _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, gc)
-
-                # Wait forever
-                _rclpy.rclpy_wait(wait_set, -1)
-                guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set)
+            # Wait forever
+            self._wait_set.wait(-1)
 
             # notify graph event subscribers
             with self._lock:
-                for gc_handle in guards_ready:
-                    node_handle = gc_handle_to_node_handle[gc_handle]
-                    if node_handle in self._subscriptions:
-                        for condition in self._subscriptions[node_handle]:
-                            condition.acquire()
-                            try:
-                                condition.notify_all()
-                            finally:
-                                condition.release()
+                if not self._thread:
+                    # Asked to shut down thread
+                    return
+                ready_callbacks = []
+                # Guard conditions
+                for gc_pointer, callback_list in self._callbacks.items():
+                    if self._wait_set.is_ready(gc_pointer):
+                        for callback in callback_list:
+                            ready_callbacks.append(callback)
+                # Timers
+                for tmr, callback in self._timers.items():
+                    if self._wait_set.is_ready(tmr.timer_pointer):
+                        ready_callbacks.append(callback)
+                        _rclpy.rclpy_call_timer(tmr.timer_handle)
+                # Call callbacks
+                for callback in ready_callbacks:
+                    try:
+                        callback()
+                    except:
+                        _rclpy.logging.logwarn(traceback.format_exc())
 
 
 class GraphEventSubscription:
-    """Manage a subscription to graph event updates."""
+    """Manage subscription to node graph updates."""
 
-    def __init__(self, node_handle):
+    def __init__(self, node_handle, callback, timeout_ns=-1, timeout_callback=None):
         self._listener = GraphListenerSingleton()
         self._node_handle = node_handle
-        self._condition = None
+        self._callback = callback
+        self._listener.add_callback(self._node_handle, self._callback)
+        self._timeout_callback = timeout_callback
+        self._timer = None
+        if timeout_ns >= 0:
+            self._timer = self._listener.add_timer(timeout_ns, self.on_timeout)
+
+    def on_timeout(self):
+        if self._callback:
+            self._listener.remove_callback(self._node_handle, self._callback)
+            self._callback = None
+        self._listener.remove_timer(self.timer)
+        self._timeout_callback()
+
+    def _unsubscribe(self):
+        if self._callback:
+            self._listener.remove_callback(self._node_handle, self._callback)
+        if self._timer:
+            self._listener.remove_timer(self.timer)
+
+    def __del__(self):
+        self._unsubscribe()
 
     def __enter__(self):
-        self._condition = self._listener.add_node(self._node_handle)
         return self
 
     def __exit__(self, t, v, tb):
-        self._listener.remove_condition(self._node_handle, self._condition)
-
-    def wait_for(self, predicate, timeout_sec=None):
-        """
-        Wait for either the predicate to return true, or timeout.
-
-        The predicate is called at the beginning, and every time the graph is updated.
-
-        :param predicate: a callable that returns a boolean
-        :type predicate: callable
-        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
-        :type timeout_sec: float or None
-        :rtype: the returned value of the last call to the predicate
-        """
-        result = predicate()
-        if result or (timeout_sec is not None and timeout_sec <= 0):
-            return result
-
-        self._condition.acquire()
-        try:
-            return self._condition.wait_for(predicate, timeout=timeout_sec)
-        finally:
-            self._condition.release()
+        self._unsubscribe()

From b17f49717a3fc5078f43428b97bc8d3ea4a7dcb5 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 09:58:51 -0700
Subject: [PATCH 05/49] Added unit tests for timeout_sec_to_nsec()

---
 rclpy/rclpy/utilities.py     |  7 ++++++-
 rclpy/test/test_utilities.py | 30 ++++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+), 1 deletion(-)
 create mode 100644 rclpy/test/test_utilities.py

diff --git a/rclpy/rclpy/utilities.py b/rclpy/rclpy/utilities.py
index eb5597b94..e47c35be3 100644
--- a/rclpy/rclpy/utilities.py
+++ b/rclpy/rclpy/utilities.py
@@ -42,7 +42,12 @@ def get_rmw_implementation_identifier():
 
 
 def timeout_sec_to_nsec(timeout_sec):
-    """Convert timeout in seconds to rcl compatible timeout in nanoseconds."""
+    """
+    Convert timeout in seconds to rcl compatible timeout in nanoseconds.
+
+    Python tends to use floating point numbers in seconds for timeouts. This utility converts a
+    python-style timeout to an integer in nanoseconds that can be used by rcl_wait.
+    """
     if timeout_sec is None:
         # Block forever
         return -1
diff --git a/rclpy/test/test_utilities.py b/rclpy/test/test_utilities.py
new file mode 100644
index 000000000..53599e66d
--- /dev/null
+++ b/rclpy/test/test_utilities.py
@@ -0,0 +1,30 @@
+# Copyright 2017 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.utilities
+
+
+class TestUtilities(unittest.TestCase):
+
+    def test_timeout_sec_to_nsec(self):
+        self.assertGreater(0, rclpy.utilities.timeout_sec_to_nsec(None))
+        self.assertEqual(0, rclpy.utilities.timeout_sec_to_nsec(0))
+        self.assertEqual(0, rclpy.utilities.timeout_sec_to_nsec(-1))
+        self.assertEqual(1000000000, rclpy.utilities.timeout_sec_to_nsec(1))
+
+
+if __name__ == '__main__':
+    unittest.main()

From 35898a697f7fdfb73a5647780d4e916aa049c3dc Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 10:53:14 -0700
Subject: [PATCH 06/49] Added test for WaitSet class

---
 rclpy/test/test_wait_set.py | 118 ++++++++++++++++++++++++++++++++++++
 1 file changed, 118 insertions(+)
 create mode 100644 rclpy/test/test_wait_set.py

diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
new file mode 100644
index 000000000..ee6d7beec
--- /dev/null
+++ b/rclpy/test/test_wait_set.py
@@ -0,0 +1,118 @@
+# Copyright 2017 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
+
+from rcl_interfaces.srv import GetParameters
+import rclpy
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.wait_set import WaitSet
+from std_msgs.msg import String
+
+
+class TestWaitSet(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+        cls.node = rclpy.create_node('TestWaitSet', namespace='/')
+
+    @classmethod
+    def tearDownClass(cls):
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def test_guard_condition_ready(self):
+        ws = WaitSet()
+        gc_handle, gc_pointer = _rclpy.rclpy_create_guard_condition()
+        try:
+            ws.add_guard_condition(gc_handle, gc_pointer)
+            self.assertFalse(ws.is_ready(gc_pointer))
+
+            ws.wait(1)
+            self.assertFalse(ws.is_ready(gc_pointer))
+
+            _rclpy.rclpy_trigger_guard_condition(gc_handle)
+            # TODO(sloretz) why does the next assertion fail with wait(0)?
+            ws.wait(1)
+            self.assertTrue(ws.is_ready(gc_pointer))
+        finally:
+            _rclpy.rclpy_destroy_entity('guard_condition', gc_handle)
+
+    def test_timer_ready(self):
+        ws = WaitSet()
+        timer_handle, timer_pointer = _rclpy.rclpy_create_timer(100000)
+        try:
+            ws.add_timer(timer_handle, timer_pointer)
+            self.assertFalse(ws.is_ready(timer_pointer))
+
+            ws.wait(1)
+            self.assertFalse(ws.is_ready(timer_pointer))
+
+            ws.wait(100000)
+            self.assertTrue(ws.is_ready(timer_pointer))
+        finally:
+            _rclpy.rclpy_destroy_entity('timer', timer_handle)
+
+    def test_subscriber_ready(self):
+        ws = WaitSet()
+        sub = self.node.create_subscription(String, 'chatter', lambda msg: print(msg))
+        pub = self.node.create_publisher(String, 'chatter')
+
+        try:
+            ws.add_subscription(sub.subscription_handle, sub.subscription_pointer)
+            self.assertFalse(ws.is_ready(sub.subscription_pointer))
+
+            ws.wait(1)
+            self.assertFalse(ws.is_ready(sub.subscription_pointer))
+
+            msg = String()
+            msg.data = "Hello World"
+            pub.publish(msg)
+
+            ws.wait(5000000000)
+            self.assertTrue(ws.is_ready(sub.subscription_pointer))
+        finally:
+            self.node.destroy_publisher(pub)
+            self.node.destroy_subscription(sub)
+
+    def test_server_ready(self):
+        ws = WaitSet()
+        cli = self.node.create_client(GetParameters, 'get/parameters')
+        srv = self.node.create_service(
+            GetParameters, 'get/parameters', lambda req: GetParameters.Response())
+
+        try:
+            ws.add_client(cli.client_handle, cli.client_pointer)
+            ws.add_service(srv.service_handle, srv.service_pointer)
+            self.assertFalse(ws.is_ready(cli.client_pointer))
+            self.assertFalse(ws.is_ready(srv.service_pointer))
+
+            ws.wait(1)
+            self.assertFalse(ws.is_ready(cli.client_pointer))
+            self.assertFalse(ws.is_ready(srv.service_pointer))
+
+            cli.wait_for_service()
+            cli.call(GetParameters.Request())
+
+            ws.wait(5000000000)
+            # TODO(sloretz) test client after it's wait_for_future() API is sorted out
+            self.assertTrue(ws.is_ready(srv.service_pointer))
+        finally:
+            self.node.destroy_client(cli)
+            self.node.destroy_service(srv)
+
+
+if __name__ == '__main__':
+    unittest.main()

From 5b1ca62bee91ff9af49e44213f227a2370091857 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 11:04:54 -0700
Subject: [PATCH 07/49] Use negative timeouts to mean block forever

---
 rclpy/rclpy/executors.py     | 11 ++++++-----
 rclpy/rclpy/utilities.py     |  5 ++++-
 rclpy/test/test_utilities.py |  2 +-
 3 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 659b86c48..cea8fec31 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -47,10 +47,12 @@ 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
         """
+        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:
@@ -90,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
         """
@@ -142,7 +144,7 @@ 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
         """
@@ -250,14 +252,13 @@ 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 = timeout_sec_to_nsec(timeout_sec)
         if timeout_nsec > 0:
             timeout_timer = _WallTimer(None, None, timeout_nsec)
diff --git a/rclpy/rclpy/utilities.py b/rclpy/rclpy/utilities.py
index e47c35be3..12881e968 100644
--- a/rclpy/rclpy/utilities.py
+++ b/rclpy/rclpy/utilities.py
@@ -51,9 +51,12 @@ def timeout_sec_to_nsec(timeout_sec):
     if timeout_sec is None:
         # Block forever
         return -1
-    elif timeout_sec <= 0:
+    elif timeout_sec > -1.0 / S_TO_NS and timeout_sec < 1.0 / S_TO_NS:
         # Return immediately
         return 0
+    elif timeout_sec < 0:
+        # block forever
+        return -1
     else:
         # wait for given time
         return int(float(timeout_sec) * S_TO_NS)
diff --git a/rclpy/test/test_utilities.py b/rclpy/test/test_utilities.py
index 53599e66d..c04ad63a6 100644
--- a/rclpy/test/test_utilities.py
+++ b/rclpy/test/test_utilities.py
@@ -21,8 +21,8 @@ class TestUtilities(unittest.TestCase):
 
     def test_timeout_sec_to_nsec(self):
         self.assertGreater(0, rclpy.utilities.timeout_sec_to_nsec(None))
+        self.assertGreater(0, rclpy.utilities.timeout_sec_to_nsec(-1))
         self.assertEqual(0, rclpy.utilities.timeout_sec_to_nsec(0))
-        self.assertEqual(0, rclpy.utilities.timeout_sec_to_nsec(-1))
         self.assertEqual(1000000000, rclpy.utilities.timeout_sec_to_nsec(1))
 
 

From 489bc0eb403f4cbec264c43de1db63bcae5dc268 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 11:09:37 -0700
Subject: [PATCH 08/49] Double quotes to single quotes

---
 rclpy/test/test_wait_set.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
index ee6d7beec..27c0245cc 100644
--- a/rclpy/test/test_wait_set.py
+++ b/rclpy/test/test_wait_set.py
@@ -78,7 +78,7 @@ def test_subscriber_ready(self):
             self.assertFalse(ws.is_ready(sub.subscription_pointer))
 
             msg = String()
-            msg.data = "Hello World"
+            msg.data = 'Hello World'
             pub.publish(msg)
 
             ws.wait(5000000000)

From f854466a5e9b6ce66840cdd3f4ac3b8c020012b1 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 11:25:55 -0700
Subject: [PATCH 09/49] Added wait_for_service() tests and fixed bugs it caught

---
 rclpy/rclpy/graph_listener.py | 15 +++++----
 rclpy/test/test_client.py     | 61 +++++++++++++++++++++++++++++++++++
 2 files changed, 69 insertions(+), 7 deletions(-)
 create mode 100644 rclpy/test/test_client.py

diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index c813e4724..f5ea5aa2c 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -16,6 +16,7 @@
 import traceback
 
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+import rclpy.logging
 from rclpy.timer import WallTimer as _WallTimer
 from rclpy.wait_set import WaitSet as _WaitSet
 
@@ -82,7 +83,7 @@ def remove_timer(self, timer):
         with self._lock:
             if timer in self._timers:
                 del self._timers[timer]
-                self._wait_set.remove_timer(timer.timer_handle, timer.timer_pointer)
+                self._wait_set.remove_timer(timer.timer_pointer)
 
     def add_callback(self, node_handle, callback):
         """
@@ -158,7 +159,7 @@ def _runner(self):
                     try:
                         callback()
                     except:
-                        _rclpy.logging.logwarn(traceback.format_exc())
+                        rclpy.logging.logwarn(traceback.format_exc())
 
 
 class GraphEventSubscription:
@@ -175,17 +176,17 @@ def __init__(self, node_handle, callback, timeout_ns=-1, timeout_callback=None):
             self._timer = self._listener.add_timer(timeout_ns, self.on_timeout)
 
     def on_timeout(self):
-        if self._callback:
-            self._listener.remove_callback(self._node_handle, self._callback)
-            self._callback = None
-        self._listener.remove_timer(self.timer)
         self._timeout_callback()
+        self._unsubscribe()
 
     def _unsubscribe(self):
         if self._callback:
             self._listener.remove_callback(self._node_handle, self._callback)
+            self._callback = None
         if self._timer:
-            self._listener.remove_timer(self.timer)
+            self._listener.remove_timer(self._timer)
+            self._timeout_callback = None
+            self._timer = None
 
     def __del__(self):
         self._unsubscribe()
diff --git a/rclpy/test/test_client.py b/rclpy/test/test_client.py
new file mode 100644
index 000000000..dd68c297d
--- /dev/null
+++ b/rclpy/test/test_client.py
@@ -0,0 +1,61 @@
+# Copyright 2017 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 time
+import unittest
+
+from rcl_interfaces.srv import GetParameters
+import rclpy
+
+
+TIME_FUDGE = 0.1
+
+
+class TestClient(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+        cls.node = rclpy.create_node('TestClient', namespace='/')
+
+    @classmethod
+    def tearDownClass(cls):
+        cls.node.destroy_node()
+        rclpy.shutdown()
+
+    def test_wait_for_service_5sec(self):
+        cli = self.node.create_client(GetParameters, 'get/parameters')
+        try:
+            start = time.time()
+            cli.wait_for_service(timeout_sec=5.0)
+            end = time.time()
+            self.assertGreater(5.0, end - start - TIME_FUDGE)
+            self.assertLess(5.0, end - start + TIME_FUDGE)
+        finally:
+            self.node.destroy_client(cli)
+
+    def test_wait_for_service_nowait(self):
+        cli = self.node.create_client(GetParameters, 'get/parameters')
+        try:
+            start = time.time()
+            cli.wait_for_service(timeout_sec=0)
+            end = time.time()
+            self.assertGreater(0, end - start - TIME_FUDGE)
+            self.assertLess(0, end - start + TIME_FUDGE)
+        finally:
+            self.node.destroy_client(cli)
+
+
+if __name__ == '__main__':
+    unittest.main()

From 9255e1f96180cba6f0e939b6214fcd5196739cad Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 12:49:09 -0700
Subject: [PATCH 10/49] Eliminate blind exception warning

---
 rclpy/rclpy/graph_listener.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index f5ea5aa2c..08c92af3c 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -158,7 +158,7 @@ def _runner(self):
                 for callback in ready_callbacks:
                     try:
                         callback()
-                    except:
+                    except Exception:
                         rclpy.logging.logwarn(traceback.format_exc())
 
 

From f89a222b2f7c56f0c82cbe63c105c5c96ac55a8a Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Fri, 13 Oct 2017 12:56:30 -0700
Subject: [PATCH 11/49] Reduce flakiness of test by increasing time to 0.1s

---
 rclpy/test/test_wait_set.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
index 27c0245cc..b5825708c 100644
--- a/rclpy/test/test_wait_set.py
+++ b/rclpy/test/test_wait_set.py
@@ -52,7 +52,7 @@ def test_guard_condition_ready(self):
 
     def test_timer_ready(self):
         ws = WaitSet()
-        timer_handle, timer_pointer = _rclpy.rclpy_create_timer(100000)
+        timer_handle, timer_pointer = _rclpy.rclpy_create_timer(100000000)
         try:
             ws.add_timer(timer_handle, timer_pointer)
             self.assertFalse(ws.is_ready(timer_pointer))
@@ -60,7 +60,7 @@ def test_timer_ready(self):
             ws.wait(1)
             self.assertFalse(ws.is_ready(timer_pointer))
 
-            ws.wait(100000)
+            ws.wait(100000000)
             self.assertTrue(ws.is_ready(timer_pointer))
         finally:
             _rclpy.rclpy_destroy_entity('timer', timer_handle)

From 39ac2316fb13cf69a04fb972a6c35b1efc8a8dce Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:29:30 -0700
Subject: [PATCH 12/49] Comment says negative timeouts block forever

---
 rclpy/rclpy/client.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py
index 5e8f9784c..67eaab1bb 100644
--- a/rclpy/rclpy/client.py
+++ b/rclpy/rclpy/client.py
@@ -86,7 +86,7 @@ def wait_for_service(self, timeout_sec=None):
         """
         Block until the service is available.
 
-        :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 the service is available
         """

From 3237ed84d595c56f063322cf63ead73057679f84 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:33:39 -0700
Subject: [PATCH 13/49] Use :returns:

---
 rclpy/rclpy/client.py         | 3 ++-
 rclpy/rclpy/executors.py      | 3 ++-
 rclpy/rclpy/graph_listener.py | 2 +-
 rclpy/rclpy/utilities.py      | 5 +++++
 4 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py
index 67eaab1bb..1f3d91948 100644
--- a/rclpy/rclpy/client.py
+++ b/rclpy/rclpy/client.py
@@ -88,7 +88,8 @@ def wait_for_service(self, timeout_sec=None):
 
         :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 the service is available
+        :rtype: bool
+        :returns: true if the service is available
         """
         timeout_nsec = rclpy.utilities.timeout_sec_to_nsec(timeout_sec)
         result = self.service_is_ready()
diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index cea8fec31..17cfa0a14 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -49,7 +49,8 @@ def wait(self, timeout_sec=None):
 
         :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
diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index 08c92af3c..25c9361f6 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -63,7 +63,7 @@ def add_timer(self, timer_period_ns, callback):
         :type timer_period_ns: integer
         :param callback: called when the graph updates
         :type callback: callable
-        :rtype: rclpy.timer.WallTimer instance
+        :rtype: rclpy.timer.WallTimer
         """
         with self._lock:
             tmr = _WallTimer(callback, None, timer_period_ns)
diff --git a/rclpy/rclpy/utilities.py b/rclpy/rclpy/utilities.py
index 12881e968..bbe36f0d4 100644
--- a/rclpy/rclpy/utilities.py
+++ b/rclpy/rclpy/utilities.py
@@ -47,6 +47,11 @@ def timeout_sec_to_nsec(timeout_sec):
 
     Python tends to use floating point numbers in seconds for timeouts. This utility converts a
     python-style timeout to an integer in nanoseconds that can be used by rcl_wait.
+
+    :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
+    :type timeout_sec: float or None
+    :rtype: int
+    :returns: rcl_wait compatible timeout in nanoseconds
     """
     if timeout_sec is None:
         # Block forever

From 7acb2a6897862168861e4c2b465d09929521c16f Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:35:08 -0700
Subject: [PATCH 14/49] Move add_subscriptions()

---
 rclpy/rclpy/wait_set.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py
index 1d852b645..2648431aa 100644
--- a/rclpy/rclpy/wait_set.py
+++ b/rclpy/rclpy/wait_set.py
@@ -46,15 +46,15 @@ def add_subscription(self, subscription_handle, subscription_pointer):
         self._ready_pointers[subscription_pointer] = False
         self._needs_building = True
 
+    def add_subscriptions(self, subscriptions):
+        for sub in subscriptions:
+            self.add_subscription(sub.subscription_handle, sub.subscription_pointer)
+
     def remove_subscription(self, subscription_pointer):
         del self._subscriptions[subscription_pointer]
         del self._ready_pointers[subscription_pointer]
         self._needs_building = True
 
-    def add_subscriptions(self, subscriptions):
-        for sub in subscriptions:
-            self.add_subscription(sub.subscription_handle, sub.subscription_pointer)
-
     def add_guard_condition(self, gc_handle, gc_pointer):
         self._guard_conditions[gc_pointer] = gc_handle
         self._ready_pointers[gc_pointer] = False

From 4720d3f7e0492d75b89714b0fa5fcd848e639914 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:36:06 -0700
Subject: [PATCH 15/49] arugments -> arguments

---
 rclpy/src/rclpy/_rclpy.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index bf120b51f..c274729ca 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -1580,7 +1580,7 @@ rclpy_service_server_is_available(PyObject * Py_UNUSED(self), PyObject * args)
     return NULL;
   }
   if (!PyCapsule_CheckExact(pynode) || !PyCapsule_CheckExact(pyclient)) {
-    PyErr_Format(PyExc_ValueError, "Expected two PyCapsule as arugments");
+    PyErr_Format(PyExc_ValueError, "Expected two PyCapsule as arguments");
     return NULL;
   }
 

From 395934a2c38aebd82a84fc05bc90b671a4a67562 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:43:23 -0700
Subject: [PATCH 16/49] Daemon as keyword arg

---
 rclpy/rclpy/graph_listener.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index 25c9361f6..f2c183bd9 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -51,8 +51,7 @@ def __del__(self):
     def _try_start_thread(self):
         # Assumes lock is already held
         if self._thread is None:
-            self._thread = threading.Thread(target=self._runner)
-            self._thread.daemon = True
+            self._thread = threading.Thread(target=self._runner, daemon=True)
             self._thread.start()
 
     def add_timer(self, timer_period_ns, callback):

From 1c9a098e4d549bb9d6fc750b29497451b6a44753 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:45:15 -0700
Subject: [PATCH 17/49] Remove unnecessary namespace argument

---
 rclpy/test/test_wait_set.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
index b5825708c..ef82983e4 100644
--- a/rclpy/test/test_wait_set.py
+++ b/rclpy/test/test_wait_set.py
@@ -26,7 +26,7 @@ class TestWaitSet(unittest.TestCase):
     @classmethod
     def setUpClass(cls):
         rclpy.init()
-        cls.node = rclpy.create_node('TestWaitSet', namespace='/')
+        cls.node = rclpy.create_node('TestWaitSet')
 
     @classmethod
     def tearDownClass(cls):

From 655425fabcdd20cba75ad051ee00289fc1f3cb4c Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:48:06 -0700
Subject: [PATCH 18/49] Use S_TO_NS in test

---
 rclpy/test/test_utilities.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/rclpy/test/test_utilities.py b/rclpy/test/test_utilities.py
index c04ad63a6..5fdfef1bf 100644
--- a/rclpy/test/test_utilities.py
+++ b/rclpy/test/test_utilities.py
@@ -14,6 +14,7 @@
 
 import unittest
 
+from rclpy.constants import S_TO_NS
 import rclpy.utilities
 
 
@@ -23,7 +24,7 @@ def test_timeout_sec_to_nsec(self):
         self.assertGreater(0, rclpy.utilities.timeout_sec_to_nsec(None))
         self.assertGreater(0, rclpy.utilities.timeout_sec_to_nsec(-1))
         self.assertEqual(0, rclpy.utilities.timeout_sec_to_nsec(0))
-        self.assertEqual(1000000000, rclpy.utilities.timeout_sec_to_nsec(1))
+        self.assertEqual(1.5 * S_TO_NS, rclpy.utilities.timeout_sec_to_nsec(1.5))
 
 
 if __name__ == '__main__':

From ccd93d8137db56bd8580053710ef2caa4ff2df0f Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:54:38 -0700
Subject: [PATCH 19/49] More tests using S_TO_NS

---
 rclpy/test/test_wait_set.py | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
index ef82983e4..62a3b645c 100644
--- a/rclpy/test/test_wait_set.py
+++ b/rclpy/test/test_wait_set.py
@@ -16,6 +16,7 @@
 
 from rcl_interfaces.srv import GetParameters
 import rclpy
+from rclpy.constants import S_TO_NS
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.wait_set import WaitSet
 from std_msgs.msg import String
@@ -52,7 +53,7 @@ def test_guard_condition_ready(self):
 
     def test_timer_ready(self):
         ws = WaitSet()
-        timer_handle, timer_pointer = _rclpy.rclpy_create_timer(100000000)
+        timer_handle, timer_pointer = _rclpy.rclpy_create_timer(int(0.1 * S_TO_NS))
         try:
             ws.add_timer(timer_handle, timer_pointer)
             self.assertFalse(ws.is_ready(timer_pointer))
@@ -60,7 +61,7 @@ def test_timer_ready(self):
             ws.wait(1)
             self.assertFalse(ws.is_ready(timer_pointer))
 
-            ws.wait(100000000)
+            ws.wait(int(0.1 * S_TO_NS))
             self.assertTrue(ws.is_ready(timer_pointer))
         finally:
             _rclpy.rclpy_destroy_entity('timer', timer_handle)
@@ -81,7 +82,7 @@ def test_subscriber_ready(self):
             msg.data = 'Hello World'
             pub.publish(msg)
 
-            ws.wait(5000000000)
+            ws.wait(5 * S_TO_NS)
             self.assertTrue(ws.is_ready(sub.subscription_pointer))
         finally:
             self.node.destroy_publisher(pub)
@@ -106,7 +107,7 @@ def test_server_ready(self):
             cli.wait_for_service()
             cli.call(GetParameters.Request())
 
-            ws.wait(5000000000)
+            ws.wait(5 * S_TO_NS)
             # TODO(sloretz) test client after it's wait_for_future() API is sorted out
             self.assertTrue(ws.is_ready(srv.service_pointer))
         finally:

From 441bbb5184fc94d07c999670437d8382007056c3 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:57:39 -0700
Subject: [PATCH 20/49] Use monotonic clock for testing timer time

---
 rclpy/test/test_client.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/rclpy/test/test_client.py b/rclpy/test/test_client.py
index dd68c297d..ff3a60945 100644
--- a/rclpy/test/test_client.py
+++ b/rclpy/test/test_client.py
@@ -37,9 +37,9 @@ def tearDownClass(cls):
     def test_wait_for_service_5sec(self):
         cli = self.node.create_client(GetParameters, 'get/parameters')
         try:
-            start = time.time()
+            start = time.monotonic()
             cli.wait_for_service(timeout_sec=5.0)
-            end = time.time()
+            end = time.monotonic()
             self.assertGreater(5.0, end - start - TIME_FUDGE)
             self.assertLess(5.0, end - start + TIME_FUDGE)
         finally:
@@ -48,9 +48,9 @@ def test_wait_for_service_5sec(self):
     def test_wait_for_service_nowait(self):
         cli = self.node.create_client(GetParameters, 'get/parameters')
         try:
-            start = time.time()
+            start = time.monotonic()
             cli.wait_for_service(timeout_sec=0)
-            end = time.time()
+            end = time.monotonic()
             self.assertGreater(0, end - start - TIME_FUDGE)
             self.assertLess(0, end - start + TIME_FUDGE)
         finally:

From 8733c14cd941e6ea5b02f0d50c91acaa2aa81067 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 08:59:12 -0700
Subject: [PATCH 21/49] Increased test timeout by 30 seconds

---
 rclpy/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt
index b28e62020..0a0ef0c41 100644
--- a/rclpy/CMakeLists.txt
+++ b/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()

From 3f26246da06e1bb49aa2a453eb93c702aa3cffbc Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 09:01:49 -0700
Subject: [PATCH 22/49] CheckExact -> IsValid

---
 rclpy/src/rclpy/_rclpy.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index c274729ca..b2884534e 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -1579,7 +1579,7 @@ rclpy_service_server_is_available(PyObject * Py_UNUSED(self), PyObject * args)
   if (!PyArg_ParseTuple(args, "OO", &pynode, &pyclient)) {
     return NULL;
   }
-  if (!PyCapsule_CheckExact(pynode) || !PyCapsule_CheckExact(pyclient)) {
+  if (!PyCapsule_IsValid(pynode, NULL) || !PyCapsule_IsValid(pyclient, NULL)) {
     PyErr_Format(PyExc_ValueError, "Expected two PyCapsule as arguments");
     return NULL;
   }

From 0a79f281dfe2c588d78141e96c82930f5da8b9e0 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 17 Oct 2017 09:18:09 -0700
Subject: [PATCH 23/49] Fixed wait_set not clearing ready_pointers

---
 rclpy/rclpy/wait_set.py     | 32 ++++++++------------------------
 rclpy/test/test_wait_set.py | 15 +++++++++++++++
 2 files changed, 23 insertions(+), 24 deletions(-)

diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py
index 2648431aa..de17cd014 100644
--- a/rclpy/rclpy/wait_set.py
+++ b/rclpy/rclpy/wait_set.py
@@ -21,8 +21,8 @@ class WaitSet:
     """Provide conveneint methods and destroy the wait set when garbage collected."""
 
     def __init__(self):
-        # maps entity pointers (the python integer, not the PyCapsule) to a bool
-        self._ready_pointers = {}
+        # List of entity pointers (the python integer, not the PyCapsule) that are ready
+        self._ready_pointers = []
 
         # maps pointers (integers) to handles (PyCapsule)
         self._subscriptions = {}
@@ -43,7 +43,6 @@ def __del__(self):
 
     def add_subscription(self, subscription_handle, subscription_pointer):
         self._subscriptions[subscription_pointer] = subscription_handle
-        self._ready_pointers[subscription_pointer] = False
         self._needs_building = True
 
     def add_subscriptions(self, subscriptions):
@@ -52,12 +51,10 @@ def add_subscriptions(self, subscriptions):
 
     def remove_subscription(self, subscription_pointer):
         del self._subscriptions[subscription_pointer]
-        del self._ready_pointers[subscription_pointer]
         self._needs_building = True
 
     def add_guard_condition(self, gc_handle, gc_pointer):
         self._guard_conditions[gc_pointer] = gc_handle
-        self._ready_pointers[gc_pointer] = False
         self._needs_building = True
 
     def add_guard_conditions(self, guards):
@@ -66,12 +63,10 @@ def add_guard_conditions(self, guards):
 
     def remove_guard_condition(self, gc_pointer):
         del self._guard_conditions[gc_pointer]
-        del self._ready_pointers[gc_pointer]
         self._needs_building = True
 
     def add_timer(self, timer_handle, timer_pointer):
         self._timers[timer_pointer] = timer_handle
-        self._ready_pointers[timer_pointer] = False
         self._needs_building = True
 
     def add_timers(self, timers):
@@ -80,12 +75,10 @@ def add_timers(self, timers):
 
     def remove_timer(self, timer_pointer):
         del self._timers[timer_pointer]
-        del self._ready_pointers[timer_pointer]
         self._needs_building = True
 
     def add_client(self, client_handle, client_pointer):
         self._clients[client_pointer] = client_handle
-        self._ready_pointers[client_pointer] = False
         self._needs_building = True
 
     def add_clients(self, clients):
@@ -94,12 +87,10 @@ def add_clients(self, clients):
 
     def remove_client(self, client_pointer):
         del self._clients[client_pointer]
-        del self._ready_pointers[client_pointer]
         self._needs_building = True
 
     def add_service(self, service_handle, service_pointer):
         self._services[service_pointer] = service_handle
-        self._ready_pointers[service_pointer] = False
         self._needs_building = True
 
     def add_services(self, services):
@@ -108,7 +99,6 @@ def add_services(self, services):
 
     def remove_service(self, service_pointer):
         del self._services[service_pointer]
-        del self._ready_pointers[service_pointer]
         self._needs_building = True
 
     def wait(self, timeout_nsec):
@@ -146,17 +136,11 @@ def wait(self, timeout_nsec):
             _rclpy.rclpy_wait(self._wait_set, timeout_nsec)
 
             ws = self._wait_set
-            sub = [(e, True) for e in _rclpy.rclpy_get_ready_entities('subscription', ws)]
-            grd = [(e, True) for e in _rclpy.rclpy_get_ready_entities('guard_condition', ws)]
-            tmr = [(e, True) for e in _rclpy.rclpy_get_ready_entities('timer', ws)]
-            cli = [(e, True) for e in _rclpy.rclpy_get_ready_entities('client', ws)]
-            srv = [(e, True) for e in _rclpy.rclpy_get_ready_entities('service', ws)]
-
-        self._ready_pointers.update(sub)
-        self._ready_pointers.update(grd)
-        self._ready_pointers.update(tmr)
-        self._ready_pointers.update(cli)
-        self._ready_pointers.update(srv)
+            self._ready_pointers = _rclpy.rclpy_get_ready_entities('subscription', ws)
+            self._ready_pointers.extend(_rclpy.rclpy_get_ready_entities('guard_condition', ws))
+            self._ready_pointers.extend(_rclpy.rclpy_get_ready_entities('timer', ws))
+            self._ready_pointers.extend(_rclpy.rclpy_get_ready_entities('client', ws))
+            self._ready_pointers.extend(_rclpy.rclpy_get_ready_entities('service', ws))
 
     def is_ready(self, entity_pointer):
-        return self._ready_pointers[entity_pointer]
+        return entity_pointer in self._ready_pointers
diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
index 62a3b645c..2c0e769eb 100644
--- a/rclpy/test/test_wait_set.py
+++ b/rclpy/test/test_wait_set.py
@@ -48,6 +48,9 @@ def test_guard_condition_ready(self):
             # TODO(sloretz) why does the next assertion fail with wait(0)?
             ws.wait(1)
             self.assertTrue(ws.is_ready(gc_pointer))
+
+            ws.wait(1)
+            self.assertFalse(ws.is_ready(gc_pointer))
         finally:
             _rclpy.rclpy_destroy_entity('guard_condition', gc_handle)
 
@@ -63,6 +66,10 @@ def test_timer_ready(self):
 
             ws.wait(int(0.1 * S_TO_NS))
             self.assertTrue(ws.is_ready(timer_pointer))
+
+            _rclpy.rclpy_call_timer(timer_handle)
+            ws.wait(1)
+            self.assertFalse(ws.is_ready(timer_pointer))
         finally:
             _rclpy.rclpy_destroy_entity('timer', timer_handle)
 
@@ -84,6 +91,10 @@ def test_subscriber_ready(self):
 
             ws.wait(5 * S_TO_NS)
             self.assertTrue(ws.is_ready(sub.subscription_pointer))
+
+            _rclpy.rclpy_take(sub.subscription_handle, sub.msg_type)
+            ws.wait(1)
+            self.assertFalse(ws.is_ready(sub.subscription_pointer))
         finally:
             self.node.destroy_publisher(pub)
             self.node.destroy_subscription(sub)
@@ -110,6 +121,10 @@ def test_server_ready(self):
             ws.wait(5 * S_TO_NS)
             # TODO(sloretz) test client after it's wait_for_future() API is sorted out
             self.assertTrue(ws.is_ready(srv.service_pointer))
+
+            _rclpy.rclpy_take_request(srv.service_handle, srv.srv_type.Request)
+            ws.wait(1)
+            self.assertFalse(ws.is_ready(srv.service_pointer))
         finally:
             self.node.destroy_client(cli)
             self.node.destroy_service(srv)

From 65c545f79f3e6e2e1d2e53968e24a84693938fa4 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 18 Oct 2017 16:02:36 -0700
Subject: [PATCH 24/49] Remove unnecessary namespace keyword arg

---
 rclpy/test/test_client.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclpy/test/test_client.py b/rclpy/test/test_client.py
index ff3a60945..24e9a2f3f 100644
--- a/rclpy/test/test_client.py
+++ b/rclpy/test/test_client.py
@@ -27,7 +27,7 @@ class TestClient(unittest.TestCase):
     @classmethod
     def setUpClass(cls):
         rclpy.init()
-        cls.node = rclpy.create_node('TestClient', namespace='/')
+        cls.node = rclpy.create_node('TestClient')
 
     @classmethod
     def tearDownClass(cls):

From d81684c355afd11c79f64b33d7d88c17f554f1a3 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 18 Oct 2017 16:12:44 -0700
Subject: [PATCH 25/49] Non-blocking wait

---
 rclpy/test/test_wait_set.py | 19 +++++++++----------
 1 file changed, 9 insertions(+), 10 deletions(-)

diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
index 2c0e769eb..505c4fe8c 100644
--- a/rclpy/test/test_wait_set.py
+++ b/rclpy/test/test_wait_set.py
@@ -41,15 +41,14 @@ def test_guard_condition_ready(self):
             ws.add_guard_condition(gc_handle, gc_pointer)
             self.assertFalse(ws.is_ready(gc_pointer))
 
-            ws.wait(1)
+            ws.wait(0)
             self.assertFalse(ws.is_ready(gc_pointer))
 
             _rclpy.rclpy_trigger_guard_condition(gc_handle)
-            # TODO(sloretz) why does the next assertion fail with wait(0)?
-            ws.wait(1)
+            ws.wait(0)
             self.assertTrue(ws.is_ready(gc_pointer))
 
-            ws.wait(1)
+            ws.wait(0)
             self.assertFalse(ws.is_ready(gc_pointer))
         finally:
             _rclpy.rclpy_destroy_entity('guard_condition', gc_handle)
@@ -61,14 +60,14 @@ def test_timer_ready(self):
             ws.add_timer(timer_handle, timer_pointer)
             self.assertFalse(ws.is_ready(timer_pointer))
 
-            ws.wait(1)
+            ws.wait(0)
             self.assertFalse(ws.is_ready(timer_pointer))
 
             ws.wait(int(0.1 * S_TO_NS))
             self.assertTrue(ws.is_ready(timer_pointer))
 
             _rclpy.rclpy_call_timer(timer_handle)
-            ws.wait(1)
+            ws.wait(0)
             self.assertFalse(ws.is_ready(timer_pointer))
         finally:
             _rclpy.rclpy_destroy_entity('timer', timer_handle)
@@ -82,7 +81,7 @@ def test_subscriber_ready(self):
             ws.add_subscription(sub.subscription_handle, sub.subscription_pointer)
             self.assertFalse(ws.is_ready(sub.subscription_pointer))
 
-            ws.wait(1)
+            ws.wait(0)
             self.assertFalse(ws.is_ready(sub.subscription_pointer))
 
             msg = String()
@@ -93,7 +92,7 @@ def test_subscriber_ready(self):
             self.assertTrue(ws.is_ready(sub.subscription_pointer))
 
             _rclpy.rclpy_take(sub.subscription_handle, sub.msg_type)
-            ws.wait(1)
+            ws.wait(0)
             self.assertFalse(ws.is_ready(sub.subscription_pointer))
         finally:
             self.node.destroy_publisher(pub)
@@ -111,7 +110,7 @@ def test_server_ready(self):
             self.assertFalse(ws.is_ready(cli.client_pointer))
             self.assertFalse(ws.is_ready(srv.service_pointer))
 
-            ws.wait(1)
+            ws.wait(0)
             self.assertFalse(ws.is_ready(cli.client_pointer))
             self.assertFalse(ws.is_ready(srv.service_pointer))
 
@@ -123,7 +122,7 @@ def test_server_ready(self):
             self.assertTrue(ws.is_ready(srv.service_pointer))
 
             _rclpy.rclpy_take_request(srv.service_handle, srv.srv_type.Request)
-            ws.wait(1)
+            ws.wait(0)
             self.assertFalse(ws.is_ready(srv.service_pointer))
         finally:
             self.node.destroy_client(cli)

From 4bbc3236955fb33581a7f5db35072ca71856cb51 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 18 Oct 2017 16:23:18 -0700
Subject: [PATCH 26/49] remove expression that always evaluates to True

---
 rclpy/rclpy/executors.py | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 17cfa0a14..3caf75efd 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -55,11 +55,10 @@ def wait(self, timeout_sec=None):
         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
 
 

From 99280ae7ca068e99b7b56fa5b1633f5349d14b3c Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Thu, 19 Oct 2017 07:54:46 -0700
Subject: [PATCH 27/49] Raise ValueError on invalid capsule

---
 rclpy/src/rclpy/_rclpy.c | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index b2884534e..049fb865c 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -46,7 +46,7 @@ static void catch_function(int signo)
 
 /// Get a guard condition for node graph events
 /**
- * Raises TypeError if the provided argument is not a PyCapsule.
+ * Raises ValueError if the provided argument is not a PyCapsule.
  *
  * A successful call will return a list with two elements:
  *
@@ -65,12 +65,12 @@ rclpy_get_graph_guard_condition(PyObject * Py_UNUSED(self), PyObject * args)
   if (!PyArg_ParseTuple(args, "O", &pynode)) {
     return NULL;
   }
-  if (!PyCapsule_IsValid(pynode, NULL)) {
-    PyErr_Format(PyExc_TypeError, "Expected a PyCapsule as an argument");
+
+  rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, NULL);
+  if (!node) {
     return NULL;
   }
 
-  rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, NULL);
   rcl_guard_condition_t * guard_condition =
     (rcl_guard_condition_t *)rcl_node_get_graph_guard_condition(node);
 
@@ -1579,13 +1579,15 @@ rclpy_service_server_is_available(PyObject * Py_UNUSED(self), PyObject * args)
   if (!PyArg_ParseTuple(args, "OO", &pynode, &pyclient)) {
     return NULL;
   }
-  if (!PyCapsule_IsValid(pynode, NULL) || !PyCapsule_IsValid(pyclient, NULL)) {
-    PyErr_Format(PyExc_ValueError, "Expected two PyCapsule as arguments");
-    return NULL;
-  }
 
   rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, NULL);
+  if (!node) {
+    return NULL;
+  }
   rcl_client_t * client = (rcl_client_t *)PyCapsule_GetPointer(pyclient, NULL);
+  if (!client) {
+    return NULL;
+  }
 
   bool is_ready;
   rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready);

From 1d2beebcfe146a97082588be9b64dfd058491902 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Thu, 19 Oct 2017 08:12:45 -0700
Subject: [PATCH 28/49] Simplified timeout_sec_to_nsec

---
 rclpy/rclpy/utilities.py | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

diff --git a/rclpy/rclpy/utilities.py b/rclpy/rclpy/utilities.py
index bbe36f0d4..551a14628 100644
--- a/rclpy/rclpy/utilities.py
+++ b/rclpy/rclpy/utilities.py
@@ -48,20 +48,14 @@ def timeout_sec_to_nsec(timeout_sec):
     Python tends to use floating point numbers in seconds for timeouts. This utility converts a
     python-style timeout to an integer in nanoseconds that can be used by rcl_wait.
 
-    :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
+    :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if < 1ns
     :type timeout_sec: float or None
     :rtype: int
     :returns: rcl_wait compatible timeout in nanoseconds
     """
-    if timeout_sec is None:
+    if timeout_sec is None or timeout_sec < 0:
         # Block forever
         return -1
-    elif timeout_sec > -1.0 / S_TO_NS and timeout_sec < 1.0 / S_TO_NS:
-        # Return immediately
-        return 0
-    elif timeout_sec < 0:
-        # block forever
-        return -1
     else:
         # wait for given time
         return int(float(timeout_sec) * S_TO_NS)

From 69807113666fea68607601a08879016a69b3ca83 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 24 Oct 2017 09:46:34 -0700
Subject: [PATCH 29/49] Added 'WaitSet.destroy()' and made executor use it

---
 rclpy/rclpy/executors.py | 131 ++++++++++++++++++++-------------------
 rclpy/rclpy/wait_set.py  |  12 +++-
 2 files changed, 77 insertions(+), 66 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 3caf75efd..8928f9ca0 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -290,73 +290,74 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                 timers.append(timeout_timer)
 
             # Construct a wait set
-            wait_set = _WaitSet()
-            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
-            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 any handlers 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):
+            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
+                wait_set.wait(timeout_nsec)
+
+                # Check sigint guard condition
+                if wait_set.is_ready(sigint_gc_handle):
+                    raise KeyboardInterrupt
+                _rclpy.rclpy_destroy_entity('guard_condition', 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
+
+                    for gc in [g for g in node.guards if g._executor_triggered]:
+                        if gc.callback_group.can_execute(gc):
                             handler = self._make_handler(
-                                tmr, self._take_timer, self._execute_timer)
+                                gc, self._take_guard_condition, self._execute_guard_condition)
                             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
-
-                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 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
-
-                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(
-                            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 wait_set.is_ready(
-                        timeout_timer.timer_pointer))):
-                break
+                            yield handler, gc, node
+
+                    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
+
+                    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(
+                                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 wait_set.is_ready(
+                            timeout_timer.timer_pointer))):
+                    break
 
 
 class SingleThreadedExecutor(Executor):
diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py
index de17cd014..70905bfeb 100644
--- a/rclpy/rclpy/wait_set.py
+++ b/rclpy/rclpy/wait_set.py
@@ -37,9 +37,19 @@ def __init__(self):
         # rcl_wait is not thread safe, so prevent multiple wait calls at once
         self._wait_lock = threading.Lock()
 
-    def __del__(self):
+    def destroy(self):
         if self._wait_set is not None:
             _rclpy.rclpy_destroy_wait_set(self._wait_set)
+            self._wait_set = None
+
+    def __del__(self):
+        self.destroy()
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, t, v, tb):
+        self.destroy()
 
     def add_subscription(self, subscription_handle, subscription_pointer):
         self._subscriptions[subscription_pointer] = subscription_handle

From c69858f2679ad030dc7a57329639d05840db3c1d Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 24 Oct 2017 10:19:22 -0700
Subject: [PATCH 30/49] GraphListener periodically checks if rclpy is shutdown

---
 rclpy/rclpy/graph_listener.py | 29 ++++++++++++++++++++---------
 1 file changed, 20 insertions(+), 9 deletions(-)

diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index f2c183bd9..91a812418 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -15,9 +15,11 @@
 import threading
 import traceback
 
+from rclpy.constants import S_TO_NS
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 import rclpy.logging
 from rclpy.timer import WallTimer as _WallTimer
+from rclpy.utilities import ok as _ok
 from rclpy.wait_set import WaitSet as _WaitSet
 
 
@@ -41,12 +43,16 @@ def __init__(self):
         self._wait_set.add_guard_condition(self._gc_handle, self._gc_pointer)
 
     def __del__(self):
-        with self._lock:
-            th = self._thread
-            self._thread = None
-        if th:
-            th.join()
-        _rclpy.rclpy_destroy_entity('guard_condition', self._gc_handle)
+        self.destroy()
+
+    @classmethod
+    def destroy(cls):
+        self = getattr(cls, '__singleton')
+        if self is not None:
+            with self._lock:
+                setattr(cls, '__singleton', None)
+                self._thread = None
+            _rclpy.rclpy_destroy_entity('guard_condition', self._gc_handle)
 
     def _try_start_thread(self):
         # Assumes lock is already held
@@ -134,11 +140,16 @@ def remove_callback(self, node_handle, callback):
 
     def _runner(self):
         while True:
-            # Wait forever
-            self._wait_set.wait(-1)
+            # Wait 1 second
+            self._wait_set.wait(S_TO_NS)
 
-            # notify graph event subscribers
             with self._lock:
+                # Shutdown if necessary
+                if not _ok():
+                    self.destroy()
+                    break
+
+                # notify graph event subscribers
                 if not self._thread:
                     # Asked to shut down thread
                     return

From 4cd2fb1c8c17d1011d00100700898044c28cd9b6 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 25 Oct 2017 13:09:25 -0700
Subject: [PATCH 31/49] Misc fixes after pycapsule names

---
 rclpy/rclpy/executors.py      | 2 +-
 rclpy/rclpy/graph_listener.py | 2 +-
 rclpy/src/rclpy/_rclpy.c      | 8 ++++----
 rclpy/test/test_wait_set.py   | 4 ++--
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 8928f9ca0..de19ff5df 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -305,7 +305,7 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                 # Check sigint guard condition
                 if wait_set.is_ready(sigint_gc_handle):
                     raise KeyboardInterrupt
-                _rclpy.rclpy_destroy_entity('guard_condition', sigint_gc)
+                _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)]:
diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index 91a812418..0b8a5ad49 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -52,7 +52,7 @@ def destroy(cls):
             with self._lock:
                 setattr(cls, '__singleton', None)
                 self._thread = None
-            _rclpy.rclpy_destroy_entity('guard_condition', self._gc_handle)
+            _rclpy.rclpy_destroy_entity(self._gc_handle)
 
     def _try_start_thread(self):
         # Assumes lock is already held
diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index 049fb865c..3e167178b 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -66,7 +66,7 @@ rclpy_get_graph_guard_condition(PyObject * Py_UNUSED(self), PyObject * args)
     return NULL;
   }
 
-  rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, NULL);
+  rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t");
   if (!node) {
     return NULL;
   }
@@ -75,7 +75,7 @@ rclpy_get_graph_guard_condition(PyObject * Py_UNUSED(self), PyObject * args)
     (rcl_guard_condition_t *)rcl_node_get_graph_guard_condition(node);
 
   PyObject * pylist = PyList_New(2);
-  PyList_SET_ITEM(pylist, 0, PyCapsule_New(guard_condition, NULL, NULL));
+  PyList_SET_ITEM(pylist, 0, PyCapsule_New(guard_condition, "rcl_guard_condition_t", NULL));
   PyList_SET_ITEM(pylist, 1, PyLong_FromUnsignedLongLong((uint64_t)&guard_condition->impl));
 
   return pylist;
@@ -1580,11 +1580,11 @@ rclpy_service_server_is_available(PyObject * Py_UNUSED(self), PyObject * args)
     return NULL;
   }
 
-  rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, NULL);
+  rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t");
   if (!node) {
     return NULL;
   }
-  rcl_client_t * client = (rcl_client_t *)PyCapsule_GetPointer(pyclient, NULL);
+  rcl_client_t * client = (rcl_client_t *)PyCapsule_GetPointer(pyclient, "rcl_client_t");
   if (!client) {
     return NULL;
   }
diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
index 505c4fe8c..f58d7f25d 100644
--- a/rclpy/test/test_wait_set.py
+++ b/rclpy/test/test_wait_set.py
@@ -51,7 +51,7 @@ def test_guard_condition_ready(self):
             ws.wait(0)
             self.assertFalse(ws.is_ready(gc_pointer))
         finally:
-            _rclpy.rclpy_destroy_entity('guard_condition', gc_handle)
+            _rclpy.rclpy_destroy_entity(gc_handle)
 
     def test_timer_ready(self):
         ws = WaitSet()
@@ -70,7 +70,7 @@ def test_timer_ready(self):
             ws.wait(0)
             self.assertFalse(ws.is_ready(timer_pointer))
         finally:
-            _rclpy.rclpy_destroy_entity('timer', timer_handle)
+            _rclpy.rclpy_destroy_entity(timer_handle)
 
     def test_subscriber_ready(self):
         ws = WaitSet()

From 0cf2aebf61566da6fcb5b51952d5cd70b082171c Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 31 Oct 2017 08:57:23 -0700
Subject: [PATCH 32/49] Wait set class always clears entities before waiting

---
 rclpy/rclpy/wait_set.py | 57 +++++++++++++++++++++--------------------
 1 file changed, 29 insertions(+), 28 deletions(-)

diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py
index 70905bfeb..58c420a96 100644
--- a/rclpy/rclpy/wait_set.py
+++ b/rclpy/rclpy/wait_set.py
@@ -32,7 +32,7 @@ def __init__(self):
         self._services = {}
 
         # Set when the wait set needs to be built or rebuilt
-        self._needs_building = True
+        self._need_resize = True
         self._wait_set = None
         # rcl_wait is not thread safe, so prevent multiple wait calls at once
         self._wait_lock = threading.Lock()
@@ -53,7 +53,7 @@ def __exit__(self, t, v, tb):
 
     def add_subscription(self, subscription_handle, subscription_pointer):
         self._subscriptions[subscription_pointer] = subscription_handle
-        self._needs_building = True
+        self._need_resize = True
 
     def add_subscriptions(self, subscriptions):
         for sub in subscriptions:
@@ -61,11 +61,11 @@ def add_subscriptions(self, subscriptions):
 
     def remove_subscription(self, subscription_pointer):
         del self._subscriptions[subscription_pointer]
-        self._needs_building = True
+        self._need_resize = True
 
     def add_guard_condition(self, gc_handle, gc_pointer):
         self._guard_conditions[gc_pointer] = gc_handle
-        self._needs_building = True
+        self._need_resize = True
 
     def add_guard_conditions(self, guards):
         for gc in guards:
@@ -73,11 +73,11 @@ def add_guard_conditions(self, guards):
 
     def remove_guard_condition(self, gc_pointer):
         del self._guard_conditions[gc_pointer]
-        self._needs_building = True
+        self._need_resize = True
 
     def add_timer(self, timer_handle, timer_pointer):
         self._timers[timer_pointer] = timer_handle
-        self._needs_building = True
+        self._need_resize = True
 
     def add_timers(self, timers):
         for tmr in timers:
@@ -85,11 +85,11 @@ def add_timers(self, timers):
 
     def remove_timer(self, timer_pointer):
         del self._timers[timer_pointer]
-        self._needs_building = True
+        self._need_resize = True
 
     def add_client(self, client_handle, client_pointer):
         self._clients[client_pointer] = client_handle
-        self._needs_building = True
+        self._need_resize = True
 
     def add_clients(self, clients):
         for cli in clients:
@@ -97,11 +97,11 @@ def add_clients(self, clients):
 
     def remove_client(self, client_pointer):
         del self._clients[client_pointer]
-        self._needs_building = True
+        self._need_resize = True
 
     def add_service(self, service_handle, service_pointer):
         self._services[service_pointer] = service_handle
-        self._needs_building = True
+        self._need_resize = True
 
     def add_services(self, services):
         for srv in services:
@@ -109,11 +109,12 @@ def add_services(self, services):
 
     def remove_service(self, service_pointer):
         del self._services[service_pointer]
-        self._needs_building = True
+        self._need_resize = True
 
     def wait(self, timeout_nsec):
         with self._wait_lock:
-            if self._needs_building:
+            if self._need_resize:
+                self._need_resize = False
                 if self._wait_set is not None:
                     _rclpy.rclpy_destroy_wait_set(self._wait_set)
 
@@ -126,22 +127,22 @@ def wait(self, timeout_nsec):
                     len(self._clients),
                     len(self._services))
 
-                _rclpy.rclpy_wait_set_clear_entities('subscription', self._wait_set)
-                _rclpy.rclpy_wait_set_clear_entities('guard_condition', self._wait_set)
-                _rclpy.rclpy_wait_set_clear_entities('timer', self._wait_set)
-                _rclpy.rclpy_wait_set_clear_entities('client', self._wait_set)
-                _rclpy.rclpy_wait_set_clear_entities('service', self._wait_set)
-
-                for handle in self._subscriptions.values():
-                    _rclpy.rclpy_wait_set_add_entity('subscription', self._wait_set, handle)
-                for handle in self._guard_conditions.values():
-                    _rclpy.rclpy_wait_set_add_entity('guard_condition', self._wait_set, handle)
-                for handle in self._timers.values():
-                    _rclpy.rclpy_wait_set_add_entity('timer', self._wait_set, handle)
-                for handle in self._clients.values():
-                    _rclpy.rclpy_wait_set_add_entity('client', self._wait_set, handle)
-                for handle in self._services.values():
-                    _rclpy.rclpy_wait_set_add_entity('service', self._wait_set, handle)
+            _rclpy.rclpy_wait_set_clear_entities('subscription', self._wait_set)
+            _rclpy.rclpy_wait_set_clear_entities('guard_condition', self._wait_set)
+            _rclpy.rclpy_wait_set_clear_entities('timer', self._wait_set)
+            _rclpy.rclpy_wait_set_clear_entities('client', self._wait_set)
+            _rclpy.rclpy_wait_set_clear_entities('service', self._wait_set)
+
+            for handle in self._subscriptions.values():
+                _rclpy.rclpy_wait_set_add_entity('subscription', self._wait_set, handle)
+            for handle in self._guard_conditions.values():
+                _rclpy.rclpy_wait_set_add_entity('guard_condition', self._wait_set, handle)
+            for handle in self._timers.values():
+                _rclpy.rclpy_wait_set_add_entity('timer', self._wait_set, handle)
+            for handle in self._clients.values():
+                _rclpy.rclpy_wait_set_add_entity('client', self._wait_set, handle)
+            for handle in self._services.values():
+                _rclpy.rclpy_wait_set_add_entity('service', self._wait_set, handle)
 
             _rclpy.rclpy_wait(self._wait_set, timeout_nsec)
 

From 7843c5c381be9fd0482d67076a0df430ff5b815a Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 31 Oct 2017 09:13:53 -0700
Subject: [PATCH 33/49] Remove underscore on import

---
 rclpy/rclpy/executors.py      | 20 ++++++++++----------
 rclpy/rclpy/graph_listener.py | 12 ++++++------
 2 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index de19ff5df..96bff4ad5 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -12,16 +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.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
 from rclpy.utilities import timeout_sec_to_nsec
-from rclpy.wait_set import WaitSet as _WaitSet
+from rclpy.wait_set import WaitSet
 
 
 class _WorkTracker:
@@ -30,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."""
@@ -77,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
@@ -261,7 +261,7 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
         timeout_timer = None
         timeout_nsec = timeout_sec_to_nsec(timeout_sec)
         if timeout_nsec > 0:
-            timeout_timer = _WallTimer(None, None, timeout_nsec)
+            timeout_timer = WallTimer(None, None, timeout_nsec)
 
         if nodes is None:
             nodes = self.get_nodes()
@@ -290,7 +290,7 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                 timers.append(timeout_timer)
 
             # Construct a wait set
-            with _WaitSet() as wait_set:
+            with WaitSet() as wait_set:
                 wait_set.add_subscriptions(subscriptions)
                 wait_set.add_clients(clients)
                 wait_set.add_services(services)
@@ -392,7 +392,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:
diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index 0b8a5ad49..8d75b96ad 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -18,9 +18,9 @@
 from rclpy.constants import S_TO_NS
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 import rclpy.logging
-from rclpy.timer import WallTimer as _WallTimer
-from rclpy.utilities import ok as _ok
-from rclpy.wait_set import WaitSet as _WaitSet
+from rclpy.timer import WallTimer
+from rclpy.utilities import ok
+from rclpy.wait_set import WaitSet
 
 
 class GraphListenerSingleton:
@@ -39,7 +39,7 @@ def __init__(self):
         self._gc_handle, self._gc_pointer = _rclpy.rclpy_create_guard_condition()
         self._thread = None
         self._lock = threading.RLock()
-        self._wait_set = _WaitSet()
+        self._wait_set = WaitSet()
         self._wait_set.add_guard_condition(self._gc_handle, self._gc_pointer)
 
     def __del__(self):
@@ -71,7 +71,7 @@ def add_timer(self, timer_period_ns, callback):
         :rtype: rclpy.timer.WallTimer
         """
         with self._lock:
-            tmr = _WallTimer(callback, None, timer_period_ns)
+            tmr = WallTimer(callback, None, timer_period_ns)
             self._timers[tmr] = callback
             self._wait_set.add_timer(tmr.timer_handle, tmr.timer_pointer)
 
@@ -145,7 +145,7 @@ def _runner(self):
 
             with self._lock:
                 # Shutdown if necessary
-                if not _ok():
+                if not ok():
                     self.destroy()
                     break
 

From ded618f8b32e3c78de723cf5130d16eed3976bf0 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 31 Oct 2017 09:15:21 -0700
Subject: [PATCH 34/49] Reformat timeout line

---
 rclpy/rclpy/executors.py | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 96bff4ad5..5728fc5b6 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -354,9 +354,10 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                             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))):
+                if (
+                    timeout_nsec == 0 or
+                    (timeout_timer is not None and wait_set.is_ready(timeout_timer.timer_pointer))
+                ):
                     break
 
 

From e8f7fd6588b9b0079faf72673ebbf3a201933fc0 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 31 Oct 2017 09:19:43 -0700
Subject: [PATCH 35/49] Use () when raising exceptions

---
 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 5728fc5b6..3d0364e3d 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -148,7 +148,7 @@ def spin_once(self, timeout_sec=None):
         :type timeout_sec: float or None
         :rtype: None
         """
-        raise NotImplementedError
+        raise NotImplementedError()
 
     def _take_timer(self, tmr):
         _rclpy.rclpy_call_timer(tmr.timer_handle)
@@ -304,7 +304,7 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
 
                 # Check sigint guard condition
                 if wait_set.is_ready(sigint_gc_handle):
-                    raise KeyboardInterrupt
+                    raise KeyboardInterrupt()
                 _rclpy.rclpy_destroy_entity(sigint_gc)
 
                 # Mark all guards as triggered before yielding since they're auto-taken

From 149ad4312c41729dc83a46f6ff512feaaec8c41d Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Tue, 31 Oct 2017 09:23:40 -0700
Subject: [PATCH 36/49] Removed _ on imports

---
 rclpy/rclpy/client.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py
index 1f3d91948..ee96f09f6 100644
--- a/rclpy/rclpy/client.py
+++ b/rclpy/rclpy/client.py
@@ -14,7 +14,7 @@
 
 import threading
 
-from rclpy.graph_listener import GraphEventSubscription as _GraphEventSubscription
+from rclpy.graph_listener import GraphEventSubscription
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 import rclpy.utilities
 
@@ -110,7 +110,7 @@ def on_timeout():
             nonlocal event
             event.set()
 
-        with _GraphEventSubscription(self.node_handle, on_graph_event, timeout_nsec, on_timeout):
+        with GraphEventSubscription(self.node_handle, on_graph_event, timeout_nsec, on_timeout):
             event.wait()
 
         return result

From dd86a13f49d969492d3330deb57b9a6e61b64428 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 1 Nov 2017 11:08:17 -0700
Subject: [PATCH 37/49] Executor optimizations

~5% less overhead in wait_for_ready_callbacks()
---
 rclpy/rclpy/executors.py | 49 ++++++++++++++++++++++++----------------
 1 file changed, 29 insertions(+), 20 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 3d0364e3d..bd8e900a6 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -82,6 +82,10 @@ def __init__(self):
         gc, gc_handle = _rclpy.rclpy_create_guard_condition()
         self._guard_condition = gc
         self._guard_condition_handle = gc_handle
+        # Triggered by signal handler for sigint
+        (sigint_gc, sigint_gc_handle) = _rclpy.rclpy_get_sigint_guard_condition()
+        self._sigint_gc = sigint_gc
+        self._sigint_gc_handle = sigint_gc_handle
         # True if shutdown has been called
         self._is_shutdown = False
         self._work_tracker = _WorkTracker()
@@ -238,15 +242,15 @@ def handler():
                     _rclpy.rclpy_trigger_guard_condition(gc)
         return handler
 
-    def _filter_eligible_entities(self, entities):
+    def _can_execute(self, entity):
         """
-        Filter entities that should not be put onto the wait list.
+        Return true if an entity is eligible for execution.
 
-        :param entity_list: Entities to be checked for eligibility
-        :type entity_list: list
-        :rtype: list
+        :param entity: an entity to be checked
+        :type entity_list: Client, Service, Publisher, Subscriber
+        :rtype: bool
         """
-        return [e for e in entities if e.callback_group.can_execute(e) and not e._executor_event]
+        return entity.callback_group.can_execute(entity) and not entity._executor_event
 
     def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
         """
@@ -275,17 +279,23 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
             clients = []
             services = []
             for node in nodes:
-                subscriptions.extend(self._filter_eligible_entities(node.subscriptions))
-                timers.extend(self._filter_eligible_entities(node.timers))
-                clients.extend(self._filter_eligible_entities(node.clients))
-                services.extend(self._filter_eligible_entities(node.services))
-                node_guards = self._filter_eligible_entities(node.guards)
-                # retrigger a guard condition that was triggered but not handled
-                for gc in node_guards:
-                    if gc._executor_triggered:
-                        gc.trigger()
-                guards.extend(node_guards)
-            (sigint_gc, sigint_gc_handle) = _rclpy.rclpy_get_sigint_guard_condition()
+                subscriptions.extend(node.subscriptions)
+                timers.extend(node.timers)
+                clients.extend(node.clients)
+                services.extend(node.services)
+                guards.extend(node.guards)
+
+            subscriptions = [e for e in filter(self._can_execute, subscriptions)]
+            guards = [e for e in filter(self._can_execute, guards)]
+            timers = [e for e in filter(self._can_execute, timers)]
+            clients = [e for e in filter(self._can_execute, clients)]
+            services = [e for e in filter(self._can_execute, services)]
+
+            # retrigger a guard condition that was triggered but not handled
+            for gc in guards:
+                if gc._executor_triggered:
+                    gc.trigger()
+
             if timeout_timer is not None:
                 timers.append(timeout_timer)
 
@@ -296,16 +306,15 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                 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._sigint_gc, self._sigint_gc_handle)
                 wait_set.add_guard_condition(self._guard_condition, self._guard_condition_handle)
 
                 # Wait for something to become ready
                 wait_set.wait(timeout_nsec)
 
                 # Check sigint guard condition
-                if wait_set.is_ready(sigint_gc_handle):
+                if wait_set.is_ready(self._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)]:

From 6352e86cf85f510c082aca2b047b28d8ed3e233b Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 1 Nov 2017 11:24:55 -0700
Subject: [PATCH 38/49] Fixed executor yielding entities to wrong node

Also refactored some code to a sub-generator
---
 rclpy/rclpy/executors.py | 92 ++++++++++++++++++++++------------------
 1 file changed, 51 insertions(+), 41 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index bd8e900a6..00dea6fa6 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -252,6 +252,56 @@ def _can_execute(self, entity):
         """
         return entity.callback_group.can_execute(entity) and not entity._executor_event
 
+    def _new_callbacks(self, nodes, wait_set):
+        """
+        Yields brand new work to executor implementations.
+
+        :param nodes: nodes to yield work for
+        :type nodes: list
+        :param wait_set: wait set that has already been waited on
+        :type wait_set: rclpy.wait_set.WaitSet
+        :rtype: Generator[(callable, entity, :class:`rclpy.node.Node`)]
+        """
+        yielded_work = False
+        # Process ready entities one node at a time
+        for node in nodes:
+            for tmr in node.timers:
+                if wait_set.is_ready(tmr.timer_pointer) and tmr.callback_group.can_execute(tmr):
+                    # TODO(Sloretz) Which rcl cancelled timer bug does this workaround?
+                    if not _rclpy.rclpy_is_timer_ready(tmr.timer_handle):
+                        continue
+                    handler = self._make_handler(tmr, self._take_timer, self._execute_timer)
+                    yielded_work = True
+                    yield handler, tmr, node
+
+            for sub in node.subscriptions:
+                if (wait_set.is_ready(sub.subscription_pointer) and
+                        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 node.guards:
+                if gc._executor_triggered and 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 cli in node.clients:
+                if wait_set.is_ready(cli.client_pointer) and cli.callback_group.can_execute(cli):
+                    handler = self._make_handler(cli, self._take_client, self._execute_client)
+                    yielded_work = True
+                    yield handler, cli, node
+
+            for srv in node.services:
+                if wait_set.is_ready(srv.service_pointer) and srv.callback_group.can_execute(srv):
+                    handler = self._make_handler(srv, self._take_service, self._execute_service)
+                    yielded_work = True
+                    yield handler, srv, node
+        return yielded_work
+
     def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
         """
         Yield callbacks that are ready to be performed.
@@ -320,47 +370,7 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                 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
-
-                    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 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
-
-                    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(
-                                srv, self._take_service, self._execute_service)
-                            yielded_work = True
-                            yield handler, srv, node
+                yielded_work = yield from self._new_callbacks(nodes, wait_set)
 
                 # Check timeout timer
                 if (

From 0dbf8c796321c34508dd26fb28bac1e2be3de1ee Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 1 Nov 2017 11:33:36 -0700
Subject: [PATCH 39/49] Use list() only where necessary

---
 rclpy/rclpy/executors.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 00dea6fa6..4dc6ac3b4 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -335,11 +335,11 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                 services.extend(node.services)
                 guards.extend(node.guards)
 
-            subscriptions = [e for e in filter(self._can_execute, subscriptions)]
-            guards = [e for e in filter(self._can_execute, guards)]
-            timers = [e for e in filter(self._can_execute, timers)]
-            clients = [e for e in filter(self._can_execute, clients)]
-            services = [e for e in filter(self._can_execute, services)]
+            subscriptions = filter(self._can_execute, subscriptions)
+            guards = list(filter(self._can_execute, guards))
+            timers = list(filter(self._can_execute, timers))
+            clients = filter(self._can_execute, clients)
+            services = filter(self._can_execute, services)
 
             # retrigger a guard condition that was triggered but not handled
             for gc in guards:

From 1a2ce9604747317ce1239a8d7c361583083fe241 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 1 Nov 2017 12:05:16 -0700
Subject: [PATCH 40/49] Docstring in imperitive mood

---
 rclpy/rclpy/executors.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 4dc6ac3b4..15bfe86a3 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -254,7 +254,7 @@ def _can_execute(self, entity):
 
     def _new_callbacks(self, nodes, wait_set):
         """
-        Yields brand new work to executor implementations.
+        Yield brand new work to executor implementations.
 
         :param nodes: nodes to yield work for
         :type nodes: list

From 7ee5ead99cf8d495818101a97266ca49fa9103bb Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 1 Nov 2017 15:09:28 -0700
Subject: [PATCH 41/49] Executors reuse iterator

---
 rclpy/rclpy/executors.py | 22 ++++++++++++++++------
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 15bfe86a3..37a070961 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -385,13 +385,18 @@ class SingleThreadedExecutor(Executor):
 
     def __init__(self):
         super().__init__()
+        self._callback_iter = None
 
     def spin_once(self, timeout_sec=None):
+        # Reuse the same callback iterator to avoid unecessary calls to rcl_wait
+        if self._callback_iter is None:
+            self._callback_iter = self.wait_for_ready_callbacks(timeout_sec=timeout_sec)
         try:
-            handler, entity, node = next(self.wait_for_ready_callbacks(timeout_sec=timeout_sec))
-            handler()
+            handler, entity, node = next(self._callback_iter)
         except StopIteration:
-            pass
+            self._callback_iter = None
+        else:
+            handler()
 
 
 class MultiThreadedExecutor(Executor):
@@ -407,6 +412,7 @@ def __init__(self, num_threads=None):
         :type num_threads: int
         """
         super().__init__()
+        self._callback_iter = None
         if num_threads is None:
             try:
                 num_threads = multiprocessing.cpu_count()
@@ -415,8 +421,12 @@ def __init__(self, num_threads=None):
         self._executor = ThreadPoolExecutor(num_threads)
 
     def spin_once(self, timeout_sec=None):
+        # Reuse the same callback iterator to avoid unecessary calls to rcl_wait
+        if self._callback_iter is None:
+            self._callback_iter = self.wait_for_ready_callbacks(timeout_sec=timeout_sec)
         try:
-            handler, entity, node = next(self.wait_for_ready_callbacks(timeout_sec=timeout_sec))
-            self._executor.submit(handler)
+            handler, entity, node = next(self._callback_iter)
         except StopIteration:
-            pass
+            self._callback_iter = None
+        else:
+            self._executor.submit(handler)

From 3628db44038ef08344eab244040a820f99dd3795 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 1 Nov 2017 18:29:25 -0700
Subject: [PATCH 42/49] moved some wait_set code into C

---
 rclpy/rclpy/executors.py       |  48 ++++-----
 rclpy/rclpy/graph_listener.py  | 104 ++++++++++---------
 rclpy/rclpy/guard_condition.py |  12 +++
 rclpy/rclpy/wait_set.py        | 145 +++++---------------------
 rclpy/src/rclpy/_rclpy.c       | 183 +++++++++++++++++----------------
 rclpy/test/test_timer.py       |   5 +
 rclpy/test/test_wait_set.py    | 104 +++++++++----------
 7 files changed, 264 insertions(+), 337 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 37a070961..b3e02b223 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -17,6 +17,8 @@
 from threading import Condition
 from threading import Lock
 
+from rclpy.guard_condition import GuardCondition
+from rclpy.guard_condition import KeyboardInterruptGuardCondition
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.timer import WallTimer
 from rclpy.utilities import ok
@@ -79,13 +81,9 @@ def __init__(self):
         self._nodes = set()
         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
-        self._guard_condition_handle = gc_handle
+        self._guard_condition = GuardCondition(None, None)
         # Triggered by signal handler for sigint
-        (sigint_gc, sigint_gc_handle) = _rclpy.rclpy_get_sigint_guard_condition()
-        self._sigint_gc = sigint_gc
-        self._sigint_gc_handle = sigint_gc_handle
+        self._sigint_gc = KeyboardInterruptGuardCondition()
         # True if shutdown has been called
         self._is_shutdown = False
         self._work_tracker = _WorkTracker()
@@ -106,14 +104,17 @@ def shutdown(self, timeout_sec=None):
         # Clean up stuff that won't be used anymore
         with self._nodes_lock:
             self._nodes = set()
-        _rclpy.rclpy_destroy_entity(self._guard_condition)
+        _rclpy.rclpy_destroy_entity(self._guard_condition.guard_handle)
+        _rclpy.rclpy_destroy_entity(self._sigint_gc.guard_handle)
 
         self._guard_condition = None
+        self._sigint_gc = None
         return True
 
     def __del__(self):
         if self._guard_condition is not None:
-            _rclpy.rclpy_destroy_entity(self._guard_condition)
+            _rclpy.rclpy_destroy_entity(self._guard_condition.guard_handle)
+            _rclpy.rclpy_destroy_entity(self._sigint_gc.guard_handle)
 
     def add_node(self, node):
         """
@@ -126,7 +127,7 @@ def add_node(self, node):
         with self._nodes_lock:
             self._nodes.add(node)
             # Rebuild the wait set so it includes this new node
-            _rclpy.rclpy_trigger_guard_condition(self._guard_condition)
+            self._guard_condition.trigger()
 
     def get_nodes(self):
         """
@@ -224,14 +225,14 @@ def handler():
             if is_shutdown or not entity.callback_group.beginning_execution(entity):
                 # Didn't get the callback, or the executor has been ordered to stop
                 entity._executor_event = False
-                _rclpy.rclpy_trigger_guard_condition(gc)
+                gc.trigger()
                 return
             with work_tracker:
                 arg = take_from_wait_list(entity)
 
                 # Signal that this has been 'taken' and can be added back to the wait list
                 entity._executor_event = False
-                _rclpy.rclpy_trigger_guard_condition(gc)
+                gc.trigger()
 
                 try:
                     call_callback(entity, arg)
@@ -239,7 +240,7 @@ def handler():
                     entity.callback_group.ending_execution(entity)
                     # Signal that work has been done so the next callback in a mutually exclusive
                     # callback group can get executed
-                    _rclpy.rclpy_trigger_guard_condition(gc)
+                    gc.trigger()
         return handler
 
     def _can_execute(self, entity):
@@ -349,26 +350,25 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
             if timeout_timer is not None:
                 timers.append(timeout_timer)
 
-            # Construct a wait set
-            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(self._sigint_gc, self._sigint_gc_handle)
-                wait_set.add_guard_condition(self._guard_condition, self._guard_condition_handle)
+            guards.append(self._sigint_gc)
+            guards.append(self._guard_condition)
 
+            # Construct a wait set
+            with WaitSet(subscriptions, guards, timers, clients, services) as wait_set:
                 # Wait for something to become ready
                 wait_set.wait(timeout_nsec)
 
                 # Check sigint guard condition
-                if wait_set.is_ready(self._sigint_gc_handle):
+                if wait_set.is_ready(self._sigint_gc.guard_handle):
                     raise KeyboardInterrupt()
 
+                guards.remove(self._sigint_gc)
+                guards.remove(self._guard_condition)
+
                 # 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
+                for gc in guards:
+                    if wait_set.is_ready(gc.guard_pointer):
+                        gc._executor_triggered = True
 
                 yielded_work = yield from self._new_callbacks(nodes, wait_set)
 
diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index 8d75b96ad..7354360c9 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -16,6 +16,8 @@
 import traceback
 
 from rclpy.constants import S_TO_NS
+from rclpy.guard_condition import GuardCondition
+from rclpy.guard_condition import NodeGraphGuardCondition
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 import rclpy.logging
 from rclpy.timer import WallTimer
@@ -32,15 +34,15 @@ def __new__(cls, *args, **kwargs):
         return getattr(cls, '__singleton')
 
     def __init__(self):
+        # Maps guard_condition pointers to guard condition instances
+        self._guards = {}
         # Maps guard_condition pointers to a list of subscriber callbacks
         self._callbacks = {}
         # Maps timer instances to timer callbacks
         self._timers = {}
-        self._gc_handle, self._gc_pointer = _rclpy.rclpy_create_guard_condition()
+        self._gc = GuardCondition(None, None)
         self._thread = None
         self._lock = threading.RLock()
-        self._wait_set = WaitSet()
-        self._wait_set.add_guard_condition(self._gc_handle, self._gc_pointer)
 
     def __del__(self):
         self.destroy()
@@ -52,7 +54,7 @@ def destroy(cls):
             with self._lock:
                 setattr(cls, '__singleton', None)
                 self._thread = None
-            _rclpy.rclpy_destroy_entity(self._gc_handle)
+            _rclpy.rclpy_destroy_entity(self._gc.guard_handle)
 
     def _try_start_thread(self):
         # Assumes lock is already held
@@ -73,8 +75,7 @@ def add_timer(self, timer_period_ns, callback):
         with self._lock:
             tmr = WallTimer(callback, None, timer_period_ns)
             self._timers[tmr] = callback
-            self._wait_set.add_timer(tmr.timer_handle, tmr.timer_pointer)
-
+            self._gc.trigger()
             self._try_start_thread()
             return tmr
 
@@ -88,7 +89,7 @@ def remove_timer(self, timer):
         with self._lock:
             if timer in self._timers:
                 del self._timers[timer]
-                self._wait_set.remove_timer(timer.timer_pointer)
+                self._gc.trigger()
 
     def add_callback(self, node_handle, callback):
         """
@@ -100,15 +101,15 @@ def add_callback(self, node_handle, callback):
         :type callback: callable
         """
         with self._lock:
-            gc_handle, gc_pointer = _rclpy.rclpy_get_graph_guard_condition(node_handle)
-            if gc_pointer not in self._callbacks:
+            gc = NodeGraphGuardCondition(node_handle)
+            if gc.guard_pointer not in self._callbacks:
                 # new node, rebuild wait set
-                self._callbacks[gc_pointer] = []
-                self._wait_set.add_guard_condition(gc_handle, gc_pointer)
-                _rclpy.rclpy_trigger_guard_condition(self._gc_handle)
+                self._callbacks[gc.guard_pointer] = []
+                self._guards[gc.guard_pointer] = gc
+                self._gc.trigger()
 
             # Add a callback
-            self._callbacks[gc_pointer].append(callback)
+            self._callbacks[gc.guard_pointer].append(callback)
 
             self._try_start_thread()
             # start the thread if necessary
@@ -127,49 +128,56 @@ def remove_callback(self, node_handle, callback):
         :type callback: callable
         """
         with self._lock:
-            gc_handle, gc_pointer = _rclpy.rclpy_get_graph_guard_condition(node_handle)
-            if gc_pointer in self._callbacks:
+            gc = NodeGraphGuardCondition(node_handle)
+            if gc.guard_pointer in self._callbacks:
                 # Remove the callback
-                self._callbacks[gc_pointer].remove(callback)
+                callbacks = self._callbacks[gc.guard_pointer]
+                callbacks.remove(callback)
 
-                if not self._callbacks[gc_pointer]:
+                if not callbacks:
                     # last subscriber for this node, remove the node and rebuild the wait set
-                    del self._callbacks[gc_pointer]
-                    self._wait_set.remove_guard_condition(gc_pointer)
-                    _rclpy.rclpy_trigger_guard_condition(self._gc_handle)
+                    del self._callbacks[gc.guard_pointer]
+                    del self._guards[gc.guard_pointer]
+                    self._gc.trigger()
 
     def _runner(self):
         while True:
-            # Wait 1 second
-            self._wait_set.wait(S_TO_NS)
-
             with self._lock:
-                # Shutdown if necessary
-                if not ok():
-                    self.destroy()
-                    break
-
-                # notify graph event subscribers
-                if not self._thread:
-                    # Asked to shut down thread
-                    return
-                ready_callbacks = []
-                # Guard conditions
-                for gc_pointer, callback_list in self._callbacks.items():
-                    if self._wait_set.is_ready(gc_pointer):
-                        for callback in callback_list:
+                guards = list(self._guards.values())
+                timers = list(self._timers.keys())
+                guards.append(self._gc)
+
+            with WaitSet([], guards, timers, [], []) as wait_set:
+                # Wait 1 second
+                wait_set.wait(S_TO_NS)
+
+                with self._lock:
+                    # Shutdown if necessary
+                    if not ok():
+                        self.destroy()
+                        break
+
+                    # notify graph event subscribers
+                    if not self._thread:
+                        # Asked to shut down thread
+                        return
+                    ready_callbacks = []
+                    # Guard conditions
+                    for gc_pointer, callback_list in self._callbacks.items():
+                        if wait_set.is_ready(gc_pointer):
+                            for callback in callback_list:
+                                ready_callbacks.append(callback)
+                    # Timers
+                    for tmr, callback in self._timers.items():
+                        if wait_set.is_ready(tmr.timer_pointer):
                             ready_callbacks.append(callback)
-                # Timers
-                for tmr, callback in self._timers.items():
-                    if self._wait_set.is_ready(tmr.timer_pointer):
-                        ready_callbacks.append(callback)
-                        _rclpy.rclpy_call_timer(tmr.timer_handle)
-                # Call callbacks
-                for callback in ready_callbacks:
-                    try:
-                        callback()
-                    except Exception:
-                        rclpy.logging.logwarn(traceback.format_exc())
+                            _rclpy.rclpy_call_timer(tmr.timer_handle)
+                    # Call callbacks
+                    for callback in ready_callbacks:
+                        try:
+                            callback()
+                        except Exception:
+                            rclpy.logging.logwarn(traceback.format_exc())
 
 
 class GraphEventSubscription:
diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py
index fe60b3caf..5ec881f00 100644
--- a/rclpy/rclpy/guard_condition.py
+++ b/rclpy/rclpy/guard_condition.py
@@ -28,3 +28,15 @@ def __init__(self, callback, callback_group):
 
     def trigger(self):
         _rclpy.rclpy_trigger_guard_condition(self.guard_handle)
+
+
+class KeyboardInterruptGuardCondition:
+
+    def __init__(self):
+        self.guard_handle, self.guard_pointer = _rclpy.rclpy_get_sigint_guard_condition()
+
+
+class NodeGraphGuardCondition:
+
+    def __init__(self, node_handle):
+        self.guard_handle, self.guard_pointer = _rclpy.rclpy_get_graph_guard_condition(node_handle)
diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py
index 58c420a96..5e9115c9b 100644
--- a/rclpy/rclpy/wait_set.py
+++ b/rclpy/rclpy/wait_set.py
@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import threading
+import itertools
 
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 
@@ -20,138 +20,43 @@
 class WaitSet:
     """Provide conveneint methods and destroy the wait set when garbage collected."""
 
-    def __init__(self):
+    def __init__(self, subscriptions, guards, timers, clients, services):
         # List of entity pointers (the python integer, not the PyCapsule) that are ready
         self._ready_pointers = []
+        self._subscriptions = [s.subscription_handle for s in subscriptions]
+        self._guards = [g.guard_handle for g in guards]
+        self._timers = [t.timer_handle for t in timers]
+        self._clients = [c.client_handle for c in clients]
+        self._services = [s.service_handle for s in services]
 
-        # maps pointers (integers) to handles (PyCapsule)
-        self._subscriptions = {}
-        self._guard_conditions = {}
-        self._timers = {}
-        self._clients = {}
-        self._services = {}
-
-        # Set when the wait set needs to be built or rebuilt
-        self._need_resize = True
         self._wait_set = None
-        # rcl_wait is not thread safe, so prevent multiple wait calls at once
-        self._wait_lock = threading.Lock()
-
-    def destroy(self):
-        if self._wait_set is not None:
-            _rclpy.rclpy_destroy_wait_set(self._wait_set)
-            self._wait_set = None
-
-    def __del__(self):
-        self.destroy()
 
     def __enter__(self):
+        self._wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
+        _rclpy.rclpy_wait_set_init(
+            self._wait_set,
+            len(self._subscriptions),
+            len(self._guards),
+            len(self._timers),
+            len(self._clients),
+            len(self._services))
         return self
 
     def __exit__(self, t, v, tb):
-        self.destroy()
-
-    def add_subscription(self, subscription_handle, subscription_pointer):
-        self._subscriptions[subscription_pointer] = subscription_handle
-        self._need_resize = True
-
-    def add_subscriptions(self, subscriptions):
-        for sub in subscriptions:
-            self.add_subscription(sub.subscription_handle, sub.subscription_pointer)
-
-    def remove_subscription(self, subscription_pointer):
-        del self._subscriptions[subscription_pointer]
-        self._need_resize = True
-
-    def add_guard_condition(self, gc_handle, gc_pointer):
-        self._guard_conditions[gc_pointer] = gc_handle
-        self._need_resize = True
-
-    def add_guard_conditions(self, guards):
-        for gc in guards:
-            self.add_guard_condition(gc.guard_handle, gc.guard_pointer)
-
-    def remove_guard_condition(self, gc_pointer):
-        del self._guard_conditions[gc_pointer]
-        self._need_resize = True
-
-    def add_timer(self, timer_handle, timer_pointer):
-        self._timers[timer_pointer] = timer_handle
-        self._need_resize = True
-
-    def add_timers(self, timers):
-        for tmr in timers:
-            self.add_timer(tmr.timer_handle, tmr.timer_pointer)
-
-    def remove_timer(self, timer_pointer):
-        del self._timers[timer_pointer]
-        self._need_resize = True
-
-    def add_client(self, client_handle, client_pointer):
-        self._clients[client_pointer] = client_handle
-        self._need_resize = True
-
-    def add_clients(self, clients):
-        for cli in clients:
-            self.add_client(cli.client_handle, cli.client_pointer)
-
-    def remove_client(self, client_pointer):
-        del self._clients[client_pointer]
-        self._need_resize = True
-
-    def add_service(self, service_handle, service_pointer):
-        self._services[service_pointer] = service_handle
-        self._need_resize = True
-
-    def add_services(self, services):
-        for srv in services:
-            self.add_service(srv.service_handle, srv.service_pointer)
-
-    def remove_service(self, service_pointer):
-        del self._services[service_pointer]
-        self._need_resize = True
+        _rclpy.rclpy_destroy_wait_set(self._wait_set)
 
     def wait(self, timeout_nsec):
-        with self._wait_lock:
-            if self._need_resize:
-                self._need_resize = False
-                if self._wait_set is not None:
-                    _rclpy.rclpy_destroy_wait_set(self._wait_set)
-
-                self._wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
-                _rclpy.rclpy_wait_set_init(
-                    self._wait_set,
-                    len(self._subscriptions),
-                    len(self._guard_conditions),
-                    len(self._timers),
-                    len(self._clients),
-                    len(self._services))
-
-            _rclpy.rclpy_wait_set_clear_entities('subscription', self._wait_set)
-            _rclpy.rclpy_wait_set_clear_entities('guard_condition', self._wait_set)
-            _rclpy.rclpy_wait_set_clear_entities('timer', self._wait_set)
-            _rclpy.rclpy_wait_set_clear_entities('client', self._wait_set)
-            _rclpy.rclpy_wait_set_clear_entities('service', self._wait_set)
-
-            for handle in self._subscriptions.values():
-                _rclpy.rclpy_wait_set_add_entity('subscription', self._wait_set, handle)
-            for handle in self._guard_conditions.values():
-                _rclpy.rclpy_wait_set_add_entity('guard_condition', self._wait_set, handle)
-            for handle in self._timers.values():
-                _rclpy.rclpy_wait_set_add_entity('timer', self._wait_set, handle)
-            for handle in self._clients.values():
-                _rclpy.rclpy_wait_set_add_entity('client', self._wait_set, handle)
-            for handle in self._services.values():
-                _rclpy.rclpy_wait_set_add_entity('service', self._wait_set, handle)
+        # Populate wait set
+        _rclpy.rclpy_wait_set_clear_entities(self._wait_set)
+        entities = itertools.chain(
+            self._subscriptions, self._guards, self._timers, self._clients, self._services)
+        _rclpy.rclpy_wait_set_add_entities(self._wait_set, entities)
 
-            _rclpy.rclpy_wait(self._wait_set, timeout_nsec)
+        # Wait
+        _rclpy.rclpy_wait(self._wait_set, timeout_nsec)
 
-            ws = self._wait_set
-            self._ready_pointers = _rclpy.rclpy_get_ready_entities('subscription', ws)
-            self._ready_pointers.extend(_rclpy.rclpy_get_ready_entities('guard_condition', ws))
-            self._ready_pointers.extend(_rclpy.rclpy_get_ready_entities('timer', ws))
-            self._ready_pointers.extend(_rclpy.rclpy_get_ready_entities('client', ws))
-            self._ready_pointers.extend(_rclpy.rclpy_get_ready_entities('service', ws))
+        # Get results
+        self._ready_pointers = _rclpy.rclpy_get_ready_entities(self._wait_set)
 
     def is_ready(self, entity_pointer):
         return entity_pointer in self._ready_pointers
diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index 3e167178b..640e82916 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -1801,21 +1801,19 @@ rclpy_wait_set_init(PyObject * Py_UNUSED(self), PyObject * args)
   Py_RETURN_NONE;
 }
 
-/// Clear all the pointers of a given wait_set field
+/// Clear all the pointers of all wait_set fields
 /**
  * Raises RuntimeError if the entity type is unknown or any rcl error occurs
  *
- * \param[in] entity_type string defining the entity ["subscription, client, service"]
  * \param[in] pywait_set Capsule pointing to the waitset structure
- * \return NULL
+ * \return None
  */
 static PyObject *
 rclpy_wait_set_clear_entities(PyObject * Py_UNUSED(self), PyObject * args)
 {
-  const char * entity_type;
   PyObject * pywait_set;
 
-  if (!PyArg_ParseTuple(args, "zO", &entity_type, &pywait_set)) {
+  if (!PyArg_ParseTuple(args, "O", &pywait_set)) {
     return NULL;
   }
 
@@ -1823,86 +1821,91 @@ rclpy_wait_set_clear_entities(PyObject * Py_UNUSED(self), PyObject * args)
   if (!wait_set) {
     return NULL;
   }
-  rcl_ret_t ret;
-  if (0 == strcmp(entity_type, "subscription")) {
-    ret = rcl_wait_set_clear_subscriptions(wait_set);
-  } else if (0 == strcmp(entity_type, "client")) {
-    ret = rcl_wait_set_clear_clients(wait_set);
-  } else if (0 == strcmp(entity_type, "service")) {
-    ret = rcl_wait_set_clear_services(wait_set);
-  } else if (0 == strcmp(entity_type, "timer")) {
-    ret = rcl_wait_set_clear_timers(wait_set);
-  } else if (0 == strcmp(entity_type, "guard_condition")) {
-    ret = rcl_wait_set_clear_guard_conditions(wait_set);
-  } else {
-    ret = RCL_RET_ERROR;  // to avoid a linter warning
-    PyErr_Format(PyExc_RuntimeError,
-      "'%s' is not a known entity", entity_type);
-    return NULL;
-  }
-  if (ret != RCL_RET_OK) {
+
+  if (
+    RCL_RET_OK != rcl_wait_set_clear_subscriptions(wait_set) ||
+    RCL_RET_OK != rcl_wait_set_clear_clients(wait_set) ||
+    RCL_RET_OK != rcl_wait_set_clear_services(wait_set) ||
+    RCL_RET_OK != rcl_wait_set_clear_timers(wait_set) ||
+    RCL_RET_OK != rcl_wait_set_clear_guard_conditions(wait_set))
+  {
     PyErr_Format(PyExc_RuntimeError,
-      "Failed to clear '%s' from wait set: %s", entity_type, rcl_get_error_string_safe());
+      "Failed to clear from wait set: %s", rcl_get_error_string_safe());
     rcl_reset_error();
     return NULL;
   }
-  Py_RETURN_TRUE;
+  Py_RETURN_NONE;
 }
 
-/// Add an entity to the waitset structure
+/// Add entities to the waitset structure
 /**
- * Raises RuntimeError if the entity type is unknown or any rcl error occurrs
+ * Raises RuntimeError if any entity type is unknown or any rcl error occurs
+ * Raises ValueError if an entity is not a valid PyCapsule
  *
- * \param[in] entity_type string defining the entity ["subscription, client, service"]
  * \param[in] pywait_set Capsule pointing to the waitset structure
- * \param[in] pyentity Capsule pointing to the entity to add
+ * \param[in] pyentities Iterable of PyCapsule belonging to an entity (sub, pub, timer, ...)
  * \return None
  */
 static PyObject *
-rclpy_wait_set_add_entity(PyObject * Py_UNUSED(self), PyObject * args)
+rclpy_wait_set_add_entities(PyObject * Py_UNUSED(self), PyObject * args)
 {
-  const char * entity_type;
   PyObject * pywait_set;
-  PyObject * pyentity;
+  PyObject * pyentities;
 
-  if (!PyArg_ParseTuple(args, "zOO", &entity_type, &pywait_set, &pyentity)) {
+  if (!PyArg_ParseTuple(args, "OO", &pywait_set, &pyentities)) {
     return NULL;
   }
+
   rcl_ret_t ret;
   rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
   if (!wait_set) {
     return NULL;
   }
-  if (0 == strcmp(entity_type, "subscription")) {
-    rcl_subscription_t * subscription =
-      (rcl_subscription_t *)PyCapsule_GetPointer(pyentity, "rcl_subscription_t");
-    ret = rcl_wait_set_add_subscription(wait_set, subscription);
-  } else if (0 == strcmp(entity_type, "client")) {
-    rcl_client_t * client =
-      (rcl_client_t *)PyCapsule_GetPointer(pyentity, "rcl_client_t");
-    ret = rcl_wait_set_add_client(wait_set, client);
-  } else if (0 == strcmp(entity_type, "service")) {
-    rcl_service_t * service =
-      (rcl_service_t *)PyCapsule_GetPointer(pyentity, "rcl_service_t");
-    ret = rcl_wait_set_add_service(wait_set, service);
-  } else if (0 == strcmp(entity_type, "timer")) {
-    rcl_timer_t * timer =
-      (rcl_timer_t *)PyCapsule_GetPointer(pyentity, "rcl_timer_t");
-    ret = rcl_wait_set_add_timer(wait_set, timer);
-  } else if (0 == strcmp(entity_type, "guard_condition")) {
-    rcl_guard_condition_t * guard_condition =
-      (rcl_guard_condition_t *)PyCapsule_GetPointer(pyentity, "rcl_guard_condition_t");
-    ret = rcl_wait_set_add_guard_condition(wait_set, guard_condition);
-  } else {
-    ret = RCL_RET_ERROR;  // to avoid a linter warning
-    PyErr_Format(PyExc_RuntimeError,
-      "'%s' is not a known entity", entity_type);
+
+  PyObject * pyiterator = PyObject_GetIter(pyentities);
+  if (!pyiterator) {
     return NULL;
   }
-  if (ret != RCL_RET_OK) {
-    PyErr_Format(PyExc_RuntimeError,
-      "Failed to add '%s' to wait set: %s", entity_type, rcl_get_error_string_safe());
-    rcl_reset_error();
+
+  PyObject * pyentity;
+  while ((pyentity = PyIter_Next(pyiterator))) {
+    if (PyCapsule_IsValid(pyentity, "rcl_subscription_t")) {
+      rcl_subscription_t * subscription =
+        (rcl_subscription_t *)PyCapsule_GetPointer(pyentity, "rcl_subscription_t");
+      ret = rcl_wait_set_add_subscription(wait_set, subscription);
+    } else if (PyCapsule_IsValid(pyentity, "rcl_client_t")) {
+      rcl_client_t * client = (rcl_client_t *)PyCapsule_GetPointer(pyentity, "rcl_client_t");
+      ret = rcl_wait_set_add_client(wait_set, client);
+    } else if (PyCapsule_IsValid(pyentity, "rcl_service_t")) {
+      rcl_service_t * service = (rcl_service_t *)PyCapsule_GetPointer(pyentity, "rcl_service_t");
+      ret = rcl_wait_set_add_service(wait_set, service);
+    } else if (PyCapsule_IsValid(pyentity, "rcl_timer_t")) {
+      rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pyentity, "rcl_timer_t");
+      ret = rcl_wait_set_add_timer(wait_set, timer);
+    } else if (PyCapsule_IsValid(pyentity, "rcl_guard_condition_t")) {
+      rcl_guard_condition_t * guard_condition =
+        (rcl_guard_condition_t *)PyCapsule_GetPointer(pyentity, "rcl_guard_condition_t");
+      ret = rcl_wait_set_add_guard_condition(wait_set, guard_condition);
+    }
+
+    if (ret != RCL_RET_OK) {
+      const char * entity_type = PyCapsule_GetName(pyentity);
+      if (entity_type) {
+        PyErr_Format(PyExc_RuntimeError,
+          "Failed to add '%s' to wait set: %s", entity_type, rcl_get_error_string_safe());
+      }
+      // else { // PyCapsule_GetName set an exception }
+      rcl_reset_error();
+      Py_DECREF(pyentity);
+      break;
+    } else {
+      Py_DECREF(pyentity);
+    }
+  }
+
+  Py_DECREF(pyiterator);
+
+  if (PyErr_Occurred()) {
     return NULL;
   }
   Py_RETURN_NONE;
@@ -1947,33 +1950,38 @@ rclpy_destroy_wait_set(PyObject * Py_UNUSED(self), PyObject * args)
 }
 
 #define GET_LIST_READY_ENTITIES(ENTITY_TYPE) \
-  size_t idx; \
-  size_t idx_max; \
-  idx_max = wait_set->size_of_ ## ENTITY_TYPE ## s; \
-  const rcl_ ## ENTITY_TYPE ## _t ** struct_ptr = wait_set->ENTITY_TYPE ## s; \
-  for (idx = 0; idx < idx_max; idx ++) { \
-    if (struct_ptr[idx]) { \
-      PyList_Append( \
-        entity_ready_list, \
-        PyLong_FromUnsignedLongLong((uint64_t) & struct_ptr[idx]->impl)); \
+  { \
+    size_t idx; \
+    size_t idx_max; \
+    idx_max = wait_set->size_of_ ## ENTITY_TYPE ## s; \
+    const rcl_ ## ENTITY_TYPE ## _t ** struct_ptr = wait_set->ENTITY_TYPE ## s; \
+    for (idx = 0; idx < idx_max; idx ++) { \
+      if (struct_ptr[idx]) { \
+        PyObject * pyptr = PyLong_FromUnsignedLongLong((uint64_t) & struct_ptr[idx]->impl); \
+        if (!pyptr) { \
+          Py_DECREF(entity_ready_list); \
+          return NULL; \
+        } \
+        if (-1 == PyList_Append(entity_ready_list, pyptr)) { \
+          Py_DECREF(pyptr); \
+          Py_DECREF(entity_ready_list); \
+          return NULL; \
+        } \
+      } \
     } \
-  } \
-  return entity_ready_list;
+  }
 /// Get list of non-null entities in waitset
 /**
  * Raises ValueError if pywait_set is not a wait_set capsule
- * Raises RuntimeError if the entity type is not known
  *
- * \param[in] entity_type string defining the entity ["subscription, client, service"]
  * \param[in] pywait_set Capsule pointing to the waitset structure
  * \return List of wait_set entities pointers ready for take
  */
 static PyObject *
 rclpy_get_ready_entities(PyObject * Py_UNUSED(self), PyObject * args)
 {
-  const char * entity_type;
   PyObject * pywait_set;
-  if (!PyArg_ParseTuple(args, "zO", &entity_type, &pywait_set)) {
+  if (!PyArg_ParseTuple(args, "O", &pywait_set)) {
     return NULL;
   }
 
@@ -1983,21 +1991,14 @@ rclpy_get_ready_entities(PyObject * Py_UNUSED(self), PyObject * args)
   }
 
   PyObject * entity_ready_list = PyList_New(0);
-  if (0 == strcmp(entity_type, "subscription")) {
-    GET_LIST_READY_ENTITIES(subscription)
-  } else if (0 == strcmp(entity_type, "client")) {
-    GET_LIST_READY_ENTITIES(client)
-  } else if (0 == strcmp(entity_type, "service")) {
-    GET_LIST_READY_ENTITIES(service)
-  } else if (0 == strcmp(entity_type, "timer")) {
-    GET_LIST_READY_ENTITIES(timer)
-  } else if (0 == strcmp(entity_type, "guard_condition")) {
-    GET_LIST_READY_ENTITIES(guard_condition)
-  } else {
-    PyErr_Format(PyExc_RuntimeError,
-      "'%s' is not a known entity", entity_type);
+  if (!entity_ready_list) {
     return NULL;
   }
+  GET_LIST_READY_ENTITIES(subscription)
+  GET_LIST_READY_ENTITIES(client)
+  GET_LIST_READY_ENTITIES(service)
+  GET_LIST_READY_ENTITIES(timer)
+  GET_LIST_READY_ENTITIES(guard_condition)
 
   return entity_ready_list;
 }
@@ -2748,8 +2749,8 @@ static PyMethodDef rclpy_methods[] = {
   },
 
   {
-    "rclpy_wait_set_add_entity", rclpy_wait_set_add_entity, METH_VARARGS,
-    "rclpy_wait_set_add_entity."
+    "rclpy_wait_set_add_entities", rclpy_wait_set_add_entities, METH_VARARGS,
+    "rclpy_wait_set_add_entities."
   },
 
   {
diff --git a/rclpy/test/test_timer.py b/rclpy/test/test_timer.py
index 37c63390b..0f513a22f 100644
--- a/rclpy/test/test_timer.py
+++ b/rclpy/test/test_timer.py
@@ -193,3 +193,8 @@ def test_timer_cancel_reset_1000hertz():
         raise SkipTest
     func_launch(
         func_cancel_reset_timer, ['0.001'], "didn't receive the expected number of callbacks")
+
+
+if __name__ == '__main__':
+    import cProfile
+    cProfile.run("func_number_callbacks(['0.001'])", "timer_test.prof")
diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
index f58d7f25d..08f9dac7b 100644
--- a/rclpy/test/test_wait_set.py
+++ b/rclpy/test/test_wait_set.py
@@ -17,7 +17,9 @@
 from rcl_interfaces.srv import GetParameters
 import rclpy
 from rclpy.constants import S_TO_NS
+from rclpy.guard_condition import GuardCondition
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.timer import WallTimer
 from rclpy.wait_set import WaitSet
 from std_msgs.msg import String
 
@@ -35,95 +37,89 @@ def tearDownClass(cls):
         rclpy.shutdown()
 
     def test_guard_condition_ready(self):
-        ws = WaitSet()
-        gc_handle, gc_pointer = _rclpy.rclpy_create_guard_condition()
+        gc = GuardCondition(None, None)
         try:
-            ws.add_guard_condition(gc_handle, gc_pointer)
-            self.assertFalse(ws.is_ready(gc_pointer))
+            with WaitSet([], [gc], [], [], []) as ws:
+                self.assertFalse(ws.is_ready(gc.guard_pointer))
 
-            ws.wait(0)
-            self.assertFalse(ws.is_ready(gc_pointer))
+                ws.wait(0)
+                self.assertFalse(ws.is_ready(gc.guard_pointer))
 
-            _rclpy.rclpy_trigger_guard_condition(gc_handle)
-            ws.wait(0)
-            self.assertTrue(ws.is_ready(gc_pointer))
+                gc.trigger()
+                ws.wait(0)
+                self.assertTrue(ws.is_ready(gc.guard_pointer))
 
-            ws.wait(0)
-            self.assertFalse(ws.is_ready(gc_pointer))
+                ws.wait(0)
+                self.assertFalse(ws.is_ready(gc.guard_pointer))
         finally:
-            _rclpy.rclpy_destroy_entity(gc_handle)
+            _rclpy.rclpy_destroy_entity(gc.guard_handle)
 
     def test_timer_ready(self):
-        ws = WaitSet()
-        timer_handle, timer_pointer = _rclpy.rclpy_create_timer(int(0.1 * S_TO_NS))
+        timer = WallTimer(None, None, int(0.1 * S_TO_NS))
         try:
-            ws.add_timer(timer_handle, timer_pointer)
-            self.assertFalse(ws.is_ready(timer_pointer))
+            with WaitSet([], [], [timer], [], []) as ws:
+                self.assertFalse(ws.is_ready(timer.timer_pointer))
 
-            ws.wait(0)
-            self.assertFalse(ws.is_ready(timer_pointer))
+                ws.wait(0)
+                self.assertFalse(ws.is_ready(timer.timer_pointer))
 
-            ws.wait(int(0.1 * S_TO_NS))
-            self.assertTrue(ws.is_ready(timer_pointer))
+                ws.wait(int(0.1 * S_TO_NS))
+                self.assertTrue(ws.is_ready(timer.timer_pointer))
 
-            _rclpy.rclpy_call_timer(timer_handle)
-            ws.wait(0)
-            self.assertFalse(ws.is_ready(timer_pointer))
+                _rclpy.rclpy_call_timer(timer.timer_handle)
+                ws.wait(0)
+                self.assertFalse(ws.is_ready(timer.timer_pointer))
         finally:
-            _rclpy.rclpy_destroy_entity(timer_handle)
+            _rclpy.rclpy_destroy_entity(timer.timer_handle)
 
     def test_subscriber_ready(self):
-        ws = WaitSet()
         sub = self.node.create_subscription(String, 'chatter', lambda msg: print(msg))
         pub = self.node.create_publisher(String, 'chatter')
-
         try:
-            ws.add_subscription(sub.subscription_handle, sub.subscription_pointer)
-            self.assertFalse(ws.is_ready(sub.subscription_pointer))
+            with WaitSet([sub], [], [], [], []) as ws:
+                self.assertFalse(ws.is_ready(sub.subscription_pointer))
 
-            ws.wait(0)
-            self.assertFalse(ws.is_ready(sub.subscription_pointer))
+                ws.wait(0)
+                self.assertFalse(ws.is_ready(sub.subscription_pointer))
 
-            msg = String()
-            msg.data = 'Hello World'
-            pub.publish(msg)
+                msg = String()
+                msg.data = 'Hello World'
+                pub.publish(msg)
 
-            ws.wait(5 * S_TO_NS)
-            self.assertTrue(ws.is_ready(sub.subscription_pointer))
+                ws.wait(5 * S_TO_NS)
+                self.assertTrue(ws.is_ready(sub.subscription_pointer))
 
-            _rclpy.rclpy_take(sub.subscription_handle, sub.msg_type)
-            ws.wait(0)
-            self.assertFalse(ws.is_ready(sub.subscription_pointer))
+                _rclpy.rclpy_take(sub.subscription_handle, sub.msg_type)
+                ws.wait(0)
+                self.assertFalse(ws.is_ready(sub.subscription_pointer))
         finally:
             self.node.destroy_publisher(pub)
             self.node.destroy_subscription(sub)
 
     def test_server_ready(self):
-        ws = WaitSet()
         cli = self.node.create_client(GetParameters, 'get/parameters')
         srv = self.node.create_service(
             GetParameters, 'get/parameters', lambda req: GetParameters.Response())
 
         try:
-            ws.add_client(cli.client_handle, cli.client_pointer)
-            ws.add_service(srv.service_handle, srv.service_pointer)
-            self.assertFalse(ws.is_ready(cli.client_pointer))
-            self.assertFalse(ws.is_ready(srv.service_pointer))
+            with WaitSet([], [], [], [cli], [srv]) as ws:
+                self.assertFalse(ws.is_ready(cli.client_pointer))
+                self.assertFalse(ws.is_ready(srv.service_pointer))
 
-            ws.wait(0)
-            self.assertFalse(ws.is_ready(cli.client_pointer))
-            self.assertFalse(ws.is_ready(srv.service_pointer))
+                ws.wait(0)
+                self.assertFalse(ws.is_ready(cli.client_pointer))
+                self.assertFalse(ws.is_ready(srv.service_pointer))
 
-            cli.wait_for_service()
-            cli.call(GetParameters.Request())
+                cli.wait_for_service()
+                cli.call(GetParameters.Request())
 
-            ws.wait(5 * S_TO_NS)
-            # TODO(sloretz) test client after it's wait_for_future() API is sorted out
-            self.assertTrue(ws.is_ready(srv.service_pointer))
+                ws.wait(5 * S_TO_NS)
+                # TODO(sloretz) test client after it's wait_for_future() API is sorted out
+                self.assertTrue(ws.is_ready(srv.service_pointer))
 
-            _rclpy.rclpy_take_request(srv.service_handle, srv.srv_type.Request)
-            ws.wait(0)
-            self.assertFalse(ws.is_ready(srv.service_pointer))
+                _rclpy.rclpy_take_request(srv.service_handle, srv.srv_type.Request)
+                ws.wait(0)
+                self.assertFalse(ws.is_ready(srv.service_pointer))
         finally:
             self.node.destroy_client(cli)
             self.node.destroy_service(srv)

From b71d1c5407062c7eb6d0dd8bcb9fa093dc8848ae Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 1 Nov 2017 18:59:42 -0700
Subject: [PATCH 43/49] Avoid another list comprehension

---
 rclpy/rclpy/executors.py |  6 +++---
 rclpy/rclpy/wait_set.py  | 25 +++++++++++++++----------
 2 files changed, 18 insertions(+), 13 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index b3e02b223..0fbb63a7b 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -336,11 +336,11 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                 services.extend(node.services)
                 guards.extend(node.guards)
 
-            subscriptions = filter(self._can_execute, subscriptions)
+            subscriptions = list(filter(self._can_execute, subscriptions))
             guards = list(filter(self._can_execute, guards))
             timers = list(filter(self._can_execute, timers))
-            clients = filter(self._can_execute, clients)
-            services = filter(self._can_execute, services)
+            clients = list(filter(self._can_execute, clients))
+            services = list(filter(self._can_execute, services))
 
             # retrigger a guard condition that was triggered but not handled
             for gc in guards:
diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py
index 5e9115c9b..5d470e72d 100644
--- a/rclpy/rclpy/wait_set.py
+++ b/rclpy/rclpy/wait_set.py
@@ -12,8 +12,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import itertools
-
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 
 
@@ -23,11 +21,11 @@ class WaitSet:
     def __init__(self, subscriptions, guards, timers, clients, services):
         # List of entity pointers (the python integer, not the PyCapsule) that are ready
         self._ready_pointers = []
-        self._subscriptions = [s.subscription_handle for s in subscriptions]
-        self._guards = [g.guard_handle for g in guards]
-        self._timers = [t.timer_handle for t in timers]
-        self._clients = [c.client_handle for c in clients]
-        self._services = [s.service_handle for s in services]
+        self._subscriptions = subscriptions
+        self._guards = guards
+        self._timers = timers
+        self._clients = clients
+        self._services = services
 
         self._wait_set = None
 
@@ -48,9 +46,16 @@ def __exit__(self, t, v, tb):
     def wait(self, timeout_nsec):
         # Populate wait set
         _rclpy.rclpy_wait_set_clear_entities(self._wait_set)
-        entities = itertools.chain(
-            self._subscriptions, self._guards, self._timers, self._clients, self._services)
-        _rclpy.rclpy_wait_set_add_entities(self._wait_set, entities)
+        _rclpy.rclpy_wait_set_add_entities(
+            self._wait_set, map(lambda sub: sub.subscription_handle, self._subscriptions))
+        _rclpy.rclpy_wait_set_add_entities(
+            self._wait_set, map(lambda gc: gc.guard_handle, self._guards))
+        _rclpy.rclpy_wait_set_add_entities(
+            self._wait_set, map(lambda tmr: tmr.timer_handle, self._timers))
+        _rclpy.rclpy_wait_set_add_entities(
+            self._wait_set, map(lambda cli: cli.client_handle, self._clients))
+        _rclpy.rclpy_wait_set_add_entities(
+            self._wait_set, map(lambda srv: srv.service_handle, self._services))
 
         # Wait
         _rclpy.rclpy_wait(self._wait_set, timeout_nsec)

From 9d9113ddaf4ca05eb23747e5cdeb1172115a844a Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Thu, 2 Nov 2017 17:50:53 -0700
Subject: [PATCH 44/49] Replaced WaitSet with C code in executor

---
 rclpy/CMakeLists.txt                         |  26 +
 rclpy/rclpy/client.py                        |   2 +-
 rclpy/rclpy/executors.py                     |  79 +--
 rclpy/rclpy/impl/implementation_singleton.py |   1 +
 rclpy/src/rclpy/_rclpy.c                     |  19 +-
 rclpy/src/rclpy/_rclpy_wait_set.c            | 696 +++++++++++++++++++
 rclpy/src/rclpy/sigint_gc.c                  |  33 +
 rclpy/src/rclpy/sigint_gc.h                  |  27 +
 rclpy/test/test_wait_set.py                  | 103 +--
 9 files changed, 874 insertions(+), 112 deletions(-)
 create mode 100644 rclpy/src/rclpy/_rclpy_wait_set.c
 create mode 100644 rclpy/src/rclpy/sigint_gc.c
 create mode 100644 rclpy/src/rclpy/sigint_gc.h

diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt
index 0a0ef0c41..66ba91b41 100644
--- a/rclpy/CMakeLists.txt
+++ b/rclpy/CMakeLists.txt
@@ -73,10 +73,24 @@ function(configure_python_c_extension_library _library_name)
   )
 endfunction()
 
+
+# Sigint handling stuff
+add_library(
+  rclpy_sigint
+  SHARED src/rclpy/sigint_gc.c
+)
+target_include_directories(rclpy_sigint PUBLIC ${CMAKE_SOURCE_DIR})
+configure_python_c_extension_library(rclpy_sigint)
+ament_target_dependencies(rclpy_sigint
+  "rcl"
+)
+
+# Main library
 add_library(
   rclpy
   SHARED src/rclpy/_rclpy.c
 )
+target_link_libraries(rclpy rclpy_sigint)
 configure_python_c_extension_library(rclpy)
 ament_target_dependencies(rclpy
   "rcl"
@@ -93,6 +107,18 @@ ament_target_dependencies(rclpy_logging
   "rcutils"
 )
 
+# WaitSet wrapper library
+add_library(
+  rclpy_wait_set
+  SHARED src/rclpy/_rclpy_wait_set.c
+)
+target_link_libraries(rclpy_wait_set rclpy_sigint)
+configure_python_c_extension_library(rclpy_wait_set)
+ament_target_dependencies(rclpy_wait_set
+  "rcl"
+  "rcutils"
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py
index ee96f09f6..86720c43f 100644
--- a/rclpy/rclpy/client.py
+++ b/rclpy/rclpy/client.py
@@ -32,7 +32,7 @@ def __init__(self, client):
 
     def run(self):
         [sigint_gc, sigint_gc_handle] = _rclpy.rclpy_get_sigint_guard_condition()
-        _rclpy.rclpy_wait_set_add_entity('guard_condition', self.wait_set, sigint_gc)
+        _rclpy.rclpy_wait_set_add_entity(self.wait_set, sigint_gc)
 
         _rclpy.rclpy_wait(self.wait_set, -1)
 
diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index 0fbb63a7b..ad6133575 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -20,10 +20,10 @@
 from rclpy.guard_condition import GuardCondition
 from rclpy.guard_condition import KeyboardInterruptGuardCondition
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.impl.implementation_singleton import rclpy_wait_set_implementation as _rclpy_wait_set
 from rclpy.timer import WallTimer
 from rclpy.utilities import ok
 from rclpy.utilities import timeout_sec_to_nsec
-from rclpy.wait_set import WaitSet
 
 
 class _WorkTracker:
@@ -87,6 +87,7 @@ def __init__(self):
         # True if shutdown has been called
         self._is_shutdown = False
         self._work_tracker = _WorkTracker()
+        self._wait_set = _rclpy_wait_set.WaitSet()
 
     def shutdown(self, timeout_sec=None):
         """
@@ -136,7 +137,7 @@ def get_nodes(self):
         :rtype: list
         """
         with self._nodes_lock:
-            return [node for node in self._nodes]
+            return list(self._nodes)
 
     def spin(self):
         """Execute callbacks until shutdown."""
@@ -251,7 +252,7 @@ def _can_execute(self, entity):
         :type entity_list: Client, Service, Publisher, Subscriber
         :rtype: bool
         """
-        return entity.callback_group.can_execute(entity) and not entity._executor_event
+        return not entity._executor_event and entity.callback_group.can_execute(entity)
 
     def _new_callbacks(self, nodes, wait_set):
         """
@@ -267,7 +268,7 @@ def _new_callbacks(self, nodes, wait_set):
         # Process ready entities one node at a time
         for node in nodes:
             for tmr in node.timers:
-                if wait_set.is_ready(tmr.timer_pointer) and tmr.callback_group.can_execute(tmr):
+                if wait_set.is_ready(tmr) and tmr.callback_group.can_execute(tmr):
                     # TODO(Sloretz) Which rcl cancelled timer bug does this workaround?
                     if not _rclpy.rclpy_is_timer_ready(tmr.timer_handle):
                         continue
@@ -276,7 +277,7 @@ def _new_callbacks(self, nodes, wait_set):
                     yield handler, tmr, node
 
             for sub in node.subscriptions:
-                if (wait_set.is_ready(sub.subscription_pointer) and
+                if (wait_set.is_ready(sub) and
                         sub.callback_group.can_execute(sub)):
                     handler = self._make_handler(
                         sub, self._take_subscription, self._execute_subscription)
@@ -291,13 +292,13 @@ def _new_callbacks(self, nodes, wait_set):
                     yield handler, gc, node
 
             for cli in node.clients:
-                if wait_set.is_ready(cli.client_pointer) and cli.callback_group.can_execute(cli):
+                if wait_set.is_ready(cli) and cli.callback_group.can_execute(cli):
                     handler = self._make_handler(cli, self._take_client, self._execute_client)
                     yielded_work = True
                     yield handler, cli, node
 
             for srv in node.services:
-                if wait_set.is_ready(srv.service_pointer) and srv.callback_group.can_execute(srv):
+                if wait_set.is_ready(srv) and srv.callback_group.can_execute(srv):
                     handler = self._make_handler(srv, self._take_service, self._execute_service)
                     yielded_work = True
                     yield handler, srv, node
@@ -323,24 +324,15 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
 
         yielded_work = False
         while not yielded_work and not self._is_shutdown:
+            self._wait_set.clear()
             # Gather entities that can be waited on
-            subscriptions = []
             guards = []
-            timers = []
-            clients = []
-            services = []
             for node in nodes:
-                subscriptions.extend(node.subscriptions)
-                timers.extend(node.timers)
-                clients.extend(node.clients)
-                services.extend(node.services)
-                guards.extend(node.guards)
-
-            subscriptions = list(filter(self._can_execute, subscriptions))
-            guards = list(filter(self._can_execute, guards))
-            timers = list(filter(self._can_execute, timers))
-            clients = list(filter(self._can_execute, clients))
-            services = list(filter(self._can_execute, services))
+                self._wait_set.add_subscriptions(filter(self._can_execute, node.subscriptions))
+                self._wait_set.add_timers(filter(self._can_execute, node.timers))
+                self._wait_set.add_clients(filter(self._can_execute, node.clients))
+                self._wait_set.add_services(filter(self._can_execute, node.services))
+                guards.extend(filter(self._can_execute, node.guards))
 
             # retrigger a guard condition that was triggered but not handled
             for gc in guards:
@@ -348,36 +340,31 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
                     gc.trigger()
 
             if timeout_timer is not None:
-                timers.append(timeout_timer)
+                self._wait_set.add_timers((timeout_timer,))
 
-            guards.append(self._sigint_gc)
-            guards.append(self._guard_condition)
+            self._wait_set.add_guard_conditions((self._sigint_gc, self._guard_condition))
+            self._wait_set.add_guard_conditions(guards)
 
-            # Construct a wait set
-            with WaitSet(subscriptions, guards, timers, clients, services) as wait_set:
-                # Wait for something to become ready
-                wait_set.wait(timeout_nsec)
+            # Wait for something to become ready
+            self._wait_set.wait(timeout_nsec)
 
-                # Check sigint guard condition
-                if wait_set.is_ready(self._sigint_gc.guard_handle):
-                    raise KeyboardInterrupt()
+            # Check sigint guard condition
+            if self._wait_set.is_ready(self._sigint_gc):
+                raise KeyboardInterrupt()
 
-                guards.remove(self._sigint_gc)
-                guards.remove(self._guard_condition)
-
-                # Mark all guards as triggered before yielding since they're auto-taken
-                for gc in guards:
-                    if wait_set.is_ready(gc.guard_pointer):
-                        gc._executor_triggered = True
+            # Mark all guards as triggered before yielding since they're auto-taken
+            for gc in guards:
+                if self._wait_set.is_ready(gc):
+                    gc._executor_triggered = True
 
-                yielded_work = yield from self._new_callbacks(nodes, wait_set)
+            yielded_work = yield from self._new_callbacks(nodes, self._wait_set)
 
-                # Check timeout timer
-                if (
-                    timeout_nsec == 0 or
-                    (timeout_timer is not None and wait_set.is_ready(timeout_timer.timer_pointer))
-                ):
-                    break
+            # Check timeout timer
+            if (
+                timeout_nsec == 0 or
+                (timeout_timer is not None and self._wait_set.is_ready(timeout_timer))
+            ):
+                break
 
 
 class SingleThreadedExecutor(Executor):
diff --git a/rclpy/rclpy/impl/implementation_singleton.py b/rclpy/rclpy/impl/implementation_singleton.py
index 62bf50cdf..95707e742 100644
--- a/rclpy/rclpy/impl/implementation_singleton.py
+++ b/rclpy/rclpy/impl/implementation_singleton.py
@@ -31,6 +31,7 @@
 
 try:
     rclpy_implementation = importlib.import_module('._rclpy', package='rclpy')
+    rclpy_wait_set_implementation = importlib.import_module('._rclpy_wait_set', package='rclpy')
 except ImportError as e:
     if os.path.isfile(e.path):
         e.msg += \
diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index 640e82916..2cd1294db 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -28,21 +28,8 @@
 #include <rmw/validate_node_name.h>
 #include <rosidl_generator_c/message_type_support_struct.h>
 
-#include <signal.h>
+#include "src/rclpy/sigint_gc.h"
 
-static rcl_guard_condition_t * g_sigint_gc_handle;
-
-/// Catch signals
-static void catch_function(int signo)
-{
-  (void) signo;
-  rcl_ret_t ret = rcl_trigger_guard_condition(g_sigint_gc_handle);
-  if (ret != RCL_RET_OK) {
-    PyErr_Format(PyExc_RuntimeError,
-      "Failed to trigger guard_condition: %s", rcl_get_error_string_safe());
-    rcl_reset_error();
-  }
-}
 
 /// Get a guard condition for node graph events
 /**
@@ -110,7 +97,7 @@ rclpy_get_sigint_guard_condition(PyObject * Py_UNUSED(self), PyObject * Py_UNUSE
     rcl_reset_error();
     return NULL;
   }
-  g_sigint_gc_handle = sigint_gc;
+  g_rclpy_sigint_gc_handle = sigint_gc;
   PyObject * pylist = PyList_New(2);
   PyList_SET_ITEM(pylist, 0, pygc);
   PyList_SET_ITEM(pylist, 1, PyLong_FromUnsignedLongLong((uint64_t)&sigint_gc->impl));
@@ -2028,7 +2015,7 @@ rclpy_wait(PyObject * Py_UNUSED(self), PyObject * args)
 #else
   sig_t
 #endif  // _WIN32
-  previous_handler = signal(SIGINT, catch_function);
+  previous_handler = signal(SIGINT, rclpy_catch_function);
   rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
   if (!wait_set) {
     return NULL;
diff --git a/rclpy/src/rclpy/_rclpy_wait_set.c b/rclpy/src/rclpy/_rclpy_wait_set.c
new file mode 100644
index 000000000..ab1fbfae2
--- /dev/null
+++ b/rclpy/src/rclpy/_rclpy_wait_set.c
@@ -0,0 +1,696 @@
+// Copyright 2017 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.
+
+#include <Python.h>
+#include <structmember.h>
+
+#include <rcl/error_handling.h>
+#include <rcl/rcl.h>
+
+#include "src/rclpy/sigint_gc.h"
+
+
+/// Storage for custom python type _rclpy_wait_set.WaitSet
+typedef struct
+{
+  PyObject_HEAD
+  rcl_wait_set_t wait_set;
+  /// A set of pycapsules in the wait set
+  PyObject * pysubs;
+  PyObject * pytmrs;
+  PyObject * pygcs;
+  PyObject * pyclis;
+  PyObject * pysrvs;
+  /// A set of pycapsule that are ready
+  PyObject * pyready;
+} rclpy_wait_set_t;
+
+/// Destructor
+static void
+rclpy_wait_set_dealloc(rclpy_wait_set_t * self)
+{
+  rcl_ret_t ret = rcl_wait_set_fini(&(self->wait_set));
+  if (ret != RCL_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError,
+      "Failed to fini wait set: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+  }
+
+  // X because it could be NULL if PySet_New failed during rclpy_wait_set_new
+  Py_XDECREF(self->pysubs);
+  Py_XDECREF(self->pytmrs);
+  Py_XDECREF(self->pygcs);
+  Py_XDECREF(self->pyclis);
+  Py_XDECREF(self->pysrvs);
+  Py_XDECREF(self->pyready);
+
+  Py_TYPE(self)->tp_free((PyObject *)self);
+}
+
+/// Constructor
+static PyObject *
+rclpy_wait_set_new(PyTypeObject * type, PyObject * Py_UNUSED(args), PyObject * Py_UNUSED(kwds))
+{
+  rclpy_wait_set_t * self;
+
+  self = (rclpy_wait_set_t *)type->tp_alloc(type, 0);
+  if (self) {
+    self->wait_set = rcl_get_zero_initialized_wait_set();
+  }
+
+  return (PyObject *)self;
+}
+
+/// Initializer
+static int
+rclpy_wait_set_init(
+  rclpy_wait_set_t * self, PyObject * Py_UNUSED(args), PyObject * Py_UNUSED(kwds))
+{
+  // rclpy_wait_set_dealloc will take care of decref
+  self->pysubs = PySet_New(NULL);
+  if (!self->pysubs) {
+    return -1;
+  }
+  self->pytmrs = PySet_New(NULL);
+  if (!self->pytmrs) {
+    return -1;
+  }
+  self->pygcs = PySet_New(NULL);
+  if (!self->pygcs) {
+    return -1;
+  }
+  self->pyclis = PySet_New(NULL);
+  if (!self->pyclis) {
+    return -1;
+  }
+  self->pysrvs = PySet_New(NULL);
+  if (!self->pysrvs) {
+    return -1;
+  }
+  self->pyready = PySet_New(NULL);
+  if (!self->pyready) {
+    return -1;
+  }
+
+  rcl_ret_t ret = rcl_wait_set_init(&(self->wait_set), 0, 0, 0, 0, 0, rcl_get_default_allocator());
+  if (ret != RCL_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError,
+      "Failed to initialize wait set: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+    return -1;
+  }
+
+  return 0;
+}
+
+/// Return a pycapsule handle from an rclpy type
+/*
+ * Raises any exception raised by an object with a custom __getattr__
+ *
+ * \param[in] pyentity
+ * \returns NULL on error else something that is probably a pycapsule
+ */
+static inline PyObject *
+_rclpy_to_pycapsule(PyObject * pyentity)
+{
+  if (PyObject_HasAttrString(pyentity, "subscription_handle")) {
+    pyentity = PyObject_GetAttrString(pyentity, "subscription_handle");
+  } else if (PyObject_HasAttrString(pyentity, "guard_handle")) {
+    pyentity = PyObject_GetAttrString(pyentity, "guard_handle");
+  } else if (PyObject_HasAttrString(pyentity, "timer_handle")) {
+    pyentity = PyObject_GetAttrString(pyentity, "timer_handle");
+  } else if (PyObject_HasAttrString(pyentity, "client_handle")) {
+    pyentity = PyObject_GetAttrString(pyentity, "client_handle");
+  } else if (PyObject_HasAttrString(pyentity, "service_handle")) {
+    pyentity = PyObject_GetAttrString(pyentity, "service_handle");
+  }
+  return pyentity;
+}
+
+
+/// Add entities of a known type to the correct set
+/*
+ * Raises ValueError if capsule is invalid
+ * Handles adding entities of a given type to the wait set
+ *
+ * \param[in] pyset a set that the entities should be added to
+ * \param[in] pyentities an iterable of (sub, pub, client, serv, guard)
+ * \param[in] handle_attr an attribute of an entity where the handle is stored
+ * \param[in] handle_type a pycapsule name this entity uses
+ * \return RCL_RET_OK if everything succeded
+ */
+static inline rcl_ret_t
+_rclpy_add_entity(
+  PyObject * pyset, PyObject * pyentities, const char * handle_attr,
+  const char * handle_type)
+{
+  // It's possible for arbitrary python code to be invoked that invalidates this reference
+  Py_INCREF(pyset);
+  Py_INCREF(pyentities);
+
+  PyObject * pyiter = PyObject_GetIter(pyentities);
+  if (!pyiter) {
+    // exception set
+    return RCL_RET_ERROR;
+  }
+
+  PyObject * pyentity;
+  while ((pyentity = PyIter_Next(pyiter))) {
+    // Accept an instance of an rclpy type for convenience
+    if (PyObject_HasAttrString(pyentity, handle_attr)) {
+      pyentity = PyObject_GetAttrString(pyentity, handle_attr);
+    }
+
+    // No chance of arbitracy python code below, so decref early
+    Py_DECREF(pyentity);
+
+    if (!pyentity) {
+      // Exception set
+      break;
+    }
+
+    if (PyCapsule_IsValid(pyentity, handle_type)) {
+      if (-1 == PySet_Add(pyset, pyentity)) {
+        break;
+      }
+    } else {
+      const char * entity_type = PyCapsule_GetName(pyentity);
+      if (entity_type) {
+        PyErr_Format(PyExc_ValueError, "Unknown capsule type: '%s'", entity_type);
+      }  // else PyCapsule_GetName raised
+      break;
+    }
+  }
+
+  Py_DECREF(pyiter);
+  Py_DECREF(pyentities);
+  Py_DECREF(pyset);
+
+  if (PyErr_Occurred()) {
+    // Return down here after references have been cleaned dup
+    return RCL_RET_ERROR;
+  }
+  return RCL_RET_OK;
+}
+
+/// Add subscriptions to be waited on to the appropriate set
+/*
+ * Raises ValueError if a capsule is invalid
+ * Raises TypeError if the argument is not iterable
+ *
+ * \param[in] self instance of rclpy_wait_set_t
+ * \param[in] pyentities Iterable of subscriptions
+ */
+static PyObject *
+rclpy_wait_set_add_subscriptions(rclpy_wait_set_t * self, PyObject * args)
+{
+  PyObject * pyentities;
+  if (!PyArg_ParseTuple(args, "O", &pyentities)) {
+    return NULL;
+  }
+
+  if (RCL_RET_OK != _rclpy_add_entity(self->pysubs, pyentities, "subscription_handle",
+    "rcl_subscription_t"))
+  {
+    // Exception set
+    return NULL;
+  }
+  Py_RETURN_NONE;
+}
+
+/// Add guard_conditions to be waited on to the appropriate set
+/*
+ * Raises ValueError if a capsule is invalid
+ * Raises TypeError if the argument is not iterable
+ *
+ * \param[in] self instance of rclpy_wait_set_t
+ * \param[in] pyentities Iterable of guard_conditions
+ */
+static PyObject *
+rclpy_wait_set_add_guard_conditions(rclpy_wait_set_t * self, PyObject * args)
+{
+  PyObject * pyentities;
+  if (!PyArg_ParseTuple(args, "O", &pyentities)) {
+    return NULL;
+  }
+
+  if (RCL_RET_OK != _rclpy_add_entity(self->pygcs, pyentities, "guard_handle",
+    "rcl_guard_condition_t"))
+  {
+    // Exception set
+    return NULL;
+  }
+  Py_RETURN_NONE;
+}
+
+/// Add timers to be waited on to the appropriate set
+/*
+ * Raises ValueError if a capsule is invalid
+ * Raises TypeError if the argument is not iterable
+ *
+ * \param[in] self instance of rclpy_wait_set_t
+ * \param[in] pyentities Iterable of timers
+ */
+static PyObject *
+rclpy_wait_set_add_timers(rclpy_wait_set_t * self, PyObject * args)
+{
+  PyObject * pyentities;
+  if (!PyArg_ParseTuple(args, "O", &pyentities)) {
+    return NULL;
+  }
+
+  if (RCL_RET_OK != _rclpy_add_entity(self->pytmrs, pyentities, "timer_handle",
+    "rcl_timer_t"))
+  {
+    // Exception set
+    return NULL;
+  }
+  Py_RETURN_NONE;
+}
+
+/// Add clients to be waited on to the appropriate set
+/*
+ * Raises ValueError if a capsule is invalid
+ * Raises TypeError if the argument is not iterable
+ *
+ * \param[in] self instance of rclpy_wait_set_t
+ * \param[in] pyentities Iterable of clients
+ */
+static PyObject *
+rclpy_wait_set_add_clients(rclpy_wait_set_t * self, PyObject * args)
+{
+  PyObject * pyentities;
+  if (!PyArg_ParseTuple(args, "O", &pyentities)) {
+    return NULL;
+  }
+
+  if (RCL_RET_OK != _rclpy_add_entity(self->pyclis, pyentities, "client_handle",
+    "rcl_client_t"))
+  {
+    // Exception set
+    return NULL;
+  }
+  Py_RETURN_NONE;
+}
+
+/// Add services to be waited on to the appropriate set
+/*
+ * Raises ValueError if a capsule is invalid
+ * Raises TypeError if the argument is not iterable
+ *
+ * \param[in] self instance of rclpy_wait_set_t
+ * \param[in] pyentities Iterable of services
+ */
+static PyObject *
+rclpy_wait_set_add_services(rclpy_wait_set_t * self, PyObject * args)
+{
+  PyObject * pyentities;
+  if (!PyArg_ParseTuple(args, "O", &pyentities)) {
+    return NULL;
+  }
+
+  if (RCL_RET_OK != _rclpy_add_entity(self->pysrvs, pyentities, "service_handle",
+    "rcl_service_t"))
+  {
+    // Exception set
+    return NULL;
+  }
+  Py_RETURN_NONE;
+}
+
+/// Build a wait set
+/*
+ * Raises RuntimeError if an RCL error occurs
+ *
+ * This method adds all of the PyCapsule to the rcl_wait_set_t instance, resizing it in the process
+ *
+ * \return number of entities in the wait set
+*/
+static inline rcl_ret_t
+_rclpy_build_wait_set(rclpy_wait_set_t * self)
+{
+  rcl_ret_t ret;
+  ret = rcl_wait_set_fini(&(self->wait_set));
+  if (ret != RCL_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError,
+      "Failed to finalize wait set: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+    return ret;
+  }
+
+  Py_ssize_t num_subs = PyObject_Length(self->pysubs);
+  if (-1 == num_subs) {
+    return RCL_RET_ERROR;
+  }
+  Py_ssize_t num_gcs = PyObject_Length(self->pygcs);
+  if (-1 == num_gcs) {
+    return RCL_RET_ERROR;
+  }
+  Py_ssize_t num_tmrs = PyObject_Length(self->pytmrs);
+  if (-1 == num_tmrs) {
+    return RCL_RET_ERROR;
+  }
+  Py_ssize_t num_clis = PyObject_Length(self->pyclis);
+  if (-1 == num_clis) {
+    return RCL_RET_ERROR;
+  }
+  Py_ssize_t num_srvs = PyObject_Length(self->pysrvs);
+  if (-1 == num_srvs) {
+    return RCL_RET_ERROR;
+  }
+
+  ret = rcl_wait_set_init(&(self->wait_set), num_subs, num_gcs, num_tmrs, num_clis, num_srvs,
+      rcl_get_default_allocator());
+  if (ret != RCL_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError,
+      "Failed to initialize wait set: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+    return ret;
+  }
+
+#define RCLPY_ADD_ENTITY(ETYPE, WSFUNC, ESET) \
+  do { \
+    PyObject * pyiter = PyObject_GetIter((ESET)); \
+    if (!pyiter) { \
+      return RCL_RET_ERROR; \
+    } \
+    PyObject * pyentity; \
+    while ((pyentity = PyIter_Next(pyiter))) { \
+      ETYPE * entity = (ETYPE *)PyCapsule_GetPointer(pyentity, #ETYPE); \
+      if (!entity) { \
+        Py_DECREF(pyentity); \
+        Py_DECREF(pyiter); \
+        return RCL_RET_ERROR; \
+      } \
+      rcl_ret_t ret = WSFUNC(&(self->wait_set), entity); \
+      if (ret != RCL_RET_OK) { \
+        PyErr_Format(PyExc_RuntimeError, \
+          "Failed to add entity '" #ETYPE "' to wait set: %s", rcl_get_error_string_safe()); \
+        rcl_reset_error(); \
+        Py_DECREF(pyentity); \
+        Py_DECREF(pyiter); \
+        return ret; \
+      } \
+      Py_DECREF(pyentity); \
+    } \
+    Py_DECREF(pyiter); \
+  } while (false)
+
+  RCLPY_ADD_ENTITY(rcl_subscription_t, rcl_wait_set_add_subscription, self->pysubs);
+  RCLPY_ADD_ENTITY(rcl_guard_condition_t, rcl_wait_set_add_guard_condition, self->pygcs);
+  RCLPY_ADD_ENTITY(rcl_timer_t, rcl_wait_set_add_timer, self->pytmrs);
+  RCLPY_ADD_ENTITY(rcl_client_t, rcl_wait_set_add_client, self->pyclis);
+  RCLPY_ADD_ENTITY(rcl_service_t, rcl_wait_set_add_service, self->pysrvs);
+#undef RCLPY_ADD_ENTITY
+
+  return RCL_RET_OK;
+}
+
+/// Fill pyready with entities that are ready
+/*
+ * \param[in] self an instance of _rclpy_wait_set.WaitSet
+ * \param[in] pyset the set of entities to check
+ * \param[in] type the name of the PyCapsule
+ */
+static inline rcl_ret_t
+_rclpy_build_ready_entities(rclpy_wait_set_t * self)
+{
+  if (-1 == PySet_Clear(self->pyready)) {
+    return RCL_RET_ERROR;
+  }
+
+#define GET_READY_ENTITIES(ETYPE, ESET) \
+  do { \
+    PyObject * pyiter = PyObject_GetIter((ESET)); \
+    if (!pyiter) { \
+      return RCL_RET_ERROR; \
+    } \
+    PyObject * pyentity; \
+    while ((pyentity = PyIter_Next(pyiter))) { \
+      rcl_ ## ETYPE ## _t * entity = \
+        (rcl_ ## ETYPE ## _t *)PyCapsule_GetPointer(pyentity, "rcl_" #ETYPE "_t"); \
+      if (!entity) { \
+        Py_DECREF(pyentity); \
+        Py_DECREF(pyiter); \
+        return RCL_RET_ERROR; \
+      } \
+      size_t idx; \
+      size_t idx_max; \
+      idx_max = self->wait_set.size_of_ ## ETYPE ## s; \
+      const rcl_ ## ETYPE ## _t ** struct_ptr = self->wait_set.ETYPE ## s; \
+      for (idx = 0; idx < idx_max; ++idx) { \
+        if (struct_ptr[idx] == entity) { \
+          if (-1 == PySet_Add(self->pyready, pyentity)) { \
+            Py_DECREF(pyentity); \
+            Py_DECREF(pyiter); \
+            return RCL_RET_ERROR; \
+          } \
+        } \
+      } \
+      Py_DECREF(pyentity); \
+    } \
+    Py_DECREF(pyiter); \
+  } while (false)
+
+  GET_READY_ENTITIES(subscription, self->pysubs);
+  GET_READY_ENTITIES(guard_condition, self->pygcs);
+  GET_READY_ENTITIES(timer, self->pytmrs);
+  GET_READY_ENTITIES(client, self->pyclis);
+  GET_READY_ENTITIES(service, self->pysrvs);
+#undef GET_READY_ENTITIES
+  return RCL_RET_OK;
+}
+
+/// Wait until timeout is reached or an event happened
+/**
+ * Raises RuntimeError if there was an error while waiting
+ *
+ * This function will wait for an event to happen or for the timeout to expire.
+ * A negative timeout means wait forever, a timeout of 0 means no wait
+ * \param[in] timeout optional time to wait before waking up (in nanoseconds)
+ * \return NULL
+ */
+static PyObject *
+rclpy_wait_set_wait(rclpy_wait_set_t * self, PyObject * args)
+{
+  PY_LONG_LONG timeout = -1;
+
+  if (!PyArg_ParseTuple(args, "|K", &timeout)) {
+    return NULL;
+  }
+
+  if (RCL_RET_OK != _rclpy_build_wait_set(self)) {
+    // exception set
+    return NULL;
+  }
+
+#ifdef _WIN32
+  _crt_signal_t
+#else
+  sig_t
+#endif  // _WIN32
+  previous_handler = signal(SIGINT, rclpy_catch_function);
+  rcl_ret_t ret;
+
+  // Reference could be invalidated by arbitrary python code running while GIL is released
+  Py_INCREF(self);
+  Py_BEGIN_ALLOW_THREADS;
+  ret = rcl_wait(&(self->wait_set), timeout);
+  Py_END_ALLOW_THREADS;
+  Py_DECREF(self);
+
+  signal(SIGINT, previous_handler);
+  if (ret != RCL_RET_OK && ret != RCL_RET_TIMEOUT) {
+    PyErr_Format(PyExc_RuntimeError,
+      "Failed to wait on wait set: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+    return NULL;
+  }
+
+  if (RCL_RET_OK != _rclpy_build_ready_entities(self)) {
+    // exception set
+    return NULL;
+  }
+
+  Py_RETURN_NONE;
+}
+
+/// Remove all stored entities from the wait set
+/**
+ * Raises RuntimeError if there was an error while clearing
+ *
+ * \return None
+ */
+static PyObject *
+rclpy_wait_set_clear(rclpy_wait_set_t * self)
+{
+  if (-1 == PySet_Clear(self->pysubs) ||
+    -1 == PySet_Clear(self->pytmrs) ||
+    -1 == PySet_Clear(self->pygcs) ||
+    -1 == PySet_Clear(self->pyclis) ||
+    -1 == PySet_Clear(self->pysrvs))
+  {
+    return NULL;
+  }
+
+  Py_RETURN_NONE;
+}
+
+
+/// Return True if an entity is ready
+/**
+ * Raises RuntimeError if there was an error while clearing
+ * Raises ValueError if the capsule is invalid
+ *
+ * \return True if the entity is ready
+ */
+static PyObject *
+rclpy_wait_set_is_ready(rclpy_wait_set_t * self, PyObject * args)
+{
+  PyObject * pyentity;
+  if (!PyArg_ParseTuple(args, "O", &pyentity)) {
+    return NULL;
+  }
+
+  pyentity = _rclpy_to_pycapsule(pyentity);
+
+  // Arbitrary python code could invalidate references in __hash__
+  Py_INCREF(self);
+  Py_INCREF(pyentity);
+  int contains = PySet_Contains(self->pyready, pyentity);
+  Py_DECREF(pyentity);
+  Py_DECREF(self);
+
+  if (-1 == contains) {
+    // Exception set
+    return NULL;
+  }
+  if (contains) {
+    Py_RETURN_TRUE;
+  }
+  Py_RETURN_FALSE;
+}
+
+static PyMemberDef rclpy_wait_set_members[] = {
+  {"_pysubs", T_OBJECT_EX, offsetof(rclpy_wait_set_t, pysubs), 0,
+    "subscription capsules"},
+  {"_pygcs", T_OBJECT_EX, offsetof(rclpy_wait_set_t, pygcs), 0,
+    "guard_condition capsules"},
+  {"_pytmrs", T_OBJECT_EX, offsetof(rclpy_wait_set_t, pytmrs), 0,
+    "timer capsules"},
+  {"_pyclis", T_OBJECT_EX, offsetof(rclpy_wait_set_t, pyclis), 0,
+    "client capsules"},
+  {"_pysrvs", T_OBJECT_EX, offsetof(rclpy_wait_set_t, pysrvs), 0,
+    "service capsules"},
+  {"_pyready", T_OBJECT_EX, offsetof(rclpy_wait_set_t, pyready), 0,
+    "ready capsules"},
+  {NULL}  /* Sentinel */
+};
+
+/// Define methods of _rclpy_wait_set.WaitSet
+static PyMethodDef rclpy_wait_set_methods[] = {
+  {"add_subscriptions", (PyCFunction)rclpy_wait_set_add_subscriptions, METH_VARARGS,
+    "Add a bunch of subscription instances or handles to the wait_set."},
+  {"add_guard_conditions", (PyCFunction)rclpy_wait_set_add_guard_conditions, METH_VARARGS,
+    "Add a bunch of guard_condition instances or handles to the wait_set."},
+  {"add_timers", (PyCFunction)rclpy_wait_set_add_timers, METH_VARARGS,
+    "Add a bunch of timer instances or handles to the wait_set."},
+  {"add_clients", (PyCFunction)rclpy_wait_set_add_clients, METH_VARARGS,
+    "Add a bunch of client instances or handles to the wait_set."},
+  {"add_services", (PyCFunction)rclpy_wait_set_add_services, METH_VARARGS,
+    "Add a bunch of service instances or handles to the wait_set."},
+  {"wait", (PyCFunction)rclpy_wait_set_wait, METH_VARARGS,
+    "Wait until timeout is reached or an event happened."},
+  {"clear", (PyCFunction)rclpy_wait_set_clear, METH_NOARGS,
+    "Remove all entities from the wait set."},
+  {"is_ready", (PyCFunction)rclpy_wait_set_is_ready, METH_VARARGS,
+    "Return True if an entity is ready."},
+  {NULL}  /* Sentinel */
+};
+
+
+// Partially initializing is recommended in the python docs
+// I don't see a way to complete the initializer without duplicating #ifdef in cpython header
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+/// Python type _rclpy_wait_set.WaitSet
+static PyTypeObject rclpy_wait_set_type_t = {
+  PyVarObject_HEAD_INIT(NULL, 0)
+  "_rclpy_wait_set.WaitSet",    /* tp_name */
+  sizeof(rclpy_wait_set_t),     /* tp_basicsize */
+  0,                            /* tp_itemsize */
+  (destructor)rclpy_wait_set_dealloc,  /* tp_dealloc */
+  0,                            /* tp_print */
+  0,                            /* tp_getattr */
+  0,                            /* tp_setattr */
+  0,                            /* tp_reserved */
+  0,                            /* tp_repr */
+  0,                            /* tp_as_number */
+  0,                            /* tp_as_sequence */
+  0,                            /* tp_as_mapping */
+  0,                            /* tp_hash  */
+  0,                            /* tp_call */
+  0,                            /* tp_str */
+  0,                            /* tp_getattro */
+  0,                            /* tp_setattro */
+  0,                            /* tp_as_buffer */
+  Py_TPFLAGS_DEFAULT,           /* tp_flags */
+  "Interface to a wait set.",   /* tp_doc */
+  0,                            /* tp_traverse */
+  0,                            /* tp_clear */
+  0,                            /* tp_richcompare */
+  0,                            /* tp_weaklistoffset */
+  0,                            /* tp_iter */
+  0,                            /* tp_iternext */
+  rclpy_wait_set_methods,       /* tp_methods */
+  rclpy_wait_set_members,       /* tp_members */
+  0,                            /* tp_getset */
+  0,                            /* tp_base */
+  0,                            /* tp_dict */
+  0,                            /* tp_descr_get */
+  0,                            /* tp_descr_set */
+  0,                            /* tp_dictoffset */
+  (initproc)rclpy_wait_set_init,  /* tp_init */
+  0,                            /* tp_alloc */
+  rclpy_wait_set_new,           /* tp_new */
+};
+#pragma GCC diagnostic pop
+
+static PyModuleDef wait_set_module = {
+  PyModuleDef_HEAD_INIT,
+  "_rclpy_wait_set",
+  "Extention module for a wait set class.",
+  -1,
+  NULL, NULL, NULL, NULL, NULL
+};
+
+PyMODINIT_FUNC
+PyInit__rclpy_wait_set(void)
+{
+  PyObject * m;
+
+  rclpy_wait_set_type_t.tp_new = PyType_GenericNew;
+  if (PyType_Ready(&rclpy_wait_set_type_t) < 0) {
+    return NULL;
+  }
+
+  m = PyModule_Create(&wait_set_module);
+  if (m == NULL) {
+    return NULL;
+  }
+
+  Py_INCREF(&rclpy_wait_set_type_t);
+  PyModule_AddObject(m, "WaitSet", (PyObject *)&rclpy_wait_set_type_t);
+  return m;
+}
diff --git a/rclpy/src/rclpy/sigint_gc.c b/rclpy/src/rclpy/sigint_gc.c
new file mode 100644
index 000000000..74a83960c
--- /dev/null
+++ b/rclpy/src/rclpy/sigint_gc.c
@@ -0,0 +1,33 @@
+// Copyright 2017 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.
+
+#include "src/rclpy/sigint_gc.h"
+
+#include <Python.h>
+
+#include <rcl/error_handling.h>
+
+rcl_guard_condition_t * g_rclpy_sigint_gc_handle;
+
+/// Catch signals
+void rclpy_catch_function(int signo)
+{
+  (void) signo;
+  rcl_ret_t ret = rcl_trigger_guard_condition(g_rclpy_sigint_gc_handle);
+  if (ret != RCL_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError,
+      "Failed to trigger guard_condition: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+  }
+}
diff --git a/rclpy/src/rclpy/sigint_gc.h b/rclpy/src/rclpy/sigint_gc.h
new file mode 100644
index 000000000..ffc06cd93
--- /dev/null
+++ b/rclpy/src/rclpy/sigint_gc.h
@@ -0,0 +1,27 @@
+// Copyright 2017 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.
+#ifndef RCLPY__SIGINT_GC_H_
+#define RCLPY__SIGINT_GC_H_
+
+#include <signal.h>
+
+#include <rcl/rcl.h>
+
+/// Global variable with guard condition for sigint handler
+extern rcl_guard_condition_t * g_rclpy_sigint_gc_handle;
+
+/// Function that can be used as a sigint handler
+extern void rclpy_catch_function(int signo);
+
+#endif  // RCLPY__SIGINT_GC_H_
diff --git a/rclpy/test/test_wait_set.py b/rclpy/test/test_wait_set.py
index 08f9dac7b..cf3e01e54 100644
--- a/rclpy/test/test_wait_set.py
+++ b/rclpy/test/test_wait_set.py
@@ -19,8 +19,8 @@
 from rclpy.constants import S_TO_NS
 from rclpy.guard_condition import GuardCondition
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.impl.implementation_singleton import rclpy_wait_set_implementation as _rclpy_wait_set
 from rclpy.timer import WallTimer
-from rclpy.wait_set import WaitSet
 from std_msgs.msg import String
 
 
@@ -39,36 +39,38 @@ def tearDownClass(cls):
     def test_guard_condition_ready(self):
         gc = GuardCondition(None, None)
         try:
-            with WaitSet([], [gc], [], [], []) as ws:
-                self.assertFalse(ws.is_ready(gc.guard_pointer))
+            ws = _rclpy_wait_set.WaitSet()
+            ws.add_guard_conditions([gc])
+            self.assertFalse(ws.is_ready(gc))
 
-                ws.wait(0)
-                self.assertFalse(ws.is_ready(gc.guard_pointer))
+            ws.wait(0)
+            self.assertFalse(ws.is_ready(gc))
 
-                gc.trigger()
-                ws.wait(0)
-                self.assertTrue(ws.is_ready(gc.guard_pointer))
+            gc.trigger()
+            ws.wait(0)
+            self.assertTrue(ws.is_ready(gc))
 
-                ws.wait(0)
-                self.assertFalse(ws.is_ready(gc.guard_pointer))
+            ws.wait(0)
+            self.assertFalse(ws.is_ready(gc))
         finally:
             _rclpy.rclpy_destroy_entity(gc.guard_handle)
 
     def test_timer_ready(self):
         timer = WallTimer(None, None, int(0.1 * S_TO_NS))
         try:
-            with WaitSet([], [], [timer], [], []) as ws:
-                self.assertFalse(ws.is_ready(timer.timer_pointer))
+            ws = _rclpy_wait_set.WaitSet()
+            ws.add_timers([timer])
+            self.assertFalse(ws.is_ready(timer))
 
-                ws.wait(0)
-                self.assertFalse(ws.is_ready(timer.timer_pointer))
+            ws.wait(0)
+            self.assertFalse(ws.is_ready(timer))
 
-                ws.wait(int(0.1 * S_TO_NS))
-                self.assertTrue(ws.is_ready(timer.timer_pointer))
+            ws.wait(int(0.1 * S_TO_NS))
+            self.assertTrue(ws.is_ready(timer))
 
-                _rclpy.rclpy_call_timer(timer.timer_handle)
-                ws.wait(0)
-                self.assertFalse(ws.is_ready(timer.timer_pointer))
+            _rclpy.rclpy_call_timer(timer.timer_handle)
+            ws.wait(0)
+            self.assertFalse(ws.is_ready(timer))
         finally:
             _rclpy.rclpy_destroy_entity(timer.timer_handle)
 
@@ -76,22 +78,23 @@ def test_subscriber_ready(self):
         sub = self.node.create_subscription(String, 'chatter', lambda msg: print(msg))
         pub = self.node.create_publisher(String, 'chatter')
         try:
-            with WaitSet([sub], [], [], [], []) as ws:
-                self.assertFalse(ws.is_ready(sub.subscription_pointer))
+            ws = _rclpy_wait_set.WaitSet()
+            ws.add_subscriptions([sub])
+            self.assertFalse(ws.is_ready(sub))
 
-                ws.wait(0)
-                self.assertFalse(ws.is_ready(sub.subscription_pointer))
+            ws.wait(0)
+            self.assertFalse(ws.is_ready(sub))
 
-                msg = String()
-                msg.data = 'Hello World'
-                pub.publish(msg)
+            msg = String()
+            msg.data = 'Hello World'
+            pub.publish(msg)
 
-                ws.wait(5 * S_TO_NS)
-                self.assertTrue(ws.is_ready(sub.subscription_pointer))
+            ws.wait(5 * S_TO_NS)
+            self.assertTrue(ws.is_ready(sub))
 
-                _rclpy.rclpy_take(sub.subscription_handle, sub.msg_type)
-                ws.wait(0)
-                self.assertFalse(ws.is_ready(sub.subscription_pointer))
+            _rclpy.rclpy_take(sub.subscription_handle, sub.msg_type)
+            ws.wait(0)
+            self.assertFalse(ws.is_ready(sub))
         finally:
             self.node.destroy_publisher(pub)
             self.node.destroy_subscription(sub)
@@ -102,24 +105,26 @@ def test_server_ready(self):
             GetParameters, 'get/parameters', lambda req: GetParameters.Response())
 
         try:
-            with WaitSet([], [], [], [cli], [srv]) as ws:
-                self.assertFalse(ws.is_ready(cli.client_pointer))
-                self.assertFalse(ws.is_ready(srv.service_pointer))
-
-                ws.wait(0)
-                self.assertFalse(ws.is_ready(cli.client_pointer))
-                self.assertFalse(ws.is_ready(srv.service_pointer))
-
-                cli.wait_for_service()
-                cli.call(GetParameters.Request())
-
-                ws.wait(5 * S_TO_NS)
-                # TODO(sloretz) test client after it's wait_for_future() API is sorted out
-                self.assertTrue(ws.is_ready(srv.service_pointer))
-
-                _rclpy.rclpy_take_request(srv.service_handle, srv.srv_type.Request)
-                ws.wait(0)
-                self.assertFalse(ws.is_ready(srv.service_pointer))
+            ws = _rclpy_wait_set.WaitSet()
+            ws.add_clients([cli])
+            ws.add_services([srv])
+            self.assertFalse(ws.is_ready(cli))
+            self.assertFalse(ws.is_ready(srv))
+
+            ws.wait(0)
+            self.assertFalse(ws.is_ready(cli))
+            self.assertFalse(ws.is_ready(srv))
+
+            cli.wait_for_service()
+            cli.call(GetParameters.Request())
+
+            ws.wait(5 * S_TO_NS)
+            # TODO(sloretz) test client after it's wait_for_future() API is sorted out
+            self.assertTrue(ws.is_ready(srv))
+
+            _rclpy.rclpy_take_request(srv.service_handle, srv.srv_type.Request)
+            ws.wait(0)
+            self.assertFalse(ws.is_ready(srv))
         finally:
             self.node.destroy_client(cli)
             self.node.destroy_service(srv)

From adfddf63cf171aadd78a45f40844e472435b411b Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Thu, 2 Nov 2017 18:27:51 -0700
Subject: [PATCH 45/49] Remove test code

---
 rclpy/test/test_timer.py | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/rclpy/test/test_timer.py b/rclpy/test/test_timer.py
index 0f513a22f..37c63390b 100644
--- a/rclpy/test/test_timer.py
+++ b/rclpy/test/test_timer.py
@@ -193,8 +193,3 @@ def test_timer_cancel_reset_1000hertz():
         raise SkipTest
     func_launch(
         func_cancel_reset_timer, ['0.001'], "didn't receive the expected number of callbacks")
-
-
-if __name__ == '__main__':
-    import cProfile
-    cProfile.run("func_number_callbacks(['0.001'])", "timer_test.prof")

From 315db4e77a3198e8dddba95964f5868ef41a2d36 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 8 Nov 2017 15:28:54 -0800
Subject: [PATCH 46/49] Use lists instead of set

---
 rclpy/src/rclpy/_rclpy_wait_set.c | 63 ++++++++++++++-----------------
 1 file changed, 28 insertions(+), 35 deletions(-)

diff --git a/rclpy/src/rclpy/_rclpy_wait_set.c b/rclpy/src/rclpy/_rclpy_wait_set.c
index ab1fbfae2..7ed500112 100644
--- a/rclpy/src/rclpy/_rclpy_wait_set.c
+++ b/rclpy/src/rclpy/_rclpy_wait_set.c
@@ -26,13 +26,13 @@ typedef struct
 {
   PyObject_HEAD
   rcl_wait_set_t wait_set;
-  /// A set of pycapsules in the wait set
+  /// Lists of pycapsules in the wait set
   PyObject * pysubs;
   PyObject * pytmrs;
   PyObject * pygcs;
   PyObject * pyclis;
   PyObject * pysrvs;
-  /// A set of pycapsule that are ready
+  /// List of pycapsule that are ready
   PyObject * pyready;
 } rclpy_wait_set_t;
 
@@ -47,7 +47,7 @@ rclpy_wait_set_dealloc(rclpy_wait_set_t * self)
     rcl_reset_error();
   }
 
-  // X because it could be NULL if PySet_New failed during rclpy_wait_set_new
+  // X because it could be NULL if PyList_New failed during rclpy_wait_set_new
   Py_XDECREF(self->pysubs);
   Py_XDECREF(self->pytmrs);
   Py_XDECREF(self->pygcs);
@@ -78,27 +78,27 @@ rclpy_wait_set_init(
   rclpy_wait_set_t * self, PyObject * Py_UNUSED(args), PyObject * Py_UNUSED(kwds))
 {
   // rclpy_wait_set_dealloc will take care of decref
-  self->pysubs = PySet_New(NULL);
+  self->pysubs = PyList_New(0);
   if (!self->pysubs) {
     return -1;
   }
-  self->pytmrs = PySet_New(NULL);
+  self->pytmrs = PyList_New(0);
   if (!self->pytmrs) {
     return -1;
   }
-  self->pygcs = PySet_New(NULL);
+  self->pygcs = PyList_New(0);
   if (!self->pygcs) {
     return -1;
   }
-  self->pyclis = PySet_New(NULL);
+  self->pyclis = PyList_New(0);
   if (!self->pyclis) {
     return -1;
   }
-  self->pysrvs = PySet_New(NULL);
+  self->pysrvs = PyList_New(0);
   if (!self->pysrvs) {
     return -1;
   }
-  self->pyready = PySet_New(NULL);
+  self->pyready = PyList_New(0);
   if (!self->pyready) {
     return -1;
   }
@@ -139,12 +139,12 @@ _rclpy_to_pycapsule(PyObject * pyentity)
 }
 
 
-/// Add entities of a known type to the correct set
+/// Add entities of a known type to the correct list
 /*
  * Raises ValueError if capsule is invalid
  * Handles adding entities of a given type to the wait set
  *
- * \param[in] pyset a set that the entities should be added to
+ * \param[in] pylist a list that the entities should be added to
  * \param[in] pyentities an iterable of (sub, pub, client, serv, guard)
  * \param[in] handle_attr an attribute of an entity where the handle is stored
  * \param[in] handle_type a pycapsule name this entity uses
@@ -152,11 +152,11 @@ _rclpy_to_pycapsule(PyObject * pyentity)
  */
 static inline rcl_ret_t
 _rclpy_add_entity(
-  PyObject * pyset, PyObject * pyentities, const char * handle_attr,
+  PyObject * pylist, PyObject * pyentities, const char * handle_attr,
   const char * handle_type)
 {
-  // It's possible for arbitrary python code to be invoked that invalidates this reference
-  Py_INCREF(pyset);
+  // It's possible for arbitrary python code to be invoked
+  Py_INCREF(pylist);
   Py_INCREF(pyentities);
 
   PyObject * pyiter = PyObject_GetIter(pyentities);
@@ -181,7 +181,7 @@ _rclpy_add_entity(
     }
 
     if (PyCapsule_IsValid(pyentity, handle_type)) {
-      if (-1 == PySet_Add(pyset, pyentity)) {
+      if (-1 == PyList_Append(pylist, pyentity)) {
         break;
       }
     } else {
@@ -195,7 +195,7 @@ _rclpy_add_entity(
 
   Py_DECREF(pyiter);
   Py_DECREF(pyentities);
-  Py_DECREF(pyset);
+  Py_DECREF(pylist);
 
   if (PyErr_Occurred()) {
     // Return down here after references have been cleaned dup
@@ -379,9 +379,9 @@ _rclpy_build_wait_set(rclpy_wait_set_t * self)
     return ret;
   }
 
-#define RCLPY_ADD_ENTITY(ETYPE, WSFUNC, ESET) \
+#define RCLPY_ADD_ENTITY(ETYPE, WSFUNC, ELIST) \
   do { \
-    PyObject * pyiter = PyObject_GetIter((ESET)); \
+    PyObject * pyiter = PyObject_GetIter(ELIST); \
     if (!pyiter) { \
       return RCL_RET_ERROR; \
     } \
@@ -426,13 +426,13 @@ _rclpy_build_wait_set(rclpy_wait_set_t * self)
 static inline rcl_ret_t
 _rclpy_build_ready_entities(rclpy_wait_set_t * self)
 {
-  if (-1 == PySet_Clear(self->pyready)) {
+  if (-1 == PySequence_DelSlice(self->pyready, 0, PySequence_Length(self->pyready))) {
     return RCL_RET_ERROR;
   }
 
-#define GET_READY_ENTITIES(ETYPE, ESET) \
+#define GET_READY_ENTITIES(ETYPE, ELIST) \
   do { \
-    PyObject * pyiter = PyObject_GetIter((ESET)); \
+    PyObject * pyiter = PyObject_GetIter((ELIST)); \
     if (!pyiter) { \
       return RCL_RET_ERROR; \
     } \
@@ -451,7 +451,7 @@ _rclpy_build_ready_entities(rclpy_wait_set_t * self)
       const rcl_ ## ETYPE ## _t ** struct_ptr = self->wait_set.ETYPE ## s; \
       for (idx = 0; idx < idx_max; ++idx) { \
         if (struct_ptr[idx] == entity) { \
-          if (-1 == PySet_Add(self->pyready, pyentity)) { \
+          if (-1 == PyList_Append(self->pyready, pyentity)) { \
             Py_DECREF(pyentity); \
             Py_DECREF(pyiter); \
             return RCL_RET_ERROR; \
@@ -535,11 +535,11 @@ rclpy_wait_set_wait(rclpy_wait_set_t * self, PyObject * args)
 static PyObject *
 rclpy_wait_set_clear(rclpy_wait_set_t * self)
 {
-  if (-1 == PySet_Clear(self->pysubs) ||
-    -1 == PySet_Clear(self->pytmrs) ||
-    -1 == PySet_Clear(self->pygcs) ||
-    -1 == PySet_Clear(self->pyclis) ||
-    -1 == PySet_Clear(self->pysrvs))
+  if (-1 == PySequence_DelSlice(self->pysubs, 0, PySequence_Length(self->pysubs)) ||
+    -1 == PySequence_DelSlice(self->pytmrs, 0, PySequence_Length(self->pytmrs)) ||
+    -1 == PySequence_DelSlice(self->pygcs, 0, PySequence_Length(self->pygcs)) ||
+    -1 == PySequence_DelSlice(self->pyclis, 0, PySequence_Length(self->pyclis)) ||
+    -1 == PySequence_DelSlice(self->pysrvs, 0, PySequence_Length(self->pysrvs)))
   {
     return NULL;
   }
@@ -547,7 +547,6 @@ rclpy_wait_set_clear(rclpy_wait_set_t * self)
   Py_RETURN_NONE;
 }
 
-
 /// Return True if an entity is ready
 /**
  * Raises RuntimeError if there was an error while clearing
@@ -565,13 +564,7 @@ rclpy_wait_set_is_ready(rclpy_wait_set_t * self, PyObject * args)
 
   pyentity = _rclpy_to_pycapsule(pyentity);
 
-  // Arbitrary python code could invalidate references in __hash__
-  Py_INCREF(self);
-  Py_INCREF(pyentity);
-  int contains = PySet_Contains(self->pyready, pyentity);
-  Py_DECREF(pyentity);
-  Py_DECREF(self);
-
+  int contains = PySequence_Contains(self->pyready, pyentity);
   if (-1 == contains) {
     // Exception set
     return NULL;

From e078c47c0e48ff6af3c8f3603e65fb3730ae0b95 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 8 Nov 2017 15:29:24 -0800
Subject: [PATCH 47/49] Use locally defined function instead of member function

---
 rclpy/rclpy/executors.py | 24 +++++++++---------------
 1 file changed, 9 insertions(+), 15 deletions(-)

diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py
index ad6133575..2a2bb09e3 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -244,16 +244,6 @@ def handler():
                     gc.trigger()
         return handler
 
-    def _can_execute(self, entity):
-        """
-        Return true if an entity is eligible for execution.
-
-        :param entity: an entity to be checked
-        :type entity_list: Client, Service, Publisher, Subscriber
-        :rtype: bool
-        """
-        return not entity._executor_event and entity.callback_group.can_execute(entity)
-
     def _new_callbacks(self, nodes, wait_set):
         """
         Yield brand new work to executor implementations.
@@ -326,13 +316,17 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
         while not yielded_work and not self._is_shutdown:
             self._wait_set.clear()
             # Gather entities that can be waited on
+
+            def can_execute(entity):
+                return not entity._executor_event and entity.callback_group.can_execute(entity)
+
             guards = []
             for node in nodes:
-                self._wait_set.add_subscriptions(filter(self._can_execute, node.subscriptions))
-                self._wait_set.add_timers(filter(self._can_execute, node.timers))
-                self._wait_set.add_clients(filter(self._can_execute, node.clients))
-                self._wait_set.add_services(filter(self._can_execute, node.services))
-                guards.extend(filter(self._can_execute, node.guards))
+                self._wait_set.add_subscriptions(filter(can_execute, node.subscriptions))
+                self._wait_set.add_timers(filter(can_execute, node.timers))
+                self._wait_set.add_clients(filter(can_execute, node.clients))
+                self._wait_set.add_services(filter(can_execute, node.services))
+                guards.extend(filter(can_execute, node.guards))
 
             # retrigger a guard condition that was triggered but not handled
             for gc in guards:

From 5c920852d501c876fc8d5ea1d56faa555bdc31f1 Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 8 Nov 2017 15:42:31 -0800
Subject: [PATCH 48/49] Shorten code using macro

---
 rclpy/src/rclpy/_rclpy_wait_set.c | 37 +++++++++++--------------------
 1 file changed, 13 insertions(+), 24 deletions(-)

diff --git a/rclpy/src/rclpy/_rclpy_wait_set.c b/rclpy/src/rclpy/_rclpy_wait_set.c
index 7ed500112..646b99981 100644
--- a/rclpy/src/rclpy/_rclpy_wait_set.c
+++ b/rclpy/src/rclpy/_rclpy_wait_set.c
@@ -78,30 +78,19 @@ rclpy_wait_set_init(
   rclpy_wait_set_t * self, PyObject * Py_UNUSED(args), PyObject * Py_UNUSED(kwds))
 {
   // rclpy_wait_set_dealloc will take care of decref
-  self->pysubs = PyList_New(0);
-  if (!self->pysubs) {
-    return -1;
-  }
-  self->pytmrs = PyList_New(0);
-  if (!self->pytmrs) {
-    return -1;
-  }
-  self->pygcs = PyList_New(0);
-  if (!self->pygcs) {
-    return -1;
-  }
-  self->pyclis = PyList_New(0);
-  if (!self->pyclis) {
-    return -1;
-  }
-  self->pysrvs = PyList_New(0);
-  if (!self->pysrvs) {
-    return -1;
-  }
-  self->pyready = PyList_New(0);
-  if (!self->pyready) {
-    return -1;
-  }
+#define MAKE_LIST_OR_BAIL(ELIST) \
+  ELIST = PyList_New(0); \
+  if (!(ELIST)) { \
+    return -1; \
+  }
+
+  MAKE_LIST_OR_BAIL(self->pysubs);
+  MAKE_LIST_OR_BAIL(self->pytmrs);
+  MAKE_LIST_OR_BAIL(self->pygcs);
+  MAKE_LIST_OR_BAIL(self->pyclis);
+  MAKE_LIST_OR_BAIL(self->pysrvs);
+  MAKE_LIST_OR_BAIL(self->pyready);
+#undef MAKE_LIST_OR_BAIL
 
   rcl_ret_t ret = rcl_wait_set_init(&(self->wait_set), 0, 0, 0, 0, 0, rcl_get_default_allocator());
   if (ret != RCL_RET_OK) {

From 70b86524692c656cdffa2909968b59015309f0ec Mon Sep 17 00:00:00 2001
From: Shane Loretz <sloretz@osrfoundation.org>
Date: Wed, 8 Nov 2017 17:23:38 -0800
Subject: [PATCH 49/49] Move everything to new wait_set code

---
 rclpy/rclpy/client.py             |  17 +-
 rclpy/rclpy/graph_listener.py     |  72 ++++---
 rclpy/rclpy/wait_set.py           |  67 ------
 rclpy/src/rclpy/_rclpy.c          | 342 ------------------------------
 rclpy/src/rclpy/_rclpy_wait_set.c |   4 +-
 5 files changed, 46 insertions(+), 456 deletions(-)
 delete mode 100644 rclpy/rclpy/wait_set.py

diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py
index 86720c43f..92411e034 100644
--- a/rclpy/rclpy/client.py
+++ b/rclpy/rclpy/client.py
@@ -16,6 +16,7 @@
 
 from rclpy.graph_listener import GraphEventSubscription
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.impl.implementation_singleton import rclpy_wait_set_implementation as _rclpy_wait_set
 import rclpy.utilities
 
 
@@ -24,24 +25,18 @@ class ResponseThread(threading.Thread):
     def __init__(self, client):
         threading.Thread.__init__(self)
         self.client = client
-        self.wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
-        _rclpy.rclpy_wait_set_init(self.wait_set, 0, 1, 0, 1, 0)
-        _rclpy.rclpy_wait_set_clear_entities('client', self.wait_set)
-        _rclpy.rclpy_wait_set_add_entity(
-            'client', self.wait_set, self.client.client_handle)
+        self._wait_set = _rclpy_wait_set.WaitSet()
 
     def run(self):
         [sigint_gc, sigint_gc_handle] = _rclpy.rclpy_get_sigint_guard_condition()
-        _rclpy.rclpy_wait_set_add_entity(self.wait_set, sigint_gc)
+        self._wait_set.add_guard_conditions([sigint_gc])
+        self._wait_set.add_clients([self.client.client_handle])
 
-        _rclpy.rclpy_wait(self.wait_set, -1)
-
-        guard_condition_ready_list = \
-            _rclpy.rclpy_get_ready_entities('guard_condition', self.wait_set)
+        self._wait_set.wait(-1)
 
         # destroying here to make sure we dont call shutdown before cleaning up
         _rclpy.rclpy_destroy_entity(sigint_gc)
-        if sigint_gc_handle in guard_condition_ready_list:
+        if self._wait_set.is_ready(sigint_gc):
             rclpy.utilities.shutdown()
             return
         response = _rclpy.rclpy_take_response(
diff --git a/rclpy/rclpy/graph_listener.py b/rclpy/rclpy/graph_listener.py
index 7354360c9..68f6fb69e 100644
--- a/rclpy/rclpy/graph_listener.py
+++ b/rclpy/rclpy/graph_listener.py
@@ -19,10 +19,10 @@
 from rclpy.guard_condition import GuardCondition
 from rclpy.guard_condition import NodeGraphGuardCondition
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.impl.implementation_singleton import rclpy_wait_set_implementation as _rclpy_wait_set
 import rclpy.logging
 from rclpy.timer import WallTimer
 from rclpy.utilities import ok
-from rclpy.wait_set import WaitSet
 
 
 class GraphListenerSingleton:
@@ -43,6 +43,7 @@ def __init__(self):
         self._gc = GuardCondition(None, None)
         self._thread = None
         self._lock = threading.RLock()
+        self._wait_set = _rclpy_wait_set.WaitSet()
 
     def __del__(self):
         self.destroy()
@@ -142,42 +143,43 @@ def remove_callback(self, node_handle, callback):
 
     def _runner(self):
         while True:
+            self._wait_set.clear()
             with self._lock:
-                guards = list(self._guards.values())
-                timers = list(self._timers.keys())
-                guards.append(self._gc)
-
-            with WaitSet([], guards, timers, [], []) as wait_set:
-                # Wait 1 second
-                wait_set.wait(S_TO_NS)
-
-                with self._lock:
-                    # Shutdown if necessary
-                    if not ok():
-                        self.destroy()
-                        break
-
-                    # notify graph event subscribers
-                    if not self._thread:
-                        # Asked to shut down thread
-                        return
-                    ready_callbacks = []
-                    # Guard conditions
-                    for gc_pointer, callback_list in self._callbacks.items():
-                        if wait_set.is_ready(gc_pointer):
-                            for callback in callback_list:
-                                ready_callbacks.append(callback)
-                    # Timers
-                    for tmr, callback in self._timers.items():
-                        if wait_set.is_ready(tmr.timer_pointer):
+                self._wait_set.add_guard_conditions(self._guards.values())
+                self._wait_set.add_guard_conditions([self._gc])
+                self._wait_set.add_timers(self._timers.keys())
+
+            # Wait 1 second
+            self._wait_set.wait(S_TO_NS)
+
+            with self._lock:
+                # Shutdown if necessary
+                if not ok():
+                    self.destroy()
+                    break
+
+                # notify graph event subscribers
+                if not self._thread:
+                    # Asked to shut down thread
+                    return
+                ready_callbacks = []
+                # Guard conditions
+                for gc_pointer, callback_list in self._callbacks.items():
+                    gc = self._guards[gc_pointer]
+                    if self._wait_set.is_ready(gc):
+                        for callback in callback_list:
                             ready_callbacks.append(callback)
-                            _rclpy.rclpy_call_timer(tmr.timer_handle)
-                    # Call callbacks
-                    for callback in ready_callbacks:
-                        try:
-                            callback()
-                        except Exception:
-                            rclpy.logging.logwarn(traceback.format_exc())
+                # Timers
+                for tmr, callback in self._timers.items():
+                    if self._wait_set.is_ready(tmr):
+                        ready_callbacks.append(callback)
+                        _rclpy.rclpy_call_timer(tmr.timer_handle)
+                # Call callbacks
+                for callback in ready_callbacks:
+                    try:
+                        callback()
+                    except Exception:
+                        rclpy.logging.logwarn(traceback.format_exc())
 
 
 class GraphEventSubscription:
diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py
deleted file mode 100644
index 5d470e72d..000000000
--- a/rclpy/rclpy/wait_set.py
+++ /dev/null
@@ -1,67 +0,0 @@
-# Copyright 2017 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.
-
-from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
-
-
-class WaitSet:
-    """Provide conveneint methods and destroy the wait set when garbage collected."""
-
-    def __init__(self, subscriptions, guards, timers, clients, services):
-        # List of entity pointers (the python integer, not the PyCapsule) that are ready
-        self._ready_pointers = []
-        self._subscriptions = subscriptions
-        self._guards = guards
-        self._timers = timers
-        self._clients = clients
-        self._services = services
-
-        self._wait_set = None
-
-    def __enter__(self):
-        self._wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
-        _rclpy.rclpy_wait_set_init(
-            self._wait_set,
-            len(self._subscriptions),
-            len(self._guards),
-            len(self._timers),
-            len(self._clients),
-            len(self._services))
-        return self
-
-    def __exit__(self, t, v, tb):
-        _rclpy.rclpy_destroy_wait_set(self._wait_set)
-
-    def wait(self, timeout_nsec):
-        # Populate wait set
-        _rclpy.rclpy_wait_set_clear_entities(self._wait_set)
-        _rclpy.rclpy_wait_set_add_entities(
-            self._wait_set, map(lambda sub: sub.subscription_handle, self._subscriptions))
-        _rclpy.rclpy_wait_set_add_entities(
-            self._wait_set, map(lambda gc: gc.guard_handle, self._guards))
-        _rclpy.rclpy_wait_set_add_entities(
-            self._wait_set, map(lambda tmr: tmr.timer_handle, self._timers))
-        _rclpy.rclpy_wait_set_add_entities(
-            self._wait_set, map(lambda cli: cli.client_handle, self._clients))
-        _rclpy.rclpy_wait_set_add_entities(
-            self._wait_set, map(lambda srv: srv.service_handle, self._services))
-
-        # Wait
-        _rclpy.rclpy_wait(self._wait_set, timeout_nsec)
-
-        # Get results
-        self._ready_pointers = _rclpy.rclpy_get_ready_entities(self._wait_set)
-
-    def is_ready(self, entity_pointer):
-        return entity_pointer in self._ready_pointers
diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index 2cd1294db..5d38a1380 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -1730,313 +1730,6 @@ rclpy_get_rmw_implementation_identifier(PyObject * Py_UNUSED(self), PyObject * P
   return pyrmw_implementation_identifier;
 }
 
-/// Return a Capsule pointing to a zero initialized rcl_wait_set_t structure
-static PyObject *
-rclpy_get_zero_initialized_wait_set(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args))
-{
-  rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyMem_Malloc(sizeof(rcl_wait_set_t));
-  *wait_set = rcl_get_zero_initialized_wait_set();
-  PyObject * pywait_set = PyCapsule_New(wait_set, "rcl_wait_set_t", NULL);
-
-  return pywait_set;
-}
-
-/// Initialize a waitset
-/**
- * Raises RuntimeError if the wait set could not be initialized
- *
- * \param[in] pywait_set Capsule pointing to the waitset structure
- * \param[in] node_name string name of the node to be created
- * \param[in] number_of_subscriptions int
- * \param[in] number_of_guard_conditions int
- * \param[in] number_of_timers int
- * \param[in] number_of_clients int
- * \param[in] number_of_services int
- * \return None
- */
-static PyObject *
-rclpy_wait_set_init(PyObject * Py_UNUSED(self), PyObject * args)
-{
-  PyObject * pywait_set;
-  unsigned PY_LONG_LONG number_of_subscriptions;
-  unsigned PY_LONG_LONG number_of_guard_conditions;
-  unsigned PY_LONG_LONG number_of_timers;
-  unsigned PY_LONG_LONG number_of_clients;
-  unsigned PY_LONG_LONG number_of_services;
-
-  if (!PyArg_ParseTuple(
-      args, "OKKKKK", &pywait_set, &number_of_subscriptions,
-      &number_of_guard_conditions, &number_of_timers,
-      &number_of_clients, &number_of_services))
-  {
-    return NULL;
-  }
-
-  rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
-  if (!wait_set) {
-    return NULL;
-  }
-  rcl_ret_t ret = rcl_wait_set_init(
-    wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers,
-    number_of_clients, number_of_services, rcl_get_default_allocator());
-  if (ret != RCL_RET_OK) {
-    PyErr_Format(PyExc_RuntimeError,
-      "Failed to initialize wait set: %s", rcl_get_error_string_safe());
-    rcl_reset_error();
-    return NULL;
-  }
-  Py_RETURN_NONE;
-}
-
-/// Clear all the pointers of all wait_set fields
-/**
- * Raises RuntimeError if the entity type is unknown or any rcl error occurs
- *
- * \param[in] pywait_set Capsule pointing to the waitset structure
- * \return None
- */
-static PyObject *
-rclpy_wait_set_clear_entities(PyObject * Py_UNUSED(self), PyObject * args)
-{
-  PyObject * pywait_set;
-
-  if (!PyArg_ParseTuple(args, "O", &pywait_set)) {
-    return NULL;
-  }
-
-  rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
-  if (!wait_set) {
-    return NULL;
-  }
-
-  if (
-    RCL_RET_OK != rcl_wait_set_clear_subscriptions(wait_set) ||
-    RCL_RET_OK != rcl_wait_set_clear_clients(wait_set) ||
-    RCL_RET_OK != rcl_wait_set_clear_services(wait_set) ||
-    RCL_RET_OK != rcl_wait_set_clear_timers(wait_set) ||
-    RCL_RET_OK != rcl_wait_set_clear_guard_conditions(wait_set))
-  {
-    PyErr_Format(PyExc_RuntimeError,
-      "Failed to clear from wait set: %s", rcl_get_error_string_safe());
-    rcl_reset_error();
-    return NULL;
-  }
-  Py_RETURN_NONE;
-}
-
-/// Add entities to the waitset structure
-/**
- * Raises RuntimeError if any entity type is unknown or any rcl error occurs
- * Raises ValueError if an entity is not a valid PyCapsule
- *
- * \param[in] pywait_set Capsule pointing to the waitset structure
- * \param[in] pyentities Iterable of PyCapsule belonging to an entity (sub, pub, timer, ...)
- * \return None
- */
-static PyObject *
-rclpy_wait_set_add_entities(PyObject * Py_UNUSED(self), PyObject * args)
-{
-  PyObject * pywait_set;
-  PyObject * pyentities;
-
-  if (!PyArg_ParseTuple(args, "OO", &pywait_set, &pyentities)) {
-    return NULL;
-  }
-
-  rcl_ret_t ret;
-  rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
-  if (!wait_set) {
-    return NULL;
-  }
-
-  PyObject * pyiterator = PyObject_GetIter(pyentities);
-  if (!pyiterator) {
-    return NULL;
-  }
-
-  PyObject * pyentity;
-  while ((pyentity = PyIter_Next(pyiterator))) {
-    if (PyCapsule_IsValid(pyentity, "rcl_subscription_t")) {
-      rcl_subscription_t * subscription =
-        (rcl_subscription_t *)PyCapsule_GetPointer(pyentity, "rcl_subscription_t");
-      ret = rcl_wait_set_add_subscription(wait_set, subscription);
-    } else if (PyCapsule_IsValid(pyentity, "rcl_client_t")) {
-      rcl_client_t * client = (rcl_client_t *)PyCapsule_GetPointer(pyentity, "rcl_client_t");
-      ret = rcl_wait_set_add_client(wait_set, client);
-    } else if (PyCapsule_IsValid(pyentity, "rcl_service_t")) {
-      rcl_service_t * service = (rcl_service_t *)PyCapsule_GetPointer(pyentity, "rcl_service_t");
-      ret = rcl_wait_set_add_service(wait_set, service);
-    } else if (PyCapsule_IsValid(pyentity, "rcl_timer_t")) {
-      rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pyentity, "rcl_timer_t");
-      ret = rcl_wait_set_add_timer(wait_set, timer);
-    } else if (PyCapsule_IsValid(pyentity, "rcl_guard_condition_t")) {
-      rcl_guard_condition_t * guard_condition =
-        (rcl_guard_condition_t *)PyCapsule_GetPointer(pyentity, "rcl_guard_condition_t");
-      ret = rcl_wait_set_add_guard_condition(wait_set, guard_condition);
-    }
-
-    if (ret != RCL_RET_OK) {
-      const char * entity_type = PyCapsule_GetName(pyentity);
-      if (entity_type) {
-        PyErr_Format(PyExc_RuntimeError,
-          "Failed to add '%s' to wait set: %s", entity_type, rcl_get_error_string_safe());
-      }
-      // else { // PyCapsule_GetName set an exception }
-      rcl_reset_error();
-      Py_DECREF(pyentity);
-      break;
-    } else {
-      Py_DECREF(pyentity);
-    }
-  }
-
-  Py_DECREF(pyiterator);
-
-  if (PyErr_Occurred()) {
-    return NULL;
-  }
-  Py_RETURN_NONE;
-}
-
-/// Destroy the waitset structure
-/**
- * Raises RuntimeError if the wait set could not be destroyed
- *
- * \param[in] pywait_set Capsule pointing to the waitset structure
- * \return None
- */
-static PyObject *
-rclpy_destroy_wait_set(PyObject * Py_UNUSED(self), PyObject * args)
-{
-  PyObject * pywait_set;
-
-  if (!PyArg_ParseTuple(args, "O", &pywait_set)) {
-    return NULL;
-  }
-  rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
-  if (!wait_set) {
-    return NULL;
-  }
-
-  rcl_ret_t ret = rcl_wait_set_fini(wait_set);
-  if (ret != RCL_RET_OK) {
-    PyErr_Format(PyExc_RuntimeError,
-      "Failed to fini wait set: %s", rcl_get_error_string_safe());
-    rcl_reset_error();
-    return NULL;
-  }
-
-  PyMem_Free(wait_set);
-
-  if (PyCapsule_SetPointer(pywait_set, Py_None)) {
-    // exception set by PyCapsule_SetPointer
-    return NULL;
-  }
-
-  Py_RETURN_NONE;
-}
-
-#define GET_LIST_READY_ENTITIES(ENTITY_TYPE) \
-  { \
-    size_t idx; \
-    size_t idx_max; \
-    idx_max = wait_set->size_of_ ## ENTITY_TYPE ## s; \
-    const rcl_ ## ENTITY_TYPE ## _t ** struct_ptr = wait_set->ENTITY_TYPE ## s; \
-    for (idx = 0; idx < idx_max; idx ++) { \
-      if (struct_ptr[idx]) { \
-        PyObject * pyptr = PyLong_FromUnsignedLongLong((uint64_t) & struct_ptr[idx]->impl); \
-        if (!pyptr) { \
-          Py_DECREF(entity_ready_list); \
-          return NULL; \
-        } \
-        if (-1 == PyList_Append(entity_ready_list, pyptr)) { \
-          Py_DECREF(pyptr); \
-          Py_DECREF(entity_ready_list); \
-          return NULL; \
-        } \
-      } \
-    } \
-  }
-/// Get list of non-null entities in waitset
-/**
- * Raises ValueError if pywait_set is not a wait_set capsule
- *
- * \param[in] pywait_set Capsule pointing to the waitset structure
- * \return List of wait_set entities pointers ready for take
- */
-static PyObject *
-rclpy_get_ready_entities(PyObject * Py_UNUSED(self), PyObject * args)
-{
-  PyObject * pywait_set;
-  if (!PyArg_ParseTuple(args, "O", &pywait_set)) {
-    return NULL;
-  }
-
-  rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
-  if (!wait_set) {
-    return NULL;
-  }
-
-  PyObject * entity_ready_list = PyList_New(0);
-  if (!entity_ready_list) {
-    return NULL;
-  }
-  GET_LIST_READY_ENTITIES(subscription)
-  GET_LIST_READY_ENTITIES(client)
-  GET_LIST_READY_ENTITIES(service)
-  GET_LIST_READY_ENTITIES(timer)
-  GET_LIST_READY_ENTITIES(guard_condition)
-
-  return entity_ready_list;
-}
-
-/// Wait until timeout is reached or event happened
-/**
- * Raises ValueError if pywait_set is not a wait_set capsule
- * Raises RuntimeError if there was an error while waiting
- *
- * This function will wait for an event to happen or for the timeout to expire.
- * A negative timeout means wait forever, a timeout of 0 means no wait
- * \param[in] pywait_set Capsule pointing to the waitset structure
- * \param[in] timeout optional time to wait before waking up (in nanoseconds)
- * \return NULL
- */
-static PyObject *
-rclpy_wait(PyObject * Py_UNUSED(self), PyObject * args)
-{
-  PyObject * pywait_set;
-  PY_LONG_LONG timeout = -1;
-
-  if (!PyArg_ParseTuple(args, "O|K", &pywait_set, &timeout)) {
-    return NULL;
-  }
-#ifdef _WIN32
-  _crt_signal_t
-#else
-  sig_t
-#endif  // _WIN32
-  previous_handler = signal(SIGINT, rclpy_catch_function);
-  rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
-  if (!wait_set) {
-    return NULL;
-  }
-  rcl_ret_t ret;
-
-  // Could be a long wait, release the GIL
-  Py_BEGIN_ALLOW_THREADS;
-  ret = rcl_wait(wait_set, timeout);
-  Py_END_ALLOW_THREADS;
-
-  signal(SIGINT, previous_handler);
-  if (ret != RCL_RET_OK && ret != RCL_RET_TIMEOUT) {
-    PyErr_Format(PyExc_RuntimeError,
-      "Failed to wait on wait set: %s", rcl_get_error_string_safe());
-    rcl_reset_error();
-    return NULL;
-  }
-  Py_RETURN_NONE;
-}
-
 /// Take a message from a given subscription
 /**
  * \param[in] pysubscription Capsule pointing to the subscription to process the message
@@ -2720,36 +2413,6 @@ static PyMethodDef rclpy_methods[] = {
     "Send a response."
   },
 
-  {
-    "rclpy_get_zero_initialized_wait_set", rclpy_get_zero_initialized_wait_set, METH_NOARGS,
-    "rclpy_get_zero_initialized_wait_set."
-  },
-
-  {
-    "rclpy_wait_set_init", rclpy_wait_set_init, METH_VARARGS,
-    "rclpy_wait_set_init."
-  },
-
-  {
-    "rclpy_wait_set_clear_entities", rclpy_wait_set_clear_entities, METH_VARARGS,
-    "rclpy_wait_set_clear_entities."
-  },
-
-  {
-    "rclpy_wait_set_add_entities", rclpy_wait_set_add_entities, METH_VARARGS,
-    "rclpy_wait_set_add_entities."
-  },
-
-  {
-    "rclpy_destroy_wait_set", rclpy_destroy_wait_set, METH_VARARGS,
-    "rclpy_destroy_wait_set."
-  },
-
-  {
-    "rclpy_get_ready_entities", rclpy_get_ready_entities, METH_VARARGS,
-    "List non null subscriptions in waitset."
-  },
-
   {
     "rclpy_reset_timer", rclpy_reset_timer, METH_VARARGS,
     "Reset a timer."
@@ -2795,11 +2458,6 @@ static PyMethodDef rclpy_methods[] = {
     "Get the period of a timer."
   },
 
-  {
-    "rclpy_wait", rclpy_wait, METH_VARARGS,
-    "rclpy_wait."
-  },
-
   {
     "rclpy_take", rclpy_take, METH_VARARGS,
     "rclpy_take."
diff --git a/rclpy/src/rclpy/_rclpy_wait_set.c b/rclpy/src/rclpy/_rclpy_wait_set.c
index 646b99981..f15fa62a2 100644
--- a/rclpy/src/rclpy/_rclpy_wait_set.c
+++ b/rclpy/src/rclpy/_rclpy_wait_set.c
@@ -151,6 +151,8 @@ _rclpy_add_entity(
   PyObject * pyiter = PyObject_GetIter(pyentities);
   if (!pyiter) {
     // exception set
+    Py_DECREF(pylist);
+    Py_DECREF(pyentities);
     return RCL_RET_ERROR;
   }
 
@@ -161,7 +163,7 @@ _rclpy_add_entity(
       pyentity = PyObject_GetAttrString(pyentity, handle_attr);
     }
 
-    // No chance of arbitracy python code below, so decref early
+    // No chance of arbitrary python code below, so decref early
     Py_DECREF(pyentity);
 
     if (!pyentity) {