diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index e2bee1c..c53bffb 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -112,6 +112,10 @@ protected: void add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr); + RCLCPP_PUBLIC + void + remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept; + CallbackGroupType type_; // Mutex to protect the subsequent vectors of pointers. mutable std::mutex mutex_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp index 1b05a3c..e16ff8e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp @@ -48,6 +48,13 @@ public: rclcpp::Waitable::SharedPtr waitable_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); + RCLCPP_PUBLIC + virtual + void + remove_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept; + private: RCLCPP_DISABLE_COPY(NodeWaitables) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp index ea05c73..ef78f52 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp @@ -37,6 +37,14 @@ public: add_waitable( rclcpp::Waitable::SharedPtr waitable_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + + /// \note this function should not throw because it may be called in destructors + RCLCPP_PUBLIC + virtual + void + remove_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept = 0; }; } // namespace node_interfaces diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 21a1acb..b812afc 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -105,3 +105,16 @@ CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) std::lock_guard lock(mutex_); waitable_ptrs_.push_back(waitable_ptr); } + +void +CallbackGroup::remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept +{ + std::lock_guard lock(mutex_); + for (auto iter = waitable_ptrs_.begin(); iter != waitable_ptrs_.end(); ++iter) { + const auto shared_ptr = iter->lock(); + if (shared_ptr.get() == waitable_ptr.get()) { + waitable_ptrs_.erase(iter); + break; + } + } +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 04a1c5c..ee9a2da 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -51,3 +51,18 @@ NodeWaitables::add_waitable( } } } + +void +NodeWaitables::remove_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + return; + } + group->remove_waitable(waitable_ptr); + } else { + node_base_->get_default_callback_group()->remove_waitable(waitable_ptr); + } +}