diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index ace7200..82c2717 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -20,6 +20,7 @@ #include "rclcpp/visibility_control.hpp" #include "rcl/guard_condition.h" +#include "rcl/wait.h" #include "rmw/macros.h" #include "rmw/rmw.h" @@ -50,9 +51,33 @@ void shutdown(); /// Get a handle to the rmw guard condition that manages the signal handler. +/** + * The first time that this function is called for a given waitset a new guard + * condition will be created and returned; thereafter the same guard condition + * will be returned for the same waitset. This mechanism is designed to ensure + * that the same guard condition is not reused across waitsets (e.g., when + * using multiple executors in the same process). Will throw an exception if + * initialization of the guard condition fails. + * \param[waitset] waitset 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_global_sigint_guard_condition(); +get_sigint_guard_condition(rcl_wait_set_t * waitset); + +/// Release the previously allocated guard condition that manages the signal handler. +/** + * If you previously called get_sigint_guard_condition() for a given waitset + * 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 waitset. + * \param[waitset] waitset 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 * waitset); /// Use the global condition variable to block for the specified amount of time. /** diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index c5a5ea8..732d890 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -46,7 +46,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::utilities::get_global_sigint_guard_condition()); + memory_strategy_->add_guard_condition(rclcpp::utilities::get_sigint_guard_condition(&waitset_)); // Put the executor's guard condition in memory_strategy_->add_guard_condition(&interrupt_guard_condition_); @@ -77,6 +77,9 @@ Executor::~Executor() fprintf(stderr, "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); } + // Remove and release the sigint guard condition + memory_strategy_->remove_guard_condition(rclcpp::utilities::get_sigint_guard_condition(&waitset_)); + rclcpp::utilities::release_sigint_guard_condition(&waitset_); } void diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 3df249d..e1ae78b 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -35,9 +36,10 @@ /// Represent the status of the global interrupt signal. static volatile sig_atomic_t g_signal_status = 0; -/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired. -static rcl_guard_condition_t g_sigint_guard_cond_handle = - rcl_get_zero_initialized_guard_condition(); +/// Guard conditions for interrupting the rmw implementation when the global interrupt signal fired. +static std::map g_sigint_guard_cond_handles; +/// Mutex to protect g_sigint_guard_cond_handles +static std::mutex g_sigint_guard_cond_handles_mutex; /// Condition variable for timed sleep (see sleep_for). static std::condition_variable g_interrupt_condition_variable; static std::atomic g_is_interrupted(false); @@ -81,10 +83,15 @@ signal_handler(int signal_value) } #endif g_signal_status = signal_value; - rcl_ret_t status = rcl_trigger_guard_condition(&g_sigint_guard_cond_handle); - if (status != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to trigger guard condition: %s\n", rcl_get_error_string_safe()); + { + std::lock_guard lock(g_sigint_guard_cond_handles_mutex); + for (auto const & 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(); @@ -133,17 +140,12 @@ rclcpp::utilities::init(int argc, char * argv[]) #else strerror_s(error_string, error_length, errno); #endif - // *INDENT-OFF* + // *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* } - rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); - if (rcl_guard_condition_init(&g_sigint_guard_cond_handle, options) != RCL_RET_OK) { - throw std::runtime_error(std::string( - "Couldn't initialize guard condition: ") + rcl_get_error_string_safe()); - } } bool @@ -156,18 +158,60 @@ void rclcpp::utilities::shutdown() { g_signal_status = SIGINT; - if (rcl_trigger_guard_condition(&g_sigint_guard_cond_handle) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe()); + { + std::lock_guard lock(g_sigint_guard_cond_handles_mutex); + for (auto const & 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(); } rcl_guard_condition_t * -rclcpp::utilities::get_global_sigint_guard_condition() +rclcpp::utilities::get_sigint_guard_condition(rcl_wait_set_t * waitset) { - return &::g_sigint_guard_cond_handle; + std::lock_guard lock(g_sigint_guard_cond_handles_mutex); + auto kv = g_sigint_guard_cond_handles.find(waitset); + if (kv != g_sigint_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(); + if (rcl_guard_condition_init(&handle, options) != RCL_RET_OK) { + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::runtime_error(std::string( + "Couldn't initialize guard condition: ") + rcl_get_error_string_safe()); + // *INDENT-ON* + } + g_sigint_guard_cond_handles[waitset] = handle; + return &g_sigint_guard_cond_handles[waitset]; + } +} + +void +rclcpp::utilities::release_sigint_guard_condition(rcl_wait_set_t * waitset) +{ + std::lock_guard lock(g_sigint_guard_cond_handles_mutex); + auto kv = g_sigint_guard_cond_handles.find(waitset); + if (kv != g_sigint_guard_cond_handles.end()) { + if (rcl_guard_condition_fini(&kv->second) != RCL_RET_OK) { + throw std::runtime_error(std::string( + "Failed to destroy sigint guard condition: ") + + rcl_get_error_string_safe()); + } + 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 waitset")); + // *INDENT-ON* + } } bool