Allow removing a waitable (#597)

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_;

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)
 

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

rclcpp/src/rclcpp/callback_group.cpp

@@ -105,3 +105,16 @@ CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr)
   std::lock_guard<std::mutex> lock(mutex_);
   waitable_ptrs_.push_back(waitable_ptr);
 }
+
+void
+CallbackGroup::remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept
+{
+  std::lock_guard<std::mutex> 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;
+    }
+  }
+}

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);
+  }
+}