From 06f99cc6340671affeb0a90ae2539dfa2aeda8cf Mon Sep 17 00:00:00 2001
From: Michael Carroll <mjcarroll@intrinsic.ai>
Date: Tue, 2 Apr 2024 18:44:03 +0000
Subject: [PATCH] Fix callback group logic in executor

Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>
Co-authored-by: Janosch Machowinski <j.machowinski@nospam.org>
---
 rclcpp/src/rclcpp/executor.cpp                      | 13 +++++++------
 .../executors/executor_entities_collection.cpp      | 10 +++++-----
 2 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
index a54d71e21b..771eeebb02 100644
--- a/rclcpp/src/rclcpp/executor.cpp
+++ b/rclcpp/src/rclcpp/executor.cpp
@@ -689,7 +689,8 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
       auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get());
       if (entity_iter != current_collection_.timers.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (callback_group && !callback_group->can_be_taken_from()) {
+        if (!callback_group || !callback_group->can_be_taken_from()) {
+          current_timer_index++;
           continue;
         }
         // At this point the timer is either ready for execution or was perhaps
@@ -699,6 +700,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
         wait_result_->clear_timer_with_index(current_timer_index);
         // Check that the timer should be called still, i.e. it wasn't canceled.
         if (!timer->call()) {
+          current_timer_index++;
           continue;
         }
         any_executable.timer = timer;
@@ -715,7 +717,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
         subscription->get_subscription_handle().get());
       if (entity_iter != current_collection_.subscriptions.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (callback_group && !callback_group->can_be_taken_from()) {
+        if (!callback_group || !callback_group->can_be_taken_from()) {
           continue;
         }
         any_executable.subscription = subscription;
@@ -731,7 +733,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
       auto entity_iter = current_collection_.services.find(service->get_service_handle().get());
       if (entity_iter != current_collection_.services.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (callback_group && !callback_group->can_be_taken_from()) {
+        if (!callback_group || !callback_group->can_be_taken_from()) {
           continue;
         }
         any_executable.service = service;
@@ -747,7 +749,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
       auto entity_iter = current_collection_.clients.find(client->get_client_handle().get());
       if (entity_iter != current_collection_.clients.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (callback_group && !callback_group->can_be_taken_from()) {
+        if (!callback_group || !callback_group->can_be_taken_from()) {
           continue;
         }
         any_executable.client = client;
@@ -763,7 +765,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
       auto entity_iter = current_collection_.waitables.find(waitable.get());
       if (entity_iter != current_collection_.waitables.end()) {
         auto callback_group = entity_iter->second.callback_group.lock();
-        if (callback_group && !callback_group->can_be_taken_from()) {
+        if (!callback_group || !callback_group->can_be_taken_from()) {
           continue;
         }
         any_executable.waitable = waitable;
@@ -782,7 +784,6 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
     }
   }
 
-
   return valid_executable;
 }
 
diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp
index 765002b2ef..68ac56b656 100644
--- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp
+++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp
@@ -153,7 +153,7 @@ ready_executables(
         continue;
       }
       auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
+      if (!group_info || !group_info->can_be_taken_from().load()) {
         continue;
       }
       if (!entity->call()) {
@@ -176,7 +176,7 @@ ready_executables(
         continue;
       }
       auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
+      if (!group_info || !group_info->can_be_taken_from().load()) {
         continue;
       }
       rclcpp::AnyExecutable exec;
@@ -196,7 +196,7 @@ ready_executables(
         continue;
       }
       auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
+      if (!group_info || !group_info->can_be_taken_from().load()) {
         continue;
       }
       rclcpp::AnyExecutable exec;
@@ -216,7 +216,7 @@ ready_executables(
         continue;
       }
       auto group_info = group_cache(entity_iter->second.callback_group);
-      if (group_info && !group_info->can_be_taken_from().load()) {
+      if (!group_info || !group_info->can_be_taken_from().load()) {
         continue;
       }
       rclcpp::AnyExecutable exec;
@@ -236,7 +236,7 @@ ready_executables(
       continue;
     }
     auto group_info = group_cache(entry.callback_group);
-    if (group_info && !group_info->can_be_taken_from().load()) {
+    if (!group_info || !group_info->can_be_taken_from().load()) {
       continue;
     }
     rclcpp::AnyExecutable exec;