diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 61a3088..dfc33d5 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__CONTEXT_HPP_ #define RCLCPP__CONTEXT_HPP_ +#include #include #include #include @@ -26,6 +27,8 @@ #include #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 @@ public: * - 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; @@ -170,8 +173,9 @@ public: * 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 @@ public: std::shared_ptr 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 std::shared_ptr @@ -261,6 +345,16 @@ private: std::vector 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 interrupt_guard_cond_handles_; }; /// Return a copy of the list of context shared pointers. diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index b243e34..ca4f05c 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -134,6 +134,12 @@ public: 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 @@ protected: 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_; diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 3b42cd8..ae22740 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/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,53 +195,18 @@ RCLCPP_PUBLIC void on_shutdown(std::function 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 @@ -250,11 +214,6 @@ 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 diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index d3e65de..a93086a 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -22,6 +22,7 @@ #include "rcl/init.h" #include "rclcpp/exceptions.hpp" #include "rclcpp/logging.hpp" +#include "rmw/impl/cpp/demangle.hpp" /// Mutex to protect initialized contexts. static std::mutex g_contexts_mutex; @@ -127,7 +128,7 @@ Context::shutdown_reason() } bool -Context::shutdown(const std::string & reason, bool notify_all) +Context::shutdown(const std::string & reason) { // prevent races std::lock_guard init_lock(init_mutex_); @@ -147,10 +148,9 @@ Context::shutdown(const std::string & reason, bool notify_all) for (const auto & callback : on_shutdown_callbacks_) { callback(); } - // notify all blocking calls, if asked - if (notify_all) { - rclcpp::notify_all(); - } + // interrupt all blocking sleep_for() and all blocking executors or wait sets + 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(); ) { @@ -190,6 +190,99 @@ Context::get_rcl_context() return rcl_context_; } +bool +Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) +{ + std::chrono::nanoseconds time_left = nanoseconds; + { + std::unique_lock lock(interrupt_mutex_); + auto start = std::chrono::steady_clock::now(); + // this will release the lock while waiting + interrupt_condition_variable_.wait_for(lock, nanoseconds); + time_left -= std::chrono::steady_clock::now() - start; + } + if (time_left > std::chrono::nanoseconds::zero() && this->is_valid()) { + return sleep_for(time_left); + } + // Return true if the timeout elapsed successfully, otherwise false. + return this->is_valid(); +} + +void +Context::interrupt_all_sleep_for() +{ + interrupt_condition_variable_.notify_all(); +} + +rcl_guard_condition_t * +Context::get_interrupt_guard_condition(rcl_wait_set_t * wait_set) +{ + std::lock_guard lock(interrupt_guard_cond_handles_mutex_); + auto kv = interrupt_guard_cond_handles_.find(wait_set); + if (kv != interrupt_guard_cond_handles_.end()) { + return &kv->second; + } else { + rcl_guard_condition_t handle = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); + auto ret = rcl_guard_condition_init(&handle, this->get_rcl_context().get(), options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize guard condition"); + } + interrupt_guard_cond_handles_.emplace(wait_set, handle); + return &interrupt_guard_cond_handles_[wait_set]; + } +} + +void +Context::release_interrupt_guard_condition(rcl_wait_set_t * wait_set) +{ + std::lock_guard lock(interrupt_guard_cond_handles_mutex_); + auto kv = interrupt_guard_cond_handles_.find(wait_set); + if (kv != interrupt_guard_cond_handles_.end()) { + rcl_ret_t ret = rcl_guard_condition_fini(&kv->second); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to destroy sigint guard condition"); + } + interrupt_guard_cond_handles_.erase(kv); + } else { + throw std::runtime_error("Tried to release sigint guard condition for nonexistent wait set"); + } +} + +void +Context::release_interrupt_guard_condition( + rcl_wait_set_t * wait_set, + const std::nothrow_t &) noexcept +{ + try { + this->release_interrupt_guard_condition(wait_set); + } catch (const std::exception & exc) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught %s exception when releasing interrupt guard condition: %s", + rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught unknown exception when releasing interrupt guard condition"); + } +} + +void +Context::interrupt_all_wait_sets() +{ + std::lock_guard lock(interrupt_guard_cond_handles_mutex_); + for (auto & kv : interrupt_guard_cond_handles_) { + rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second)); + if (status != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to trigger guard condition in Context::interrupt_all_wait_sets(): %s", + rcl_get_error_string().str); + } + } +} + void Context::clean_up() { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 083f78a..b8deac9 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -51,8 +51,7 @@ Executor::Executor(const ExecutorArgs & args) // and one for the executor's guard cond (interrupt_guard_condition_) // Put the global ctrl-c guard condition in - memory_strategy_->add_guard_condition( - rclcpp::get_sigint_guard_condition(&wait_set_, args.context)); + memory_strategy_->add_guard_condition(args.context->get_interrupt_guard_condition(&wait_set_)); // Put the executor's guard condition in memory_strategy_->add_guard_condition(&interrupt_guard_condition_); @@ -105,9 +104,8 @@ Executor::~Executor() rcl_reset_error(); } // Remove and release the sigint guard condition - memory_strategy_->remove_guard_condition( - rclcpp::get_sigint_guard_condition(&wait_set_, context_)); - rclcpp::release_sigint_guard_condition(&wait_set_); + memory_strategy_->remove_guard_condition(context_->get_interrupt_guard_condition(&wait_set_)); + context_->release_interrupt_guard_condition(&wait_set_, std::nothrow); } void diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index d96c394..8fbad34 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -36,7 +36,8 @@ namespace graph_listener { GraphListener::GraphListener(std::shared_ptr parent_context) -: is_started_(false), +: parent_context_(parent_context), + is_started_(false), is_shutdown_(false), interrupt_guard_condition_context_(nullptr), shutdown_guard_condition_(nullptr) @@ -54,12 +55,12 @@ GraphListener::GraphListener(std::shared_ptr parent_context) throw_from_rcl_error(ret, "failed to create interrupt guard condition"); } - shutdown_guard_condition_ = rclcpp::get_sigint_guard_condition(&wait_set_, parent_context); + shutdown_guard_condition_ = parent_context->get_interrupt_guard_condition(&wait_set_); } GraphListener::~GraphListener() { - this->shutdown(); + this->shutdown(std::nothrow); } void @@ -90,7 +91,8 @@ GraphListener::start_if_not_started() [weak_this]() { auto shared_this = weak_this.lock(); if (shared_this) { - shared_this->shutdown(); + // should not throw from on_shutdown if it can be avoided + shared_this->shutdown(std::nothrow); } }); // Start the listener thread. @@ -337,7 +339,7 @@ GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_gr } void -GraphListener::shutdown() +GraphListener::__shutdown(bool should_throw) { std::lock_guard shutdown_lock(shutdown_mutex_); if (!is_shutdown_.exchange(true)) { @@ -351,7 +353,11 @@ GraphListener::shutdown() throw_from_rcl_error(ret, "failed to finalize interrupt guard condition"); } if (shutdown_guard_condition_) { - rclcpp::release_sigint_guard_condition(&wait_set_); + if (should_throw) { + parent_context_->release_interrupt_guard_condition(&wait_set_); + } else { + parent_context_->release_interrupt_guard_condition(&wait_set_, std::nothrow); + } shutdown_guard_condition_ = nullptr; } if (is_started_) { @@ -363,6 +369,29 @@ GraphListener::shutdown() } } +void +GraphListener::shutdown() +{ + this->__shutdown(true); +} + +void +GraphListener::shutdown(const std::nothrow_t &) noexcept +{ + try { + this->__shutdown(false); + } catch (const std::exception & exc) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught %s exception when shutting down GraphListener: %s", + rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught unknown exception when shutting down GraphListener"); + } +} + bool GraphListener::is_shutdown() { diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index d1e3820..127cc7a 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "rclcpp/contexts/default_context.hpp" @@ -33,6 +34,7 @@ #include "rcl/rcl.h" #include "rmw/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rmw/rmw.h" #include "rcutils/logging_macros.h" @@ -42,10 +44,238 @@ #define HAS_SIGACTION #endif -/// Mutex to protect installation and uninstallation of signal handlers. -static std::mutex g_signal_handlers_mutex; -/// Atomic bool to control setup of signal handlers. -static std::atomic g_signal_handlers_installed(false); +namespace rclcpp +{ + +class SignalHandler final +{ +public: + static + SignalHandler & + get_global_signal_handler() + { + static SignalHandler singleton; + return singleton; + } + + bool + install() + { + std::lock_guard lock(install_mutex_); + bool already_installed = installed_.exchange(true); + if (already_installed) { + return false; + } + try { + signal_received.exchange(false); +#ifdef HAS_SIGACTION + struct sigaction action; + memset(&action, 0, sizeof(action)); + sigemptyset(&action.sa_mask); + action.sa_sigaction = signal_handler; + action.sa_flags = SA_SIGINFO; + old_action = set_sigaction(SIGINT, action); +#else + old_signal_handler = set_signal_handler(SIGINT, signal_handler); +#endif + signal_handler_thread_ = + std::thread(&SignalHandler::deferred_signal_handler, this); + } catch (...) { + installed_.exchange(false); + throw; + } + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "signal handler installed"); + return true; + } + + bool + uninstall() + { + std::lock_guard lock(install_mutex_); + bool installed = installed_.exchange(false); + if (!installed) { + return false; + } + try { +#ifdef HAS_SIGACTION + set_sigaction(SIGINT, old_action); +#else + set_signal_handler(SIGINT, old_signal_handler); +#endif + events_condition_variable.notify_one(); + signal_handler_thread_.join(); + } catch (...) { + installed_.exchange(true); + throw; + } + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "signal handler uninstalled"); + return true; + } + + bool + is_installed() + { + return installed_.load(); + } + +private: + SignalHandler() = default; + + ~SignalHandler() + { + try { + uninstall(); + } catch (const std::exception & exc) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught %s exception when uninstalling the sigint handler in rclcpp::~SignalHandler: %s", + rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught unknown exception when uninstalling the sigint handler in rclcpp::~SignalHandler"); + } + } + + // POSIX signal handler structure. +#ifdef HAS_SIGACTION + static struct sigaction old_action; +#else + typedef void (* signal_handler_t)(int); + static signal_handler_t old_signal_handler; +#endif + + static +#ifdef HAS_SIGACTION + struct sigaction + set_sigaction(int signal_value, const struct sigaction & action) +#else + signal_handler_t + set_signal_handler(int signal_value, signal_handler_t signal_handler) +#endif + { +#ifdef HAS_SIGACTION + struct sigaction old_action; + ssize_t ret = sigaction(signal_value, &action, &old_action); + if (ret == -1) +#else + signal_handler_t old_signal_handler = std::signal(signal_value, signal_handler); + // NOLINTNEXTLINE(readability/braces) + if (old_signal_handler == SIG_ERR) +#endif + { + const size_t error_length = 1024; + // NOLINTNEXTLINE(runtime/arrays) + char error_string[error_length]; +#ifndef _WIN32 +#if (defined(_GNU_SOURCE) && !defined(ANDROID)) + char * msg = strerror_r(errno, error_string, error_length); + if (msg != error_string) { + strncpy(error_string, msg, error_length); + msg[error_length - 1] = '\0'; + } +#else + int error_status = strerror_r(errno, error_string, error_length); + if (error_status != 0) { + throw std::runtime_error("Failed to get error string for errno: " + std::to_string(errno)); + } +#endif +#else + strerror_s(error_string, error_length, errno); +#endif + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::runtime_error( + std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") + + error_string); + // *INDENT-ON* + } + +#ifdef HAS_SIGACTION + return old_action; +#else + return old_signal_handler; +#endif + } + + static + void +#ifdef HAS_SIGACTION + signal_handler(int signal_value, siginfo_t * siginfo, void * context) +#else + signal_handler(int signal_value) +#endif + { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "signal_handler(signal_value=%d)", signal_value); + +#ifdef HAS_SIGACTION + if (old_action.sa_flags & SA_SIGINFO) { + if (old_action.sa_sigaction != NULL) { + old_action.sa_sigaction(signal_value, siginfo, context); + } + } else { + if ( + old_action.sa_handler != NULL && // Is set + old_action.sa_handler != SIG_DFL && // Is not default + old_action.sa_handler != SIG_IGN) // Is not ignored + { + old_action.sa_handler(signal_value); + } + } +#else + if (old_signal_handler) { + old_signal_handler(signal_value); + } +#endif + signal_received.exchange(true); + events_condition_variable.notify_one(); + } + + void + deferred_signal_handler() + { + while (true) { + if (signal_received.exchange(false)) { + for (auto context_ptr : rclcpp::get_contexts()) { + if (context_ptr->get_init_options().shutdown_on_sigint) { + context_ptr->shutdown("signal handler"); + } + } + } + if (!is_installed()) { + break; + } + std::unique_lock events_lock(events_mutex); + events_condition_variable.wait(events_lock); + } + } + + // A mutex to lock event signaling. + static std::mutex events_mutex; + // A cond var to wait on for event signaling. + static std::condition_variable events_condition_variable; + // Whether a signal has been received or not. + static std::atomic signal_received; + // A thread to defer signal handling tasks to. + std::thread signal_handler_thread_; + + // A mutex to synchronize the install() and uninstall() methods. + std::mutex install_mutex_; + // Whether this handler has been installed or not. + std::atomic installed_{false}; +}; + +} // namespace rclcpp + +// Declare static class variables for SignalHandler +std::mutex rclcpp::SignalHandler::events_mutex; +std::condition_variable rclcpp::SignalHandler::events_condition_variable; +std::atomic rclcpp::SignalHandler::signal_received; +#ifdef HAS_SIGACTION +struct sigaction rclcpp::SignalHandler::old_action; +#else +typedef void (* signal_handler_t)(int); +signal_handler_t rclcpp::SignalHandler::old_signal_handler = 0; +#endif /// Mutex to protect g_sigint_guard_cond_handles static std::mutex g_sigint_guard_cond_handles_mutex; @@ -57,102 +287,6 @@ static std::condition_variable g_interrupt_condition_variable; /// Mutex for protecting the global condition variable. static std::mutex g_interrupt_mutex; -#ifdef HAS_SIGACTION -static struct sigaction old_action; -#else -typedef void (* signal_handler_t)(int); -static signal_handler_t old_signal_handler = 0; -#endif - -#ifdef HAS_SIGACTION -struct sigaction -set_sigaction(int signal_value, const struct sigaction & action) -#else -signal_handler_t -set_signal_handler(int signal_value, signal_handler_t signal_handler) -#endif -{ -#ifdef HAS_SIGACTION - struct sigaction old_action; - ssize_t ret = sigaction(signal_value, &action, &old_action); - if (ret == -1) -#else - signal_handler_t old_signal_handler = std::signal(signal_value, signal_handler); - // NOLINTNEXTLINE(readability/braces) - if (old_signal_handler == SIG_ERR) -#endif - { - const size_t error_length = 1024; - // NOLINTNEXTLINE(runtime/arrays) - char error_string[error_length]; -#ifndef _WIN32 -#if (defined(_GNU_SOURCE) && !defined(ANDROID)) - char * msg = strerror_r(errno, error_string, error_length); - if (msg != error_string) { - strncpy(error_string, msg, error_length); - msg[error_length - 1] = '\0'; - } -#else - int error_status = strerror_r(errno, error_string, error_length); - if (error_status != 0) { - throw std::runtime_error("Failed to get error string for errno: " + std::to_string(errno)); - } -#endif -#else - strerror_s(error_string, error_length, errno); -#endif - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") + - error_string); - // *INDENT-ON* - } - -#ifdef HAS_SIGACTION - return old_action; -#else - return old_signal_handler; -#endif -} - -void -#ifdef HAS_SIGACTION -signal_handler(int signal_value, siginfo_t * siginfo, void * context) -#else -signal_handler(int signal_value) -#endif -{ - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "signal_handler(signal_value=%d)", signal_value); - -#ifdef HAS_SIGACTION - if (old_action.sa_flags & SA_SIGINFO) { - if (old_action.sa_sigaction != NULL) { - old_action.sa_sigaction(signal_value, siginfo, context); - } - } else { - if ( - old_action.sa_handler != NULL && // Is set - old_action.sa_handler != SIG_DFL && // Is not default - old_action.sa_handler != SIG_IGN) // Is not ignored - { - old_action.sa_handler(signal_value); - } - } -#else - if (old_signal_handler) { - old_signal_handler(signal_value); - } -#endif - - for (auto context_ptr : rclcpp::get_contexts()) { - if (context_ptr->get_init_options().shutdown_on_sigint) { - // do not notify all, instead do that once after all are shutdown - context_ptr->shutdown("signal handler", false /* notify_all */); - } - } - rclcpp::notify_all(); -} - void rclcpp::init(int argc, char const * const argv[], const rclcpp::InitOptions & init_options) { @@ -165,46 +299,19 @@ rclcpp::init(int argc, char const * const argv[], const rclcpp::InitOptions & in bool rclcpp::install_signal_handlers() { - std::lock_guard lock(g_signal_handlers_mutex); - bool already_installed = g_signal_handlers_installed.exchange(true); - if (already_installed) { - return false; - } -#ifdef HAS_SIGACTION - struct sigaction action; - memset(&action, 0, sizeof(action)); - sigemptyset(&action.sa_mask); - action.sa_sigaction = ::signal_handler; - action.sa_flags = SA_SIGINFO; - ::old_action = set_sigaction(SIGINT, action); -#else - ::old_signal_handler = set_signal_handler(SIGINT, ::signal_handler); -#endif - RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "signal handler installed"); - return true; + return rclcpp::SignalHandler::get_global_signal_handler().install(); } bool rclcpp::signal_handlers_installed() { - return g_signal_handlers_installed.load(); + return rclcpp::SignalHandler::get_global_signal_handler().is_installed(); } bool rclcpp::uninstall_signal_handlers() { - std::lock_guard lock(g_signal_handlers_mutex); - bool installed = g_signal_handlers_installed.exchange(false); - if (!installed) { - return false; - } -#ifdef HAS_SIGACTION - set_sigaction(SIGINT, ::old_action); -#else - set_signal_handler(SIGINT, ::old_signal_handler); -#endif - RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "signal handler uninstalled"); - return true; + return rclcpp::SignalHandler::get_global_signal_handler().uninstall(); } std::vector @@ -300,14 +407,10 @@ rclcpp::shutdown(rclcpp::Context::SharedPtr context, const std::string & reason) if (nullptr == context) { context = default_context; } - bool ret = context->shutdown(reason); - - // Uninstall the signal handlers if this is the default context's shutdown. if (context == default_context) { - uninstall_signal_handlers(); + rclcpp::uninstall_signal_handlers(); } - return ret; } @@ -321,84 +424,14 @@ rclcpp::on_shutdown(std::function callback, rclcpp::Context::SharedPtr c context->on_shutdown(callback); } -rcl_guard_condition_t * -rclcpp::get_sigint_guard_condition(rcl_wait_set_t * wait_set, rclcpp::Context::SharedPtr context) -{ - std::lock_guard lock(g_sigint_guard_cond_handles_mutex); - auto kv = g_sigint_guard_cond_handles.find(wait_set); - if (kv != g_sigint_guard_cond_handles.end()) { - return &kv->second; - } else { - using rclcpp::contexts::default_context::get_global_default_context; - if (nullptr == context) { - context = get_global_default_context(); - } - rcl_guard_condition_t handle = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); - auto ret = rcl_guard_condition_init(&handle, context->get_rcl_context().get(), options); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize guard condition: "); - } - g_sigint_guard_cond_handles[wait_set] = handle; - return &g_sigint_guard_cond_handles[wait_set]; - } -} - -void -rclcpp::release_sigint_guard_condition(rcl_wait_set_t * wait_set) -{ - std::lock_guard lock(g_sigint_guard_cond_handles_mutex); - auto kv = g_sigint_guard_cond_handles.find(wait_set); - if (kv != g_sigint_guard_cond_handles.end()) { - if (rcl_guard_condition_fini(&kv->second) != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error(std::string( - "Failed to destroy sigint guard condition: ") + - rcl_get_error_string().str); - // *INDENT-ON* - } - g_sigint_guard_cond_handles.erase(kv); - } else { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error(std::string( - "Tried to release sigint guard condition for nonexistent wait set")); - // *INDENT-ON* - } -} - bool rclcpp::sleep_for(const std::chrono::nanoseconds & nanoseconds, rclcpp::Context::SharedPtr context) { - std::chrono::nanoseconds time_left = nanoseconds; - { - std::unique_lock lock(::g_interrupt_mutex); - auto start = std::chrono::steady_clock::now(); - // this will release the lock while waiting - ::g_interrupt_condition_variable.wait_for(lock, nanoseconds); - time_left -= std::chrono::steady_clock::now() - start; + using rclcpp::contexts::default_context::get_global_default_context; + if (nullptr == context) { + context = get_global_default_context(); } - if (time_left > std::chrono::nanoseconds::zero() && ok(context)) { - return sleep_for(time_left); - } - // Return true if the timeout elapsed successfully, otherwise false. - return ok(context); -} - -void -rclcpp::notify_all() -{ - { - std::lock_guard lock(g_sigint_guard_cond_handles_mutex); - for (auto & kv : g_sigint_guard_cond_handles) { - rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second)); - if (status != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to trigger guard condition: %s", rcl_get_error_string().str); - } - } - } - g_interrupt_condition_variable.notify_all(); + return context->sleep_for(nanoseconds); } const char *