defer signal handling to a singleton thread

rclcpp/include/rclcpp/context.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__CONTEXT_HPP_
 #define RCLCPP__CONTEXT_HPP_
 
+#include <condition_variable>
 #include <functional>
 #include <memory>
 #include <mutex>
@@ -26,6 +27,8 @@
 #include <vector>
 
 #include "rcl/context.h"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
 #include "rclcpp/init_options.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -147,21 +150,21 @@ class Context : public std::enable_shared_from_this<Context>
    * - rcl_shutdown() is called on the internal rcl_context_t instance
    * - the shutdown reason is set
    * - each on_shutdown callback is called, in the order that they were added
-   * - if notify_all is true, rclcpp::notify_all is called to unblock some ROS functions
+   * - interrupt blocking sleep_for() calls, so they return early due to shutdown
+   * - interrupt blocking executors and wait sets
    *
    * The underlying rcl context is not finalized by this function.
    *
    * This function is thread-safe.
    *
    * \param[in] reason the description of why shutdown happened
-   * \param[in] notify_all if true, then rclcpp::notify_all will be called
    * \return true if shutdown was successful, false if context was already shutdown
    * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails
    */
   RCLCPP_PUBLIC
   virtual
   bool
-  shutdown(const std::string & reason, bool notify_all = true);
+  shutdown(const std::string & reason);
 
   using OnShutdownCallback = std::function<void ()>;
 
@@ -170,8 +173,9 @@ class Context : public std::enable_shared_from_this<Context>
    * These callbacks will be called in the order they are added as the second
    * to last step in shutdown().
    *
-   * These callbacks may be run in the signal handler as the signal handler
-   * may call shutdown() on this context.
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronoulsy 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
@@ -211,6 +215,86 @@ class Context : public std::enable_shared_from_this<Context>
   std::shared_ptr<rcl_context_t>
   get_rcl_context();
 
+  /// Sleep for a given period of time or until shutdown() is called.
+  /**
+   * This function can be interrupted early if:
+   *
+   *   - this context is shutdown()
+   *   - this context is destructed (resulting in shutdown)
+   *   - this context has shutdown_on_sigint=true and SIGINT occurs (resulting in shutdown)
+   *   - interrupt_all_sleep_for() is called
+   *
+   * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
+   * \return true if the condition variable did not timeout, i.e. you were interrupted.
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_for(const std::chrono::nanoseconds & nanoseconds);
+
+  /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  interrupt_all_sleep_for();
+
+  /// Get a handle to the guard condition which is triggered when interrupted.
+  /**
+   * This guard condition is triggered any time interrupt_all_wait_sets() is
+   * called, which may be called by the user, or shutdown().
+   * And in turn, shutdown() may be called by the user, the destructor of this
+   * context, or the signal handler if installed and shutdown_on_sigint is true
+   * for this context.
+   *
+   * The first time that this function is called for a given wait set a new guard
+   * condition will be created and returned; thereafter the same guard condition
+   * will be returned for the same wait set.
+   * This mechanism is designed to ensure that the same guard condition is not
+   * reused across wait sets (e.g., when using multiple executors in the same
+   * process).
+   * This method will throw an exception if initialization of the guard
+   * condition fails.
+   *
+   * The returned guard condition needs to be released with the
+   * release_interrupt_guard_condition() method in order to reclaim resources.
+   *
+   * \param[in] wait_set Pointer to the rcl_wait_set_t that will be using the
+   *   resulting guard condition.
+   * \return Pointer to the guard condition.
+   */
+  RCLCPP_PUBLIC
+  rcl_guard_condition_t *
+  get_interrupt_guard_condition(rcl_wait_set_t * wait_set);
+
+  /// Release the previously allocated guard condition which is triggered when interrupted.
+  /**
+   * If you previously called get_interrupt_guard_condition() for a given wait
+   * set to get a interrupt guard condition, then you should call
+   * release_interrupt_guard_condition() when you're done, to free that
+   * condition.
+   * Will throw an exception if get_interrupt_guard_condition() wasn't
+   * previously called for the given wait set.
+   *
+   * After calling this, the pointer returned by get_interrupt_guard_condition()
+   * for the given wait_set is invalid.
+   *
+   * \param[in] wait_set Pointer to the rcl_wait_set_t that was using the
+   *   resulting guard condition.
+   */
+  RCLCPP_PUBLIC
+  void
+  release_interrupt_guard_condition(rcl_wait_set_t * wait_set);
+
+  /// Nothrow version of release_interrupt_guard_condition(), logs to RCLCPP_ERROR instead.
+  RCLCPP_PUBLIC
+  void
+  release_interrupt_guard_condition(rcl_wait_set_t * wait_set, const std::nothrow_t &) noexcept;
+
+  /// Interrupt any blocking executors, or wait sets associated with this context.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  interrupt_all_wait_sets();
+
   /// Return a singleton instance for the SubContext type, constructing one if necessary.
   template<typename SubContext, typename ... Args>
   std::shared_ptr<SubContext>
