Fix potential Construction/Destruction order problem between global contexts vector and Context of static lifetime

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<Context>
   std::mutex interrupt_guard_cond_handles_mutex_;
   /// Guard conditions for interrupting of associated wait sets on interrupt_all_wait_sets().
   std::unordered_map<rcl_wait_set_t *, rcl_guard_condition_t> interrupt_guard_cond_handles_;
+
+  /// Keep shared ownership of global vector of weak contexts
+  std::shared_ptr<WeakContextsWrapper> weak_contexts_;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/src/rclcpp/context.cpp

@@ -35,21 +35,90 @@
 
 #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;
+
+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<std::mutex> guard(mutex_);
+    weak_contexts_.push_back(context);
+  }
+
+  void
+  remove_context(const Context * context)
+  {
+    std::lock_guard<std::mutex> 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<Context::SharedPtr>
+  get_contexts()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<Context::SharedPtr> 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<std::weak_ptr<rclcpp::Context>> 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()
 {
   static size_t ref_count = 0;
   return ref_count;
 }
 
-using rclcpp::Context;
-
 extern "C"
 {
 static
@@ -168,8 +237,8 @@ Context::init(
 
     init_options_ = init_options;
 
-    std::lock_guard<std::mutex> 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<std::mutex> 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<Context::SharedPtr>
 rclcpp::get_contexts()
 {
-  std::lock_guard<std::mutex> lock(g_contexts_mutex);
-  std::vector<Context::SharedPtr> 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();
 }