diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 61eb27ac88..0bfd61955e 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -44,6 +44,9 @@ class ContextAlreadyInitialized : public std::runtime_error : std::runtime_error("context is already initialized") {} }; +/// Forward declare WeakContextsWrapper +class WeakContextsWrapper; + /// Context which encapsulates shared state between nodes and other similar entities. /** * A context also represents the lifecycle between init and shutdown of rclcpp. @@ -358,6 +361,9 @@ class Context : public std::enable_shared_from_this std::mutex interrupt_guard_cond_handles_mutex_; /// Guard conditions for interrupting of associated wait sets on interrupt_all_wait_sets(). std::unordered_map interrupt_guard_cond_handles_; + + /// Keep shared ownership of global vector of weak contexts + std::shared_ptr weak_contexts_; }; /// Return a copy of the list of context shared pointers. diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index ea1c3ec95a..d938268d9b 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -35,12 +35,83 @@ #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; + +namespace rclcpp +{ +/// Class to manage vector of weak pointers to all created contexts +class WeakContextsWrapper +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(WeakContextsWrapper) + + void + add_context(const Context::SharedPtr & context) + { + std::lock_guard guard(mutex_); + weak_contexts_.push_back(context); + } + + void + remove_context(const Context * context) + { + std::lock_guard guard(mutex_); + weak_contexts_.erase( + std::remove_if( + weak_contexts_.begin(), + weak_contexts_.end(), + [context](const Context::WeakPtr weak_context) { + auto locked_context = weak_context.lock(); + if (!locked_context) { + // take advantage and removed expired contexts + return true; + } + return locked_context.get() == context; + } + ), + weak_contexts_.end()); + } + + std::vector + get_contexts() + { + std::lock_guard lock(mutex_); + std::vector shared_contexts; + for (auto it = weak_contexts_.begin(); it != weak_contexts_.end(); /* noop */) { + auto context_ptr = it->lock(); + if (!context_ptr) { + // remove invalid weak context pointers + it = weak_contexts_.erase(it); + } else { + ++it; + shared_contexts.push_back(context_ptr); + } + } + return shared_contexts; + } + +private: + std::vector> weak_contexts_; + std::mutex mutex_; +}; +} // namespace rclcpp + +using rclcpp::WeakContextsWrapper; + +/// Global vector of weak pointers to all contexts +static +WeakContextsWrapper::SharedPtr +get_weak_contexts() +{ + static WeakContextsWrapper::SharedPtr weak_contexts = WeakContextsWrapper::make_shared(); + if (!weak_contexts) { + throw std::runtime_error("weak contexts vector is not valid"); + } + return weak_contexts; +} /// Count of contexts that wanted to initialize the logging system. +static size_t & get_logging_reference_count() { @@ -48,8 +119,6 @@ get_logging_reference_count() return ref_count; } -using rclcpp::Context; - extern "C" { static @@ -168,8 +237,8 @@ Context::init( init_options_ = init_options; - std::lock_guard lock(g_contexts_mutex); - g_contexts.push_back(this->shared_from_this()); + weak_contexts_ = get_weak_contexts(); + weak_contexts_->add_context(this->shared_from_this()); } catch (const std::exception & e) { ret = rcl_shutdown(rcl_context_.get()); rcl_context_.reset(); @@ -238,16 +307,7 @@ Context::shutdown(const std::string & reason) this->interrupt_all_sleep_for(); this->interrupt_all_wait_sets(); // remove self from the global contexts - std::lock_guard context_lock(g_contexts_mutex); - for (auto it = g_contexts.begin(); it != g_contexts.end(); ) { - auto shared_context = it->lock(); - if (shared_context.get() == this) { - it = g_contexts.erase(it); - break; - } else { - ++it; - } - } + weak_contexts_->remove_context(this); // shutdown logger if (logging_mutex_) { // logging was initialized by this context @@ -396,17 +456,6 @@ Context::clean_up() std::vector rclcpp::get_contexts() { - std::lock_guard lock(g_contexts_mutex); - std::vector shared_contexts; - for (auto it = g_contexts.begin(); it != g_contexts.end(); /* noop */) { - auto context_ptr = it->lock(); - if (!context_ptr) { - // remove invalid weak context pointers - it = g_contexts.erase(it); - } else { - ++it; - shared_contexts.push_back(context_ptr); - } - } - return shared_contexts; + WeakContextsWrapper::SharedPtr weak_contexts = get_weak_contexts(); + return weak_contexts->get_contexts(); }