From 629a22c542f451d597899c6054601cffe1e2e1b4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 3 Mar 2020 16:48:53 -0300 Subject: [PATCH 1/4] Ensure logging is initialized only once Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/context.py | 23 +++++++++++++++- rclpy/src/rclpy/_rclpy.c | 59 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 81 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index c5c056e64..9e4d0eaba 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: + self._logging_initialized = True + with g_logging_configure_lock: + g_logging_ref_count += 1 + if g_logging_ref_count == 1: + rclpy_implementation.rclpy_logging_configure(self._handle) def ok(self): """Check if context hasn't been shut down.""" @@ -95,3 +107,12 @@ def on_shutdown(self, callback: Callable[[], None]): callback() else: self._callbacks.append(weakref.WeakMethod(callback, self._remove_callback)) + + def __del__(self): + from rclpy.impl.implementation_singleton import rclpy_implementation + global g_logging_ref_count + 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() diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 355301b90..6134fe9e3 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,56 @@ 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 +5308,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." From da86a77a40fc8556f8f70b9ead0f4d8db3f9ac91 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 5 Mar 2020 11:32:20 -0300 Subject: [PATCH 2/4] Correction after rebasing Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/context.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index 9e4d0eaba..bfbf566c4 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -62,7 +62,7 @@ def init(self, args: Optional[List[str]] = None, *, initialize_logging: bool = T with g_logging_configure_lock: g_logging_ref_count += 1 if g_logging_ref_count == 1: - rclpy_implementation.rclpy_logging_configure(self._handle) + rclpy_implementation.rclpy_logging_configure(capsule) def ok(self): """Check if context hasn't been shut down.""" From 6247d0cb9ecbf19ecff7aa951c988fedabf6e0e2 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 21 Apr 2020 13:29:40 -0300 Subject: [PATCH 3/4] Fini logging when shutdown is called Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/context.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index bfbf566c4..eaf867b12 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -85,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.""" @@ -108,7 +109,7 @@ def on_shutdown(self, callback: Callable[[], None]): else: self._callbacks.append(weakref.WeakMethod(callback, self._remove_callback)) - def __del__(self): + def _logging_fini(self): from rclpy.impl.implementation_singleton import rclpy_implementation global g_logging_ref_count if self._logging_initialized: From a5814ed5b7c5788ecd6e77aa11fa36580ed8efaa Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 23 Apr 2020 13:53:46 -0300 Subject: [PATCH 4/4] Address peer review comments Signed-off-by: Ivan Santiago Paunovic --- rclpy/rclpy/context.py | 19 ++++++++++++------- rclpy/src/rclpy/_rclpy.c | 3 +-- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index eaf867b12..f72116597 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -57,12 +57,12 @@ def init(self, args: Optional[List[str]] = None, *, initialize_logging: bool = T 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: - self._logging_initialized = True + 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.""" @@ -112,8 +112,13 @@ def on_shutdown(self, callback: Callable[[], None]): def _logging_fini(self): from rclpy.impl.implementation_singleton import rclpy_implementation global g_logging_ref_count - 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() + 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 6134fe9e3..ae693ffe4 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -654,8 +654,7 @@ rclpy_logging_configure(PyObject * Py_UNUSED(self), PyObject * args) } rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_logging_configure( - &context->global_arguments, - &allocator); + &context->global_arguments, &allocator); if (RCL_RET_OK != ret) { PyErr_Format( RCLError,