From 111424a019c7d888ab8ca748d240402a8f97981c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 20 Feb 2020 16:54:32 -0300 Subject: [PATCH 1/5] Ensure logging is initialized just once Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/context.hpp | 3 ++ rclcpp/src/rclcpp/context.cpp | 56 ++++++++++++++++++++++++++++++- 2 files changed, 58 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 76d3cb1dca..5d7a6bcecf 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -338,6 +338,9 @@ 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_; + std::unordered_map> 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/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 57937d35fb..5c1ab00950 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -22,6 +22,7 @@ #include #include "rcl/init.h" +#include "rcl/logging.h" #include "rclcpp/detail/utilities.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/logging.hpp" @@ -34,8 +35,31 @@ 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 +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_(get_global_logging_configure_mutex()) +{ + if (!logging_configure_mutex_) { + throw std::runtime_error("global logging configure mutex is 'nullptr'"); + } +} Context::~Context() { @@ -94,6 +118,22 @@ Context::init( rcl_context_.reset(); rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl"); } + + { + std::lock_guard 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"); + } + } + ++count; + } + try { std::vector unparsed_ros_arguments = detail::get_unparsed_ros_arguments( argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator()); @@ -309,6 +349,20 @@ Context::clean_up() { shutdown_reason_ = ""; rcl_context_.reset(); + { + std::lock_guard 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(); + } + } + } sub_contexts_.clear(); } From 05a8dd845c5b7487f820201cff2e4cfee299c12b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 26 Feb 2020 18:26:20 -0300 Subject: [PATCH 2/5] Add init option to initialize logging Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/init_options.hpp | 11 +++++++++++ rclcpp/src/rclcpp/context.cpp | 21 +++++++++++++-------- rclcpp/src/rclcpp/init_options.cpp | 13 +++++++++++++ 3 files changed, 37 insertions(+), 8 deletions(-) diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp index fd0e4f82d7..3af785073c 100644 --- a/rclcpp/include/rclcpp/init_options.hpp +++ b/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. + RCLCPP_PUBLIC + bool + initialize_logging() const; + + /// Set flag indicating if logging should be initialized or not. + RCLCPP_PUBLIC + InitOptions & + initialize_logging(bool initialize_logging); + /// Assignment operator. RCLCPP_PUBLIC InitOptions & @@ -62,6 +72,7 @@ class InitOptions private: std::unique_ptr init_options_; + bool initialize_logging_ = true; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 5c1ab00950..4868f41e7e 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -54,12 +54,8 @@ get_logging_reference_count() Context::Context() : rcl_context_(nullptr), shutdown_reason_(""), - logging_configure_mutex_(get_global_logging_configure_mutex()) -{ - if (!logging_configure_mutex_) { - throw std::runtime_error("global logging configure mutex is 'nullptr'"); - } -} + logging_configure_mutex_(nullptr) +{} Context::~Context() { @@ -119,7 +115,11 @@ Context::init( rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl"); } - { + if (init_options.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_); size_t & count = get_logging_reference_count(); if (0u == count) { @@ -130,6 +130,10 @@ Context::init( 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; } @@ -349,7 +353,8 @@ Context::clean_up() { shutdown_reason_ = ""; rcl_context_.reset(); - { + if (logging_configure_mutex_) { + // logging was initialized by this context std::lock_guard guard(*logging_configure_mutex_); size_t & count = get_logging_reference_count(); if (0u == --count) { diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp index ba46edbec1..38496ce3e2 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::initialize_logging() const +{ + return initialize_logging_; +} + +InitOptions & +InitOptions::initialize_logging(bool initialize_logging) +{ + initialize_logging_ = initialize_logging; + return *this; +} + InitOptions & InitOptions::operator=(const InitOptions & other) { From e85034dddee3af6124ea70a78a1cfa39d7bc6c49 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 5 Mar 2020 12:47:54 -0300 Subject: [PATCH 3/5] Rename methods Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/init_options.hpp | 4 ++-- rclcpp/src/rclcpp/context.cpp | 2 +- rclcpp/src/rclcpp/init_options.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp index 3af785073c..df2e667ba8 100644 --- a/rclcpp/include/rclcpp/init_options.hpp +++ b/rclcpp/include/rclcpp/init_options.hpp @@ -45,12 +45,12 @@ class InitOptions /// Return `true` if logging should be initialized. RCLCPP_PUBLIC bool - initialize_logging() const; + auto_initialize_logging() const; /// Set flag indicating if logging should be initialized or not. RCLCPP_PUBLIC InitOptions & - initialize_logging(bool initialize_logging); + auto_initialize_logging(bool initialize_logging); /// Assignment operator. RCLCPP_PUBLIC diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 4868f41e7e..824810cc51 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -115,7 +115,7 @@ Context::init( rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl"); } - if (init_options.initialize_logging()) { + 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'"); diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp index 38496ce3e2..ad2a900b5f 100644 --- a/rclcpp/src/rclcpp/init_options.cpp +++ b/rclcpp/src/rclcpp/init_options.cpp @@ -47,13 +47,13 @@ InitOptions::InitOptions(const InitOptions & other) } bool -InitOptions::initialize_logging() const +InitOptions::auto_initialize_logging() const { return initialize_logging_; } InitOptions & -InitOptions::initialize_logging(bool initialize_logging) +InitOptions::auto_initialize_logging(bool initialize_logging) { initialize_logging_ = initialize_logging; return *this; From 5c25c7844e5c0bd1e92683244e7532b4eebc85f2 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 21 Apr 2020 13:30:28 -0300 Subject: [PATCH 4/5] Fini logging when shutdown is called Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/context.cpp | 31 ++++++++++++++++--------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 824810cc51..1918a77a02 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -227,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 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; } @@ -353,21 +369,6 @@ Context::clean_up() { shutdown_reason_ = ""; rcl_context_.reset(); - if (logging_configure_mutex_) { - // logging was initialized by this context - std::lock_guard 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(); - } - } - } sub_contexts_.clear(); } From 0c95f168d584923b8ccb00d6080561df82b95db5 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 23 Apr 2020 13:59:34 -0300 Subject: [PATCH 5/5] Address peer review comments Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/init_options.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp index df2e667ba8..7b05ee8d00 100644 --- a/rclcpp/include/rclcpp/init_options.hpp +++ b/rclcpp/include/rclcpp/init_options.hpp @@ -42,7 +42,7 @@ class InitOptions RCLCPP_PUBLIC InitOptions(const InitOptions & other); - /// Return `true` if logging should be initialized. + /// Return `true` if logging should be initialized when `rclcpp::Context::init` is called. RCLCPP_PUBLIC bool auto_initialize_logging() const; @@ -72,7 +72,7 @@ class InitOptions private: std::unique_ptr init_options_; - bool initialize_logging_ = true; + bool initialize_logging_{true}; }; } // namespace rclcpp