diff --git a/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp index 2b02f9ec6e..c761ca67dc 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -16,19 +16,12 @@ #include #include +#include +#include #include #include #include -// includes for semaphore notification code -#if defined(_WIN32) -#include -#elif defined(__APPLE__) -#include -#else // posix -#include -#endif - #include "rclcpp/logging.hpp" #include "rclcpp/utilities.hpp" #include "rcutils/strerror.h" @@ -102,6 +95,21 @@ SignalHandler::signal_handler(int signum) } #endif +void rclcpp::SignalHandler::signal_handler_common(int signum) noexcept +{ + switch(signum) { + case SIGTERM: + notify_deferred_handler(Input::SigTerm); + break; + case SIGINT: + notify_deferred_handler(Input::SigInt); + break; + default: + break; + } +} + + rclcpp::Logger & SignalHandler::get_logger() { @@ -119,17 +127,20 @@ bool SignalHandler::install(SignalHandlerOptions signal_handler_options) { std::lock_guard lock(install_mutex_); - bool already_installed = installed_.exchange(true); - if (already_installed) { + if (installed_) { return false; } if (signal_handler_options == SignalHandlerOptions::None) { return true; } + + // Reset state in case someone uninstalls and reinstall handlers + got_sig_int = false; + got_sig_term = false; + terminate_handler_ = false; + signal_handlers_options_ = signal_handler_options; try { - setup_wait_for_signal(); - signal_received_.store(false); SignalHandler::signal_handler_type handler_argument; #if defined(RCLCPP_HAS_SIGACTION) @@ -156,9 +167,10 @@ SignalHandler::install(SignalHandlerOptions signal_handler_options) signal_handler_thread_ = std::thread(&SignalHandler::deferred_signal_handler, this); } catch (...) { - installed_.store(false); throw; } + installed_ = true; + RCLCPP_DEBUG(get_logger(), "signal handler installed"); return true; } @@ -167,11 +179,18 @@ bool SignalHandler::uninstall() { std::lock_guard lock(install_mutex_); - bool installed = installed_.exchange(false); - if (!installed) { + if (!installed_) { return false; } + + RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): shutting down deferred signal handler"); + notify_deferred_handler(Input::TerminateHandler); + if (signal_handler_thread_.joinable()) { + signal_handler_thread_.join(); + } + try { + RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): restoring signal handlers"); // TODO(wjwwood): what happens if someone overrides our signal handler then calls uninstall? // I think we need to assert that we're the current signal handler, and mitigate if not. if ( @@ -187,16 +206,10 @@ SignalHandler::uninstall() set_signal_handler(SIGTERM, old_sigterm_handler_); } signal_handlers_options_ = SignalHandlerOptions::None; - RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler"); - notify_signal_handler(); - if (signal_handler_thread_.joinable()) { - signal_handler_thread_.join(); - } - teardown_wait_for_signal(); } catch (...) { - installed_.exchange(true); throw; } + installed_ = false; RCLCPP_DEBUG(get_logger(), "signal handler uninstalled"); return true; } @@ -204,7 +217,8 @@ SignalHandler::uninstall() bool SignalHandler::is_installed() { - return installed_.load(); + std::lock_guard lock(install_mutex_); + return installed_; } SignalHandler::~SignalHandler() @@ -242,143 +256,79 @@ SignalHandler::get_old_signal_handler(int signum) #endif } -void -SignalHandler::signal_handler_common(int signum) -{ - auto & instance = SignalHandler::get_global_signal_handler(); - instance.signal_number_.store(signum); - instance.signal_received_.store(true); - instance.notify_signal_handler(); -} - void SignalHandler::deferred_signal_handler() { - while (true) { - if (signal_received_.exchange(false)) { - int signum = signal_number_.load(); - RCLCPP_INFO(get_logger(), "signal_handler(signum=%d)", signum); - RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): shutting down"); - for (auto context_ptr : rclcpp::get_contexts()) { - if (context_ptr->get_init_options().shutdown_on_signal) { - RCLCPP_DEBUG( + bool running = true; + while (running) { + std::optional next; + + RCLCPP_DEBUG( + get_logger(), "deferred_signal_handler(): waiting for SIGINT/SIGTERM or uninstall"); + { + std::unique_lock l(signal_mutex_); + signal_conditional_.wait(l, [this] () { + return terminate_handler_ || got_sig_int || got_sig_term; + }); + + if(terminate_handler_.exchange(false)) { + next = Input::TerminateHandler; + } + if(got_sig_int.exchange(false)) { + RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(SIGINT)"); + next = Input::SigInt; + } + if(got_sig_term.exchange(false)) { + RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(SIGTERM)"); + next = Input::SigTerm; + } + } + RCLCPP_DEBUG( + get_logger(), "deferred_signal_handler(): woken up due to SIGINT/SIGTERM or uninstall"); + + // Note 'next' must always be valid at this point, if not + // a throw is the expected behaviour + switch(next.value()) { + case Input::SigInt: + [[fallthrough]]; + case Input::SigTerm: + { + RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): shutting down"); + for (auto context_ptr : rclcpp::get_contexts()) { + if (context_ptr->get_init_options().shutdown_on_signal) { + RCLCPP_DEBUG( get_logger(), "deferred_signal_handler(): " "shutting down rclcpp::Context @ %p, because it had shutdown_on_signal == true", static_cast(context_ptr.get())); - context_ptr->shutdown("signal handler"); + context_ptr->shutdown("signal handler"); + } + } + break; } - } + case Input::TerminateHandler: + running = false; + break; } - if (!is_installed()) { - RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): signal handling uninstalled"); - break; - } - RCLCPP_DEBUG( - get_logger(), "deferred_signal_handler(): waiting for SIGINT/SIGTERM or uninstall"); - wait_for_signal(); - RCLCPP_DEBUG( - get_logger(), "deferred_signal_handler(): woken up due to SIGINT/SIGTERM or uninstall"); - } -} - -void -SignalHandler::setup_wait_for_signal() -{ -#if defined(_WIN32) - signal_handler_sem_ = CreateSemaphore( - NULL, // default security attributes - 0, // initial semaphore count - 1, // maximum semaphore count - NULL); // unnamed semaphore - if (NULL == signal_handler_sem_) { - throw std::runtime_error("CreateSemaphore() failed in setup_wait_for_signal()"); - } -#elif defined(__APPLE__) - signal_handler_sem_ = dispatch_semaphore_create(0); -#else // posix - if (-1 == sem_init(&signal_handler_sem_, 0, 0)) { - throw std::runtime_error(std::string("sem_init() failed: ") + strerror(errno)); - } -#endif - wait_for_signal_is_setup_.store(true); -} - -void -SignalHandler::teardown_wait_for_signal() noexcept -{ - if (!wait_for_signal_is_setup_.exchange(false)) { - return; } -#if defined(_WIN32) - CloseHandle(signal_handler_sem_); -#elif defined(__APPLE__) - dispatch_release(signal_handler_sem_); -#else // posix - if (-1 == sem_destroy(&signal_handler_sem_)) { - RCLCPP_ERROR(get_logger(), "invalid semaphore in teardown_wait_for_signal()"); - } -#endif } void -SignalHandler::wait_for_signal() +SignalHandler::notify_deferred_handler(Input input) noexcept { - if (!wait_for_signal_is_setup_.load()) { - RCLCPP_ERROR(get_logger(), "called wait_for_signal() before setup_wait_for_signal()"); - return; - } -#if defined(_WIN32) - DWORD dw_wait_result = WaitForSingleObject(signal_handler_sem_, INFINITE); - switch (dw_wait_result) { - case WAIT_ABANDONED: - RCLCPP_ERROR( - get_logger(), "WaitForSingleObject() failed in wait_for_signal() with WAIT_ABANDONED: %s", - GetLastError()); - break; - case WAIT_OBJECT_0: - // successful + switch(input) { + case Input::SigInt: + got_sig_int.exchange(true); break; - case WAIT_TIMEOUT: - RCLCPP_ERROR(get_logger(), "WaitForSingleObject() timedout out in wait_for_signal()"); + case Input::SigTerm: + got_sig_term.exchange(true); break; - case WAIT_FAILED: - RCLCPP_ERROR( - get_logger(), "WaitForSingleObject() failed in wait_for_signal(): %s", GetLastError()); + case Input::TerminateHandler: + terminate_handler_.exchange(true); break; - default: - RCLCPP_ERROR( - get_logger(), "WaitForSingleObject() gave unknown return in wait_for_signal(): %s", - GetLastError()); } -#elif defined(__APPLE__) - dispatch_semaphore_wait(signal_handler_sem_, DISPATCH_TIME_FOREVER); -#else // posix - int s; - do { - s = sem_wait(&signal_handler_sem_); - } while (-1 == s && EINTR == errno); -#endif -} -void -SignalHandler::notify_signal_handler() noexcept -{ - if (!wait_for_signal_is_setup_.load()) { - return; - } -#if defined(_WIN32) - if (!ReleaseSemaphore(signal_handler_sem_, 1, NULL)) { - RCLCPP_ERROR( - get_logger(), "ReleaseSemaphore() failed in notify_signal_handler(): %s", GetLastError()); - } -#elif defined(__APPLE__) - dispatch_semaphore_signal(signal_handler_sem_); -#else // posix - if (-1 == sem_post(&signal_handler_sem_)) { - RCLCPP_ERROR(get_logger(), "sem_post failed in notify_signal_handler()"); - } -#endif + signal_conditional_.notify_one(); } rclcpp::SignalHandlerOptions diff --git a/rclcpp/src/rclcpp/signal_handler.hpp b/rclcpp/src/rclcpp/signal_handler.hpp index fdaae6d684..36ab7b2da3 100644 --- a/rclcpp/src/rclcpp/signal_handler.hpp +++ b/rclcpp/src/rclcpp/signal_handler.hpp @@ -17,21 +17,13 @@ #include #include +#include #include #include #include "rclcpp/logging.hpp" #include "rclcpp/utilities.hpp" -// includes for semaphore notification code -#if defined(_WIN32) -#include -#elif defined(__APPLE__) -#include -#else // posix -#include -#endif - // Determine if sigaction is available #if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE #define RCLCPP_HAS_SIGACTION @@ -114,10 +106,6 @@ class SignalHandler final SignalHandler && operator=(SignalHandler &&) = delete; - /// Common signal handler code between sigaction and non-sigaction versions. - void - signal_handler_common(int signum); - #if defined(RCLCPP_HAS_SIGACTION) /// Signal handler function. static @@ -130,37 +118,21 @@ class SignalHandler final signal_handler(int signal_value); #endif - /// Target of the dedicated signal handling thread. - void - deferred_signal_handler(); - - /// Setup anything that is necessary for wait_for_signal() or notify_signal_handler(). - /** - * This must be called before wait_for_signal() or notify_signal_handler(). - * This is not thread-safe. - */ void - setup_wait_for_signal(); + signal_handler_common(int signum) noexcept; - /// Undo all setup done in setup_wait_for_signal(). - /** - * Must not call wait_for_signal() or notify_signal_handler() after calling this. - * - * This is not thread-safe. - */ + /// Target of the dedicated signal handling thread. void - teardown_wait_for_signal() noexcept; + deferred_signal_handler(); - /// Wait for a notification from notify_signal_handler() in a signal safe way. - /** - * This static method may throw if posting the semaphore fails. - * - * This is not thread-safe. - */ - void - wait_for_signal(); + enum class Input + { + TerminateHandler, + SigInt, + SigTerm, + }; - /// Notify blocking wait_for_signal() calls in a signal safe way. + /// Notify blocking deferred_signal_handler in a signal safe way. /** * This is used to notify the deferred_signal_handler() thread to start work * from the signal handler. @@ -168,7 +140,7 @@ class SignalHandler final * This is thread-safe. */ void - notify_signal_handler() noexcept; + notify_deferred_handler(Input input) noexcept; static signal_handler_type @@ -187,28 +159,24 @@ class SignalHandler final // logger instance rclcpp::Logger logger_ = rclcpp::get_logger("rclcpp"); - // Whether or not a signal has been received. - std::atomic_bool signal_received_ = false; - // The signal number that was received. - std::atomic_int signal_number_ = 0; // A thread to which signal handling tasks are deferred. std::thread signal_handler_thread_; // A mutex used to synchronize the install() and uninstall() methods. std::mutex install_mutex_; // Whether or not the signal handler has been installed. - std::atomic_bool installed_ = false; - - // Whether or not the semaphore for wait_for_signal is setup. - std::atomic_bool wait_for_signal_is_setup_; - // Storage for the wait_for_signal semaphore. -#if defined(_WIN32) - HANDLE signal_handler_sem_; -#elif defined(__APPLE__) - dispatch_semaphore_t signal_handler_sem_; -#else // posix - sem_t signal_handler_sem_; -#endif + bool installed_ = false; + + // Set to true, if signals handlers are called + std::atomic_bool got_sig_int = false; + std::atomic_bool got_sig_term = false; + // If set to true, the background thread shall terminate + std::atomic_bool terminate_handler_ = false; + + // Mutex and conditional used to signal changes of the above variables + std::mutex signal_mutex_; + std::condition_variable signal_conditional_; + }; } // namespace rclcpp