From 699bf4b022cd9199af88218be17c9ca8c31442a4 Mon Sep 17 00:00:00 2001
From: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
Date: Tue, 3 Mar 2020 16:48:53 -0300
Subject: [PATCH] Ensure logging is initialized only once

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
---
 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 22defea3b..c8e01f170 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.
@@ -35,12 +39,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.
 
@@ -48,9 +53,16 @@ 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._lock:
             rclpy_implementation.rclpy_init(
                 args if args is not None else sys.argv, self._handle)
+            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 b42ef4ec3..ba51de31b 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>
@@ -618,6 +619,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 = (rcl_context_t *)PyCapsule_GetPointer(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;
+}
+
 /// PyCapsule destructor for node
 static void
 _rclpy_destroy_node(PyObject * pyentity)
@@ -5016,6 +5067,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."