@@ -261,6 +345,16 @@ class Context : public std::enable_shared_from_this<Context>
 
   std::vector<OnShutdownCallback> on_shutdown_callbacks_;
   std::mutex on_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_;
+
+  /// Mutex to protect sigint_guard_cond_handles_.
+  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_;
 };
 
 /// Return a copy of the list of context shared pointers.

rclcpp/include/rclcpp/graph_listener.hpp

@@ -134,6 +134,12 @@ class GraphListener : public std::enable_shared_from_this<GraphListener>
   void
   shutdown();
 
+  /// Nothrow version of shutdown(), logs to RCLCPP_ERROR instead.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  shutdown(const std::nothrow_t &) noexcept;
+
   /// Return true if shutdown() has been called, else false.
   RCLCPP_PUBLIC
   virtual
@@ -155,6 +161,12 @@ class GraphListener : public std::enable_shared_from_this<GraphListener>
 private:
   RCLCPP_DISABLE_COPY(GraphListener)
 
+  /** \internal */
+  void
+  __shutdown(bool);
+
+  rclcpp::Context::SharedPtr parent_context_;
+
   std::thread listener_thread_;
   bool is_started_;
   std::atomic_bool is_shutdown_;

rclcpp/include/rclcpp/utilities.hpp

@@ -24,9 +24,6 @@
 #include "rclcpp/init_options.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-#include "rcl/guard_condition.h"
-#include "rcl/wait.h"
-
 #include "rmw/macros.h"
 #include "rmw/rmw.h"
 
@@ -184,9 +181,11 @@ shutdown(
  * If nullptr is given for the context, then the global context is used, i.e.
  * the context initialized by rclcpp::init().
  *
- * Note that these callbacks should be non-blocking and not throw exceptions,
- * as they may be called from signal handlers and from the destructor of the
- * context.
+ * These callbacks are called when the associated Context is shutdown with the
+ * Context::shutdown() method.
+ * When shutdown by the SIGINT handler, shutdown, and therefore these callbacks,
+ * is called asynchronously from the dedicated signal handling thread, at some
+ * point after the SIGINT signal is received.
  *
  * \sa rclcpp::Context::on_shutdown()
  * \param[in] callback to be called when the given context is shutdown
@@ -196,65 +195,25 @@ RCLCPP_PUBLIC
 void
 on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr context = nullptr);
 
-/// Get a handle to the rmw guard condition that manages the signal handler.
-/**
- * The first time that this function is called for a given wait set a new guard
- * condition will be created and returned; thereafter the same guard condition
- * will be returned for the same wait set. This mechanism is designed to ensure
- * that the same guard condition is not reused across wait sets (e.g., when
- * using multiple executors in the same process). Will throw an exception if
- * initialization of the guard condition fails.
- *
- * \param[in] wait_set Pointer to the rcl_wait_set_t that will be using the
- *   resulting guard condition.
- * \param[in] context The context associated with the guard condition.
- * \return Pointer to the guard condition.
- */
-RCLCPP_PUBLIC
-rcl_guard_condition_t *
-get_sigint_guard_condition(rcl_wait_set_t * wait_set, rclcpp::Context::SharedPtr context);
-
-/// Release the previously allocated guard condition that manages the signal handler.
-/**
- * If you previously called get_sigint_guard_condition() for a given wait set
- * to get a sigint guard condition, then you should call
- * release_sigint_guard_condition() when you're done, to free that condition.
- * Will throw an exception if get_sigint_guard_condition() wasn't previously
- * called for the given wait set.
- *
- * If nullptr is given for the context, then the global context is used, i.e.
- * the context initialized by rclcpp::init().
- *
- * \param[in] wait_set Pointer to the rcl_wait_set_t that was using the
- *   resulting guard condition.
- */
-RCLCPP_PUBLIC
-void
-release_sigint_guard_condition(rcl_wait_set_t * wait_set);
-
 /// Use the global condition variable to block for the specified amount of time.
 /**
  * This function can be interrupted early if the associated context becomes
- * invalid due to rclcpp::shutdown() or the signal handler.
+ * invalid due to shutdown() or the signal handler.
+ * \sa rclcpp::Context::sleep_for
  *
  * If nullptr is given for the context, then the global context is used, i.e.
  * the context initialized by rclcpp::init().
  *
  * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
  * \param[in] context which may interrupt this sleep
- * \return True if the condition variable did not timeout.
+ * \return true if the condition variable did not timeout.
  */
 RCLCPP_PUBLIC
 bool
 sleep_for(
   const std::chrono::nanoseconds & nanoseconds,
   rclcpp::Context::SharedPtr context = nullptr);
 
-/// Notify all blocking calls so they can check for changes in rclcpp::ok().
-RCLCPP_PUBLIC
-void
-notify_all();
-
 /// Safely check if addition will overflow.
 /**
  * The type of the operands, T, should have defined