Fix thread safety issues related to logging

rclcpp/CMakeLists.txt

@@ -54,6 +54,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/init_options.cpp
   src/rclcpp/intra_process_manager.cpp
   src/rclcpp/logger.cpp
+  src/rclcpp/logging_mutex.cpp
   src/rclcpp/memory_strategies.cpp
   src/rclcpp/memory_strategy.cpp
   src/rclcpp/message_info.cpp

rclcpp/include/rclcpp/context.hpp

@@ -338,8 +338,8 @@ 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_;
+  // Keep shared ownership of the global logging mutex.
+  std::shared_ptr<std::recursive_mutex> logging_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

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -150,6 +150,9 @@ class NodeBase : public NodeBaseInterface
   mutable std::recursive_mutex notify_guard_condition_mutex_;
   rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
   bool notify_guard_condition_is_valid_;
+
+  // Keep shared ownership of the global logging mutex.
+  std::shared_ptr<std::recursive_mutex> logging_mutex_;
 };
 
 }  // namespace node_interfaces

rclcpp/src/rclcpp/context.cpp

@@ -1,4 +1,4 @@
-// Copyright 2015 Open Source Robotics Foundation, Inc.
+// Copyright 2015-2020 Open Source Robotics Foundation, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -23,25 +23,46 @@
 
 #include "rcl/init.h"
 #include "rcl/logging.h"
+
 #include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logging.hpp"
+
+#include "rcutils/error_handling.h"
+#include "rcutils/macros.h"
+
 #include "rmw/impl/cpp/demangle.hpp"
 
+#include "./logging_mutex.hpp"
+
 /// Mutex to protect initialized contexts.
 static std::mutex g_contexts_mutex;
 /// Weak list of context to be shutdown by the signal handler.
 static std::vector<std::weak_ptr<rclcpp::Context>> g_contexts;
 
 using rclcpp::Context;
 
-static
-std::shared_ptr<std::mutex>
-get_global_logging_configure_mutex()
+extern "C"
 {
-  static auto mutex = std::make_shared<std::mutex>();
-  return mutex;
+void
+rclcpp_logging_output_handler(
+  const rcutils_log_location_t * location,
+  int severity, const char * name, rcutils_time_point_value_t timestamp,
+  const char * format, va_list * args)
+{
+  std::shared_ptr<std::recursive_mutex> logging_mutex = nullptr;
+  try {
+    logging_mutex = get_global_logging_mutex();
+  } catch (std::exception & ex) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR(ex.what());
+    return;
+  }
+
+  std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
+  return rcl_logging_multiple_output_handler(
+    location, severity, name, timestamp, format, args);
 }
+}  // extern "C"
 
 static
 size_t &
@@ -54,7 +75,7 @@ get_logging_reference_count()
 Context::Context()
 : rcl_context_(nullptr),
   shutdown_reason_(""),
-  logging_configure_mutex_(nullptr)
+  logging_mutex_(nullptr)
 {}
 
 Context::~Context()
@@ -116,16 +137,14 @@ Context::init(
   }
 
   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_);
+    logging_mutex_ = get_global_logging_mutex();
+    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
     size_t & count = get_logging_reference_count();
     if (0u == count) {
-      ret = rcl_logging_configure(
+      ret = rcl_logging_configure_with_output_handler(
         &rcl_context_->global_arguments,
-        rcl_init_options_get_allocator(init_options_.get_rcl_init_options()));
+        rcl_init_options_get_allocator(init_options_.get_rcl_init_options()),
+        rclcpp_logging_output_handler);
       if (RCL_RET_OK != ret) {
         rcl_context_.reset();
         rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging");
@@ -228,9 +247,9 @@ Context::shutdown(const std::string & reason)
     }
   }
   // shutdown logger
-  if (logging_configure_mutex_) {
+  if (logging_mutex_) {
     // logging was initialized by this context
-    std::lock_guard<std::mutex> guard(*logging_configure_mutex_);
+    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
     size_t & count = get_logging_reference_count();
     if (0u == --count) {
       rcl_ret_t rcl_ret = rcl_logging_fini();

rclcpp/src/rclcpp/logging_mutex.cpp

@@ -0,0 +1,28 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <mutex>
+
+#include "rcutils/macros.h"
+
+std::shared_ptr<std::recursive_mutex>
+get_global_logging_mutex()
+{
+  static auto mutex = std::make_shared<std::recursive_mutex>();
+  if (RCUTILS_UNLIKELY(!mutex)) {
+    throw std::runtime_error("rclcpp global logging mutex is a nullptr");
+  }
+  return mutex;
+}

rclcpp/src/rclcpp/logging_mutex.hpp

@@ -0,0 +1,36 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__LOGGING_MUTEX_HPP_
+#define RCLCPP__LOGGING_MUTEX_HPP_
+
+#include <memory>
+#include <mutex>
+
+/// Global logging mutex
+/**
+ * This mutex is locked in the following situations:
+ *   - In initialization/destruction of contexts.
+ *   - In initialization/destruction of nodes.
+ *   - In the rcl logging output handler installed by rclcpp,
+ *     i.e.: in all calls to the logger macros, including RCUTILS_* ones.
+ */
+// Implementation detail:
+// A shared pointer to the mutex is used, so that objects that need to use
+// it at destruction time can hold it alive.
+// In that way, a destruction ordering problem between static objects is avoided.
+std::shared_ptr<std::recursive_mutex>
+get_global_logging_mutex();
+
+#endif  // RCLCPP__LOGGING_MUTEX_HPP_

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -25,6 +25,8 @@
 #include "rmw/validate_namespace.h"
 #include "rmw/validate_node_name.h"
 
+#include "../logging_mutex.hpp"
+
 using rclcpp::exceptions::throw_from_rcl_error;
 
 using rclcpp::node_interfaces::NodeBase;
@@ -65,10 +67,17 @@ NodeBase::NodeBase(
   // Create the rcl node and store it in a shared_ptr with a custom destructor.
   std::unique_ptr<rcl_node_t> rcl_node(new rcl_node_t(rcl_get_zero_initialized_node()));
 
-  ret = rcl_node_init(
-    rcl_node.get(),
-    node_name.c_str(), namespace_.c_str(),
-    context_->get_rcl_context().get(), &rcl_node_options);
+  logging_mutex_ = get_global_logging_mutex();
+  {
+    std::lock_guard<std::recursive_mutex> guard(*logging_mutex_);
+    // TODO(ivanpauno): Instead of mutually excluding rcl_node_init with the global logger mutex,
+    // rcl_logging_rosout_init_publisher_for_node could be decoupled from there and be called
+    // here directly.
+    ret = rcl_node_init(
+      rcl_node.get(),
+      node_name.c_str(), namespace_.c_str(),
+      context_->get_rcl_context().get(), &rcl_node_options);
+  }
   if (ret != RCL_RET_OK) {
     // Finalize the interrupt guard condition.
     finalize_notify_guard_condition();
@@ -123,7 +132,11 @@ NodeBase::NodeBase(
 
   node_handle_.reset(
     rcl_node.release(),
-    [](rcl_node_t * node) -> void {
+    [logging_mutex = logging_mutex_](rcl_node_t * node) -> void {
+      std::lock_guard<std::recursive_mutex> guard(*logging_mutex);
+      // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex,
+      // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called
+      // here directly.
       if (rcl_node_fini(node) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",