diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 9140f3f117..ae6d3d0a99 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -47,6 +48,17 @@ class ContextAlreadyInitialized : public std::runtime_error /// Forward declare WeakContextsWrapper class WeakContextsWrapper; +class OnShutdownCallbackHandle +{ + friend class Context; + +public: + using OnShutdownCallbackType = std::function; + +private: + std::weak_ptr callback; +}; + /// Context which encapsulates shared state between nodes and other similar entities. /** * A context also represents the lifecycle between init and shutdown of rclcpp. @@ -177,7 +189,7 @@ class Context : public std::enable_shared_from_this bool shutdown(const std::string & reason); - using OnShutdownCallback = std::function; + using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType; /// Add a on_shutdown callback to be called when shutdown is called for this context. /** @@ -203,23 +215,47 @@ class Context : public std::enable_shared_from_this OnShutdownCallback on_shutdown(OnShutdownCallback callback); - /// Return the shutdown callbacks as const. + /// Add a on_shutdown callback to be called when shutdown is called for this context. + /** + * These callbacks will be called in the order they are added as the second + * to last step in shutdown(). + * + * When shutdown occurs due to the signal handler, these callbacks are run + * asynchronously 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 + * occurred. + * + * On shutdown callbacks may be registered before init and after shutdown, + * and persist on repeated init's. + * + * \param[in] callback the on_shutdown callback to be registered + * \return the created callback handle + */ + RCLCPP_PUBLIC + virtual + OnShutdownCallbackHandle + add_on_shutdown_callback(OnShutdownCallback callback); + + /// Remove an registered on_shutdown callbacks. /** - * Using the returned reference is not thread-safe with calls that modify - * the list of "on shutdown" callbacks, i.e. on_shutdown(). + * \param[in] callback_handle the on_shutdown callback handle to be removed. + * \return true if the callback is found and removed, otherwise false. */ RCLCPP_PUBLIC - const std::vector & - get_on_shutdown_callbacks() const; + virtual + bool + remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle); /// Return the shutdown callbacks. /** - * Using the returned reference is not thread-safe with calls that modify - * the list of "on shutdown" callbacks, i.e. on_shutdown(). + * Returned callbacks are a copy of the registered callbacks. */ RCLCPP_PUBLIC - std::vector & - get_on_shutdown_callbacks(); + std::vector + get_on_shutdown_callbacks() const; /// Return the internal rcl context. RCLCPP_PUBLIC @@ -299,8 +335,8 @@ class Context : public std::enable_shared_from_this // attempt to acquire another sub context. std::recursive_mutex sub_contexts_mutex_; - std::vector on_shutdown_callbacks_; - std::mutex on_shutdown_callbacks_mutex_; + std::unordered_set> on_shutdown_callbacks_; + mutable std::mutex on_shutdown_callbacks_mutex_; /// Condition variable for timed sleep (see sleep_for). std::condition_variable interrupt_condition_variable_; diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index bc4e167a1e..13ed108348 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -30,6 +30,7 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" +#include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/executor_options.hpp" @@ -571,6 +572,9 @@ class Executor /// nodes that are associated with the executor std::list weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// shutdown callback handle registered to Context + rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 90ea3db908..580689d694 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -315,9 +315,13 @@ Context::shutdown(const std::string & reason) // set shutdown reason shutdown_reason_ = reason; // call each shutdown callback - for (const auto & callback : on_shutdown_callbacks_) { - callback(); + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + for (const auto & callback : on_shutdown_callbacks_) { + (*callback)(); + } } + // interrupt all blocking sleep_for() and all blocking executors or wait sets this->interrupt_all_sleep_for(); // remove self from the global contexts @@ -344,20 +348,48 @@ Context::shutdown(const std::string & reason) rclcpp::Context::OnShutdownCallback Context::on_shutdown(OnShutdownCallback callback) { - on_shutdown_callbacks_.push_back(callback); + add_on_shutdown_callback(callback); return callback; } -const std::vector & -Context::get_on_shutdown_callbacks() const +rclcpp::OnShutdownCallbackHandle +Context::add_on_shutdown_callback(OnShutdownCallback callback) { - return on_shutdown_callbacks_; + auto callback_shared_ptr = std::make_shared(callback); + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + on_shutdown_callbacks_.emplace(callback_shared_ptr); + } + + OnShutdownCallbackHandle callback_handle; + callback_handle.callback = callback_shared_ptr; + return callback_handle; } -std::vector & -Context::get_on_shutdown_callbacks() +bool +Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) { - return on_shutdown_callbacks_; + std::lock_guard lock(on_shutdown_callbacks_mutex_); + auto callback_shared_ptr = callback_handle.callback.lock(); + if (callback_shared_ptr == nullptr) { + return false; + } + return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1; +} + +std::vector +Context::get_on_shutdown_callbacks() const +{ + std::vector callbacks; + + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + for (auto & iter : on_shutdown_callbacks_) { + callbacks.emplace_back(*iter); + } + } + + return callbacks; } std::shared_ptr diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 630db50b9b..20876668a4 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -56,7 +56,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor"); } - context_->on_shutdown( + shutdown_callback_handle_ = context_->add_on_shutdown_callback( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); if (strong_gc) { @@ -138,6 +138,14 @@ Executor::~Executor() } // Remove and release the sigint guard condition memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + + // Remove shutdown callback handle registered to Context + if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to remove registered on_shutdown callback"); + rcl_reset_error(); + } } std::vector