From fe68a6ccfbaf5b57c61a00a79c25f4c06188e1ae Mon Sep 17 00:00:00 2001
From: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
Date: Thu, 23 Apr 2020 17:34:11 -0300
Subject: [PATCH] Ensure logging is initialized only once (#518)

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
---
 rclpy/rclpy/context.py   | 29 +++++++++++++++++++-
 rclpy/src/rclpy/_rclpy.c | 58 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 86 insertions(+), 1 deletion(-)

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 <rcl/error_handling.h>
 #include <rcl/expand_topic_name.h>
 #include <rcl/graph.h>
+#include <rcl/logging.h>
 #include <rcl/node.h>
 #include <rcl/publisher.h>
 #include <rcl/rcl.h>
@@ -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."