diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index e511f93b4b..dee864c37f 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -192,7 +193,7 @@ class Context : public std::enable_shared_from_this * to last step in shutdown(). * * When shutdown occurs due to the signal handler, these callbacks are run - * asynchronoulsy in the dedicated singal handling thread. + * 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. @@ -203,21 +204,21 @@ class Context : public std::enable_shared_from_this * and persist on repeated init's. * * \param[in] callback the on shutdown callback to be registered - * \return the created shared pointer of callback handler + * \return the weak pointer to registered callback handler */ RCLCPP_PUBLIC virtual - OnShutdownCallbackHandle::SharedPtr + OnShutdownCallbackHandle::WeakPtr on_shutdown(OnShutdownCallbackHandle::OnShutdownCallbackType callback); - /// Remove a on_shutdown callback handle registered with on_shutdown. + /// Remove a specified shutdown callback handle. /** - * \param[in] callback_handle the on shutdown callback handle to be removed. + * \param[in] callback_handle the weak pointer to a shutdown callback handle */ RCLCPP_PUBLIC virtual void - remove_on_shutdown_callback(OnShutdownCallbackHandle::SharedPtr callback_handle); + remove_on_shutdown_callback(const OnShutdownCallbackHandle::WeakPtr callback_handle); /// Return the shutdown callback handles as const. /** @@ -225,7 +226,7 @@ class Context : public std::enable_shared_from_this * the list of "on shutdown" callback handles, i.e. on_shutdown(). */ RCLCPP_PUBLIC - const std::vector & + const std::unordered_set & get_on_shutdown_callbacks() const; /// Return the shutdown callback handles @@ -234,7 +235,7 @@ class Context : public std::enable_shared_from_this * the list of "on shutdown" callback handles, i.e. on_shutdown(). */ RCLCPP_PUBLIC - std::vector & + std::unordered_set & get_on_shutdown_callbacks(); /// Return the internal rcl context. @@ -315,7 +316,7 @@ 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::unordered_set on_shutdown_callbacks_; std::mutex on_shutdown_callbacks_mutex_; /// Condition variable for timed sleep (see sleep_for). diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 8fbf2417ed..2c5a167f1a 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -574,7 +574,7 @@ class Executor weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// shutdown callback handle registered to Context - rclcpp::OnShutdownCallbackHandle::SharedPtr shutdown_callback_handle_; + rclcpp::OnShutdownCallbackHandle::WeakPtr shutdown_callback_handle_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index 30305ad2ff..7161d7052e 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -187,6 +187,9 @@ class GraphListener : public std::enable_shared_from_this mutable std::mutex node_graph_interfaces_mutex_; std::vector node_graph_interfaces_; + /// shutdown callback handle registered to Context + rclcpp::OnShutdownCallbackHandle::WeakPtr shutdown_callback_handle_; + rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); }; diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 5011a12c6a..0a4713b9f4 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -177,11 +177,23 @@ shutdown( * \sa rclcpp::Context::on_shutdown() * \param[in] callback to be called when the given context is shutdown * \param[in] context with which to associate the context + * \return the weak pointer to registered callback handler */ RCLCPP_PUBLIC -void +OnShutdownCallbackHandle::WeakPtr on_shutdown(std::function callback, rclcpp::Context::SharedPtr context = nullptr); +/// Remove a specified shutdown callback handle. +/** + * \param[in] callback_handle the weak pointer to a shutdown callback handle + * \param[in] context with which to associate the context + */ +RCLCPP_PUBLIC +void +remove_on_shutdown( + const OnShutdownCallbackHandle::WeakPtr callback_handle, + rclcpp::Context::SharedPtr context = nullptr); + /// Use the global condition variable to block for the specified amount of time. /** * This function can be interrupted early if the associated context becomes diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index ed3054dcb3..c2bbd791d2 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "rcl/init.h" @@ -317,11 +318,8 @@ Context::shutdown(const std::string & reason) // call each shutdown callback { std::lock_guard lock(on_shutdown_callbacks_mutex_); - for (const auto & callback_handle_weak_ptr : on_shutdown_callbacks_) { - auto callback_handle = callback_handle_weak_ptr.lock(); - if (callback_handle) { - callback_handle->callback(); - } + for (const auto & callback_handle : on_shutdown_callbacks_) { + callback_handle->callback(); } } @@ -348,38 +346,35 @@ Context::shutdown(const std::string & reason) return true; } -rclcpp::OnShutdownCallbackHandle::SharedPtr +rclcpp::OnShutdownCallbackHandle::WeakPtr Context::on_shutdown(OnShutdownCallbackHandle::OnShutdownCallbackType callback) { - std::lock_guard lock(on_shutdown_callbacks_mutex_); auto handle = std::make_shared(); handle->callback = callback; - on_shutdown_callbacks_.push_back(handle); + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + on_shutdown_callbacks_.emplace(handle); + } return handle; } void -Context::remove_on_shutdown_callback(OnShutdownCallbackHandle::SharedPtr callback_handle) +Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle::WeakPtr callback_handle) { - std::lock_guard lock(on_shutdown_callbacks_mutex_); - auto it = std::find_if( - on_shutdown_callbacks_.begin(), - on_shutdown_callbacks_.end(), - [callback_handle](const auto & weak_handle) { - return callback_handle.get() == weak_handle.lock().get(); - }); - if (it != on_shutdown_callbacks_.end()) { - on_shutdown_callbacks_.erase(it); + auto callback_handle_shared_ptr = callback_handle.lock(); + if (callback_handle_shared_ptr) { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + on_shutdown_callbacks_.erase(callback_handle_shared_ptr); } } -const std::vector & +const std::unordered_set & Context::get_on_shutdown_callbacks() const { return on_shutdown_callbacks_; } -std::vector & +std::unordered_set & Context::get_on_shutdown_callbacks() { return on_shutdown_callbacks_; diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 0af47acc5b..885d45f103 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -57,6 +57,10 @@ GraphListener::GraphListener(const std::shared_ptr & parent_context) GraphListener::~GraphListener() { + auto parent_context = weak_parent_context_.lock(); + if (parent_context) { + parent_context->remove_on_shutdown_callback(shutdown_callback_handle_); + } this->shutdown(std::nothrow); } @@ -90,7 +94,7 @@ GraphListener::start_if_not_started() // This is important to ensure that the wait set is finalized before // destruction of static objects occurs. std::weak_ptr weak_this = shared_from_this(); - parent_context->on_shutdown( + shutdown_callback_handle_ = parent_context->on_shutdown( [weak_this]() { auto shared_this = weak_this.lock(); if (shared_this) { diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 9b467d57f6..f2bcf07d69 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -20,6 +20,7 @@ #include #include "./signal_handler.hpp" +#include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/detail/utilities.hpp" #include "rclcpp/exceptions.hpp" @@ -177,14 +178,26 @@ shutdown(Context::SharedPtr context, const std::string & reason) return ret; } -void +OnShutdownCallbackHandle::WeakPtr on_shutdown(std::function callback, Context::SharedPtr context) { using rclcpp::contexts::get_global_default_context; if (nullptr == context) { context = get_global_default_context(); } - context->on_shutdown(callback); + return context->on_shutdown(callback); +} + +void +remove_on_shutdown( + const OnShutdownCallbackHandle::WeakPtr callback_handle, + Context::SharedPtr context) +{ + using rclcpp::contexts::get_global_default_context; + if (nullptr == context) { + context = get_global_default_context(); + } + context->remove_on_shutdown_callback(callback_handle); } bool