diff --git a/rclpy/rclpy/wait_for_message.py b/rclpy/rclpy/wait_for_message.py index 5b2a8de3c..9d966c528 100644 --- a/rclpy/rclpy/wait_for_message.py +++ b/rclpy/rclpy/wait_for_message.py @@ -39,24 +39,27 @@ def wait_for_message( wait_set.clear_entities() sub = node.create_subscription(msg_type, topic, lambda _: None, 1) - wait_set.add_subscription(sub.handle) - sigint_gc = SignalHandlerGuardCondition(context=context) - wait_set.add_guard_condition(sigint_gc.handle) - - timeout_nsec = timeout_sec_to_nsec(time_to_wait) - wait_set.wait(timeout_nsec) - - subs_ready = wait_set.get_ready_entities('subscription') - guards_ready = wait_set.get_ready_entities('guard_condition') - - if guards_ready: - if sigint_gc.handle.pointer in guards_ready: - return (False, None) - - if subs_ready: - if sub.handle.pointer in subs_ready: - msg_info = sub.handle.take_message(sub.msg_type, sub.raw) - if msg_info is not None: - return (True, msg_info[0]) - - return (False, None) + try: + wait_set.add_subscription(sub.handle) + sigint_gc = SignalHandlerGuardCondition(context=context) + wait_set.add_guard_condition(sigint_gc.handle) + + timeout_nsec = timeout_sec_to_nsec(time_to_wait) + wait_set.wait(timeout_nsec) + + subs_ready = wait_set.get_ready_entities('subscription') + guards_ready = wait_set.get_ready_entities('guard_condition') + + if guards_ready: + if sigint_gc.handle.pointer in guards_ready: + return (False, None) + + if subs_ready: + if sub.handle.pointer in subs_ready: + msg_info = sub.handle.take_message(sub.msg_type, sub.raw) + if msg_info is not None: + return (True, msg_info[0]) + + return (False, None) + finally: + node.destroy_subscription(sub)