Workaround race in Waitables handling

Signed-off-by: Michel Hidalgo <[email protected]>

bdai_ros2_wrappers/bdai_ros2_wrappers/node.py

@@ -1,13 +1,32 @@
 # Copyright (c) 2023 Boston Dynamics AI Institute Inc.  All rights reserved.
-from typing import Any, Optional
+import contextlib
+from typing import Any, Iterable, Optional, cast
 
 from rclpy.callback_groups import CallbackGroup
+from rclpy.exceptions import InvalidHandle
 from rclpy.node import Node as BaseNode
+from rclpy.wait_set import WaitSet
+from rclpy.waitables import Waitable
 
 from bdai_ros2_wrappers.callback_groups import NonReentrantCallbackGroup
 from bdai_ros2_wrappers.logging import MemoizingRcutilsLogger, as_memoizing_logger
 
 
+class SafeWaitable:
+    """A decorator to workaround https://github.com/ros2/rclpy/issues/1284."""
+
+    def __init__(self, wrapped: Waitable) -> None:
+        self._wrapped = wrapped
+
+    def __getattr__(self, name: str) -> Any:
+        return getattr(self._wrapped, name)
+
+    def add_to_wait_set(self, wait_set: WaitSet) -> None:
+        """Add waitable to `wait_set` safely."""
+        with contextlib.suppress(InvalidHandle):
+            self._wrapped.add_to_wait_set(wait_set)
+
+
 class Node(BaseNode):
     """An rclpy.node.Node subclass that:
 
@@ -40,6 +59,12 @@ def default_callback_group(self) -> CallbackGroup:
         # NOTE(hidmic): this overrides the hardcoded default group in rclpy.node.Node implementation
         return self._default_callback_group_override
 
+    @property
+    def waitables(self) -> Iterable[Waitable]:
+        """Get node waitables."""
+        for waitable in super().waitables:
+            yield cast(Waitable, SafeWaitable(waitable))
+
     @property
     def destruction_requested(self) -> bool:
         """Checks whether destruction was requested or not."""