diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index dfeab2cab..eabc0ac0c 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from collections.abc import Callable import threading import time from types import TracebackType @@ -46,12 +47,16 @@ def __init__( srv_type: type[Srv[SrvRequestT, SrvResponseT]], srv_name: str, qos_profile: QoSProfile, + *, + on_destroy: Optional[Callable[['BaseClient[SrvRequestT, SrvResponseT]'], None]] = None, ) -> None: self.context = context self.__client = client_impl self.srv_type = srv_type self.srv_name = srv_name self.qos_profile = qos_profile + self._on_destroy = on_destroy + self._destroyed = False def service_is_ready(self) -> bool: """ @@ -95,13 +100,17 @@ def logger_name(self) -> str: return self.__client.get_logger_name() def destroy(self) -> None: - """ - Destroy a container for a ROS service client. - - .. warning:: Users should not destroy a service client with this destructor, instead they - should call :meth:`.Node.destroy_client`. - """ - self.__client.destroy_when_not_in_use() + """Destroy the client, notifying the owning node and releasing the handle.""" + if self._destroyed: + return + self._destroyed = True + if self._on_destroy is not None: + self._on_destroy(self) + self._on_destroy = None + self._destroy() + + def _destroy(self) -> None: + self.handle.destroy_when_not_in_use() def __enter__(self) -> Self: return self @@ -124,6 +133,7 @@ def __init__( srv_name: str, qos_profile: QoSProfile, *, + on_destroy: Optional[Callable[['BaseClient[SrvRequestT, SrvResponseT]'], None]] = None, callback_group: CallbackGroup ) -> None: """ @@ -145,7 +155,8 @@ def __init__( client_impl=client_impl, srv_type=srv_type, srv_name=srv_name, - qos_profile=qos_profile + qos_profile=qos_profile, + on_destroy=on_destroy ) # Key is a sequence number, value is an instance of a Future self._pending_requests: Dict[int, Future[SrvResponseT]] = {} diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 2313e8318..62b21d906 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -144,6 +144,7 @@ def __init__( self._state_machine.service_change_state.name, self.__on_change_state, QoSProfile(**self._state_machine.service_change_state.qos), + on_destroy=self._on_destroy_service, callback_group=callback_group) self._service_get_state = Service( self._state_machine.service_get_state, @@ -151,6 +152,7 @@ def __init__( self._state_machine.service_get_state.name, self.__on_get_state, QoSProfile(**self._state_machine.service_get_state.qos), + on_destroy=self._on_destroy_service, callback_group=callback_group) self._service_get_available_states = Service( self._state_machine.service_get_available_states, @@ -158,6 +160,7 @@ def __init__( self._state_machine.service_get_available_states.name, self.__on_get_available_states, QoSProfile(**self._state_machine.service_get_available_states.qos), + on_destroy=self._on_destroy_service, callback_group=callback_group) self._service_get_available_transitions = Service( self._state_machine.service_get_available_transitions, @@ -165,6 +168,7 @@ def __init__( self._state_machine.service_get_available_transitions.name, self.__on_get_available_transitions, QoSProfile(**self._state_machine.service_get_available_transitions.qos), + on_destroy=self._on_destroy_service, callback_group=callback_group) self._service_get_transition_graph = Service( self._state_machine.service_get_transition_graph, @@ -172,6 +176,7 @@ def __init__( self._state_machine.service_get_transition_graph.name, self.__on_get_transition_graph, QoSProfile(**self._state_machine.service_get_transition_graph.qos), + on_destroy=self._on_destroy_service, callback_group=callback_group) lifecycle_services = [ diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index 4163b9cab..2ece2950b 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -14,7 +14,8 @@ from __future__ import annotations -from typing import Generic, Tuple, Type, TYPE_CHECKING, TypedDict, Union +from collections.abc import Callable +from typing import Generic, Optional, Tuple, Type, TYPE_CHECKING, TypedDict, Union from rclpy.callback_groups import CallbackGroup from rclpy.event_handler import PublisherEventCallbacks @@ -37,6 +38,7 @@ class LifecyclePublisherKWArgs(TypedDict, Generic[MsgT]): msg_type: Type[MsgT] topic: str qos_profile: QoSProfile + on_destroy: Optional[Callable[['Publisher[MsgT]'], None]] event_callbacks: PublisherEventCallbacks callback_group: CallbackGroup diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 341070f2b..56bf695f9 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -50,7 +50,7 @@ from rclpy.callback_groups import CallbackGroup from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.client import Client +from rclpy.client import BaseClient, Client from rclpy.clock import Clock from rclpy.clock_type import ClockType from rclpy.constants import S_TO_NS @@ -85,6 +85,7 @@ from rclpy.qos import QoSProfile from rclpy.qos_overriding_options import _declare_qos_parameters from rclpy.qos_overriding_options import QoSOverridingOptions +from rclpy.service import BaseService from rclpy.service import Service from rclpy.service import ServiceCallbackUnion from rclpy.subscription import GenericSubscriptionCallbackUnion @@ -92,7 +93,7 @@ from rclpy.subscription import SubscriptionCallbackUnion from rclpy.subscription_content_filter_options import ContentFilterOptions from rclpy.time_source import TimeSource -from rclpy.timer import Rate +from rclpy.timer import BaseTimer, Rate from rclpy.timer import Timer from rclpy.timer import TimerCallbackUnion from rclpy.type_description_service import TypeDescriptionService @@ -1622,6 +1623,7 @@ def create_publisher( try: publisher = publisher_class( publisher_object, msg_type, topic, qos_profile, + on_destroy=self._on_destroy_publisher, event_callbacks=event_callbacks or PublisherEventCallbacks(), callback_group=callback_group) except Exception: @@ -1748,6 +1750,7 @@ def create_subscription( subscription = Subscription( subscription_object, msg_type, topic, callback, qos_profile, raw, + on_destroy=self._on_destroy_subscription, callback_group=callback_group, event_callbacks=event_callbacks or SubscriptionEventCallbacks()) except Exception: @@ -1798,6 +1801,7 @@ def create_client( client = Client( self.context, client_impl, srv_type, srv_name, qos_profile, + on_destroy=self._on_destroy_client, callback_group=callback_group) callback_group.add_entity(client) self._clients.append(client) @@ -1843,6 +1847,7 @@ def create_service( service = Service( service_impl, srv_type, srv_name, callback, qos_profile, + on_destroy=self._on_destroy_service, callback_group=callback_group) callback_group.add_entity(service) self._services.append(service) @@ -1881,6 +1886,7 @@ def create_timer( timer = Timer( callback, timer_period_nsec, clock, callback_group=callback_group, + on_destroy=self._on_destroy_timer, context=self.context, autostart=autostart ) @@ -1934,6 +1940,30 @@ def create_rate( timer = self.create_timer(period, callback, group, clock) return Rate(timer, context=self.context) + def _on_destroy_publisher(self, publisher: Publisher) -> None: + self._publishers.remove(publisher) + for event_handler in publisher.event_handlers: + self.__waitables.remove(event_handler) + self._wake_executor() + + def _on_destroy_subscription(self, subscription: Subscription[Any]) -> None: + self._subscriptions.remove(subscription) + for event_handler in subscription.event_handlers: + self.__waitables.remove(event_handler) + self._wake_executor() + + def _on_destroy_client(self, client: BaseClient[Any, Any]) -> None: + self._clients.remove(client) + self._wake_executor() + + def _on_destroy_service(self, service: BaseService[Any, Any]) -> None: + self._services.remove(service) + self._wake_executor() + + def _on_destroy_timer(self, timer: BaseTimer) -> None: + self._timers.remove(timer) + self._wake_executor() + def destroy_publisher(self, publisher: Publisher[Any]) -> bool: """ Destroy a publisher created by the node. @@ -1941,14 +1971,7 @@ def destroy_publisher(self, publisher: Publisher[Any]) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ if publisher in self._publishers: - self._publishers.remove(publisher) - for event_handler in publisher.event_handlers: - self.__waitables.remove(event_handler) - try: - publisher.destroy() - except InvalidHandle: - return False - self._wake_executor() + publisher.destroy() return True return False @@ -1959,14 +1982,7 @@ def destroy_subscription(self, subscription: Subscription[Any]) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ if subscription in self._subscriptions: - self._subscriptions.remove(subscription) - for event_handler in subscription.event_handlers: - self.__waitables.remove(event_handler) - try: - subscription.destroy() - except InvalidHandle: - return False - self._wake_executor() + subscription.destroy() return True return False @@ -1977,12 +1993,7 @@ def destroy_client(self, client: Client[Any, Any]) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ if client in self._clients: - self._clients.remove(client) - try: - client.destroy() - except InvalidHandle: - return False - self._wake_executor() + client.destroy() return True return False @@ -1993,12 +2004,7 @@ def destroy_service(self, service: Service[Any, Any]) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ if service in self._services: - self._services.remove(service) - try: - service.destroy() - except InvalidHandle: - return False - self._wake_executor() + service.destroy() return True return False @@ -2009,12 +2015,7 @@ def destroy_timer(self, timer: Timer) -> bool: :return: ``True`` if successful, ``False`` otherwise. """ if timer in self._timers: - self._timers.remove(timer) - try: - timer.destroy() - except InvalidHandle: - return False - self._wake_executor() + timer.destroy() return True return False @@ -2078,7 +2079,6 @@ def destroy_node(self) -> None: self.destroy_timer(self._timers[0]) while self._guards: self.destroy_guard_condition(self._guards[0]) - self._type_description_service.destroy() self.__node.destroy_when_not_in_use() self._wake_executor() diff --git a/rclpy/rclpy/publisher.py b/rclpy/rclpy/publisher.py index dd1def580..201959b48 100644 --- a/rclpy/rclpy/publisher.py +++ b/rclpy/rclpy/publisher.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from collections.abc import Callable from types import TracebackType from typing import Generic, List, Optional, Type, TypeVar, Union @@ -37,11 +38,15 @@ def __init__( msg_type: Type[MsgT], topic: str, qos_profile: QoSProfile, + *, + on_destroy: Optional[Callable[['BasePublisher[MsgT]'], None]] = None, ) -> None: self.__publisher = publisher_impl self.msg_type = msg_type self.topic = topic self.qos_profile = qos_profile + self._on_destroy = on_destroy + self._destroyed = False def publish(self, msg: Union[MsgT, bytes]) -> None: """ @@ -80,7 +85,17 @@ def logger_name(self) -> str: return self.__publisher.get_logger_name() def destroy(self) -> None: - self.__publisher.destroy_when_not_in_use() + """Destroy the publisher, notifying the owning node and releasing the handle.""" + if self._destroyed: + return + self._destroyed = True + if self._on_destroy is not None: + self._on_destroy(self) + self._on_destroy = None + self._destroy() + + def _destroy(self) -> None: + self.handle.destroy_when_not_in_use() def assert_liveliness(self) -> None: """ @@ -113,6 +128,7 @@ def __init__( topic: str, qos_profile: QoSProfile, *, + on_destroy: Optional[Callable[['Publisher[MsgT]'], None]] = None, event_callbacks: PublisherEventCallbacks, callback_group: CallbackGroup, ) -> None: @@ -134,8 +150,9 @@ def __init__( publisher_impl=publisher_impl, msg_type=msg_type, topic=topic, - qos_profile=qos_profile + qos_profile=qos_profile, ) + self._on_destroy = on_destroy self.event_handlers: List[EventHandler] = event_callbacks.create_event_handlers( callback_group, publisher_impl, topic) @@ -160,13 +177,7 @@ def wait_for_all_acked(self, timeout: Duration = Duration(seconds=-1)) -> bool: with self.handle: return self.handle.wait_for_all_acked(timeout._duration_handle) - def destroy(self) -> None: - """ - Destroy a container for a ROS publisher. - - .. warning:: Users should not destroy a publisher with this method, instead they should - call :meth:`.Node.destroy_publisher`. - """ + def _destroy(self) -> None: for handler in self.event_handlers: handler.destroy() - super().destroy() + super()._destroy() diff --git a/rclpy/rclpy/service.py b/rclpy/rclpy/service.py index f6d53e890..26903dedb 100644 --- a/rclpy/rclpy/service.py +++ b/rclpy/rclpy/service.py @@ -53,13 +53,17 @@ def __init__( srv_type: type[Srv[SrvRequestT, SrvResponseT]], srv_name: str, callback: ServiceCallbackUnion[SrvRequestT, SrvResponseT], - qos_profile: QoSProfile + qos_profile: QoSProfile, + *, + on_destroy: Optional[Callable[['BaseService[SrvRequestT, SrvResponseT]'], None]] = None, ) -> None: self.__service = service_impl self.srv_type = srv_type self.srv_name = srv_name self.callback = callback self.qos_profile = qos_profile + self._on_destroy = on_destroy + self._destroyed = False def _send_response( self, @@ -109,13 +113,17 @@ def logger_name(self) -> str: return self.__service.get_logger_name() def destroy(self) -> None: - """ - Destroy a container for a ROS service server. - - .. warning:: Users should not destroy a service server with this destructor, instead they - should call :meth:`.Node.destroy_service`. - """ - self.__service.destroy_when_not_in_use() + """Destroy the service, notifying the owning node and releasing the handle.""" + if self._destroyed: + return + self._destroyed = True + if self._on_destroy is not None: + self._on_destroy(self) + self._on_destroy = None + self._destroy() + + def _destroy(self) -> None: + self.handle.destroy_when_not_in_use() def __enter__(self) -> Self: return self @@ -138,6 +146,7 @@ def __init__( callback: ServiceCallbackUnion[SrvRequestT, SrvResponseT], qos_profile: QoSProfile, *, + on_destroy: Optional[Callable[['Service[SrvRequestT, SrvResponseT]'], None]] = None, callback_group: CallbackGroup ) -> None: """ @@ -160,7 +169,8 @@ def __init__( srv_type=srv_type, srv_name=srv_name, callback=callback, - qos_profile=qos_profile + qos_profile=qos_profile, + on_destroy=on_destroy ) self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index 3cb9602d8..53b49914d 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -84,6 +84,8 @@ def __init__( callback: GenericSubscriptionCallbackUnion[bytes], qos_profile: QoSProfile, raw: Literal[True], + *, + on_destroy: Optional[Callable[['BaseSubscription[MsgT]'], None]] = None, ) -> None: ... @overload @@ -95,6 +97,8 @@ def __init__( callback: GenericSubscriptionCallbackUnion[MsgT], qos_profile: QoSProfile, raw: Literal[False], + *, + on_destroy: Optional[Callable[['BaseSubscription[MsgT]'], None]] = None, ) -> None: ... @overload @@ -106,6 +110,8 @@ def __init__( callback: SubscriptionCallbackUnion[MsgT], qos_profile: QoSProfile, raw: bool, + *, + on_destroy: Optional[Callable[['BaseSubscription[MsgT]'], None]] = None, ) -> None: ... def __init__( @@ -116,6 +122,8 @@ def __init__( callback: SubscriptionCallbackUnion[MsgT], qos_profile: QoSProfile, raw: bool, + *, + on_destroy: Optional[Callable[['BaseSubscription[MsgT]'], None]] = None, ) -> None: self.__subscription = subscription_impl self.msg_type = msg_type @@ -123,6 +131,8 @@ def __init__( self.callback = callback self.qos_profile = qos_profile self.raw = raw + self._on_destroy = on_destroy + self._destroyed = False def get_publisher_count(self) -> int: """Get the number of publishers that this subscription has.""" @@ -134,6 +144,16 @@ def handle(self) -> '_rclpy.Subscription[MsgT]': return self.__subscription def destroy(self) -> None: + """Destroy the subscription, notifying the owning node and releasing the handle.""" + if self._destroyed: + return + self._destroyed = True + if self._on_destroy is not None: + self._on_destroy(self) + self._on_destroy = None + self._destroy() + + def _destroy(self) -> None: self.handle.destroy_when_not_in_use() @property @@ -231,6 +251,7 @@ def __init__( qos_profile: QoSProfile, raw: Literal[True], *, + on_destroy: Optional[Callable[['Subscription[MsgT]'], None]] = None, callback_group: CallbackGroup, event_callbacks: SubscriptionEventCallbacks, ) -> None: ... @@ -245,6 +266,7 @@ def __init__( qos_profile: QoSProfile, raw: Literal[False], *, + on_destroy: Optional[Callable[['Subscription[MsgT]'], None]] = None, callback_group: CallbackGroup, event_callbacks: SubscriptionEventCallbacks, ) -> None: ... @@ -259,6 +281,7 @@ def __init__( qos_profile: QoSProfile, raw: bool, *, + on_destroy: Optional[Callable[['Subscription[MsgT]'], None]] = None, callback_group: CallbackGroup, event_callbacks: SubscriptionEventCallbacks, ) -> None: ... @@ -272,6 +295,7 @@ def __init__( qos_profile: QoSProfile, raw: bool, *, + on_destroy: Optional[Callable[['Subscription[MsgT]'], None]] = None, callback_group: CallbackGroup, event_callbacks: SubscriptionEventCallbacks, ) -> None: @@ -301,6 +325,7 @@ def __init__( qos_profile=qos_profile, raw=raw ) + self._on_destroy = on_destroy self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor self._executor_event = False @@ -308,13 +333,7 @@ def __init__( self.event_handlers = event_callbacks.create_event_handlers( callback_group, subscription_impl, topic) - def destroy(self) -> None: - """ - Destroy a container for a ROS subscription. - - .. warning:: Users should not destroy a subscription with this method, instead they - should call :meth:`.Node.destroy_subscription`. - """ + def _destroy(self) -> None: for handler in self.event_handlers: handler.destroy() - super().destroy() + super()._destroy() diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index 23296e08c..9a418c769 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -81,6 +81,7 @@ def __init__( timer_period_ns: int, clock: Clock, *, + on_destroy: Optional[Callable[['BaseTimer'], None]] = None, context: Optional[Context] = None, autostart: bool = True ) -> None: @@ -92,18 +93,24 @@ def __init__( self.__timer = _rclpy.Timer( self._clock.handle, self._context.handle, timer_period_ns, autostart) self.callback = callback + self._on_destroy = on_destroy + self._destroyed = False @property def handle(self) -> _rclpy.Timer: return self.__timer def destroy(self) -> None: - """ - Destroy a container for a ROS timer. - - .. warning:: Users should not destroy a timer with this method, instead they should - call :meth:`.Node.destroy_timer`. - """ + """Destroy the timer, notifying the owning node and releasing the handle.""" + if self._destroyed: + return + self._destroyed = True + if self._on_destroy is not None: + self._on_destroy(self) + self._on_destroy = None + self._destroy() + + def _destroy(self) -> None: self.__timer.destroy_when_not_in_use() @property @@ -166,6 +173,7 @@ def __init__( timer_period_ns: int, clock: Clock, *, + on_destroy: Optional[Callable[['BaseTimer'], None]] = None, context: Optional[Context] = None, autostart: bool = True, callback_group: Optional[CallbackGroup] = None @@ -195,7 +203,8 @@ def __init__( timer_period_ns=timer_period_ns, clock=clock, context=context, - autostart=autostart + autostart=autostart, + on_destroy=on_destroy ) self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor diff --git a/rclpy/rclpy/type_description_service.py b/rclpy/rclpy/type_description_service.py index 3f896e095..4caf1f661 100644 --- a/rclpy/rclpy/type_description_service.py +++ b/rclpy/rclpy/type_description_service.py @@ -77,12 +77,6 @@ def __init__(self, node: 'Node'): if self.enabled: self._start_service() - def destroy(self) -> None: - # Required manual destruction because this is not managed by rclpy.Service - if self._type_description_srv is not None: - self._type_description_srv.destroy_when_not_in_use() - self._type_description_srv = None - def _start_service(self) -> None: node = self._get_node() self._type_description_srv = _rclpy.TypeDescriptionService(node.handle) @@ -94,6 +88,7 @@ def _start_service(self) -> None: srv_type=GetTypeDescription, srv_name=self.service_name, callback=self._service_callback, + on_destroy=node._on_destroy_service, callback_group=node.default_callback_group, qos_profile=qos_profile_services_default) node.default_callback_group.add_entity(service) diff --git a/rclpy/test/test_client.py b/rclpy/test/test_client.py index 43e5c1199..49fbcaf38 100644 --- a/rclpy/test/test_client.py +++ b/rclpy/test/test_client.py @@ -309,6 +309,15 @@ def _service(request: Empty.Request, response: Empty.Response) -> Empty.Response finally: executor.shutdown() + def test_client_direct_destroy(self) -> None: + cli = self.node.create_client(Empty, '/test_direct_destroy') + + self.assertIn(cli, list(self.node.clients)) + cli.destroy() + self.assertNotIn(cli, list(self.node.clients)) + cli.destroy() + self.assertFalse(self.node.destroy_client(cli)) + if __name__ == '__main__': unittest.main() diff --git a/rclpy/test/test_publisher.py b/rclpy/test/test_publisher.py index 31e780938..4389b6ccf 100644 --- a/rclpy/test/test_publisher.py +++ b/rclpy/test/test_publisher.py @@ -138,6 +138,15 @@ def test_logger_name_is_equal_to_node_name(self) -> None: with self.node.create_publisher(BasicTypes, TEST_TOPIC, 10) as pub: self.assertEqual(pub.logger_name, 'node') + def test_direct_destroy(self) -> None: + pub = self.node.create_publisher(BasicTypes, 'topic', 1) + + self.assertIn(pub, list(self.node.publishers)) + pub.destroy() + self.assertNotIn(pub, list(self.node.publishers)) + pub.destroy() + self.assertFalse(self.node.destroy_publisher(pub)) + def test_publisher_context_manager() -> None: rclpy.init() diff --git a/rclpy/test/test_service.py b/rclpy/test/test_service.py index 5c31b35c9..3b71195c2 100644 --- a/rclpy/test/test_service.py +++ b/rclpy/test/test_service.py @@ -115,6 +115,19 @@ def test_service_context_manager() -> None: assert srv.service_name == '/empty_service' +def test_service_direct_destroy(test_node: Node) -> None: + srv = test_node.create_service( + srv_type=Empty, + srv_name='test_direct_destroy_srv', + callback=lambda _req, res: res) + + assert srv in list(test_node.services) + srv.destroy() + assert srv not in list(test_node.services) + srv.destroy() + assert not test_node.destroy_service(srv) + + def test_set_on_new_request_callback(test_node: Node) -> None: cli = test_node.create_client(Empty, '/service') srv = test_node.create_service(Empty, '/service', lambda req, res: res) diff --git a/rclpy/test/test_subscription.py b/rclpy/test/test_subscription.py index 8eb8b2aaa..eb6b7cdaf 100644 --- a/rclpy/test/test_subscription.py +++ b/rclpy/test/test_subscription.py @@ -486,6 +486,20 @@ def publish_messages(pub: Publisher[BasicTypes]) -> None: assert len(received_msgs) == expected_msg_count +def test_subscription_direct_destroy(test_node: Node) -> None: + sub = test_node.create_subscription( + msg_type=Empty, + topic='test_direct_destroy_topic', + callback=lambda msg: None, + qos_profile=10) + + assert sub in list(test_node.subscriptions) + sub.destroy() + assert sub not in list(test_node.subscriptions) + sub.destroy() + assert not test_node.destroy_subscription(sub) + + def test_subscription_content_filter_at_create_subscription(test_node: Node) -> None: topic_name = '/topic' diff --git a/rclpy/test/test_timer.py b/rclpy/test/test_timer.py index 26a55d1f7..3afd5924d 100644 --- a/rclpy/test/test_timer.py +++ b/rclpy/test/test_timer.py @@ -362,3 +362,13 @@ def test_on_reset_callback(test_node: Node) -> None: tmr.handle.clear_on_reset_callback() tmr.reset() cb.assert_called_once() + + +def test_timer_direct_destroy(test_node: Node) -> None: + tmr = test_node.create_timer(1, lambda: None) + + assert tmr in list(test_node.timers) + tmr.destroy() + assert tmr not in list(test_node.timers) + tmr.destroy() + assert not test_node.destroy_timer(tmr)