Use OnShutdown callback handle instead of OnShutdown callback (#1639) (…

…#1650)

1. Add remove_on_shutdown_callback() in rclcpp::Context

Signed-off-by: Barry Xu <barry.xu@sony.com>

2. Add add_on_shutdown_callback(), which returns a handle that can be removed by remove_on_shutdown_callback().

Signed-off-by: Barry Xu <barry.xu@sony.com>
(cherry picked from commit 6806cdf)

Co-authored-by: Barry Xu <barry.xu@sony.com>

rclcpp/include/rclcpp/context.hpp

@@ -23,6 +23,7 @@
 #include <typeindex>
 #include <typeinfo>
 #include <unordered_map>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -47,6 +48,17 @@ class ContextAlreadyInitialized : public std::runtime_error
 /// Forward declare WeakContextsWrapper
 class WeakContextsWrapper;
 
+class OnShutdownCallbackHandle
+{
+  friend class Context;
+
+public:
+  using OnShutdownCallbackType = std::function<void ()>;
+
+private:
+  std::weak_ptr<OnShutdownCallbackType> callback;
+};
+
 /// Context which encapsulates shared state between nodes and other similar entities.
 /**
  * A context also represents the lifecycle between init and shutdown of rclcpp.
@@ -177,7 +189,7 @@ class Context : public std::enable_shared_from_this<Context>
   bool
   shutdown(const std::string & reason);
 
-  using OnShutdownCallback = std::function<void ()>;
+  using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType;
 
   /// Add a on_shutdown callback to be called when shutdown is called for this context.
   /**
@@ -203,23 +215,47 @@ class Context : public std::enable_shared_from_this<Context>
   OnShutdownCallback
   on_shutdown(OnShutdownCallback callback);
 
-  /// Return the shutdown callbacks as const.
+  /// Add a on_shutdown callback to be called when shutdown is called for this context.
+  /**
+   * These callbacks will be called in the order they are added as the second
+   * to last step in shutdown().
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronously in the dedicated singal handling thread.
+   *
+   * Also, shutdown() may be called from the destructor of this function.
+   * Therefore, it is not safe to throw exceptions from these callbacks.
+   * Instead, log errors or use some other mechanism to indicate an error has
+   * occurred.
+   *
+   * On shutdown callbacks may be registered before init and after shutdown,
+   * and persist on repeated init's.
+   *
+   * \param[in] callback the on_shutdown callback to be registered
+   * \return the created callback handle
+   */
+  RCLCPP_PUBLIC
+  virtual
+  OnShutdownCallbackHandle
+  add_on_shutdown_callback(OnShutdownCallback callback);
+
+  /// Remove an registered on_shutdown callbacks.
   /**
-   * Using the returned reference is not thread-safe with calls that modify
-   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   * \param[in] callback_handle the on_shutdown callback handle to be removed.
+   * \return true if the callback is found and removed, otherwise false.
    */
   RCLCPP_PUBLIC
-  const std::vector<OnShutdownCallback> &
-  get_on_shutdown_callbacks() const;
+  virtual
+  bool
+  remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle);
 
   /// Return the shutdown callbacks.
   /**
-   * Using the returned reference is not thread-safe with calls that modify
-   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   * Returned callbacks are a copy of the registered callbacks.
    */
   RCLCPP_PUBLIC
-  std::vector<OnShutdownCallback> &
-  get_on_shutdown_callbacks();
+  std::vector<OnShutdownCallback>
+  get_on_shutdown_callbacks() const;
 
   /// Return the internal rcl context.
   RCLCPP_PUBLIC
@@ -299,8 +335,8 @@ class Context : public std::enable_shared_from_this<Context>
   // attempt to acquire another sub context.
   std::recursive_mutex sub_contexts_mutex_;
 
-  std::vector<OnShutdownCallback> on_shutdown_callbacks_;
-  std::mutex on_shutdown_callbacks_mutex_;
+  std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
+  mutable std::mutex on_shutdown_callbacks_mutex_;
 
   /// Condition variable for timed sleep (see sleep_for).
   std::condition_variable interrupt_condition_variable_;

rclcpp/include/rclcpp/executor.hpp

@@ -30,6 +30,7 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/executor_options.hpp"
@@ -571,6 +572,9 @@ class Executor
   /// nodes that are associated with the executor
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
   weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+
+  /// shutdown callback handle registered to Context
+  rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_;
 };
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/context.cpp

@@ -315,9 +315,13 @@ Context::shutdown(const std::string & reason)
   // set shutdown reason
   shutdown_reason_ = reason;
   // call each shutdown callback
-  for (const auto & callback : on_shutdown_callbacks_) {
-    callback();
+  {
+    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+    for (const auto & callback : on_shutdown_callbacks_) {
+      (*callback)();
+    }
   }
+
   // interrupt all blocking sleep_for() and all blocking executors or wait sets
   this->interrupt_all_sleep_for();
   // remove self from the global contexts
@@ -344,20 +348,48 @@ Context::shutdown(const std::string & reason)
 rclcpp::Context::OnShutdownCallback
 Context::on_shutdown(OnShutdownCallback callback)
 {
-  on_shutdown_callbacks_.push_back(callback);
+  add_on_shutdown_callback(callback);
   return callback;
 }
 
-const std::vector<rclcpp::Context::OnShutdownCallback> &
-Context::get_on_shutdown_callbacks() const
+rclcpp::OnShutdownCallbackHandle
+Context::add_on_shutdown_callback(OnShutdownCallback callback)
 {
-  return on_shutdown_callbacks_;
+  auto callback_shared_ptr = std::make_shared<OnShutdownCallback>(callback);
+  {
+    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+    on_shutdown_callbacks_.emplace(callback_shared_ptr);
+  }
+
+  OnShutdownCallbackHandle callback_handle;
+  callback_handle.callback = callback_shared_ptr;
+  return callback_handle;
 }
 
-std::vector<rclcpp::Context::OnShutdownCallback> &
-Context::get_on_shutdown_callbacks()
+bool
+Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle)
 {
-  return on_shutdown_callbacks_;
+  std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+  auto callback_shared_ptr = callback_handle.callback.lock();
+  if (callback_shared_ptr == nullptr) {
+    return false;
+  }
+  return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1;
+}
+
+std::vector<rclcpp::Context::OnShutdownCallback>
+Context::get_on_shutdown_callbacks() const
+{
+  std::vector<OnShutdownCallback> callbacks;
+
+  {
+    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+    for (auto & iter : on_shutdown_callbacks_) {
+      callbacks.emplace_back(*iter);
+    }
+  }
+
+  return callbacks;
 }
 
 std::shared_ptr<rcl_context_t>

rclcpp/src/rclcpp/executor.cpp

@@ -56,7 +56,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
     throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor");
   }
 
-  context_->on_shutdown(
+  shutdown_callback_handle_ = context_->add_on_shutdown_callback(
     [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{shutdown_guard_condition_}]() {
       auto strong_gc = weak_gc.lock();
       if (strong_gc) {
@@ -138,6 +138,14 @@ Executor::~Executor()
   }
   // Remove and release the sigint guard condition
   memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition());
+
+  // Remove shutdown callback handle registered to Context
+  if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) {
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to remove registered on_shutdown callback");
+    rcl_reset_error();
+  }
 }
 
 std::vector<rclcpp::CallbackGroup::WeakPtr>