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