From 084bdff575240f22ce4365a47b0c92c5e54b9e30 Mon Sep 17 00:00:00 2001
From: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
Date: Thu, 20 Feb 2020 16:54:32 -0300
Subject: [PATCH] Ensure logging is initialized just once

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
---
 rclcpp/include/rclcpp/context.hpp |  3 ++
 rclcpp/src/rclcpp/context.cpp     | 56 ++++++++++++++++++++++++++++++-
 2 files changed, 58 insertions(+), 1 deletion(-)

diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp
index dfc33d5e4f..d59f5755c0 100644
--- a/rclcpp/include/rclcpp/context.hpp
+++ b/rclcpp/include/rclcpp/context.hpp
@@ -338,6 +338,9 @@ class Context : public std::enable_shared_from_this<Context>
   rclcpp::InitOptions init_options_;
   std::string shutdown_reason_;
 
+  // Keep shared ownership of global logging configure mutex.
+  std::shared_ptr<std::mutex> logging_configure_mutex_;
+
   std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
   // This mutex is recursive so that the constructor of a sub context may
   // attempt to acquire another sub context.
diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp
index 57937d35fb..5c1ab00950 100644
--- a/rclcpp/src/rclcpp/context.cpp
+++ b/rclcpp/src/rclcpp/context.cpp
@@ -22,6 +22,7 @@
 #include <utility>
 
 #include "rcl/init.h"
+#include "rcl/logging.h"
 #include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logging.hpp"
@@ -34,8 +35,31 @@ static std::vector<std::weak_ptr<rclcpp::Context>> g_contexts;
 
 using rclcpp::Context;
 
+static
+std::shared_ptr<std::mutex>
+get_global_logging_configure_mutex()
+{
+  static auto mutex = std::make_shared<std::mutex>();
+  return mutex;
+}
+
+static
+size_t &
+get_logging_reference_count()
+{
+  static size_t ref_count = 0;
+  return ref_count;
+}
+
 Context::Context()
-: rcl_context_(nullptr), shutdown_reason_("") {}
+: rcl_context_(nullptr),
+  shutdown_reason_(""),
+  logging_configure_mutex_(get_global_logging_configure_mutex())
+{
+  if (!logging_configure_mutex_) {
+    throw std::runtime_error("global logging configure mutex is 'nullptr'");
+  }
+}
 
 Context::~Context()
 {
@@ -94,6 +118,22 @@ Context::init(
     rcl_context_.reset();
     rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
   }
+
+  {
+    std::lock_guard<std::mutex> guard(*logging_configure_mutex_);
+    size_t & count = get_logging_reference_count();
+    if (0u == count) {
+      ret = rcl_logging_configure(
+        &rcl_context_->global_arguments,
+        rcl_init_options_get_allocator(init_options_.get_rcl_init_options()));
+      if (RCL_RET_OK != ret) {
+        rcl_context_.reset();
+        rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging");
+      }
+    }
+    ++count;
+  }
+
   try {
     std::vector<std::string> unparsed_ros_arguments = detail::get_unparsed_ros_arguments(
       argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator());
@@ -309,6 +349,20 @@ Context::clean_up()
 {
   shutdown_reason_ = "";
   rcl_context_.reset();
+  {
+    std::lock_guard<std::mutex> guard(*logging_configure_mutex_);
+    size_t & count = get_logging_reference_count();
+    if (0u == --count) {
+      rcl_ret_t rcl_ret = rcl_logging_fini();
+      if (RCL_RET_OK != rcl_ret) {
+        RCUTILS_SAFE_FWRITE_TO_STDERR(
+          RCUTILS_STRINGIFY(__file__) ":"
+          RCUTILS_STRINGIFY(__LINE__)
+          " failed to fini logging");
+        rcl_reset_error();
+      }
+    }
+  }
   sub_contexts_.clear();
 }