From 80e8dcad026d0bd7da1b584ad8b1f5a7941bd1a8 Mon Sep 17 00:00:00 2001
From: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
Date: Thu, 23 Apr 2020 17:33:56 -0300
Subject: [PATCH] Ensure logging is initialized just once (#998)

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
---
 rclcpp/include/rclcpp/context.hpp      |  3 ++
 rclcpp/include/rclcpp/init_options.hpp | 11 +++++
 rclcpp/src/rclcpp/context.cpp          | 62 +++++++++++++++++++++++++-
 rclcpp/src/rclcpp/init_options.cpp     | 13 ++++++
 4 files changed, 88 insertions(+), 1 deletion(-)

diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp
index 76d3cb1..5d7a6bc 100644
--- a/rclcpp/include/rclcpp/context.hpp
+++ b/rclcpp/include/rclcpp/context.hpp
@@ -338,6 +338,9 @@ private:
   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/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp
index fd0e4f8..7b05ee8 100644
--- a/rclcpp/include/rclcpp/init_options.hpp
+++ b/rclcpp/include/rclcpp/init_options.hpp
@@ -42,6 +42,16 @@ public:
   RCLCPP_PUBLIC
   InitOptions(const InitOptions & other);
 
+  /// Return `true` if logging should be initialized when `rclcpp::Context::init` is called.
+  RCLCPP_PUBLIC
+  bool
+  auto_initialize_logging() const;
+
+  /// Set flag indicating if logging should be initialized or not.
+  RCLCPP_PUBLIC
+  InitOptions &
+  auto_initialize_logging(bool initialize_logging);
+
   /// Assignment operator.
   RCLCPP_PUBLIC
   InitOptions &
@@ -62,6 +72,7 @@ protected:
 
 private:
   std::unique_ptr<rcl_init_options_t> init_options_;
+  bool initialize_logging_{true};
 };
 
 }  // namespace rclcpp
diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp
index 57937d3..1918a77 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,27 @@ 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_(nullptr)
+{}
 
 Context::~Context()
 {
@@ -94,6 +114,30 @@ Context::init(
     rcl_context_.reset();
     rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
   }
+
+  if (init_options.auto_initialize_logging()) {
+    logging_configure_mutex_ = get_global_logging_configure_mutex();
+    if (!logging_configure_mutex_) {
+      throw std::runtime_error("global logging configure mutex is 'nullptr'");
+    }
+    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");
+      }
+    } else {
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "logging was initialized more than once");
+    }
+    ++count;
+  }
+
   try {
     std::vector<std::string> unparsed_ros_arguments = detail::get_unparsed_ros_arguments(
       argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator());
@@ -183,6 +227,22 @@ Context::shutdown(const std::string & reason)
       ++it;
     }
   }
+  // shutdown logger
+  if (logging_configure_mutex_) {
+    // logging was initialized by this context
+    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();
+      }
+    }
+  }
   return true;
 }
 
diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp
index ba46edb..ad2a900 100644
--- a/rclcpp/src/rclcpp/init_options.cpp
+++ b/rclcpp/src/rclcpp/init_options.cpp
@@ -46,6 +46,19 @@ InitOptions::InitOptions(const InitOptions & other)
   shutdown_on_sigint = other.shutdown_on_sigint;
 }
 
+bool
+InitOptions::auto_initialize_logging() const
+{
+  return initialize_logging_;
+}
+
+InitOptions &
+InitOptions::auto_initialize_logging(bool initialize_logging)
+{
+  initialize_logging_ = initialize_logging;
+  return *this;
+}
+
 InitOptions &
 InitOptions::operator=(const InitOptions & other)
 {