diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index c5c056e64..f72116597 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -20,6 +20,10 @@ import weakref +g_logging_configure_lock = threading.Lock() +g_logging_ref_count = 0 + + class Context: """ Encapsulates the lifecycle of init and shutdown. @@ -36,12 +40,13 @@ def __init__(self): self._lock = threading.Lock() self._callbacks = [] self._callbacks_lock = threading.Lock() + self._logging_initialized = False @property def handle(self): return self._handle - def init(self, args: Optional[List[str]] = None): + def init(self, args: Optional[List[str]] = None, *, initialize_logging: bool = True): """ Initialize ROS communications for a given context. @@ -49,8 +54,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 + global g_logging_ref_count with self._handle as capsule, self._lock: rclpy_implementation.rclpy_init(args if args is not None else sys.argv, capsule) + if initialize_logging and not self._logging_initialized: + with g_logging_configure_lock: + g_logging_ref_count += 1 + if g_logging_ref_count == 1: + rclpy_implementation.rclpy_logging_configure(capsule) + self._logging_initialized = True def ok(self): """Check if context hasn't been shut down.""" @@ -73,6 +85,7 @@ def shutdown(self): with self._handle as capsule, self._lock: rclpy_implementation.rclpy_shutdown(capsule) self._call_on_shutdown_callbacks() + self._logging_fini() def try_shutdown(self): """Shutdown this context, if not already shutdown.""" @@ -95,3 +108,17 @@ def on_shutdown(self, callback: Callable[[], None]): callback() else: self._callbacks.append(weakref.WeakMethod(callback, self._remove_callback)) + + def _logging_fini(self): + from rclpy.impl.implementation_singleton import rclpy_implementation + global g_logging_ref_count + with self._lock: + if self._logging_initialized: + with g_logging_configure_lock: + g_logging_ref_count -= 1 + if g_logging_ref_count == 0: + rclpy_implementation.rclpy_logging_fini() + if g_logging_ref_count < 0: + raise RuntimeError( + 'Unexpected error: logger ref count should never be lower that zero') + self._logging_initialized = False diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 355301b90..ae693ffe4 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -634,6 +635,55 @@ rclpy_init(PyObject * Py_UNUSED(self), PyObject * args) Py_RETURN_NONE; } +/// Initialize rcl logging +/** + * Raises RuntimeError if rcl logging could not be initialized + */ +static PyObject * +rclpy_logging_configure(PyObject * Py_UNUSED(self), PyObject * args) +{ + // Expect one argument, a context. + PyObject * pycontext; + if (!PyArg_ParseTuple(args, "O", &pycontext)) { + // Exception raised + return NULL; + } + rcl_context_t * context = rclpy_handle_get_pointer_from_capsule(pycontext, "rcl_context_t"); + if (!context) { + return NULL; + } + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_logging_configure( + &context->global_arguments, &allocator); + if (RCL_RET_OK != ret) { + PyErr_Format( + RCLError, + "Failed to initialize logging: %s", rcl_get_error_string().str); + return NULL; + } + Py_RETURN_NONE; +} + +/// Finalize rcl logging +/** + * Raises RuntimeError if rcl logging could not be finalized + */ +static PyObject * +rclpy_logging_fini(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args)) +{ + rcl_ret_t ret = rcl_logging_fini(); + if (RCL_RET_OK != ret) { + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, + stack_level, + "Failed to fini logging: %s", + rcl_get_error_string().str); + return NULL; + } + Py_RETURN_NONE; +} + /// Handle destructor for node static void _rclpy_destroy_node(void * p) @@ -5257,6 +5307,14 @@ static PyMethodDef rclpy_methods[] = { "rclpy_init", rclpy_init, METH_VARARGS, "Initialize RCL." }, + { + "rclpy_logging_configure", rclpy_logging_configure, METH_VARARGS, + "Initialize RCL logging." + }, + { + "rclpy_logging_fini", rclpy_logging_fini, METH_NOARGS, + "Finalize RCL logging." + }, { "rclpy_remove_ros_args", rclpy_remove_ros_args, METH_VARARGS, "Remove ROS-specific arguments from argument vector."