Add the interface for pre-shutdown callback

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

rclcpp/include/rclcpp/context.hpp

@@ -48,17 +48,20 @@ class ContextAlreadyInitialized : public std::runtime_error
 /// Forward declare WeakContextsWrapper
 class WeakContextsWrapper;
 
-class OnShutdownCallbackHandle
+class ShutdownCallbackHandle
 {
   friend class Context;
 
 public:
-  using OnShutdownCallbackType = std::function<void ()>;
+  using ShutdownCallbackType = std::function<void ()>;
 
 private:
-  std::weak_ptr<OnShutdownCallbackType> callback;
+  std::weak_ptr<ShutdownCallbackType> callback;
 };
 
+using OnShutdownCallbackHandle = ShutdownCallbackHandle;
+using PreShutdownCallbackHandle = ShutdownCallbackHandle;
+
 /// Context which encapsulates shared state between nodes and other similar entities.
 /**
  * A context also represents the lifecycle between init and shutdown of rclcpp.
@@ -189,7 +192,7 @@ class Context : public std::enable_shared_from_this<Context>
   bool
   shutdown(const std::string & reason);
 
-  using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType;
+  using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType;
 
   /// Add a on_shutdown callback to be called when shutdown is called for this context.
   /**
@@ -249,6 +252,33 @@ class Context : public std::enable_shared_from_this<Context>
   bool
   remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle);
 
+  using PreShutdownCallback = PreShutdownCallbackHandle::ShutdownCallbackType;
+
+  /// Add a pre_shutdown callback to be called before shutdown is called for this context.
+  /**
+   * These callbacks will be called in the order they are added.
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronously in the dedicated singal handling thread.
+   *
+   * \param[in] callback the pre_shutdown callback to be registered
+   * \return the created callback handle
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PreShutdownCallbackHandle
+  add_pre_shutdown_callback(PreShutdownCallback callback);
+
+  /// Remove an registered pre_shutdown callback.
+  /**
+   * \param[in] callback_handle the pre_shutdown callback handle to be removed.
+   * \return true if the callback is found and removed, otherwise false.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  remove_pre_shutdown_callback(const PreShutdownCallbackHandle & callback_handle);
+
   /// Return the shutdown callbacks.
   /**
    * Returned callbacks are a copy of the registered callbacks.
@@ -257,6 +287,14 @@ class Context : public std::enable_shared_from_this<Context>
   std::vector<OnShutdownCallback>
   get_on_shutdown_callbacks() const;
 
+  /// Return the pre-shutdown callbacks.
+  /**
+   * Returned callbacks are a copy of the registered callbacks.
+   */
+  RCLCPP_PUBLIC
+  std::vector<PreShutdownCallback>
+  get_pre_shutdown_callbacks() const;
+
   /// Return the internal rcl context.
   RCLCPP_PUBLIC
   std::shared_ptr<rcl_context_t>
@@ -338,13 +376,37 @@ class Context : public std::enable_shared_from_this<Context>
   std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
   mutable std::mutex on_shutdown_callbacks_mutex_;
 
+  std::unordered_set<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
+  mutable std::mutex pre_shutdown_callbacks_mutex_;
+
   /// Condition variable for timed sleep (see sleep_for).
   std::condition_variable interrupt_condition_variable_;
   /// Mutex for protecting the global condition variable.
   std::mutex interrupt_mutex_;
 
   /// Keep shared ownership of global vector of weak contexts
   std::shared_ptr<WeakContextsWrapper> weak_contexts_;
+
+  enum class ShutdownType
+  {
+    pre_shutdown,
+    on_shutdown
+  };
+
+  using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType;
+
+  ShutdownCallbackHandle
+  add_shutdown_callback(
+    ShutdownType shutdown_type,
+    ShutdownCallback callback);
+
+  bool
+  remove_shutdown_callback(
+    ShutdownType shutdown_type,
+    const ShutdownCallbackHandle & callback_handle);
+
+  std::vector<rclcpp::Context::ShutdownCallback>
+  get_shutdown_callback(ShutdownType shutdown_type) const;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/include/rclcpp/utilities.hpp

