diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 3322ef4..44e41c1 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -50,76 +50,26 @@ static std::mutex g_interrupt_mutex; #ifdef HAS_SIGACTION static struct sigaction old_action; #else -static void (* old_signal_handler)(int) = 0; +typedef void (* signal_handler_t)(int); +static signal_handler_t old_signal_handler = 0; #endif -void #ifdef HAS_SIGACTION -signal_handler(int signal_value, siginfo_t * siginfo, void * context) +struct sigaction +set_sigaction(int signal_value, const struct sigaction & action) #else -signal_handler(int signal_value) +signal_handler_t +set_signal_handler(int signal_value, signal_handler_t signal_handler) #endif { - // TODO(wjwwood): remove? move to console logging at some point? - printf("signal_handler(%d)\n", 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 { - // *INDENT-OFF* - 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 - // *INDENT-ON* - { - old_action.sa_handler(signal_value); - } - } -#else - if (old_signal_handler) { - old_signal_handler(signal_value); - } -#endif - g_signal_status = signal_value; - { - 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) { - fprintf(stderr, - "[rclcpp::error] failed to trigger guard condition: %s\n", rcl_get_error_string_safe()); - } - } - } - g_is_interrupted.store(true); - g_interrupt_condition_variable.notify_all(); -} - -void -rclcpp::utilities::init(int argc, char * argv[]) -{ - g_is_interrupted.store(false); - if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to initialize rmw implementation: ") + rcl_get_error_string_safe()); - // *INDENT-ON* - } -#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; - ssize_t ret = sigaction(SIGINT, &action, &old_action); + struct sigaction old_action; + ssize_t ret = sigaction(signal_value, &action, &old_action); if (ret == -1) #else - ::old_signal_handler = std::signal(SIGINT, ::signal_handler); + signal_handler_t old_signal_handler = std::signal(signal_value, signal_handler); // NOLINTNEXTLINE(readability/braces) - if (::old_signal_handler == SIG_ERR) + if (old_signal_handler == SIG_ERR) #endif { const size_t error_length = 1024; @@ -147,6 +97,95 @@ rclcpp::utilities::init(int argc, char * argv[]) error_string); // *INDENT-ON* } + +#ifdef HAS_SIGACTION + return old_action; +#else + return old_signal_handler; +#endif +} + +void +trigger_interrupt_guard_condition(int signal_value) +{ + g_signal_status = signal_value; + { + 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) { + fprintf(stderr, + "[rclcpp::error] failed to trigger guard condition: %s\n", rcl_get_error_string_safe()); + } + } + } + g_is_interrupted.store(true); + g_interrupt_condition_variable.notify_all(); +} + +void +#ifdef HAS_SIGACTION +signal_handler(int signal_value, siginfo_t * siginfo, void * context) +#else +signal_handler(int signal_value) +#endif +{ + // TODO(wjwwood): remove? move to console logging at some point? + printf("signal_handler(%d)\n", 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 { + // *INDENT-OFF* + 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 + // *INDENT-ON* + { + old_action.sa_handler(signal_value); + } + } +#else + if (old_signal_handler) { + old_signal_handler(signal_value); + } +#endif + + trigger_interrupt_guard_condition(signal_value); +} + +void +rclcpp::utilities::init(int argc, char * argv[]) +{ + g_is_interrupted.store(false); + if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) { + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::runtime_error( + std::string("failed to initialize rmw implementation: ") + rcl_get_error_string_safe()); + // *INDENT-ON* + } +#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); + // Register an on_shutdown hook to restore the old action. + rclcpp::utilities::on_shutdown([]() { + set_sigaction(SIGINT, ::old_action); + }); +#else + ::old_signal_handler = set_signal_handler(SIGINT, ::signal_handler); + // Register an on_shutdown hook to restore the old signal handler. + rclcpp::utilities::on_shutdown([]() { + set_signal_handler(SIGINT, ::old_signal_handler); + }); +#endif } bool @@ -161,19 +200,8 @@ static std::vector> on_shutdown_callbacks_; void rclcpp::utilities::shutdown() { - g_signal_status = SIGINT; - { - std::lock_guard lock(g_sigint_guard_cond_handles_mutex); - for (auto & kv : g_sigint_guard_cond_handles) { - if (rcl_trigger_guard_condition(&(kv.second)) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to trigger sigint guard condition: %s\n", - rcl_get_error_string_safe()); - } - } - } - g_is_interrupted.store(true); - g_interrupt_condition_variable.notify_all(); + trigger_interrupt_guard_condition(SIGINT); + { std::lock_guard lock(on_shutdown_mutex_); for (auto & on_shutdown_callback : on_shutdown_callbacks_) {