Avoid losing waitable handles while using MultiThreadedExecutor

Signed-off-by: Barry Xu <[email protected]>

Author: Barry-Xu-2018

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -120,8 +120,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
       }
     }
     for (size_t i = 0; i < waitable_handles_.size(); ++i) {
-      if (!waitable_handles_[i]->is_ready(wait_set)) {
-        waitable_handles_[i].reset();
+      if (waitable_handles_[i]->is_ready(wait_set)) {
+        waitable_triggered_handles_.emplace_back(waitable_handles_[i]);
       }
     }
 
@@ -145,10 +145,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
       timer_handles_.end()
     );
 
-    waitable_handles_.erase(
-      std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
-      waitable_handles_.end()
-    );
+    waitable_handles_.clear();
   }
 
   bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
@@ -392,16 +389,18 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
     rclcpp::AnyExecutable & any_exec,
     const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
   {
-    auto it = waitable_handles_.begin();
-    while (it != waitable_handles_.end()) {
-      std::shared_ptr<Waitable> & waitable = *it;
+    auto & waitable_handles =
+      waitable_triggered_handles_.empty() ? waitable_handles_ : waitable_triggered_handles_;
+    auto it = waitable_handles.begin();
+    while (it != waitable_handles.end()) {
+      std::shared_ptr<Waitable> waitable = *it;
       if (waitable) {
         // Find the group for this handle and see if it can be serviced
         auto group = get_group_by_waitable(waitable, weak_groups_to_nodes);
         if (!group) {
           // Group was not found, meaning the waitable is not valid...
           // Remove it from the ready list and continue looking
-          it = waitable_handles_.erase(it);
+          it = waitable_handles.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -414,11 +413,11 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
         any_exec.waitable = waitable;
         any_exec.callback_group = group;
         any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes);
-        waitable_handles_.erase(it);
+        waitable_handles.erase(it);
         return;
       }
       // Else, the waitable is no longer valid, remove it and continue
-      it = waitable_handles_.erase(it);
+      it = waitable_handles.erase(it);
     }
   }
 
@@ -499,6 +498,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
   VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
   VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
 
+  VectorRebind<std::shared_ptr<Waitable>> waitable_triggered_handles_;
+
   std::shared_ptr<VoidAlloc> allocator_;
 };