@@ -211,6 +211,43 @@ RCLCPP_PUBLIC
 void
 on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr context = nullptr);
 
+/// Register a function to be called before shutdown is called on the context.
+/**
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * These callbacks are called before the associated Context is shutdown.
+ * When shutdown by the SIGINT handler, these callbacks are called asynchronously
+ * from the dedicated signal handling thread, at some point after the SIGINT
+ * signal is received.
+ *
+ * \sa rclcpp::Context::add_pre_shutdown_callback()
+ * \param[in] callback to be called before the given context is shutdown
+ * \param[in] context with which to associate the context
+ * \return the created callback handle
+ */
+RCLCPP_PUBLIC
+PreShutdownCallbackHandle
+add_pre_shutdown_callback(
+  rclcpp::PreShutdownCallbackHandle::ShutdownCallbackType callback,
+  rclcpp::Context::SharedPtr context = nullptr);
+
+/// Remove an registered pre_shutdown callback.
+/**
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * \sa rclcpp::Context::remove_pre_shutdown_callback()
+ * \param[in] callback_handle the pre_shutdown callback handle to be removed.
+ * \param[in] context with which to associate the context
+ * \return true if the callback is found and removed, otherwise false.
+ */
+RCLCPP_PUBLIC
+bool
+remove_pre_shutdown_callback(
+  const rclcpp::PreShutdownCallbackHandle & callback_handle,
+  rclcpp::Context::SharedPtr context = nullptr);
+
 /// Use the global condition variable to block for the specified amount of time.
 /**
  * This function can be interrupted early if the associated context becomes

rclcpp/src/rclcpp/context.cpp

@@ -19,6 +19,7 @@
 #include <sstream>
 #include <string>
 #include <vector>
+#include <unordered_set>
 #include <utility>
 
 #include "rcl/init.h"
@@ -307,6 +308,15 @@ Context::shutdown(const std::string & reason)
     // if it is not valid, then it cannot be shutdown
     return false;
   }
+
+  // call each pre-shutdown callback
+  {
+    std::lock_guard<std::mutex> lock{pre_shutdown_callbacks_mutex_};
+    for (const auto & callback : pre_shutdown_callbacks_) {
+      (*callback)();
+    }
+  }
+
   // rcl shutdown
   rcl_ret_t ret = rcl_shutdown(rcl_context_.get());
   if (RCL_RET_OK != ret) {
@@ -355,36 +365,118 @@ Context::on_shutdown(OnShutdownCallback callback)
 rclcpp::OnShutdownCallbackHandle
 Context::add_on_shutdown_callback(OnShutdownCallback callback)
 {
-  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);
+  return add_shutdown_callback(ShutdownType::on_shutdown, callback);
+}
+
+bool
+Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle)
+{
+  return remove_shutdown_callback(ShutdownType::on_shutdown, callback_handle);
+}
+
+rclcpp::PreShutdownCallbackHandle
+Context::add_pre_shutdown_callback(PreShutdownCallback callback)
+{
+  return add_shutdown_callback(ShutdownType::pre_shutdown, callback);
+}
+
+bool
+Context::remove_pre_shutdown_callback(
+  const PreShutdownCallbackHandle & callback_handle)
+{
+  return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle);
+}
+
+rclcpp::ShutdownCallbackHandle
+Context::add_shutdown_callback(
+  ShutdownType shutdown_type,
+  ShutdownCallback callback)
+{
+  auto callback_shared_ptr =
+    std::make_shared<ShutdownCallbackHandle::ShutdownCallbackType>(callback);
+
+  switch (shutdown_type) {
+    case ShutdownType::pre_shutdown:
+      {
+        std::lock_guard<std::mutex> lock(pre_shutdown_callbacks_mutex_);
+        pre_shutdown_callbacks_.emplace(callback_shared_ptr);
+      }
+      break;
+    case ShutdownType::on_shutdown:
+      {
+        std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+        on_shutdown_callbacks_.emplace(callback_shared_ptr);
+      }
+      break;
   }
 
-  OnShutdownCallbackHandle callback_handle;
+  ShutdownCallbackHandle callback_handle;
   callback_handle.callback = callback_shared_ptr;
   return callback_handle;
 }
 
 bool
-Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle)
+Context::remove_shutdown_callback(
+  ShutdownType shutdown_type,
+  const ShutdownCallbackHandle & callback_handle)
 {
-  std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
+  std::mutex * mutex_ptr;
+  std::unordered_set<
+    std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
+
+  switch (shutdown_type) {
+    case ShutdownType::pre_shutdown:
+      mutex_ptr = &pre_shutdown_callbacks_mutex_;
+      callback_list_ptr = &pre_shutdown_callbacks_;
+      break;
+    case ShutdownType::on_shutdown:
+      mutex_ptr = &on_shutdown_callbacks_mutex_;
+      callback_list_ptr = &on_shutdown_callbacks_;
+      break;
+  }
+
+  std::lock_guard<std::mutex> lock(*mutex_ptr);
   auto callback_shared_ptr = callback_handle.callback.lock();
   if (callback_shared_ptr == nullptr) {
     return false;
   }
-  return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1;
+  return callback_list_ptr->erase(callback_shared_ptr) == 1;
 }
 
 std::vector<rclcpp::Context::OnShutdownCallback>
 Context::get_on_shutdown_callbacks() const
 {
-  std::vector<OnShutdownCallback> callbacks;
+  return get_shutdown_callback(ShutdownType::on_shutdown);
+}
+
+std::vector<rclcpp::Context::PreShutdownCallback>
+Context::get_pre_shutdown_callbacks() const
+{
+  return get_shutdown_callback(ShutdownType::pre_shutdown);
+}
 
+std::vector<rclcpp::Context::ShutdownCallback>
+Context::get_shutdown_callback(ShutdownType shutdown_type) const
+{
+  std::mutex * mutex_ptr;
+  const std::unordered_set<
+    std::shared_ptr<ShutdownCallbackHandle::ShutdownCallbackType>> * callback_list_ptr;
+
+  switch (shutdown_type) {
+    case ShutdownType::pre_shutdown:
+      mutex_ptr = &pre_shutdown_callbacks_mutex_;
+      callback_list_ptr = &pre_shutdown_callbacks_;
+      break;
+    case ShutdownType::on_shutdown:
+      mutex_ptr = &on_shutdown_callbacks_mutex_;
+      callback_list_ptr = &on_shutdown_callbacks_;
+      break;
+  }
+
+  std::vector<rclcpp::Context::ShutdownCallback> callbacks;
   {
-    std::lock_guard<std::mutex> lock(on_shutdown_callbacks_mutex_);
-    for (auto & iter : on_shutdown_callbacks_) {
+    std::lock_guard<std::mutex> lock(*mutex_ptr);
+    for (auto & iter : *callback_list_ptr) {
       callbacks.emplace_back(*iter);
     }
   }

rclcpp/src/rclcpp/utilities.cpp

@@ -192,6 +192,32 @@ on_shutdown(std::function<void()> callback, Context::SharedPtr context)
   context->on_shutdown(callback);
 }
 
+rclcpp::PreShutdownCallbackHandle
+add_pre_shutdown_callback(
+  rclcpp::PreShutdownCallbackHandle::ShutdownCallbackType callback,
+  rclcpp::Context::SharedPtr context)
+{
+  using rclcpp::contexts::get_global_default_context;
+  if (nullptr == context) {
+    context = get_global_default_context();
+  }
+
+  return context->add_pre_shutdown_callback(callback);
+}
+
+bool
+remove_pre_shutdown_callback(
+  const rclcpp::PreShutdownCallbackHandle & callback_handle,
+  rclcpp::Context::SharedPtr context)
+{
+  using rclcpp::contexts::get_global_default_context;
+  if (nullptr == context) {
+    context = get_global_default_context();
+  }
+
+  return context->remove_pre_shutdown_callback(callback_handle);
+}
+
 bool
 sleep_for(const std::chrono::nanoseconds & nanoseconds, Context::SharedPtr context)
 {