Ensure logging is initialized just once (ros2#998)

Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>

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.

rclcpp/include/rclcpp/init_options.hpp

@@ -42,6 +42,16 @@ class InitOptions
   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 @@ class InitOptions
 
 private:
   std::unique_ptr<rcl_init_options_t> init_options_;
+  bool initialize_logging_{true};
 };
 
 }  // namespace rclcpp

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;
 }
 

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)
 {