From 71751379c534e6427e2ea1d924e975151219ea37 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 3 Dec 2019 12:51:31 -0300 Subject: [PATCH 1/2] Add node handle dependeny on context handle Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/__init__.py | 3 +- rclpy/rclpy/context.py | 17 ++++++----- rclpy/rclpy/executors.py | 3 +- rclpy/rclpy/guard_condition.py | 4 ++- rclpy/rclpy/node.py | 42 +++++++++++++------------- rclpy/rclpy/timer.py | 5 ++-- rclpy/test/test_qos_event.py | 54 ++++++++++++++++++++++------------ rclpy/test/test_waitable.py | 29 +++++++++++------- 8 files changed, 96 insertions(+), 61 deletions(-) diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 83b2e14d3..965496063 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -70,7 +70,8 @@ def init(*, args: List[str] = None, context: Context = None) -> None: context = get_default_context() if context is None else context # imported locally to avoid loading extensions on module import from rclpy.impl.implementation_singleton import rclpy_implementation - return rclpy_implementation.rclpy_init(args if args is not None else sys.argv, context.handle) + with context.handle as capsule: + return rclpy_implementation.rclpy_init(args if args is not None else sys.argv, capsule) # The global spin functions need an executor to do the work diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index b18542732..76a2fcc0c 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -28,7 +28,8 @@ class Context: def __init__(self): from rclpy.impl.implementation_singleton import rclpy_implementation - self._handle = rclpy_implementation.rclpy_create_context() + from .handle import Handle + self._handle = Handle(rclpy_implementation.rclpy_create_context()) self._lock = threading.Lock() self._callbacks = [] self._callbacks_lock = threading.Lock() @@ -41,8 +42,8 @@ def ok(self): """Check if context hasn't been shut down.""" # imported locally to avoid loading extensions on module import from rclpy.impl.implementation_singleton import rclpy_implementation - with self._lock: - return rclpy_implementation.rclpy_ok(self._handle) + with self._handle as capsule, self._lock: + return rclpy_implementation.rclpy_ok(capsule) def _call_on_shutdown_callbacks(self): with self._callbacks_lock: @@ -55,17 +56,17 @@ def shutdown(self): """Shutdown this context.""" # imported locally to avoid loading extensions on module import from rclpy.impl.implementation_singleton import rclpy_implementation - with self._lock: - rclpy_implementation.rclpy_shutdown(self._handle) + with self._handle as capsule, self._lock: + rclpy_implementation.rclpy_shutdown(capsule) self._call_on_shutdown_callbacks() def try_shutdown(self): """Shutdown this context, if not already shutdown.""" # imported locally to avoid loading extensions on module import from rclpy.impl.implementation_singleton import rclpy_implementation - with self._lock: - if rclpy_implementation.rclpy_ok(self._handle): - rclpy_implementation.rclpy_shutdown(self._handle) + with self._handle as capsule, self._lock: + if rclpy_implementation.rclpy_ok(capsule): + rclpy_implementation.rclpy_shutdown(capsule) self._call_on_shutdown_callbacks() def _remove_callback(self, weak_method): diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index e61986e50..d9c482244 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -532,6 +532,7 @@ def _wait_for_ready_callbacks( except InvalidHandle: entity_count.num_guard_conditions -= 1 + context_capsule = context_stack.enter_context(self._context.handle) _rclpy.rclpy_wait_set_init( wait_set, entity_count.num_subscriptions, @@ -540,7 +541,7 @@ def _wait_for_ready_callbacks( entity_count.num_clients, entity_count.num_services, entity_count.num_events, - self._context.handle) + context_capsule) _rclpy.rclpy_wait_set_clear_entities(wait_set) for sub_capsule in sub_capsules: diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py index d7b0d5487..4e31ce62e 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -21,7 +21,9 @@ class GuardCondition: def __init__(self, callback, callback_group, context=None): self._context = get_default_context() if context is None else context - self.__handle = Handle(_rclpy.rclpy_create_guard_condition(self._context.handle)) + with self._context.handle as capsule: + self.__handle = Handle(_rclpy.rclpy_create_guard_condition(capsule)) + self.__handle.requires(self._context.handle) self.callback = callback 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/node.py b/rclpy/rclpy/node.py index 07914e3f5..88c19044b 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -157,26 +157,28 @@ def __init__( namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') - try: - self.__handle = Handle(_rclpy.rclpy_create_node( - node_name, - namespace, - self._context.handle, - cli_args, - use_global_arguments, - enable_rosout - )) - except ValueError: - # these will raise more specific errors if the name or namespace is bad - validate_node_name(node_name) - # emulate what rcl_node_init() does to accept '' and relative namespaces - if not namespace: - namespace = '/' - if not namespace.startswith('/'): - namespace = '/' + namespace - validate_namespace(namespace) - # Should not get to this point - raise RuntimeError('rclpy_create_node failed for unknown reason') + with self._context.handle as capsule: + try: + self.__handle = Handle(_rclpy.rclpy_create_node( + node_name, + namespace, + capsule, + cli_args, + use_global_arguments, + enable_rosout + )) + except ValueError: + # these will raise more specific errors if the name or namespace is bad + validate_node_name(node_name) + # emulate what rcl_node_init() does to accept '' and relative namespaces + if not namespace: + namespace = '/' + if not namespace.startswith('/'): + namespace = '/' + namespace + validate_namespace(namespace) + # Should not get to this point + raise RuntimeError('rclpy_create_node failed for unknown reason') + self.__handle.requires(self._context.handle) with self.handle as capsule: self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(capsule)) diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index d00bd5c5e..27089dc7d 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -26,10 +26,11 @@ class Timer: def __init__(self, callback, callback_group, timer_period_ns, clock, *, context=None): self._context = get_default_context() if context is None else context self._clock = clock - with self._clock.handle as clock_capsule: + with self._clock.handle as clock_capsule, self._context.handle as context_capsule: self.__handle = Handle(_rclpy.rclpy_create_timer( - clock_capsule, self._context.handle, timer_period_ns)) + clock_capsule, context_capsule, timer_period_ns)) self.__handle.requires(self._clock.handle) + self.__handle.requires(self._context.handle) self.timer_period_ns = timer_period_ns self.callback = callback self.callback_group = callback_group diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py index 88ad7c1be..ccd9430ce 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -135,26 +135,35 @@ def test_call_publisher_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events publisher = self.node.create_publisher(EmptyMsg, 'test_topic', 10) - wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() - _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 2, self.context.handle) + wait_set = Handle(_rclpy.rclpy_get_zero_initialized_wait_set()) + wait_set.requires(self.context.handle) + with self.context.handle as context_capsule, wait_set as wait_set_capsule: + _rclpy.rclpy_wait_set_init(wait_set_capsule, 0, 0, 0, 0, 0, 2, context_capsule) deadline_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) - with deadline_event_handle as capsule: - deadline_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) + wait_set.requires(deadline_event_handle) + with deadline_event_handle as event_capsule, wait_set as wait_set_capsule: + deadline_event_index = _rclpy.rclpy_wait_set_add_entity( + 'event', wait_set_capsule, event_capsule) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST) - with liveliness_event_handle as capsule: - liveliness_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) + wait_set.requires(liveliness_event_handle) + with liveliness_event_handle as event_capsule, wait_set as wait_set_capsule: + liveliness_event_index = _rclpy.rclpy_wait_set_add_entity( + 'event', wait_set_capsule, event_capsule) self.assertIsNotNone(liveliness_event_index) # We live in our own namespace and have created no other participants, so # there can't be any of these events. - _rclpy.rclpy_wait(wait_set, 0) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) + with wait_set as wait_set_capsule: + _rclpy.rclpy_wait(wait_set_capsule, 0) + self.assertFalse( + _rclpy.rclpy_wait_set_is_ready('event', wait_set_capsule, deadline_event_index)) + self.assertFalse( + _rclpy.rclpy_wait_set_is_ready('event', wait_set_capsule, liveliness_event_index)) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side @@ -188,26 +197,35 @@ def test_call_subscription_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events subscription = self.node.create_subscription(EmptyMsg, 'test_topic', Mock(), 10) - wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() - _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 2, self.context.handle) + wait_set = Handle(_rclpy.rclpy_get_zero_initialized_wait_set()) + wait_set.requires(self.context.handle) + with self.context.handle as context_capsule, wait_set as wait_set_capsule: + _rclpy.rclpy_wait_set_init(wait_set_capsule, 0, 0, 0, 0, 0, 2, context_capsule) deadline_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) - with deadline_event_handle as capsule: - deadline_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) + wait_set.requires(deadline_event_handle) + with deadline_event_handle as event_capsule, wait_set as wait_set_capsule: + deadline_event_index = _rclpy.rclpy_wait_set_add_entity( + 'event', wait_set_capsule, event_capsule) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED) - with liveliness_event_handle as capsule: - liveliness_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) + wait_set.requires(liveliness_event_handle) + with liveliness_event_handle as event_capsule, wait_set as wait_set_capsule: + liveliness_event_index = _rclpy.rclpy_wait_set_add_entity( + 'event', wait_set_capsule, event_capsule) self.assertIsNotNone(liveliness_event_index) # We live in our own namespace and have created no other participants, so # there can't be any of these events. - _rclpy.rclpy_wait(wait_set, 0) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) + with wait_set as wait_set_capsule: + _rclpy.rclpy_wait(wait_set_capsule, 0) + self.assertFalse(_rclpy.rclpy_wait_set_is_ready( + 'event', wait_set_capsule, deadline_event_index)) + self.assertFalse(_rclpy.rclpy_wait_set_is_ready( + 'event', wait_set_capsule, liveliness_event_index)) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 0afbc8f05..74affb595 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -21,6 +21,7 @@ from rclpy.clock import Clock from rclpy.clock import ClockType from rclpy.executors import SingleThreadedExecutor +from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import check_for_type_support from rclpy.qos import QoSProfile @@ -129,9 +130,12 @@ def __init__(self, node): self._clock = Clock(clock_type=ClockType.STEADY_TIME) period_nanoseconds = 10000 - with self._clock.handle as clock_capsule: - self.timer = _rclpy.rclpy_create_timer( - clock_capsule, node.context.handle, period_nanoseconds) + with self._clock.handle as clock_capsule, node.context.handle as context_handle: + self.timer = Handle(_rclpy.rclpy_create_timer( + clock_capsule, context_handle, period_nanoseconds)) + self.timer.requires(self._clock.handle) + self.timer.requires(node.context.handle) + self.timer_index = None self.timer_is_ready = False @@ -148,7 +152,8 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" if self.timer_is_ready: self.timer_is_ready = False - _rclpy.rclpy_call_timer(self.timer) + with self.timer as capsule: + _rclpy.rclpy_call_timer(capsule) return 'timer' return None @@ -165,7 +170,8 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.timer_index = _rclpy.rclpy_wait_set_add_entity('timer', wait_set, self.timer) + with self.timer as capsule: + self.timer_index = _rclpy.rclpy_wait_set_add_entity('timer', wait_set, capsule) class SubscriptionWaitable(Waitable): @@ -216,8 +222,9 @@ class GuardConditionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - - self.guard_condition = _rclpy.rclpy_create_guard_condition(node.context.handle) + with node.context.handle as capsule: + self.guard_condition = Handle(_rclpy.rclpy_create_guard_condition(capsule)) + self.guard_condition.requires(node.context.handle) self.guard_condition_index = None self.guard_is_ready = False @@ -250,8 +257,9 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.guard_condition_index = _rclpy.rclpy_wait_set_add_entity( - 'guard_condition', wait_set, self.guard_condition) + with self.guard_condition as guard_condition_capsule: + self.guard_condition_index = _rclpy.rclpy_wait_set_add_entity( + 'guard_condition', wait_set, guard_condition_capsule) class MutuallyExclusiveWaitable(Waitable): @@ -366,7 +374,8 @@ def test_waitable_with_guard_condition(self): self.node.add_waitable(self.waitable) thr = self.start_spin_thread(self.waitable) - _rclpy.rclpy_trigger_guard_condition(self.waitable.guard_condition) + with self.waitable.guard_condition as capsule: + _rclpy.rclpy_trigger_guard_condition(capsule) thr.join() assert self.waitable.future.done() From 670673cb21beffd72c3cb778468f60e827c872a8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 3 Dec 2019 12:51:57 -0300 Subject: [PATCH 2/2] Debugging messages Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/handle.py | 3 +++ rclpy/src/rclpy/_rclpy.c | 6 ++++++ 2 files changed, 9 insertions(+) diff --git a/rclpy/rclpy/handle.py b/rclpy/rclpy/handle.py index d82216a25..b0720d365 100644 --- a/rclpy/rclpy/handle.py +++ b/rclpy/rclpy/handle.py @@ -198,3 +198,6 @@ def __destroy_self(self): self.__destroy_callbacks.pop()(self) # get rid of references to other handles to break reference cycles del self.__required_handles + + def __del__(self): + print('destroying', self.__capsule_name, ' ', self.__capsule_pointer) diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 835085281..d382162c4 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -48,6 +48,7 @@ static PyObject * RCLError; void _rclpy_context_capsule_destructor(PyObject * capsule) { + fprintf(stderr, "context being destructed\n"); rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(capsule, "rcl_context_t"); if (!context) { return; @@ -76,6 +77,7 @@ _rclpy_context_capsule_destructor(PyObject * capsule) rcl_reset_error(); } } + fprintf(stderr, "context destructed\n"); PyMem_FREE(context); } @@ -614,6 +616,7 @@ rclpy_init(PyObject * Py_UNUSED(self), PyObject * args) static void _rclpy_destroy_node(PyObject * pyentity) { + fprintf(stderr, "destroying node\n"); rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer( pyentity, "rcl_node_t"); if (!node) { @@ -635,6 +638,7 @@ _rclpy_destroy_node(PyObject * pyentity) rcl_get_error_string().str); } PyMem_Free(node); + fprintf(stderr, "destroyed node\n"); } /// Create a node @@ -2176,6 +2180,7 @@ rclpy_send_request(PyObject * Py_UNUSED(self), PyObject * args) static void _rclpy_destroy_service(PyObject * pyentity) { + fprintf(stderr, "destroying service\n"); rclpy_service_t * srv = (rclpy_service_t *)PyCapsule_GetPointer( pyentity, "rclpy_service_t"); if (!srv) { @@ -2197,6 +2202,7 @@ _rclpy_destroy_service(PyObject * pyentity) rcl_get_error_string().str); } PyMem_Free(srv); + fprintf(stderr, "destroyed service\n"); } /// Create a service server