diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 7e5702cd79..171e0f875d 100644 --- a/rclcpp/CMakeLists.txt +++ b/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 diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 5d7a6bcecf..61eb27ac88 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -338,8 +338,8 @@ class Context : public std::enable_shared_from_this rclcpp::InitOptions init_options_; std::string shutdown_reason_; - // Keep shared ownership of global logging configure mutex. - std::shared_ptr logging_configure_mutex_; + // Keep shared ownership of the global logging mutex. + std::shared_ptr logging_mutex_; std::unordered_map> sub_contexts_; // This mutex is recursive so that the constructor of a sub context may diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 1918a77a02..ea1c3ec95a 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/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,27 +23,24 @@ #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> g_contexts; -using rclcpp::Context; - -static -std::shared_ptr -get_global_logging_configure_mutex() -{ - static auto mutex = std::make_shared(); - return mutex; -} - -static +/// Count of contexts that wanted to initialize the logging system. size_t & get_logging_reference_count() { @@ -51,10 +48,36 @@ get_logging_reference_count() return ref_count; } +using rclcpp::Context; + +extern "C" +{ +static +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) +{ + try { + std::shared_ptr logging_mutex; + logging_mutex = get_global_logging_mutex(); + std::lock_guard guard(*logging_mutex); + return rcl_logging_multiple_output_handler( + location, severity, name, timestamp, format, args); + } catch (std::exception & ex) { + RCUTILS_SAFE_FWRITE_TO_STDERR(ex.what()); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } catch (...) { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to take global rclcpp logging mutex\n"); + } +} +} // extern "C" + Context::Context() : rcl_context_(nullptr), shutdown_reason_(""), - logging_configure_mutex_(nullptr) + logging_mutex_(nullptr) {} Context::~Context() @@ -116,16 +139,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 guard(*logging_configure_mutex_); + logging_mutex_ = get_global_logging_mutex(); + std::lock_guard 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 +249,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 guard(*logging_configure_mutex_); + std::lock_guard guard(*logging_mutex_); size_t & count = get_logging_reference_count(); if (0u == --count) { rcl_ret_t rcl_ret = rcl_logging_fini(); diff --git a/rclcpp/src/rclcpp/logging_mutex.cpp b/rclcpp/src/rclcpp/logging_mutex.cpp new file mode 100644 index 0000000000..5d189c4918 --- /dev/null +++ b/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 +#include + +#include "rcutils/macros.h" + +std::shared_ptr +get_global_logging_mutex() +{ + static auto mutex = std::make_shared(); + if (RCUTILS_UNLIKELY(!mutex)) { + throw std::runtime_error("rclcpp global logging mutex is a nullptr"); + } + return mutex; +} diff --git a/rclcpp/src/rclcpp/logging_mutex.hpp b/rclcpp/src/rclcpp/logging_mutex.hpp new file mode 100644 index 0000000000..0e7be39237 --- /dev/null +++ b/rclcpp/src/rclcpp/logging_mutex.hpp @@ -0,0 +1,39 @@ +// 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 +#include + +#include "rclcpp/visibility_control.hpp" + +/// 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. +RCLCPP_LOCAL +std::shared_ptr +get_global_logging_mutex(); + +#endif // RCLCPP__LOGGING_MUTEX_HPP_ diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index f055abaa25..d8c13bd826 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/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(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); + std::shared_ptr logging_mutex = get_global_logging_mutex(); + { + std::lock_guard 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](rcl_node_t * node) -> void { + std::lock_guard 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",