diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 7d7472c88b..227fc3928c 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -48,17 +48,20 @@ class ContextAlreadyInitialized : public std::runtime_error /// Forward declare WeakContextsWrapper class WeakContextsWrapper; -class OnShutdownCallbackHandle +class ShutdownCallbackHandle { friend class Context; public: - using OnShutdownCallbackType = std::function; + using ShutdownCallbackType = std::function; private: - std::weak_ptr callback; + std::weak_ptr callback; }; +using OnShutdownCallbackHandle = ShutdownCallbackHandle; +using PreShutdownCallbackHandle = ShutdownCallbackHandle; + /// Context which encapsulates shared state between nodes and other similar entities. /** * A context also represents the lifecycle between init and shutdown of rclcpp. @@ -189,7 +192,7 @@ class Context : public std::enable_shared_from_this bool shutdown(const std::string & reason); - using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType; + using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType; /// Add a on_shutdown callback to be called when shutdown is called for this context. /** @@ -197,7 +200,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. @@ -221,7 +224,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 - * asynchronously in the dedicated singal handling thread. + * asynchronously in the dedicated signal handling thread. * * Also, shutdown() may be called from the destructor of this function. * Therefore, it is not safe to throw exceptions from these callbacks. @@ -249,6 +252,33 @@ class Context : public std::enable_shared_from_this bool remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle); + using PreShutdownCallback = PreShutdownCallbackHandle::ShutdownCallbackType; + + /// Add a pre_shutdown callback to be called before shutdown is called for this context. + /** + * These callbacks will be called in the order they are added. + * + * When shutdown occurs due to the signal handler, these callbacks are run + * asynchronously in the dedicated signal handling thread. + * + * \param[in] callback the pre_shutdown callback to be registered + * \return the created callback handle + */ + RCLCPP_PUBLIC + virtual + PreShutdownCallbackHandle + add_pre_shutdown_callback(PreShutdownCallback callback); + + /// Remove an registered pre_shutdown callback. + /** + * \param[in] callback_handle the pre_shutdown callback handle to be removed. + * \return true if the callback is found and removed, otherwise false. + */ + RCLCPP_PUBLIC + virtual + bool + remove_pre_shutdown_callback(const PreShutdownCallbackHandle & callback_handle); + /// Return the shutdown callbacks. /** * Returned callbacks are a copy of the registered callbacks. @@ -257,6 +287,14 @@ class Context : public std::enable_shared_from_this std::vector get_on_shutdown_callbacks() const; + /// Return the pre-shutdown callbacks. + /** + * Returned callbacks are a copy of the registered callbacks. + */ + RCLCPP_PUBLIC + std::vector + get_pre_shutdown_callbacks() const; + /// Return the internal rcl context. RCLCPP_PUBLIC std::shared_ptr @@ -338,6 +376,9 @@ class Context : public std::enable_shared_from_this std::unordered_set> on_shutdown_callbacks_; mutable std::mutex on_shutdown_callbacks_mutex_; + std::unordered_set> pre_shutdown_callbacks_; + mutable std::mutex pre_shutdown_callbacks_mutex_; + /// Condition variable for timed sleep (see sleep_for). std::condition_variable interrupt_condition_variable_; /// Mutex for protecting the global condition variable. @@ -345,6 +386,29 @@ class Context : public std::enable_shared_from_this /// Keep shared ownership of global vector of weak contexts std::shared_ptr weak_contexts_; + + enum class ShutdownType + { + pre_shutdown, + on_shutdown + }; + + using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType; + + RCLCPP_LOCAL + ShutdownCallbackHandle + add_shutdown_callback( + ShutdownType shutdown_type, + ShutdownCallback callback); + + RCLCPP_LOCAL + bool + remove_shutdown_callback( + ShutdownType shutdown_type, + const ShutdownCallbackHandle & callback_handle); + + std::vector + get_shutdown_callback(ShutdownType shutdown_type) const; }; /// Return a copy of the list of context shared pointers. diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index b86fe8eafc..fff5753954 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" @@ -307,6 +308,15 @@ Context::shutdown(const std::string & reason) // if it is not valid, then it cannot be shutdown return false; } + + // call each pre-shutdown callback + { + std::lock_guard lock{pre_shutdown_callbacks_mutex_}; + for (const auto & callback : pre_shutdown_callbacks_) { + (*callback)(); + } + } + // rcl shutdown rcl_ret_t ret = rcl_shutdown(rcl_context_.get()); if (RCL_RET_OK != ret) { @@ -355,36 +365,118 @@ Context::on_shutdown(OnShutdownCallback callback) rclcpp::OnShutdownCallbackHandle Context::add_on_shutdown_callback(OnShutdownCallback callback) { - auto callback_shared_ptr = std::make_shared(callback); - { - std::lock_guard lock(on_shutdown_callbacks_mutex_); - on_shutdown_callbacks_.emplace(callback_shared_ptr); + return add_shutdown_callback(ShutdownType::on_shutdown, callback); +} + +bool +Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) +{ + return remove_shutdown_callback(ShutdownType::on_shutdown, callback_handle); +} + +rclcpp::PreShutdownCallbackHandle +Context::add_pre_shutdown_callback(PreShutdownCallback callback) +{ + return add_shutdown_callback(ShutdownType::pre_shutdown, callback); +} + +bool +Context::remove_pre_shutdown_callback( + const PreShutdownCallbackHandle & callback_handle) +{ + return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle); +} + +rclcpp::ShutdownCallbackHandle +Context::add_shutdown_callback( + ShutdownType shutdown_type, + ShutdownCallback callback) +{ + auto callback_shared_ptr = + std::make_shared(callback); + + switch (shutdown_type) { + case ShutdownType::pre_shutdown: + { + std::lock_guard lock(pre_shutdown_callbacks_mutex_); + pre_shutdown_callbacks_.emplace(callback_shared_ptr); + } + break; + case ShutdownType::on_shutdown: + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + on_shutdown_callbacks_.emplace(callback_shared_ptr); + } + break; } - OnShutdownCallbackHandle callback_handle; + ShutdownCallbackHandle callback_handle; callback_handle.callback = callback_shared_ptr; return callback_handle; } bool -Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) +Context::remove_shutdown_callback( + ShutdownType shutdown_type, + const ShutdownCallbackHandle & callback_handle) { - std::lock_guard lock(on_shutdown_callbacks_mutex_); + std::mutex * mutex_ptr = nullptr; + std::unordered_set< + std::shared_ptr> * callback_list_ptr; + + switch (shutdown_type) { + case ShutdownType::pre_shutdown: + mutex_ptr = &pre_shutdown_callbacks_mutex_; + callback_list_ptr = &pre_shutdown_callbacks_; + break; + case ShutdownType::on_shutdown: + mutex_ptr = &on_shutdown_callbacks_mutex_; + callback_list_ptr = &on_shutdown_callbacks_; + break; + } + + std::lock_guard lock(*mutex_ptr); auto callback_shared_ptr = callback_handle.callback.lock(); if (callback_shared_ptr == nullptr) { return false; } - return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1; + return callback_list_ptr->erase(callback_shared_ptr) == 1; } std::vector Context::get_on_shutdown_callbacks() const { - std::vector callbacks; + return get_shutdown_callback(ShutdownType::on_shutdown); +} + +std::vector +Context::get_pre_shutdown_callbacks() const +{ + return get_shutdown_callback(ShutdownType::pre_shutdown); +} +std::vector +Context::get_shutdown_callback(ShutdownType shutdown_type) const +{ + std::mutex * mutex_ptr = nullptr; + const std::unordered_set< + std::shared_ptr> * callback_list_ptr; + + switch (shutdown_type) { + case ShutdownType::pre_shutdown: + mutex_ptr = &pre_shutdown_callbacks_mutex_; + callback_list_ptr = &pre_shutdown_callbacks_; + break; + case ShutdownType::on_shutdown: + mutex_ptr = &on_shutdown_callbacks_mutex_; + callback_list_ptr = &on_shutdown_callbacks_; + break; + } + + std::vector callbacks; { - std::lock_guard lock(on_shutdown_callbacks_mutex_); - for (auto & iter : on_shutdown_callbacks_) { + std::lock_guard lock(*mutex_ptr); + for (auto & iter : *callback_list_ptr) { callbacks.emplace_back(*iter); } } diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp index c57fcb9fc3..c177efb7fc 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -111,10 +111,42 @@ TEST(TestUtilities, multi_init) { EXPECT_FALSE(rclcpp::ok(context2)); } +TEST(TestUtilities, test_pre_shutdown_callback_add_remove) { + auto context1 = std::make_shared(); + context1->init(0, nullptr); + + bool is_called1 = false; + bool is_called2 = false; + auto callback1 = [&is_called1]() {is_called1 = true;}; + auto callback2 = [&is_called2]() {is_called2 = true;}; + + EXPECT_EQ(0u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::PreShutdownCallbackHandle callback_handle1 = + context1->add_pre_shutdown_callback(callback1); + EXPECT_EQ(1u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::PreShutdownCallbackHandle callback_handle2 = + context1->add_pre_shutdown_callback(callback2); + EXPECT_EQ(2u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::PreShutdownCallbackHandle wrong_callback_handle; + EXPECT_FALSE(context1->remove_pre_shutdown_callback(wrong_callback_handle)); + + EXPECT_TRUE(context1->remove_pre_shutdown_callback(callback_handle1)); + EXPECT_EQ(1u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::shutdown(context1); + + EXPECT_FALSE(is_called1); + EXPECT_TRUE(is_called2); +} + TEST(TestUtilities, test_context_basic_access) { auto context1 = std::make_shared(); EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + EXPECT_EQ(0u, context1->get_pre_shutdown_callbacks().size()); EXPECT_EQ(std::string{""}, context1->shutdown_reason()); } @@ -123,6 +155,7 @@ TEST(TestUtilities, test_context_basic_access_const_methods) { EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + EXPECT_EQ(0u, context1->get_pre_shutdown_callbacks().size()); } MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)