diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index f91a27252d..b8ecabf6d1 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -59,7 +59,7 @@ class OnShutdownCallbackHandle using OnShutdownCallbackType = std::function; private: - OnShutdownCallbackType callback; + std::weak_ptr callback; }; /// Context which encapsulates shared state between nodes and other similar entities. @@ -234,22 +234,23 @@ class Context : public std::enable_shared_from_this * 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 shared pointer of callback handler + * \param[in] callback the on_shutdown callback to be registered + * \return the created callback handle */ RCLCPP_PUBLIC virtual - OnShutdownCallbackHandle::WeakPtr + OnShutdownCallbackHandle add_on_shutdown_callback(OnShutdownCallback callback); /// Remove an registered on_shutdown callbacks. /** - * \param[in] callback_handle the handle to be removed. + * \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 virtual - void - remove_on_shutdown_callback(const OnShutdownCallbackHandle::WeakPtr & callback_handle); + bool + remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle); /// Return the shutdown callbacks. /** @@ -337,7 +338,7 @@ class Context : public std::enable_shared_from_this // attempt to acquire another sub context. std::recursive_mutex sub_contexts_mutex_; - std::unordered_set 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 2c5a167f1a..13ed108348 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::WeakPtr shutdown_callback_handle_; + rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 915cd5434a..827ea9163d 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -318,8 +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 : on_shutdown_callbacks_) { - callback_handle->callback(); + for (const auto & callback : on_shutdown_callbacks_) { + (*callback)(); } } @@ -349,43 +349,44 @@ Context::shutdown(const std::string & reason) rclcpp::Context::OnShutdownCallback Context::on_shutdown(OnShutdownCallback callback) { - std::weak_ptr callback_handle = - add_on_shutdown_callback(callback); - return callback_handle.lock()->callback; + OnShutdownCallbackHandle callback_handle = add_on_shutdown_callback(callback); + return *(callback_handle.callback.lock()); } -rclcpp::OnShutdownCallbackHandle::WeakPtr +rclcpp::OnShutdownCallbackHandle Context::add_on_shutdown_callback(OnShutdownCallback callback) { - auto handle = std::make_shared(); - handle->callback = callback; + auto callback_shared_ptr = std::make_shared(callback); { std::lock_guard lock(on_shutdown_callbacks_mutex_); - on_shutdown_callbacks_.emplace(handle); + on_shutdown_callbacks_.emplace(callback_shared_ptr); } - return handle; + + OnShutdownCallbackHandle callback_handle; + callback_handle.callback = callback_shared_ptr; + return callback_handle; } -void -Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle::WeakPtr & callback_handle) +bool +Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) { std::lock_guard lock(on_shutdown_callbacks_mutex_); - auto callback_handle_shared_ptr = callback_handle.lock(); - if (callback_handle_shared_ptr) { - on_shutdown_callbacks_.erase(callback_handle_shared_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; } std::vector Context::get_on_shutdown_callbacks() { std::vector callbacks; - std::unordered_set::iterator iter; { std::lock_guard lock(on_shutdown_callbacks_mutex_); - for (iter = on_shutdown_callbacks_.begin(); iter != on_shutdown_callbacks_.end(); ++iter) { - callbacks.emplace_back((*iter)->callback); + for (auto & iter : on_shutdown_callbacks_) { + callbacks.emplace_back(*iter); } } diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp index 14e09cf5d6..c83c0dbaa0 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -119,6 +119,12 @@ TEST(TestUtilities, test_context_basic_access) { EXPECT_EQ(std::string{""}, context1->shutdown_reason()); } +TEST(TestUtilities, test_context_basic_access_const_methods) { + auto context1 = std::make_shared(); + + EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); +} + MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)