diff --git a/bdai_ros2_wrappers/bdai_ros2_wrappers/node.py b/bdai_ros2_wrappers/bdai_ros2_wrappers/node.py index 4550724..0e5810b 100644 --- a/bdai_ros2_wrappers/bdai_ros2_wrappers/node.py +++ b/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."""