diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 2d2157190..3fc6eb655 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -1,7 +1,11 @@ cmake_minimum_required(VERSION 3.5) -project(rclpy C) +project(rclpy) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() # Default to C11 if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 11) @@ -71,8 +75,9 @@ function(configure_python_c_extension_library _library_name) ) endfunction() -add_library(rclpy_common - SHARED src/rclpy_common/src/common.c +add_library(rclpy_common SHARED + src/rclpy_common/src/common.c + src/rclpy_common/src/handle.c ) target_link_libraries(rclpy_common ${PythonExtra_LIBRARIES} @@ -142,6 +147,9 @@ add_library( rclpy_signal_handler SHARED src/rclpy/_rclpy_signal_handler.c ) +target_link_libraries(rclpy_signal_handler + rclpy_common +) configure_python_c_extension_library(rclpy_signal_handler) ament_target_dependencies(rclpy_signal_handler "rcl" @@ -155,6 +163,19 @@ add_library( ) configure_python_c_extension_library(rclpy_pycapsule) +# Handle library, used to keep dependencies between C objects. +add_library( + rclpy_handle + SHARED src/rclpy/_rclpy_handle.c +) +target_link_libraries(rclpy_handle + rclpy_common +) +configure_python_c_extension_library(rclpy_handle) +ament_target_dependencies(rclpy_handle + "rcutils" +) + if(NOT WIN32) ament_environment_hooks( "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}" @@ -167,6 +188,7 @@ if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) find_package(rosidl_generator_py REQUIRED) + find_package(ament_cmake_gtest REQUIRED) rosidl_generator_py_get_typesupports(_typesupport_impls) ament_index_get_prefix_path(ament_index_build_path SKIP_AMENT_PREFIX_PATH) @@ -176,58 +198,63 @@ if(BUILD_TESTING) # On Windows prevent CMake errors and prevent it being evaluated as a list. string(REPLACE "\\" "/" ament_index_build_path "${ament_index_build_path}") endif() - if(NOT _typesupport_impls STREQUAL "") - # Run each test in its own pytest invocation to isolate any global state in rclpy - set(_rclpy_pytest_tests - test/test_action_client.py - test/test_action_graph.py - test/test_action_server.py - test/test_callback_group.py - test/test_client.py - test/test_clock.py - test/test_create_node.py - test/test_create_while_spinning.py - test/test_destruction.py - test/test_executor.py - test/test_expand_topic_name.py - test/test_guard_condition.py - test/test_handle.py - test/test_init_shutdown.py - test/test_logging.py - test/test_logging_rosout.py - test/test_messages.py - test/test_node.py - test/test_parameter.py - test/test_parameters_callback.py - test/test_qos.py - test/test_qos_event.py - test/test_rate.py - test/test_serialization.py - test/test_task.py - test/test_time_source.py - test/test_time.py - test/test_timer.py - test/test_topic_or_service_is_hidden.py - test/test_topic_endpoint_info.py - test/test_utilities.py - test/test_validate_full_topic_name.py - test/test_validate_namespace.py - test/test_validate_node_name.py - test/test_validate_topic_name.py - test/test_waitable.py - ) + ament_add_gtest(test_c_handle + src/test/rclpy_common/test_handle.cpp) + target_link_libraries(test_c_handle + rclpy_common) - foreach(_test_path ${_rclpy_pytest_tests}) - get_filename_component(_test_name ${_test_path} NAME_WE) - ament_add_pytest_test(${_test_name} ${_test_path} - PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" - APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path} - PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} - TIMEOUT 120 - WERROR ON + if(NOT _typesupport_impls STREQUAL "") + # Run each test in its own pytest invocation to isolate any global state in rclpy + set(_rclpy_pytest_tests + test/test_action_client.py + test/test_action_graph.py + test/test_action_server.py + test/test_callback_group.py + test/test_client.py + test/test_clock.py + test/test_create_node.py + test/test_create_while_spinning.py + test/test_destruction.py + test/test_executor.py + test/test_expand_topic_name.py + test/test_guard_condition.py + test/test_handle.py + test/test_init_shutdown.py + test/test_logging.py + test/test_logging_rosout.py + test/test_messages.py + test/test_node.py + test/test_parameter.py + test/test_parameters_callback.py + test/test_qos.py + test/test_qos_event.py + test/test_rate.py + test/test_serialization.py + test/test_task.py + test/test_time_source.py + test/test_time.py + test/test_timer.py + test/test_topic_or_service_is_hidden.py + test/test_topic_endpoint_info.py + test/test_utilities.py + test/test_validate_full_topic_name.py + test/test_validate_namespace.py + test/test_validate_node_name.py + test/test_validate_topic_name.py + test/test_waitable.py ) - endforeach() + + foreach(_test_path ${_rclpy_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" + APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path} + PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 120 + WERROR ON + ) + endforeach() endif() endif() set(PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") diff --git a/rclpy/package.xml b/rclpy/package.xml index 93373703a..f303ea61a 100644 --- a/rclpy/package.xml +++ b/rclpy/package.xml @@ -28,6 +28,7 @@ rcl_interfaces rosgraph_msgs + ament_cmake_gtest ament_cmake_pytest ament_lint_auto ament_lint_common diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index 22defea3b..c5c056e64 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -31,7 +31,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() @@ -48,16 +49,15 @@ def init(self, args: Optional[List[str]] = None): """ # imported locally to avoid loading extensions on module import from rclpy.impl.implementation_singleton import rclpy_implementation - with self._lock: - rclpy_implementation.rclpy_init( - args if args is not None else sys.argv, self._handle) + with self._handle as capsule, self._lock: + rclpy_implementation.rclpy_init(args if args is not None else sys.argv, capsule) 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: @@ -70,17 +70,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..319b55fb8 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -21,7 +21,8 @@ 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.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/handle.py b/rclpy/rclpy/handle.py index d82216a25..e99991764 100644 --- a/rclpy/rclpy/handle.py +++ b/rclpy/rclpy/handle.py @@ -1,4 +1,4 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. +# Copyright 2019-2020 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from threading import RLock -import weakref +from threading import Lock +from rclpy.impl.implementation_singleton import rclpy_handle_implementation as _rclpy_handle from rclpy.impl.implementation_singleton import rclpy_pycapsule_implementation as _rclpy_capsule @@ -24,18 +24,17 @@ class InvalidHandle(Exception): class Handle: """ - Wrap a pycapsule object for thread-safe early destruction. + Wraps a `rclpy_handle_t` pycapsule object for thread-safe early destruction. This is intended to be used as a context manager, meaning using the ``with`` keyword. - :: with subscription.handle as pycapsule: ... - This class assumes the passed pycapsule has a destructor. - When this class destroys the capsule, it will call the destructor. - Then it will set the destructor to NULL so it is not called a second time when the capsule is - garbage collected. + :meth:`destroy` allows early destruction of the pycapsule. + + A managed `rclpy_handle_t` blocks destruction as long as another `rclpy_handler_t` needs it + to exist. If :meth:`destroy` is never called then the pycapsule will be destructed when it is garbage collected. """ @@ -45,15 +44,10 @@ def __init__(self, pycapsule): self.__use_count = 0 self.__request_invalidation = False self.__valid = True - self.__rlock = RLock() - # Create this early because RLock() can raise during interpeter shutdown - self.__dependents_rlock = RLock() - self.__required_handles = [] - self.__dependent_handles = weakref.WeakSet() - self.__destroy_callbacks = [] + self.__lock = Lock() # Called to give an opportunity to raise an exception if the object is not a pycapsule. - self.__capsule_name = _rclpy_capsule.rclpy_pycapsule_name(pycapsule) - self.__capsule_pointer = _rclpy_capsule.rclpy_pycapsule_pointer(pycapsule) + self.__capsule_pointer = _rclpy_handle.rclpy_handle_get_pointer(pycapsule) + self.__handle_name = _rclpy_handle.rclpy_handle_get_name(pycapsule) def __bool__(self): """Return True if the handle is valid.""" @@ -68,58 +62,38 @@ def __hash__(self): @property def name(self): """ - Get the name of the managed pycapsule. + Get the name of the managed handle. - rclpy uses the name of the C type the pycapsule holds a pointer to. - - :return: name of the pycapsule + :return: name of the handle """ - return self.__capsule_name + return self.__handle_name @property def pointer(self): """ Get the address held by the managed pycapsule. + This is the address of the `rcl` object, not of the `rclpy_handle_t`. :return: address of the pycapsule """ return self.__capsule_pointer def destroy(self, then=None): """ - Destroy pycapsule as soon as possible without waiting for garbage collection. + Destroy the pycapsule as soon as possible without waiting for garbage collection. - :param then: callback to call after handle has been destroyed. + The managed `rclpy_handle_t` object will not be destructed immediately if another handle + called `other.requires(this)`. + In that case, the managed object will be destroyed after all its + dependents are destroyed. """ - with self.__rlock: + with self.__lock: if not self.__valid: raise InvalidHandle('Asked to destroy handle, but it was already destroyed') - if then: - self.__destroy_callbacks.append(then) self.__request_invalidation = True if 0 == self.__use_count: self.__destroy() - def requires(self, req_handle): - """ - Indicate that this handle requires another handle to live longer than itself. - - Calling :meth:`destroy` on the passed in handle will cause this handle to be - destroyed first. - This handle will hold a reference to the passed in handle so the required handle is - garbage collected after this handle in case :meth:`destroy` is not called. - """ - assert isinstance(req_handle, Handle) - with self.__rlock, req_handle.__rlock: - if not self.__valid: - raise InvalidHandle('Cannot require a new handle if already destroyed') - if req_handle.__valid: - self.__required_handles.append(req_handle) - req_handle.__dependent_handles.add(self) - else: - # required handle destroyed before we could link to it, destroy self - self.destroy() - def _get_capsule(self): """ Get the pycapsule managed by this handle. @@ -127,7 +101,7 @@ def _get_capsule(self): The capsule must be returned using :meth:`_return_capsule` when it is no longer in use. :return: PyCapsule instance """ - with self.__rlock: + with self.__lock: if not self.__valid: raise InvalidHandle('Tried to use a handle that has been destroyed.') self.__use_count += 1 @@ -139,7 +113,7 @@ def _return_capsule(self): :return: None """ - with self.__rlock: + with self.__lock: # Assume _return_capsule is not called more times than _get_capsule assert self.__use_count > 0 self.__use_count -= 1 @@ -159,42 +133,6 @@ def __destroy(self): assert self.__request_invalidation # mark as invalid so no one else tries to use it self.__valid = False - self.__destroy_dependents(then=self.__destroy_self) - - def __destroy_dependents(self, then): - # assumes self.__rlock is held - deps_lock = self.__dependents_rlock - # Turn weak references to regular references - dependent_handles = list(self.__dependent_handles) - - if not dependent_handles: - # no dependents to wait on - then() - return - - def remove_dependent(handle): - nonlocal dependent_handles - nonlocal deps_lock - nonlocal then - with deps_lock: - dependent_handles.remove(handle) - if 0 == len(dependent_handles): - # all dependents destroyed, do what comes next - then() - - for dep in self.__dependent_handles: - try: - dep.destroy(then=remove_dependent) - except InvalidHandle: - # Dependent was already destroyed - remove_dependent(dep) - - def __destroy_self(self): - with self.__rlock: - # Calls pycapsule destructor - _rclpy_capsule.rclpy_pycapsule_destroy(self.__capsule) - # Call post-destroy callbacks - while self.__destroy_callbacks: - self.__destroy_callbacks.pop()(self) - # get rid of references to other handles to break reference cycles - del self.__required_handles + + # Calls pycapsule destructor + _rclpy_capsule.rclpy_pycapsule_destroy(self.__capsule) diff --git a/rclpy/rclpy/impl/implementation_singleton.py b/rclpy/rclpy/impl/implementation_singleton.py index bff893961..d85ec7b2d 100644 --- a/rclpy/rclpy/impl/implementation_singleton.py +++ b/rclpy/rclpy/impl/implementation_singleton.py @@ -32,4 +32,5 @@ rclpy_action_implementation = _import('._rclpy_action') rclpy_logging_implementation = _import('._rclpy_logging') rclpy_signal_handler_implementation = _import('._rclpy_signal_handler') +rclpy_handle_implementation = _import('._rclpy_handle') rclpy_pycapsule_implementation = _import('._rclpy_pycapsule') diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 2ca6e3ad5..d9558d9ac 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -158,26 +158,27 @@ 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') with self.handle as capsule: self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(capsule)) @@ -1140,7 +1141,6 @@ def create_publisher( self._validate_topic_or_service_name(topic) publisher_handle = Handle(publisher_capsule) - publisher_handle.requires(self.handle) publisher = Publisher( publisher_handle, msg_type, topic, qos_profile, @@ -1199,7 +1199,6 @@ def create_subscription( self._validate_topic_or_service_name(topic) subscription_handle = Handle(subscription_capsule) - subscription_handle.requires(self.handle) subscription = Subscription( subscription_handle, msg_type, @@ -1247,7 +1246,6 @@ def create_client( self._validate_topic_or_service_name(srv_name, is_service=True) client_handle = Handle(client_capsule) - client_handle.requires(self.handle) client = Client( self.context, @@ -1295,7 +1293,6 @@ def create_service( self._validate_topic_or_service_name(srv_name, is_service=True) service_handle = Handle(service_capsule) - service_handle.requires(self.handle) service = Service( service_handle, @@ -1330,7 +1327,6 @@ def create_timer( if clock is None: clock = self._clock timer = Timer(callback, callback_group, timer_period_nsec, clock, context=self.context) - timer.handle.requires(self.handle) self.__timers.append(timer) callback_group.add_entity(timer) @@ -1346,7 +1342,6 @@ def create_guard_condition( if callback_group is None: callback_group = self.default_callback_group guard = GuardCondition(callback, callback_group, context=self.context) - guard.handle.requires(self.handle) self.__guards.append(guard) callback_group.add_entity(guard) diff --git a/rclpy/rclpy/qos_event.py b/rclpy/rclpy/qos_event.py index e4af09912..e61f3ad56 100644 --- a/rclpy/rclpy/qos_event.py +++ b/rclpy/rclpy/qos_event.py @@ -115,7 +115,6 @@ def __init__( with parent_handle as parent_capsule: event_capsule = _rclpy.rclpy_create_event(event_type, parent_capsule) self._event_handle = Handle(event_capsule) - self._event_handle.requires(self._parent_handle) self._ready_to_take_data = False self._event_index = None diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index d00bd5c5e..01917126a 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -26,10 +26,9 @@ 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)) - self.__handle.requires(self._clock.handle) + clock_capsule, context_capsule, timer_period_ns)) self.timer_period_ns = timer_period_ns self.callback = callback self.callback_group = callback_group diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index b42ef4ec3..4925b34fa 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -1,4 +1,4 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. +// Copyright 2016-2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -42,6 +42,7 @@ #include #include "rclpy_common/common.h" +#include "rclpy_common/handle.h" #include "./_rclpy_qos_event.c" static PyObject * RCLInvalidROSArgsError; @@ -50,10 +51,14 @@ static PyObject * NodeNameNonExistentError; static PyObject * RCLError; void -_rclpy_context_capsule_destructor(PyObject * capsule) +_rclpy_context_handle_destructor(void * p) { - rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(capsule, "rcl_context_t"); + rcl_context_t * context = p; if (!context) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "_rclpy_context_handle_destructor failed to get pointer"); return; } if (NULL != context->impl) { @@ -100,29 +105,26 @@ _rclpy_context_capsule_destructor(PyObject * capsule) static PyObject * rclpy_create_context(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) { - rcl_context_t * context = (rcl_context_t *)PyMem_Malloc(sizeof(rcl_context_t)); + rcl_context_t * context = PyMem_Malloc(sizeof(rcl_context_t)); if (!context) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for context"); return NULL; } *context = rcl_get_zero_initialized_context(); // if it fails, error is set and NULL is returned as it should - return PyCapsule_New(context, "rcl_context_t", _rclpy_context_capsule_destructor); + return rclpy_create_handle_capsule(context, "rcl_context_t", _rclpy_context_handle_destructor); } -/// PyCapsule destructor for guard condition +/// Handle destructor for guard condition static void -_rclpy_destroy_guard_condition(PyObject * pyentity) +_rclpy_destroy_guard_condition(void * p) { - rcl_guard_condition_t * gc = (rcl_guard_condition_t *)PyCapsule_GetPointer( - pyentity, "rcl_guard_condition_t"); + rcl_guard_condition_t * gc = p; if (!gc) { - // Don't want to raise an exception, who knows where it will get raised. - PyErr_Clear(); // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_guard_condition failed to get pointer"); + PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_guard_condition got NULL pointer"); return; } @@ -156,13 +158,16 @@ rclpy_create_guard_condition(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(pycontext, "rcl_context_t"); + rclpy_handle_t * context_handle = PyCapsule_GetPointer(pycontext, "rcl_context_t"); + if (!context_handle) { + return NULL; + } + rcl_context_t * context = _rclpy_handle_get_pointer(context_handle); if (!context) { return NULL; } - rcl_guard_condition_t * gc = - (rcl_guard_condition_t *)PyMem_Malloc(sizeof(rcl_guard_condition_t)); + rcl_guard_condition_t * gc = PyMem_Malloc(sizeof(rcl_guard_condition_t)); if (!gc) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for guard condition"); return NULL; @@ -180,13 +185,22 @@ rclpy_create_guard_condition(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - PyObject * pygc = PyCapsule_New(gc, "rcl_guard_condition_t", _rclpy_destroy_guard_condition); - if (!pygc) { - ret = rcl_guard_condition_fini(gc); - PyMem_Free(gc); + rclpy_handle_t * gc_handle = _rclpy_create_handle(gc, _rclpy_destroy_guard_condition); + if (!gc_handle) { + _rclpy_destroy_guard_condition(gc); + return NULL; + } + _rclpy_handle_add_dependency(gc_handle, context_handle); + if (PyErr_Occurred()) { + _rclpy_handle_dec_ref(gc_handle); + return NULL; + } + PyObject * gc_capsule = _rclpy_create_handle_capsule(gc_handle, "rcl_guard_condition_t"); + if (!gc_capsule) { + _rclpy_handle_dec_ref(gc_handle); return NULL; } - return pygc; + return gc_capsule; } /// Trigger a general purpose guard condition @@ -205,7 +219,7 @@ rclpy_trigger_guard_condition(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_guard_condition_t * gc = (rcl_guard_condition_t *)PyCapsule_GetPointer( + rcl_guard_condition_t * gc = rclpy_handle_get_pointer_from_capsule( pygc, "rcl_guard_condition_t"); if (!gc) { return NULL; @@ -530,7 +544,7 @@ rclpy_init(PyObject * Py_UNUSED(self), PyObject * args) } int num_args = (int)pysize_num_args; - rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(pycontext, "rcl_context_t"); + rcl_context_t * context = rclpy_handle_get_pointer_from_capsule(pycontext, "rcl_context_t"); if (!context) { Py_DECREF(pyseqlist); return NULL; @@ -618,19 +632,16 @@ rclpy_init(PyObject * Py_UNUSED(self), PyObject * args) Py_RETURN_NONE; } -/// PyCapsule destructor for node +/// Handle destructor for node static void -_rclpy_destroy_node(PyObject * pyentity) +_rclpy_destroy_node(void * p) { - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer( - pyentity, "rcl_node_t"); + rcl_node_t * node = p; if (!node) { - // Don't want to raise an exception, who knows where it will get raised. - PyErr_Clear(); // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_node failed to get pointer"); + PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_node got a NULL pointer"); return; } @@ -679,20 +690,23 @@ rclpy_create_node(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(pycontext, "rcl_context_t"); + rclpy_handle_t * context_handle = PyCapsule_GetPointer(pycontext, "rcl_context_t"); + if (!context_handle) { + return NULL; + } + rcl_context_t * context = _rclpy_handle_get_pointer(context_handle); if (!context) { return NULL; } rcl_arguments_t arguments = rcl_get_zero_initialized_arguments(); - ret = _rclpy_parse_args(py_cli_args, &arguments); if (RCL_RET_OK != ret) { // exception set return NULL; } - rcl_node_t * node = (rcl_node_t *)PyMem_Malloc(sizeof(rcl_node_t)); + rcl_node_t * node = PyMem_Malloc(sizeof(rcl_node_t)); if (!node) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for node"); return NULL; @@ -723,26 +737,42 @@ rclpy_create_node(PyObject * Py_UNUSED(self), PyObject * args) PyMem_Free(node); if (RCL_RET_OK != rcl_arguments_fini(&arguments)) { - rcl_reset_error(); // Warn because an exception is already raised // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( PyExc_RuntimeWarning, stack_level, "Failed to fini arguments during error handling: %s", rcl_get_error_string().str); + rcl_reset_error(); } return NULL; } if (RCL_RET_OK != rcl_arguments_fini(&arguments)) { - rcl_reset_error(); // Warn because the node was successfully created // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( PyExc_RuntimeWarning, stack_level, "Failed to fini arguments: %s", rcl_get_error_string().str); + rcl_reset_error(); } - return PyCapsule_New(node, "rcl_node_t", _rclpy_destroy_node); + + rclpy_handle_t * node_handle = _rclpy_create_handle(node, _rclpy_destroy_node); + if (!node_handle) { + _rclpy_destroy_node(node); + return NULL; + } + _rclpy_handle_add_dependency(node_handle, context_handle); + if (PyErr_Occurred()) { + _rclpy_handle_dec_ref(node_handle); + return NULL; + } + PyObject * node_capsule = _rclpy_create_handle_capsule(node_handle, "rcl_node_t"); + if (!node_capsule) { + _rclpy_handle_dec_ref(node_handle); + return NULL; + } + return node_capsule; } /// Get the name of a node. @@ -762,7 +792,7 @@ rclpy_get_node_name(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -792,7 +822,7 @@ rclpy_get_node_namespace(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -821,7 +851,7 @@ rclpy_get_node_logger_name(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -846,7 +876,7 @@ _count_subscribers_publishers(PyObject * args, const char * type, count_func cou return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -910,7 +940,7 @@ _get_info_by_topic( return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -1424,7 +1454,7 @@ rclpy_remap_topic_name(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - const rcl_node_t * node = (const rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + const rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (node == NULL) { return NULL; } @@ -1443,19 +1473,16 @@ rclpy_remap_topic_name(PyObject * Py_UNUSED(self), PyObject * args) return result; } -/// PyCapsule destructor for publisher +/// Handle destructor for publisher static void -_rclpy_destroy_publisher(PyObject * pyentity) +_rclpy_destroy_publisher(void * p) { - rclpy_publisher_t * pub = (rclpy_publisher_t *)PyCapsule_GetPointer( - pyentity, "rclpy_publisher_t"); + rclpy_publisher_t * pub = p; if (!pub) { - // Don't want to raise an exception, who knows where it will get raised. - PyErr_Clear(); // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_publisher failed to get pointer"); + PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_publisher got NULL pointer"); return; } @@ -1505,13 +1532,16 @@ rclpy_create_publisher(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rclpy_handle_t * node_handle = PyCapsule_GetPointer(pynode, "rcl_node_t"); + if (!node_handle) { + return NULL; + } + rcl_node_t * node = _rclpy_handle_get_pointer(node_handle); if (!node) { return NULL; } - rosidl_message_type_support_t * ts = - (rosidl_message_type_support_t *)rclpy_common_get_type_support(pymsg_type); + rosidl_message_type_support_t * ts = rclpy_common_get_type_support(pymsg_type); if (!ts) { return NULL; } @@ -1519,19 +1549,18 @@ rclpy_create_publisher(PyObject * Py_UNUSED(self), PyObject * args) rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options(); if (PyCapsule_IsValid(pyqos_profile, "rmw_qos_profile_t")) { - void * p = PyCapsule_GetPointer(pyqos_profile, "rmw_qos_profile_t"); - rmw_qos_profile_t * qos_profile = (rmw_qos_profile_t *)p; + rmw_qos_profile_t * qos_profile = PyCapsule_GetPointer(pyqos_profile, "rmw_qos_profile_t"); publisher_ops.qos = *qos_profile; // TODO(jacobperron): It is not obvious why the capsule reference should be destroyed here. // Instead, a safer pattern would be to destroy the QoS object with its own destructor. - PyMem_Free(p); + PyMem_Free(qos_profile); if (PyCapsule_SetPointer(pyqos_profile, Py_None)) { // exception set by PyCapsule_SetPointer return NULL; } } - rclpy_publisher_t * pub = (rclpy_publisher_t *)PyMem_Malloc(sizeof(rclpy_publisher_t)); + rclpy_publisher_t * pub = PyMem_Malloc(sizeof(rclpy_publisher_t)); if (!pub) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for publisher"); return NULL; @@ -1555,7 +1584,23 @@ rclpy_create_publisher(PyObject * Py_UNUSED(self), PyObject * args) PyMem_Free(pub); return NULL; } - return PyCapsule_New(pub, "rclpy_publisher_t", _rclpy_destroy_publisher); + + rclpy_handle_t * pub_handle = _rclpy_create_handle(pub, _rclpy_destroy_publisher); + if (!pub_handle) { + _rclpy_destroy_publisher(pub); + return NULL; + } + _rclpy_handle_add_dependency(pub_handle, node_handle); + if (PyErr_Occurred()) { + _rclpy_handle_dec_ref(pub_handle); + return NULL; + } + PyObject * pub_capsule = _rclpy_create_handle_capsule(pub_handle, "rclpy_publisher_t"); + if (!pub_capsule) { + _rclpy_handle_dec_ref(pub_handle); + return NULL; + } + return pub_capsule; } /// Publish a message @@ -1577,7 +1622,7 @@ rclpy_publish(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rclpy_publisher_t * pub = (rclpy_publisher_t *)PyCapsule_GetPointer( + rclpy_publisher_t * pub = rclpy_handle_get_pointer_from_capsule( pypublisher, "rclpy_publisher_t"); if (!pub) { return NULL; @@ -1616,7 +1661,7 @@ rclpy_publisher_get_subscription_count(PyObject * Py_UNUSED(self), PyObject * ar return NULL; } - const rclpy_publisher_t * pub = (rclpy_publisher_t *)PyCapsule_GetPointer( + const rclpy_publisher_t * pub = rclpy_handle_get_pointer_from_capsule( pypublisher, "rclpy_publisher_t"); if (!pub) { return NULL; @@ -1632,19 +1677,16 @@ rclpy_publisher_get_subscription_count(PyObject * Py_UNUSED(self), PyObject * ar return PyLong_FromSize_t(count); } -/// PyCapsule destructor for timer +/// Handle destructor for timer static void -_rclpy_destroy_timer(PyObject * pyentity) +_rclpy_destroy_timer(void * p) { - rcl_timer_t * tmr = (rcl_timer_t *)PyCapsule_GetPointer( - pyentity, "rcl_timer_t"); + rcl_timer_t * tmr = p; if (!tmr) { - // Don't want to raise an exception, who knows where it will get raised. - PyErr_Clear(); // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_timer failed to get pointer"); + PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_timer got NULL pointer"); return; } @@ -1687,17 +1729,25 @@ rclpy_create_timer(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(pycontext, "rcl_context_t"); + rclpy_handle_t * context_handle = PyCapsule_GetPointer(pycontext, "rcl_context_t"); + if (!context_handle) { + return NULL; + } + rcl_context_t * context = _rclpy_handle_get_pointer(context_handle); if (!context) { return NULL; } - rcl_clock_t * clock = (rcl_clock_t *) PyCapsule_GetPointer(pyclock, "rcl_clock_t"); + rclpy_handle_t * clock_handle = PyCapsule_GetPointer(pyclock, "rcl_clock_t"); + if (!clock_handle) { + return NULL; + } + rcl_clock_t * clock = _rclpy_handle_get_pointer(clock_handle); if (!clock) { return NULL; } - rcl_timer_t * timer = (rcl_timer_t *) PyMem_Malloc(sizeof(rcl_timer_t)); + rcl_timer_t * timer = PyMem_Malloc(sizeof(rcl_timer_t)); if (!timer) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for timer"); return NULL; @@ -1714,14 +1764,27 @@ rclpy_create_timer(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - PyObject * pytimer = PyCapsule_New(timer, "rcl_timer_t", _rclpy_destroy_timer); - if (!pytimer) { - ret = rcl_timer_fini(timer); - (void)ret; - PyMem_Free(timer); + rclpy_handle_t * timer_handle = _rclpy_create_handle(timer, _rclpy_destroy_timer); + if (!timer_handle) { + _rclpy_destroy_timer(timer); + return NULL; + } + _rclpy_handle_add_dependency(timer_handle, context_handle); + if (PyErr_Occurred()) { + _rclpy_handle_dec_ref(timer_handle); + return NULL; + } + _rclpy_handle_add_dependency(timer_handle, clock_handle); + if (PyErr_Occurred()) { + _rclpy_handle_dec_ref(timer_handle); + return NULL; + } + PyObject * timer_capsule = _rclpy_create_handle_capsule(timer_handle, "rcl_timer_t"); + if (!timer_capsule) { + _rclpy_handle_dec_ref(timer_handle); return NULL; } - return pytimer; + return timer_capsule; } /// Returns the period of the timer in nanoseconds @@ -1741,7 +1804,7 @@ rclpy_get_timer_period(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pytimer, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pytimer, "rcl_timer_t"); if (!timer) { return NULL; } @@ -1773,7 +1836,7 @@ rclpy_cancel_timer(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pytimer, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pytimer, "rcl_timer_t"); if (!timer) { return NULL; } @@ -1805,7 +1868,7 @@ rclpy_is_timer_canceled(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pytimer, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pytimer, "rcl_timer_t"); if (!timer) { return NULL; } @@ -1840,7 +1903,7 @@ rclpy_reset_timer(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pytimer, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pytimer, "rcl_timer_t"); if (!timer) { return NULL; } @@ -1871,7 +1934,7 @@ rclpy_is_timer_ready(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pytimer, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pytimer, "rcl_timer_t"); if (!timer) { return NULL; } @@ -1907,7 +1970,7 @@ rclpy_call_timer(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pytimer, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pytimer, "rcl_timer_t"); if (!timer) { return NULL; } @@ -1942,7 +2005,7 @@ rclpy_change_timer_period(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pytimer, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pytimer, "rcl_timer_t"); if (!timer) { return NULL; } @@ -1977,7 +2040,7 @@ rclpy_time_until_next_call(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pytimer, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pytimer, "rcl_timer_t"); if (!timer) { return NULL; } @@ -2009,7 +2072,7 @@ rclpy_time_since_last_call(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_timer_t * timer = (rcl_timer_t *)PyCapsule_GetPointer(pytimer, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pytimer, "rcl_timer_t"); if (!timer) { return NULL; } @@ -2026,19 +2089,16 @@ rclpy_time_since_last_call(PyObject * Py_UNUSED(self), PyObject * args) return PyLong_FromUnsignedLongLong(elapsed_time); } -/// PyCapsule destructor for subscription +/// Handle destructor for subscription static void -_rclpy_destroy_subscription(PyObject * pyentity) +_rclpy_destroy_subscription(void * p) { - rclpy_subscription_t * sub = (rclpy_subscription_t *)PyCapsule_GetPointer( - pyentity, "rclpy_subscription_t"); + rclpy_subscription_t * sub = p; if (!sub) { - // Don't want to raise an exception, who knows where it will get raised. - PyErr_Clear(); // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_subscrition failed to get pointer"); + PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_subscrition got NULL pointer"); return; } @@ -2091,13 +2151,16 @@ rclpy_create_subscription(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rclpy_handle_t * node_handle = PyCapsule_GetPointer(pynode, "rcl_node_t"); + if (!node_handle) { + return NULL; + } + rcl_node_t * node = _rclpy_handle_get_pointer(node_handle); if (!node) { return NULL; } - rosidl_message_type_support_t * ts = - (rosidl_message_type_support_t *)rclpy_common_get_type_support(pymsg_type); + rosidl_message_type_support_t * ts = rclpy_common_get_type_support(pymsg_type); if (!ts) { return NULL; } @@ -2106,7 +2169,7 @@ rclpy_create_subscription(PyObject * Py_UNUSED(self), PyObject * args) if (PyCapsule_IsValid(pyqos_profile, "rmw_qos_profile_t")) { void * p = PyCapsule_GetPointer(pyqos_profile, "rmw_qos_profile_t"); - rmw_qos_profile_t * qos_profile = (rmw_qos_profile_t *)p; + rmw_qos_profile_t * qos_profile = p; subscription_ops.qos = *qos_profile; // TODO(jacobperron): It is not obvious why the capsule reference should be destroyed here. // Instead, a safer pattern would be to destroy the QoS object with its own destructor. @@ -2117,8 +2180,7 @@ rclpy_create_subscription(PyObject * Py_UNUSED(self), PyObject * args) } } - rclpy_subscription_t * sub = - (rclpy_subscription_t *)PyMem_Malloc(sizeof(rclpy_subscription_t)); + rclpy_subscription_t * sub = PyMem_Malloc(sizeof(rclpy_subscription_t)); if (!sub) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for subscription"); return NULL; @@ -2143,30 +2205,36 @@ rclpy_create_subscription(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - PyObject * pysubscription = PyCapsule_New( - sub, "rclpy_subscription_t", _rclpy_destroy_subscription); - if (!pysubscription) { - ret = rcl_subscription_fini(&(sub->subscription), node); - (void)ret; - PyMem_Free(sub); + rclpy_handle_t * sub_handle = _rclpy_create_handle(sub, _rclpy_destroy_subscription); + if (!sub_handle) { + _rclpy_destroy_subscription(sub); + return NULL; + } + _rclpy_handle_add_dependency(sub_handle, node_handle); + if (PyErr_Occurred()) { + _rclpy_handle_dec_ref(sub_handle); + return NULL; + } + PyObject * sub_capsule = _rclpy_create_handle_capsule(sub_handle, "rclpy_subscription_t"); + if (!sub_capsule) { + _rclpy_handle_dec_ref(sub_handle); return NULL; } - return pysubscription; + return sub_capsule; } -/// PyCapsule destructor for client +/// Handle destructor for client static void -_rclpy_destroy_client(PyObject * pyentity) +_rclpy_destroy_client(void * p) { - rclpy_client_t * cli = (rclpy_client_t *)PyCapsule_GetPointer( - pyentity, "rclpy_client_t"); + rclpy_client_t * cli = p; if (!cli) { // Don't want to raise an exception, who knows where it will get raised. PyErr_Clear(); // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_client failed to get pointer"); + PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_client got NULL pointer"); return; } @@ -2217,13 +2285,16 @@ rclpy_create_client(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rclpy_handle_t * node_handle = PyCapsule_GetPointer(pynode, "rcl_node_t"); + if (!node_handle) { + return NULL; + } + rcl_node_t * node = _rclpy_handle_get_pointer(node_handle); if (!node) { return NULL; } - rosidl_service_type_support_t * ts = - (rosidl_service_type_support_t *)rclpy_common_get_type_support(pysrv_type); + rosidl_service_type_support_t * ts = rclpy_common_get_type_support(pysrv_type); if (!ts) { return NULL; } @@ -2232,7 +2303,7 @@ rclpy_create_client(PyObject * Py_UNUSED(self), PyObject * args) if (PyCapsule_IsValid(pyqos_profile, "rmw_qos_profile_t")) { void * p = PyCapsule_GetPointer(pyqos_profile, "rmw_qos_profile_t"); - rmw_qos_profile_t * qos_profile = (rmw_qos_profile_t *)p; + rmw_qos_profile_t * qos_profile = p; client_ops.qos = *qos_profile; // TODO(jacobperron): It is not obvious why the capsule reference should be destroyed here. // Instead, a safer pattern would be to destroy the QoS object with its own destructor. @@ -2243,7 +2314,7 @@ rclpy_create_client(PyObject * Py_UNUSED(self), PyObject * args) } } - rclpy_client_t * client = (rclpy_client_t *)PyMem_Malloc(sizeof(rclpy_client_t)); + rclpy_client_t * client = PyMem_Malloc(sizeof(rclpy_client_t)); if (!client) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for client"); return NULL; @@ -2266,15 +2337,23 @@ rclpy_create_client(PyObject * Py_UNUSED(self), PyObject * args) PyMem_Free(client); return NULL; } - PyObject * pyclient = PyCapsule_New(client, "rclpy_client_t", _rclpy_destroy_client); - if (!pyclient) { - ret = rcl_client_fini(&(client->client), node); - (void)ret; - PyMem_Free(client); + rclpy_handle_t * client_handle = _rclpy_create_handle( + client, _rclpy_destroy_client); + if (!client_handle) { + _rclpy_destroy_client(client); return NULL; } - - return pyclient; + _rclpy_handle_add_dependency(client_handle, node_handle); + if (PyErr_Occurred()) { + _rclpy_handle_dec_ref(client_handle); + return NULL; + } + PyObject * client_capsule = _rclpy_create_handle_capsule(client_handle, "rclpy_client_t"); + if (!client_capsule) { + _rclpy_handle_dec_ref(client_handle); + return NULL; + } + return client_capsule; } /// Publish a request message @@ -2295,7 +2374,7 @@ rclpy_send_request(PyObject * Py_UNUSED(self), PyObject * args) if (!PyArg_ParseTuple(args, "OO", &pyclient, &pyrequest)) { return NULL; } - rclpy_client_t * client = (rclpy_client_t *)PyCapsule_GetPointer(pyclient, "rclpy_client_t"); + rclpy_client_t * client = rclpy_handle_get_pointer_from_capsule(pyclient, "rclpy_client_t"); if (!client) { return NULL; } @@ -2319,19 +2398,16 @@ rclpy_send_request(PyObject * Py_UNUSED(self), PyObject * args) return PyLong_FromLongLong(sequence_number); } -/// PyCapsule destructor for service +/// Handle destructor for service static void -_rclpy_destroy_service(PyObject * pyentity) +_rclpy_destroy_service(void * p) { - rclpy_service_t * srv = (rclpy_service_t *)PyCapsule_GetPointer( - pyentity, "rclpy_service_t"); + rclpy_service_t * srv = p; if (!srv) { - // Don't want to raise an exception, who knows where it will get raised. - PyErr_Clear(); // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_service failed to get pointer"); + PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_service got NULL pointer"); return; } @@ -2383,13 +2459,16 @@ rclpy_create_service(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rclpy_handle_t * node_handle = PyCapsule_GetPointer(pynode, "rcl_node_t"); + if (!node_handle) { + return NULL; + } + rcl_node_t * node = _rclpy_handle_get_pointer(node_handle); if (!node) { return NULL; } - rosidl_service_type_support_t * ts = - (rosidl_service_type_support_t *)rclpy_common_get_type_support(pysrv_type); + rosidl_service_type_support_t * ts = rclpy_common_get_type_support(pysrv_type); if (!ts) { return NULL; } @@ -2398,7 +2477,7 @@ rclpy_create_service(PyObject * Py_UNUSED(self), PyObject * args) if (PyCapsule_IsValid(pyqos_profile, "rmw_qos_profile_t")) { void * p = PyCapsule_GetPointer(pyqos_profile, "rmw_qos_profile_t"); - rmw_qos_profile_t * qos_profile = (rmw_qos_profile_t *)p; + rmw_qos_profile_t * qos_profile = p; service_ops.qos = *qos_profile; // TODO(jacobperron): It is not obvious why the capsule reference should be destroyed here. // Instead, a safer pattern would be to destroy the QoS object with its own destructor. @@ -2409,7 +2488,7 @@ rclpy_create_service(PyObject * Py_UNUSED(self), PyObject * args) } } - rclpy_service_t * srv = (rclpy_service_t *)PyMem_Malloc(sizeof(rclpy_service_t)); + rclpy_service_t * srv = PyMem_Malloc(sizeof(rclpy_service_t)); if (!srv) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for service"); return NULL; @@ -2433,14 +2512,22 @@ rclpy_create_service(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - PyObject * pyservice = PyCapsule_New(srv, "rclpy_service_t", _rclpy_destroy_service); - if (!pyservice) { - ret = rcl_service_fini(&(srv->service), node); - (void)ret; - PyMem_Free(srv); + rclpy_handle_t * service_handle = _rclpy_create_handle(srv, _rclpy_destroy_service); + if (!service_handle) { + _rclpy_destroy_service(srv); + return NULL; + } + _rclpy_handle_add_dependency(service_handle, node_handle); + if (PyErr_Occurred()) { + _rclpy_handle_dec_ref(service_handle); return NULL; } - return pyservice; + PyObject * service_capsule = _rclpy_create_handle_capsule(service_handle, "rclpy_service_t"); + if (!service_capsule) { + _rclpy_handle_dec_ref(service_handle); + return NULL; + } + return service_capsule; } /// Publish a response message @@ -2463,12 +2550,12 @@ rclpy_send_response(PyObject * Py_UNUSED(self), PyObject * args) if (!PyArg_ParseTuple(args, "OOO", &pyservice, &pyresponse, &pyheader)) { return NULL; } - rclpy_service_t * srv = (rclpy_service_t *)PyCapsule_GetPointer(pyservice, "rclpy_service_t"); + rclpy_service_t * srv = rclpy_handle_get_pointer_from_capsule(pyservice, "rclpy_service_t"); if (!srv) { return NULL; } - rmw_request_id_t * header = (rmw_request_id_t *)PyCapsule_GetPointer( + rmw_request_id_t * header = PyCapsule_GetPointer( pyheader, "rmw_request_id_t"); if (!header) { return NULL; @@ -2507,7 +2594,7 @@ rclpy_service_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rclpy_client_t * client = (rclpy_client_t *)PyCapsule_GetPointer(pyclient, "rclpy_client_t"); + rclpy_client_t * client = rclpy_handle_get_pointer_from_capsule(pyclient, "rclpy_client_t"); if (!client) { return NULL; } @@ -2531,12 +2618,10 @@ rclpy_service_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) /// Destructor for a clock static void -_rclpy_destroy_clock(PyObject * pycapsule) +_rclpy_destroy_clock(void * p) { - rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer(pycapsule, "rcl_clock_t"); + rcl_clock_t * clock = p; if (!clock) { - // exception was set by PyCapsule_GetPointer - PyErr_Clear(); // Warning should use line number of the current stack frame int stack_level = 1; PyErr_WarnFormat( @@ -2576,7 +2661,7 @@ rclpy_get_rmw_implementation_identifier(PyObject * Py_UNUSED(self), PyObject * P static PyObject * rclpy_get_zero_initialized_wait_set(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) { - rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyMem_Malloc(sizeof(rcl_wait_set_t)); + rcl_wait_set_t * wait_set = PyMem_Malloc(sizeof(rcl_wait_set_t)); if (!wait_set) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for wait set"); return NULL; @@ -2620,12 +2705,12 @@ rclpy_wait_set_init(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); + rcl_wait_set_t * wait_set = PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); if (!wait_set) { return NULL; } - rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(pycontext, "rcl_context_t"); + rcl_context_t * context = rclpy_handle_get_pointer_from_capsule(pycontext, "rcl_context_t"); if (!context) { return NULL; } @@ -2666,7 +2751,7 @@ rclpy_wait_set_clear_entities(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); + rcl_wait_set_t * wait_set = PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); if (!wait_set) { return NULL; } @@ -2701,47 +2786,44 @@ rclpy_wait_set_add_entity(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } rcl_ret_t ret; - rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); + rcl_wait_set_t * wait_set = PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); if (!wait_set) { return NULL; } if (0 == strcmp(entity_type, "subscription")) { rclpy_subscription_t * sub = - (rclpy_subscription_t *)PyCapsule_GetPointer(pyentity, "rclpy_subscription_t"); + rclpy_handle_get_pointer_from_capsule(pyentity, "rclpy_subscription_t"); if (!sub) { return NULL; } ret = rcl_wait_set_add_subscription(wait_set, &(sub->subscription), &index); } else if (0 == strcmp(entity_type, "client")) { - rclpy_client_t * client = - (rclpy_client_t *)PyCapsule_GetPointer(pyentity, "rclpy_client_t"); + rclpy_client_t * client = rclpy_handle_get_pointer_from_capsule(pyentity, "rclpy_client_t"); if (!client) { return NULL; } ret = rcl_wait_set_add_client(wait_set, &(client->client), &index); } else if (0 == strcmp(entity_type, "service")) { - rclpy_service_t * srv = - (rclpy_service_t *)PyCapsule_GetPointer(pyentity, "rclpy_service_t"); + rclpy_service_t * srv = rclpy_handle_get_pointer_from_capsule(pyentity, "rclpy_service_t"); if (!srv) { return NULL; } ret = rcl_wait_set_add_service(wait_set, &(srv->service), &index); } else if (0 == strcmp(entity_type, "timer")) { - rcl_timer_t * timer = - (rcl_timer_t *)PyCapsule_GetPointer(pyentity, "rcl_timer_t"); + rcl_timer_t * timer = rclpy_handle_get_pointer_from_capsule(pyentity, "rcl_timer_t"); if (!timer) { return NULL; } ret = rcl_wait_set_add_timer(wait_set, timer, &index); } else if (0 == strcmp(entity_type, "guard_condition")) { - rcl_guard_condition_t * guard_condition = - (rcl_guard_condition_t *)PyCapsule_GetPointer(pyentity, "rcl_guard_condition_t"); + rcl_guard_condition_t * guard_condition = rclpy_handle_get_pointer_from_capsule( + pyentity, "rcl_guard_condition_t"); if (!guard_condition) { return NULL; } ret = rcl_wait_set_add_guard_condition(wait_set, guard_condition, &index); } else if (0 == strcmp(entity_type, "event")) { - rcl_event_t * event = (rcl_event_t *)PyCapsule_GetPointer(pyentity, "rcl_event_t"); + rcl_event_t * event = rclpy_handle_get_pointer_from_capsule(pyentity, "rcl_event_t"); ret = rcl_wait_set_add_event(wait_set, event, &index); } else { ret = RCL_RET_ERROR; // to avoid a linter warning @@ -2788,7 +2870,7 @@ rclpy_wait_set_is_ready(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); + rcl_wait_set_t * wait_set = PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); if (!wait_set) { return NULL; } @@ -2847,7 +2929,7 @@ rclpy_destroy_wait_set(PyObject * Py_UNUSED(self), PyObject * args) if (!PyArg_ParseTuple(args, "O", &pywait_set)) { return NULL; } - rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); + rcl_wait_set_t * wait_set = PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); if (!wait_set) { return NULL; } @@ -2910,7 +2992,7 @@ rclpy_get_ready_entities(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); + rcl_wait_set_t * wait_set = PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); if (!wait_set) { return NULL; } @@ -2953,7 +3035,7 @@ rclpy_wait(PyObject * Py_UNUSED(self), PyObject * args) if (!PyArg_ParseTuple(args, "O|K", &pywait_set, &timeout)) { return NULL; } - rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); + rcl_wait_set_t * wait_set = PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t"); if (!wait_set) { return NULL; } @@ -3043,7 +3125,7 @@ rclpy_take(PyObject * Py_UNUSED(self), PyObject * args) } rclpy_subscription_t * sub = - (rclpy_subscription_t *)PyCapsule_GetPointer(pysubscription, "rclpy_subscription_t"); + rclpy_handle_get_pointer_from_capsule(pysubscription, "rclpy_subscription_t"); if (!sub) { return NULL; } @@ -3105,8 +3187,7 @@ rclpy_take_request(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rclpy_service_t * srv = - (rclpy_service_t *)PyCapsule_GetPointer(pyservice, "rclpy_service_t"); + rclpy_service_t * srv = rclpy_handle_get_pointer_from_capsule(pyservice, "rclpy_service_t"); if (!srv) { return NULL; } @@ -3117,7 +3198,7 @@ rclpy_take_request(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rmw_request_id_t * header = (rmw_request_id_t *)PyMem_Malloc(sizeof(rmw_request_id_t)); + rmw_request_id_t * header = PyMem_Malloc(sizeof(rmw_request_id_t)); if (!header) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for request header"); return NULL; @@ -3183,8 +3264,7 @@ rclpy_take_response(PyObject * Py_UNUSED(self), PyObject * args) if (!PyArg_ParseTuple(args, "OO", &pyclient, &pyresponse_type)) { return NULL; } - rclpy_client_t * client = - (rclpy_client_t *)PyCapsule_GetPointer(pyclient, "rclpy_client_t"); + rclpy_client_t * client = rclpy_handle_get_pointer_from_capsule(pyclient, "rclpy_client_t"); if (!client) { return NULL; } @@ -3195,7 +3275,7 @@ rclpy_take_response(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rmw_request_id_t * header = (rmw_request_id_t *)PyMem_Malloc(sizeof(rmw_request_id_t)); + rmw_request_id_t * header = PyMem_Malloc(sizeof(rmw_request_id_t)); if (!header) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for response header"); return NULL; @@ -3250,7 +3330,7 @@ rclpy_ok(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(pycontext, "rcl_context_t"); + rcl_context_t * context = rclpy_handle_get_pointer_from_capsule(pycontext, "rcl_context_t"); if (!context) { return NULL; } @@ -3278,7 +3358,7 @@ rclpy_shutdown(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_context_t * context = (rcl_context_t *)PyCapsule_GetPointer(pycontext, "rcl_context_t"); + rcl_context_t * context = rclpy_handle_get_pointer_from_capsule(pycontext, "rcl_context_t"); if (!context) { return NULL; } @@ -3313,14 +3393,12 @@ rclpy_get_node_names_and_namespaces(PyObject * Py_UNUSED(self), PyObject * args) } rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } - rcutils_string_array_t node_names = - rcutils_get_zero_initialized_string_array(); - rcutils_string_array_t node_namespaces = - rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); rcl_ret_t ret = rcl_get_node_names(node, allocator, &node_names, &node_namespaces); if (ret != RCL_RET_OK) { PyErr_Format( @@ -3408,7 +3486,7 @@ rclpy_get_service_names_and_types_by_node(PyObject * Py_UNUSED(self), PyObject * return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -3460,7 +3538,7 @@ rclpy_get_client_names_and_types_by_node(PyObject * Py_UNUSED(self), PyObject * return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -3514,7 +3592,7 @@ rclpy_get_subscriber_names_and_types_by_node(PyObject * Py_UNUSED(self), PyObjec return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -3522,8 +3600,7 @@ rclpy_get_subscriber_names_and_types_by_node(PyObject * Py_UNUSED(self), PyObjec rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node( - node, &allocator, no_demangle, node_name, node_namespace, - &topic_names_and_types); + node, &allocator, no_demangle, node_name, node_namespace, &topic_names_and_types); if (ret != RCL_RET_OK) { PyObject * error = RCLError; if (ret == RCL_RET_NODE_NAME_NON_EXISTENT) { @@ -3568,7 +3645,7 @@ rclpy_get_publisher_names_and_types_by_node(PyObject * Py_UNUSED(self), PyObject return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -3576,8 +3653,7 @@ rclpy_get_publisher_names_and_types_by_node(PyObject * Py_UNUSED(self), PyObject rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node( - node, &allocator, no_demangle, node_name, node_namespace, - &topic_names_and_types); + node, &allocator, no_demangle, node_name, node_namespace, &topic_names_and_types); if (ret != RCL_RET_OK) { PyObject * error = RCLError; if (ret == RCL_RET_NODE_NAME_NON_EXISTENT) { @@ -3619,7 +3695,7 @@ rclpy_get_topic_names_and_types(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -3709,15 +3785,14 @@ rclpy_get_service_names_and_types(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = - rcl_get_service_names_and_types(node, &allocator, &service_names_and_types); + rcl_ret_t ret = rcl_get_service_names_and_types(node, &allocator, &service_names_and_types); if (ret != RCL_RET_OK) { PyErr_Format( RCLError, @@ -3789,7 +3864,7 @@ rclpy_get_service_names_and_types(PyObject * Py_UNUSED(self), PyObject * args) bool _convert_py_duration_to_rmw_time(PyObject * pyobject, rmw_time_t * out_time) { - rcl_duration_t * duration = (rcl_duration_t *)PyCapsule_GetPointer(pyobject, "rcl_duration_t"); + rcl_duration_t * duration = PyCapsule_GetPointer(pyobject, "rcl_duration_t"); if (!duration) { return false; } @@ -3838,7 +3913,7 @@ rclpy_convert_from_py_qos_policy(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rmw_qos_profile_t * qos_profile = (rmw_qos_profile_t *)PyMem_Malloc(sizeof(rmw_qos_profile_t)); + rmw_qos_profile_t * qos_profile = PyMem_Malloc(sizeof(rmw_qos_profile_t)); if (!qos_profile) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for QoS profile"); @@ -3887,7 +3962,7 @@ rclpy_convert_to_py_qos_policy(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rmw_qos_profile_t * profile = (rmw_qos_profile_t *)PyCapsule_GetPointer( + rmw_qos_profile_t * profile = PyCapsule_GetPointer( pyqos_profile, "rmw_qos_profile_t"); if (!profile) { return NULL; @@ -3959,7 +4034,7 @@ rclpy_assert_liveliness(PyObject * Py_UNUSED(self), PyObject * args) } if (PyCapsule_IsValid(pyentity, "rcl_node_t")) { - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pyentity, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pyentity, "rcl_node_t"); if (RCL_RET_OK != rcl_node_assert_liveliness(node)) { PyErr_Format( RCLError, @@ -3968,7 +4043,7 @@ rclpy_assert_liveliness(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } } else if (PyCapsule_IsValid(pyentity, "rclpy_publisher_t")) { - rclpy_publisher_t * publisher = (rclpy_publisher_t *)PyCapsule_GetPointer( + rclpy_publisher_t * publisher = rclpy_handle_get_pointer_from_capsule( pyentity, "rclpy_publisher_t"); if (RCL_RET_OK != rcl_publisher_assert_liveliness(&publisher->publisher)) { PyErr_Format( @@ -4022,7 +4097,7 @@ rclpy_create_time_point(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_time_point_t * time_point = (rcl_time_point_t *) PyMem_Malloc(sizeof(rcl_time_point_t)); + rcl_time_point_t * time_point = PyMem_Malloc(sizeof(rcl_time_point_t)); if (!time_point) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for time point."); return NULL; @@ -4052,7 +4127,7 @@ rclpy_time_point_get_nanoseconds(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_time_point_t * time_point = (rcl_time_point_t *)PyCapsule_GetPointer( + rcl_time_point_t * time_point = PyCapsule_GetPointer( pytime_point, "rcl_time_point_t"); if (!time_point) { return NULL; @@ -4090,7 +4165,7 @@ rclpy_create_duration(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_duration_t * duration = (rcl_duration_t *) PyMem_Malloc(sizeof(rcl_duration_t)); + rcl_duration_t * duration = PyMem_Malloc(sizeof(rcl_duration_t)); if (!duration) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for duration."); return NULL; @@ -4119,7 +4194,7 @@ rclpy_duration_get_nanoseconds(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_duration_t * duration = (rcl_duration_t *)PyCapsule_GetPointer( + rcl_duration_t * duration = PyCapsule_GetPointer( pyduration, "rcl_duration_t"); if (!duration) { return NULL; @@ -4149,7 +4224,7 @@ rclpy_create_clock(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_clock_t * clock = (rcl_clock_t *)PyMem_Malloc(sizeof(rcl_clock_t)); + rcl_clock_t * clock = PyMem_Malloc(sizeof(rcl_clock_t)); if (!clock) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for clock."); return NULL; @@ -4165,7 +4240,7 @@ rclpy_create_clock(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - return PyCapsule_New(clock, "rcl_clock_t", _rclpy_destroy_clock); + return rclpy_create_handle_capsule(clock, "rcl_clock_t", _rclpy_destroy_clock); } /// Returns the current value of the clock @@ -4187,13 +4262,13 @@ rclpy_clock_get_now(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( + rcl_clock_t * clock = rclpy_handle_get_pointer_from_capsule( pyclock, "rcl_clock_t"); if (!clock) { return NULL; } - rcl_time_point_t * time_point = (rcl_time_point_t *) PyMem_Malloc(sizeof(rcl_time_point_t)); + rcl_time_point_t * time_point = PyMem_Malloc(sizeof(rcl_time_point_t)); if (!time_point) { PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for time point."); return NULL; @@ -4234,7 +4309,7 @@ rclpy_clock_get_ros_time_override_is_enabled(PyObject * Py_UNUSED(self), PyObjec return NULL; } - rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( + rcl_clock_t * clock = rclpy_handle_get_pointer_from_capsule( pyclock, "rcl_clock_t"); if (!clock) { return NULL; @@ -4278,7 +4353,7 @@ rclpy_clock_set_ros_time_override_is_enabled(PyObject * Py_UNUSED(self), PyObjec return NULL; } - rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( + rcl_clock_t * clock = rclpy_handle_get_pointer_from_capsule( pyclock, "rcl_clock_t"); if (!clock) { return NULL; @@ -4326,13 +4401,13 @@ rclpy_clock_set_ros_time_override(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( + rcl_clock_t * clock = rclpy_handle_get_pointer_from_capsule( pyclock, "rcl_clock_t"); if (!clock) { return NULL; } - rcl_time_point_t * time_point = (rcl_time_point_t *)PyCapsule_GetPointer( + rcl_time_point_t * time_point = PyCapsule_GetPointer( pytime_point, "rcl_time_point_t"); if (!time_point) { return NULL; @@ -4466,8 +4541,7 @@ rclpy_add_clock_callback(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( - pyclock, "rcl_clock_t"); + rcl_clock_t * clock = rclpy_handle_get_pointer_from_capsule(pyclock, "rcl_clock_t"); if (!clock) { return NULL; } @@ -4511,8 +4585,7 @@ rclpy_remove_clock_callback(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( - pyclock, "rcl_clock_t"); + rcl_clock_t * clock = rclpy_handle_get_pointer_from_capsule(pyclock, "rcl_clock_t"); if (!clock) { return NULL; } @@ -4809,7 +4882,7 @@ rclpy_get_node_parameters(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(node_capsule, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(node_capsule, "rcl_node_t"); if (!node) { return NULL; } @@ -4920,8 +4993,7 @@ rclpy_serialize(PyObject * Py_UNUSED(self), PyObject * args) } // Get type support - rosidl_message_type_support_t * ts = - (rosidl_message_type_support_t *)rclpy_common_get_type_support(pymsg_type); + rosidl_message_type_support_t * ts = rclpy_common_get_type_support(pymsg_type); if (!ts) { return NULL; } @@ -4973,8 +5045,7 @@ rclpy_deserialize(PyObject * Py_UNUSED(self), PyObject * args) } // Get type support - rosidl_message_type_support_t * ts = - (rosidl_message_type_support_t *)rclpy_common_get_type_support(pymsg_type); + rosidl_message_type_support_t * ts = rclpy_common_get_type_support(pymsg_type); if (!ts) { return NULL; } diff --git a/rclpy/src/rclpy/_rclpy_action.c b/rclpy/src/rclpy/_rclpy_action.c index 35e7921ce..3562f08ba 100644 --- a/rclpy/src/rclpy/_rclpy_action.c +++ b/rclpy/src/rclpy/_rclpy_action.c @@ -18,6 +18,7 @@ #include #include "rclpy_common/common.h" +#include "rclpy_common/handle.h" /// Destroy an rcl_action entity. /** @@ -36,7 +37,7 @@ rclpy_action_destroy_entity(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -471,7 +472,7 @@ rclpy_action_create_client(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -588,12 +589,12 @@ rclpy_action_create_server(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } - rcl_clock_t * clock = (rcl_clock_t *) PyCapsule_GetPointer(pyclock, "rcl_clock_t"); + rcl_clock_t * clock = rclpy_handle_get_pointer_from_capsule(pyclock, "rcl_clock_t"); if (!clock) { return NULL; } @@ -664,7 +665,7 @@ rclpy_action_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -1687,7 +1688,7 @@ rclpy_action_get_client_names_and_types_by_node(PyObject * Py_UNUSED(self), PyOb return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -1727,7 +1728,7 @@ rclpy_action_get_server_names_and_types_by_node(PyObject * Py_UNUSED(self), PyOb return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } @@ -1765,7 +1766,7 @@ rclpy_action_get_names_and_types(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + rcl_node_t * node = rclpy_handle_get_pointer_from_capsule(pynode, "rcl_node_t"); if (!node) { return NULL; } diff --git a/rclpy/src/rclpy/_rclpy_handle.c b/rclpy/src/rclpy/_rclpy_handle.c new file mode 100644 index 000000000..eb2d18bb0 --- /dev/null +++ b/rclpy/src/rclpy/_rclpy_handle.c @@ -0,0 +1,96 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rcutils/allocator.h" +#include "rcutils/strdup.h" +#include "rcutils/types/rcutils_ret.h" + +#include "rclpy_common/handle.h" + +static PyObject * +rclpy_handle_get_name(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * handle_capsule; + if (!PyArg_ParseTuple(args, "O", &handle_capsule)) { + return NULL; + } + + if (PyErr_Occurred()) { + return NULL; + } + return PyUnicode_FromString(PyCapsule_GetName(handle_capsule)); +} + +static PyObject * +rclpy_handle_get_pointer(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * handle_capsule; + if (!PyArg_ParseTuple(args, "O", &handle_capsule)) { + return NULL; + } + + if (PyErr_Occurred()) { + return NULL; + } + + void * ptr = rclpy_handle_get_pointer_from_capsule( + handle_capsule, PyCapsule_GetName(handle_capsule)); + + if (!ptr || PyErr_Occurred()) { + return NULL; + } + + return PyLong_FromVoidPtr(ptr); +} + +/// Define the public methods of this module +static PyMethodDef rclpy_handle_methods[] = { + { + "rclpy_handle_get_name", rclpy_handle_get_name, + METH_VARARGS, + "Get handle name." + }, + { + "rclpy_handle_get_pointer", rclpy_handle_get_pointer, + METH_VARARGS, + "Get handle pointer." + }, + {NULL, NULL, 0, NULL} /* sentinel */ +}; + +PyDoc_STRVAR( + rclpy_handle__doc__, + "rclpy module for working with Handle objects."); + +/// Define the Python module +static struct PyModuleDef _rclpy_handle_module = { + PyModuleDef_HEAD_INIT, + "_rclpy_handle", + rclpy_handle__doc__, + -1, /* -1 means that the module keeps state in global variables */ + rclpy_handle_methods, + NULL, + NULL, + NULL, + NULL +}; + +/// Init function of this module +PyMODINIT_FUNC PyInit__rclpy_handle(void) +{ + return PyModule_Create(&_rclpy_handle_module); +} diff --git a/rclpy/src/rclpy/_rclpy_qos_event.c b/rclpy/src/rclpy/_rclpy_qos_event.c index 32d5a140a..0b6c2fdf1 100644 --- a/rclpy/src/rclpy/_rclpy_qos_event.c +++ b/rclpy/src/rclpy/_rclpy_qos_event.c @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rcl/event.h" +#include "rclpy_common/handle.h" typedef union _qos_event_callback_data { // Subscription events @@ -41,9 +42,9 @@ _check_rcl_return(rcl_ret_t ret, const char * error_msg) static void -_destroy_event_capsule(PyObject * pycapsule) +_destroy_event_capsule(void * p) { - rcl_event_t * event = (rcl_event_t *)PyCapsule_GetPointer(pycapsule, "rcl_event_t"); + rcl_event_t * event = p; if (!event) { PyErr_Clear(); int stack_level = 1; @@ -77,7 +78,7 @@ static rcl_event_t * _pycapsule_to_rcl_event(PyObject * pycapsule) { - return (rcl_event_t *)PyCapsule_GetPointer(pycapsule, "rcl_event_t"); + return rclpy_handle_get_pointer_from_capsule(pycapsule, "rcl_event_t"); } static @@ -247,19 +248,16 @@ rclpy_create_event(PyObject * Py_UNUSED(self), PyObject * args) rcl_publisher_t * publisher = NULL; rcl_event_t * event = NULL; - PyObject * pyevent = NULL; - if (!PyArg_ParseTuple(args, "KO", &event_type, &pyparent)) { return NULL; } + rclpy_handle_t * parent_handle = PyCapsule_GetPointer(pyparent, PyCapsule_GetName(pyparent)); if (_is_pycapsule_rcl_subscription(pyparent)) { - rclpy_subscription_t * py_subscription = - (rclpy_subscription_t *)PyCapsule_GetPointer(pyparent, "rclpy_subscription_t"); + rclpy_subscription_t * py_subscription = _rclpy_handle_get_pointer(parent_handle); subscription = py_subscription ? &py_subscription->subscription : NULL; } else if (_is_pycapsule_rcl_publisher(pyparent)) { - rclpy_publisher_t * py_publisher = - (rclpy_publisher_t *)PyCapsule_GetPointer(pyparent, "rclpy_publisher_t"); + rclpy_publisher_t * py_publisher = _rclpy_handle_get_pointer(parent_handle); publisher = py_publisher ? &py_publisher->publisher : NULL; } else { PyErr_Format(PyExc_TypeError, "Event parent was not a valid Publisher or Subscription."); @@ -281,15 +279,24 @@ rclpy_create_event(PyObject * Py_UNUSED(self), PyObject * args) return NULL; } - pyevent = PyCapsule_New(event, "rcl_event_t", _destroy_event_capsule); - if (!pyevent) { + rclpy_handle_t * event_handle = _rclpy_create_handle(event, _destroy_event_capsule); + if (!event_handle) { ret = rcl_event_fini(event); PyMem_Free(event); _check_rcl_return(ret, "Failed to fini 'rcl_event_t'"); return NULL; } - - return pyevent; + _rclpy_handle_add_dependency(event_handle, parent_handle); + if (PyErr_Occurred()) { + _rclpy_handle_dec_ref(event_handle); + return NULL; + } + PyObject * event_capsule = _rclpy_create_handle_capsule(event_handle, "rcl_event_t"); + if (!event_capsule) { + _rclpy_handle_dec_ref(event_handle); + return NULL; + } + return event_capsule; } /// Get a pending QoS event's data. diff --git a/rclpy/src/rclpy/_rclpy_signal_handler.c b/rclpy/src/rclpy/_rclpy_signal_handler.c index 7d9e3e4ce..1bd55e40e 100644 --- a/rclpy/src/rclpy/_rclpy_signal_handler.c +++ b/rclpy/src/rclpy/_rclpy_signal_handler.c @@ -13,13 +13,15 @@ // limitations under the License. #include +#include -#include -#include -#include -#include +#include "rcl/error_handling.h" +#include "rcl/rcl.h" -#include +#include "rclpy_common/handle.h" + +#include "rcutils/allocator.h" +#include "rcutils/stdatomic_helper.h" #if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE @@ -236,8 +238,7 @@ rclpy_register_sigint_guard_condition(PyObject * Py_UNUSED(self), PyObject * arg return NULL; } - rcl_guard_condition_t * gc = (rcl_guard_condition_t *)PyCapsule_GetPointer( - pygc, "rcl_guard_condition_t"); + rcl_guard_condition_t * gc = rclpy_handle_get_pointer_from_capsule(pygc, "rcl_guard_condition_t"); if (!gc) { return NULL; } @@ -300,8 +301,7 @@ rclpy_unregister_sigint_guard_condition(PyObject * Py_UNUSED(self), PyObject * a return NULL; } - rcl_guard_condition_t * gc = (rcl_guard_condition_t *)PyCapsule_GetPointer( - pygc, "rcl_guard_condition_t"); + rcl_guard_condition_t * gc = rclpy_handle_get_pointer_from_capsule(pygc, "rcl_guard_condition_t"); if (!gc) { return NULL; } diff --git a/rclpy/src/rclpy_common/include/rclpy_common/handle.h b/rclpy/src/rclpy_common/include/rclpy_common/handle.h new file mode 100644 index 000000000..f74f48f5d --- /dev/null +++ b/rclpy/src/rclpy_common/include/rclpy_common/handle.h @@ -0,0 +1,124 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLPY_COMMON__HANDLE_H_ +#define RCLPY_COMMON__HANDLE_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include + +#include "rcutils/types/rcutils_ret.h" + +#include "rclpy_common/visibility_control.h" + +/// Wrapper that manages the lifetime of an object, +/// allowing to establish dependencies between them. +typedef struct rclpy_handle_t rclpy_handle_t; + +/// Signature of rclpy_handle_t destructors. +typedef void (* rclpy_handle_destructor_t)(void *); + +/// Create a PyCapsule wrapping a rclpy_handle_t object. +/** + * The main reason to use them, is that destruction order cannot be guaranteed in Python. + * From PEP 442: + * > Cyclic isolate (CI) + * > A standalone subgraph of objects in which no object is referenced from the outside, + * > containing one or several reference cycles, and whose objects are still in a usable, + * > non-broken state: they can access each other from their respective finalizers. + * > + * > For CI objects, the order in which finalizers are called (step 2 above) is undefined. + * + * Handles provide a way of establishing dependencies, keeping a reference count separately from + * Python's, ensuring proper destruction order. + * No protection against reference cycles is provided. + * + * It could be replaced with `std::shared_ptr` if this library is migrated to `pybind11`. + * + * \sa _rclpy_create_handle + * \param name Name of the PyCapsule. + * \returns PyCapsule wrapping the rclpy_handle_t. + */ +RCLPY_COMMON_PUBLIC +PyObject * +rclpy_create_handle_capsule(void * ptr, const char * name, rclpy_handle_destructor_t destructor); + +/// Returns the object managed by the rclpy_handle_t wrapped in a PyCapsule. +RCLPY_COMMON_PUBLIC +void * +rclpy_handle_get_pointer_from_capsule(PyObject * capsule, const char * name); + +/// Creates a rclpy_handle_t object. +/** + * \param ptr Opaque pointer to the object being wrapped. + * \param destructor Function that will be called when the handle is destructed. + */ +RCLPY_COMMON_PUBLIC +rclpy_handle_t * +_rclpy_create_handle(void * ptr, rclpy_handle_destructor_t destructor); + +/// Create a PyCapsule wrapping a rclpy_handle_t object. +/** + * \param ptr Already constructed handle. + * \param name Name of the PyCapsule. + * \returns PyCapsule wrapping the rclpy_handle_t, using _rclpy_handle_dec_ref as destructor. + */ +RCLPY_COMMON_PUBLIC +PyObject * +_rclpy_create_handle_capsule(rclpy_handle_t * ptr, const char * name); + +/// Returns the object managed by the rclpy_handle_t. +/** + * PyExc_RuntimeError is set, if `handle` is NULL, o if the managed pointer is NULL. + * + * \param handle + */ +RCLPY_COMMON_PUBLIC +void * +_rclpy_handle_get_pointer(rclpy_handle_t * handle); + +/// Adds a dependency to a handle. +/** + * PyExc_RuntimeError is set, if `handle` is NULL, o if the managed pointer is NULL. + * + * \param dependency Handle object whose reference count will be incremented. + * \param dependent Handle object that keeps a reference to the `dependency`. + */ +RCLPY_COMMON_PUBLIC +void +_rclpy_handle_add_dependency(rclpy_handle_t * dependent, rclpy_handle_t * dependency); + +/// Decrements the reference count of a handle. +/** + * The reference count of `handle` is decremented. + * If it reaches zero: + * - `rclpy_handle_dec_ref` is called on `handle` dependencies. + * - `handle` is deallocated. + * + * \param handle Object which reference count will be decremented. + */ +RCLPY_COMMON_PUBLIC +void +_rclpy_handle_dec_ref(rclpy_handle_t * handle); + +#ifdef __cplusplus +} +#endif + +#endif // RCLPY_COMMON__HANDLE_H_ diff --git a/rclpy/src/rclpy_common/src/handle.c b/rclpy/src/rclpy_common/src/handle.c new file mode 100644 index 000000000..771802cfb --- /dev/null +++ b/rclpy/src/rclpy_common/src/handle.c @@ -0,0 +1,154 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rcutils/allocator.h" +#include "rcutils/strdup.h" +#include "rcutils/types/rcutils_ret.h" + +#include "rclpy_common/handle.h" + +struct rclpy_handle_t +{ + void * ptr; // opaque pointer to the wrapped object. + size_t ref_count; // Reference count. + struct rclpy_handle_t ** dependencies; // array of pointers to dependencies. + size_t num_of_dependencies; // size of the array. + rclpy_handle_destructor_t destructor; // destructor +}; + +/// Creates a Handle object. +rclpy_handle_t * +_rclpy_create_handle(void * ptr, rclpy_handle_destructor_t destructor) +{ + assert(ptr); + assert(destructor); + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rclpy_handle_t * handle = allocator.zero_allocate(1, sizeof(rclpy_handle_t), allocator.state); + if (!handle) { + PyErr_Format(PyExc_MemoryError, "Failed to allocate memory for handle"); + return NULL; + } + + handle->ptr = ptr; + handle->ref_count++; + handle->destructor = destructor; + + return handle; +} + +/// Adds a dependency to a handle. +void +_rclpy_handle_add_dependency(rclpy_handle_t * dependent, rclpy_handle_t * dependency) +{ + assert(dependent); + assert(dependency); + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + rclpy_handle_t ** new_dependencies = allocator.reallocate( + dependent->dependencies, + (dependent->num_of_dependencies + 1u) * sizeof(rclpy_handle_t *), + allocator.state); + if (!new_dependencies) { + PyErr_Format( + PyExc_RuntimeError, + "Failed to add dependency to handle"); + return; + } + new_dependencies[dependent->num_of_dependencies] = dependency; + dependent->num_of_dependencies++; + dependent->dependencies = new_dependencies; + dependency->ref_count++; +} + +/// Decrements the reference count of a handle. +/** + * The reference count of `handle` is decremented. + * If it reaches zero: + * - `rclpy_handle_dec_ref` is called on `handle` dependencies. + * - `handle` is deallocated. + */ +void +_rclpy_handle_dec_ref(rclpy_handle_t * handle) +{ + if (!handle) { + return; + } + assert( + (0u != handle->num_of_dependencies && NULL != handle->dependencies) || + (0u == handle->num_of_dependencies && NULL == handle->dependencies)); + + if (!--handle->ref_count) { + if (handle->destructor) { + handle->destructor(handle->ptr); + } + for (size_t i = 0; i < handle->num_of_dependencies; i++) { + _rclpy_handle_dec_ref(handle->dependencies[i]); + } + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + allocator.deallocate(handle->dependencies, allocator.state); + allocator.deallocate(handle, allocator.state); + } +} + +void +_rclpy_handle_capsule_destructor(PyObject * capsule) +{ + rclpy_handle_t * handle = PyCapsule_GetPointer(capsule, PyCapsule_GetName(capsule)); + if (!handle) { + return; + } + _rclpy_handle_dec_ref(handle); +} + +/// Creates a PyCapsule wrapping a handle object. +PyObject * +_rclpy_create_handle_capsule(rclpy_handle_t * ptr, const char * name) +{ + return PyCapsule_New(ptr, name, _rclpy_handle_capsule_destructor); +} + +/// Creates a PyCapsule wrapping a handle object. +PyObject * +rclpy_create_handle_capsule(void * ptr, const char * name, rclpy_handle_destructor_t destructor) +{ + rclpy_handle_t * handle = _rclpy_create_handle(ptr, destructor); + if (!handle) { + return NULL; + } + return PyCapsule_New(handle, name, _rclpy_handle_capsule_destructor); +} + +void * +_rclpy_handle_get_pointer(rclpy_handle_t * handle) +{ + if (!handle || !handle->ptr) { + PyErr_Format( + PyExc_RuntimeError, + "Failed to get pointer from handle"); + return NULL; + } + return handle->ptr; +} + +void * +rclpy_handle_get_pointer_from_capsule(PyObject * capsule, const char * name) +{ + rclpy_handle_t * handle = PyCapsule_GetPointer(capsule, name); + return _rclpy_handle_get_pointer(handle); +} diff --git a/rclpy/src/test/rclpy_common/test_handle.cpp b/rclpy/src/test/rclpy_common/test_handle.cpp new file mode 100644 index 000000000..2d2b33475 --- /dev/null +++ b/rclpy/src/test/rclpy_common/test_handle.cpp @@ -0,0 +1,95 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rclpy_common/handle.h" + +void int_destructor(void * p) +{ + *reinterpret_cast(p) += 1; +} + +TEST(test_handle, basic_usage) { + auto test_obj = std::make_unique(0); + rclpy_handle_t * handle = + _rclpy_create_handle(test_obj.get(), int_destructor); + ASSERT_NE(nullptr, handle); + ASSERT_EQ(test_obj.get(), _rclpy_handle_get_pointer(handle)); + EXPECT_EQ(0, *test_obj); + _rclpy_handle_dec_ref(handle); + EXPECT_EQ(1, *test_obj); +} + +TEST(test_handle, destroy_dependency_first) { + // Construct two handles. + auto dependent_obj = std::make_unique(0); + auto dependency_obj = std::make_unique(0); + rclpy_handle_t * dependent_handle = + _rclpy_create_handle(dependent_obj.get(), int_destructor); + rclpy_handle_t * dependency_handle = + _rclpy_create_handle(dependency_obj.get(), int_destructor); + ASSERT_NE(nullptr, dependent_handle); + ASSERT_EQ(dependent_obj.get(), _rclpy_handle_get_pointer(dependent_handle)); + ASSERT_NE(nullptr, dependency_handle); + ASSERT_EQ(dependency_obj.get(), _rclpy_handle_get_pointer(dependency_handle)); + EXPECT_EQ(0, *dependent_obj); + EXPECT_EQ(0, *dependency_obj); + + // Add dependency. + _rclpy_handle_add_dependency(dependent_handle, dependency_handle); + + // Decrement reference of dependency, nothing is destructed. + _rclpy_handle_dec_ref(dependency_handle); + ASSERT_EQ(dependent_obj.get(), _rclpy_handle_get_pointer(dependent_handle)); + ASSERT_EQ(dependency_obj.get(), _rclpy_handle_get_pointer(dependency_handle)); + EXPECT_EQ(0, *dependent_obj); + EXPECT_EQ(0, *dependency_obj); + + // Decrement reference of dependent, both are destructed. + _rclpy_handle_dec_ref(dependent_handle); + EXPECT_EQ(1, *dependent_obj); + EXPECT_EQ(1, *dependency_obj); +} + +TEST(test_handle, destroy_dependent_first) { + // Construct two handles. + auto dependent_obj = std::make_unique(0); + auto dependency_obj = std::make_unique(0); + rclpy_handle_t * dependent_handle = + _rclpy_create_handle(dependent_obj.get(), int_destructor); + rclpy_handle_t * dependency_handle = + _rclpy_create_handle(dependency_obj.get(), int_destructor); + ASSERT_NE(nullptr, dependent_handle); + ASSERT_EQ(dependent_obj.get(), _rclpy_handle_get_pointer(dependent_handle)); + ASSERT_NE(nullptr, dependency_handle); + ASSERT_EQ(dependency_obj.get(), _rclpy_handle_get_pointer(dependency_handle)); + EXPECT_EQ(0, *dependent_obj); + EXPECT_EQ(0, *dependency_obj); + + // Add dependency. + _rclpy_handle_add_dependency(dependent_handle, dependency_handle); + + // Decrement reference of dependent, destructed right away. + _rclpy_handle_dec_ref(dependent_handle); + ASSERT_EQ(dependency_obj.get(), _rclpy_handle_get_pointer(dependency_handle)); + EXPECT_EQ(1, *dependent_obj); + EXPECT_EQ(0, *dependency_obj); + + // Decrement reference of dependency, destructed right away. + _rclpy_handle_dec_ref(dependency_handle); + EXPECT_EQ(1, *dependent_obj); + EXPECT_EQ(1, *dependency_obj); +} diff --git a/rclpy/test/test_destruction_order.py b/rclpy/test/test_destruction_order.py new file mode 100644 index 000000000..f34cb16df --- /dev/null +++ b/rclpy/test/test_destruction_order.py @@ -0,0 +1,32 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node + + +def test_destruction_order(): + context = rclpy.context.Context() + rclpy.init(context=context) + node1 = Node('test_destruction_order_node_1', context=context) + node2 = Node('test_destruction_order_node_2', context=context) + node3 = Node('test_destruction_order_node_3', context=context) + + node1.cyclic_ref = node1 + node2.cyclic_ref = node2 + node1.node3 = node3 + node2.node3 = node3 + + node1.handle.context_handle = context.handle + node2.handle.context_handle = context.handle diff --git a/rclpy/test/test_handle.py b/rclpy/test/test_handle.py index c530cdd4c..6c9cab2e8 100644 --- a/rclpy/test/test_handle.py +++ b/rclpy/test/test_handle.py @@ -47,52 +47,3 @@ def test_handle_destroyed_when_not_used(): pass finally: rclpy.shutdown(context=context) - - -def test_handle_destroys_dependents(): - context = rclpy.context.Context() - rclpy.init(context=context) - - try: - node1 = rclpy.create_node('test_handle_destroys_dependents1', context=context) - node2 = rclpy.create_node('test_handle_destroys_dependents2', context=context) - node2.handle.requires(node1.handle) - - with node1.handle: - pass - with node2.handle: - pass - - node1.handle.destroy() - with pytest.raises(InvalidHandle): - with node1.handle: - pass - with pytest.raises(InvalidHandle): - with node2.handle: - pass - finally: - rclpy.shutdown(context=context) - - -def test_handle_does_not_destroy_requirements(): - context = rclpy.context.Context() - rclpy.init(context=context) - - try: - node1 = rclpy.create_node('test_handle_does_not_destroy_requirements1', context=context) - node2 = rclpy.create_node('test_handle_does_not_destroy_requirements2', context=context) - node2.handle.requires(node1.handle) - - with node1.handle: - pass - with node2.handle: - pass - - node2.handle.destroy() - with pytest.raises(InvalidHandle): - with node2.handle: - pass - with node1.handle: - pass - finally: - rclpy.shutdown(context=context) diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py index 88ad7c1be..d9294d0a1 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -136,7 +136,8 @@ def test_call_publisher_rclpy_event_apis(self): # 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) + with self.context.handle as context_handle: + _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 2, context_handle) deadline_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) @@ -189,7 +190,8 @@ def test_call_subscription_rclpy_event_apis(self): # 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) + with self.context.handle as context_handle: + _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 2, context_handle) deadline_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 0afbc8f05..48f80575b 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -129,9 +129,9 @@ def __init__(self, node): self._clock = Clock(clock_type=ClockType.STEADY_TIME) period_nanoseconds = 10000 - with self._clock.handle as clock_capsule: + with self._clock.handle as clock_capsule, node.context.handle as context_capsule: self.timer = _rclpy.rclpy_create_timer( - clock_capsule, node.context.handle, period_nanoseconds) + clock_capsule, context_capsule, period_nanoseconds) self.timer_index = None self.timer_is_ready = False @@ -217,7 +217,8 @@ 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 context_capsule: + self.guard_condition = _rclpy.rclpy_create_guard_condition(context_capsule) self.guard_condition_index = None self.guard_is_ready = False