From e34f4dcea3eb4451e51ffb2772c01a7ed04b9995 Mon Sep 17 00:00:00 2001
From: Michael Carroll <mjcarroll@intrinsic.ai>
Date: Tue, 2 May 2023 13:41:36 +0000
Subject: [PATCH] Introduce method to clear expired entities from a collection

Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>
---
 .../executor_entities_collection.hpp          |  6 +++++
 .../executor_entities_collection.cpp          | 25 +++++++++++++++++++
 2 files changed, 31 insertions(+)

diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp
index 166bb99119..517894a2a2 100644
--- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp
+++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp
@@ -178,6 +178,12 @@ struct ExecutorEntitiesCollection
 
   /// Clear the entities collection
   void clear();
+
+  /// Remove entities that have expired weak ownership
+  /**
+   * \return The total number of removed entities
+   */
+  size_t remove_expired_entities();
 };
 
 /// Build an entities collection from callback groups
diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp
index 88a878824a..394e4c1b89 100644
--- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp
+++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp
@@ -39,6 +39,31 @@ void ExecutorEntitiesCollection::clear()
   waitables.clear();
 }
 
+size_t ExecutorEntitiesCollection::remove_expired_entities()
+{
+  auto remove_entities = [](auto & collection) -> size_t {
+    size_t removed = 0;
+    for (auto it = collection.begin(); it != collection.end(); )
+    {
+      if (it->second.entity.expired()) {
+        ++removed;
+        it = collection.erase(it);
+      } else {
+        ++it;
+      }
+    }
+    return removed;
+  };
+
+  return (
+    remove_entities(subscriptions) +
+    remove_entities(timers) +
+    remove_entities(guard_conditions) +
+    remove_entities(clients) +
+    remove_entities(services) +
+    remove_entities(waitables));
+}
+
 void
 build_entities_collection(
   const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,