Fixed wait_set not clearing ready_pointers

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,22 +51,18 @@ 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 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):
@@ -76,12 +71,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):
@@ -90,12 +83,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):
@@ -104,7 +95,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):
@@ -142,17 +132,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

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)