From de48cbe5e5bf16bbbbca5d568aed69655a9a7f11 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 21 May 2020 18:05:59 -0300 Subject: [PATCH 1/7] Fix thread safety issues related to logging Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/context.hpp | 4 +- .../rclcpp/node_interfaces/node_base.hpp | 3 ++ rclcpp/src/rclcpp/context.cpp | 51 +++++++++++++------ rclcpp/src/rclcpp/logging_mutex.cpp | 28 ++++++++++ rclcpp/src/rclcpp/logging_mutex.hpp | 36 +++++++++++++ .../src/rclcpp/node_interfaces/node_base.cpp | 23 +++++++-- 7 files changed, 123 insertions(+), 23 deletions(-) create mode 100644 rclcpp/src/rclcpp/logging_mutex.cpp create mode 100644 rclcpp/src/rclcpp/logging_mutex.hpp 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/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 0e124f6f56..63b25cc71e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/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 logging_mutex_; }; } // namespace node_interfaces diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 1918a77a02..0529a2304d 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,11 +23,18 @@ #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. @@ -35,13 +42,27 @@ static std::vector> g_contexts; using rclcpp::Context; -static -std::shared_ptr -get_global_logging_configure_mutex() +extern "C" { - static auto mutex = std::make_shared(); - 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 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 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 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 +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 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..e0e7fbec8f --- /dev/null +++ b/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 +#include + +/// 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 +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..e91d69b99e 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); + 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 = 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", From b18fca7446db41736274293b8a37c2038105ffbf Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 22 May 2020 10:09:56 -0300 Subject: [PATCH 2/7] Use LOCAL visibility in functions not intended to be public Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/logging_mutex.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/src/rclcpp/logging_mutex.hpp b/rclcpp/src/rclcpp/logging_mutex.hpp index e0e7fbec8f..0e7be39237 100644 --- a/rclcpp/src/rclcpp/logging_mutex.hpp +++ b/rclcpp/src/rclcpp/logging_mutex.hpp @@ -18,6 +18,8 @@ #include #include +#include "rclcpp/visibility_control.hpp" + /// Global logging mutex /** * This mutex is locked in the following situations: @@ -30,6 +32,7 @@ // 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(); From bfc98d0e0be5fc89b3bc15be01885e4434f4fc48 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 22 May 2020 10:12:44 -0300 Subject: [PATCH 3/7] Use static qualifier in functions only used within a file Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/context.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 0529a2304d..e65b89c677 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -44,6 +44,7 @@ using rclcpp::Context; extern "C" { +static void rclcpp_logging_output_handler( const rcutils_log_location_t * location, From bdb1ae96b3f961e68a5438e22a03569f02d04ebf Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 22 May 2020 10:16:12 -0300 Subject: [PATCH 4/7] Delete unnecessary member variable Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/node_interfaces/node_base.hpp | 3 --- rclcpp/src/rclcpp/node_interfaces/node_base.cpp | 6 +++--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 63b25cc71e..0e124f6f56 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -150,9 +150,6 @@ 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 logging_mutex_; }; } // namespace node_interfaces diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index e91d69b99e..d8c13bd826 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -67,9 +67,9 @@ 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())); - logging_mutex_ = get_global_logging_mutex(); + std::shared_ptr logging_mutex = get_global_logging_mutex(); { - std::lock_guard guard(*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. @@ -132,7 +132,7 @@ NodeBase::NodeBase( node_handle_.reset( rcl_node.release(), - [logging_mutex = logging_mutex_](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 From 8630accf5c8ca7ec688684b51a86bfaeec8700b8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 22 May 2020 11:52:58 -0300 Subject: [PATCH 5/7] Address peer review comment Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/context.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index e65b89c677..8eb2f56ac5 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -51,7 +51,7 @@ rclcpp_logging_output_handler( int severity, const char * name, rcutils_time_point_value_t timestamp, const char * format, va_list * args) { - std::shared_ptr logging_mutex = nullptr; + std::shared_ptr logging_mutex; try { logging_mutex = get_global_logging_mutex(); } catch (std::exception & ex) { From b29cdd698b0b8778d036b73a55f49b976b37fca4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 22 May 2020 18:50:33 +0000 Subject: [PATCH 6/7] Address peer review comments Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/context.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 8eb2f56ac5..5473d9e05e 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -40,6 +40,14 @@ static std::mutex g_contexts_mutex; /// Weak list of context to be shutdown by the signal handler. static std::vector> g_contexts; +/// Count of contexts that wanted to initialize the logging system. +size_t & +get_logging_reference_count() +{ + static size_t ref_count = 0; + return ref_count; +} + using rclcpp::Context; extern "C" @@ -51,28 +59,20 @@ rclcpp_logging_output_handler( int severity, const char * name, rcutils_time_point_value_t timestamp, const char * format, va_list * args) { - std::shared_ptr logging_mutex; 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()); - return; + } catch (...) { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to take global rclcpp logging mutex\n"); } - - std::lock_guard guard(*logging_mutex); - return rcl_logging_multiple_output_handler( - location, severity, name, timestamp, format, args); } } // extern "C" -static -size_t & -get_logging_reference_count() -{ - static size_t ref_count = 0; - return ref_count; -} - Context::Context() : rcl_context_(nullptr), shutdown_reason_(""), From 26eee4756938225fd5962fa33123abbe18139527 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 22 May 2020 17:10:53 -0300 Subject: [PATCH 7/7] Add missing new line in message Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/context.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 5473d9e05e..ea1c3ec95a 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -67,6 +67,7 @@ rclcpp_logging_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"); }