Fix callback group logic in executor

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

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;

rclcpp/test/rclcpp/CMakeLists.txt

@@ -473,6 +473,15 @@ if(TARGET test_executors)
   target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS})
 endif()
 
+ament_add_gtest(
+  test_executors_callback_group_behavior
+  executors/test_executors_callback_group_behavior.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 180)
+if(TARGET test_executors)
+  target_link_libraries(test_executors_callback_group_behavior ${PROJECT_NAME})
+endif()
+
 ament_add_gtest(
   test_executors_intraprocess
   executors/test_executors_intraprocess.cpp

rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp

@@ -0,0 +1,139 @@
+// Copyright 2024 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/**
+ * This test checks that when callback groups go out of scope, that their associated executable
+ * entities should not be returned as valid executables.
+ *
+ * The test makes use of a bit of executor internals, but is meant to prevent regressions of behavior.
+ * Ref: https://github.com/ros2/rclcpp/issues/2474
+ */
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/executor.hpp>
+#include <rclcpp/node.hpp>
+
+class CustomExecutor: public rclcpp::Executor
+{
+public:
+  explicit CustomExecutor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions())
+    : rclcpp::Executor(options)
+  {
+  }
+
+  ~CustomExecutor() override = default;
+
+  void spin() override {};
+
+  void collect() {
+    this->collect_entities();
+  }
+
+  void wait() {
+    this->wait_for_work(std::chrono::milliseconds(10));
+  }
+
+  size_t collected_timers() const {
+    return this->current_collection_.timers.size();
+  }
+
+  rclcpp::AnyExecutable next() {
+    rclcpp::AnyExecutable ret;
+    this->get_next_ready_executable(ret);
+    return ret;
+  }
+};
+
+
+TEST(TestCallbackGroup, ValidCbg)
+{
+  rclcpp::init(0, nullptr);
+
+  // Create a timer associated with a callback group
+  auto node = std::make_shared<rclcpp::Node>("node");
+
+  // Add the callback group to the executor
+  auto executor = CustomExecutor();
+  executor.add_node(node);
+  executor.spin_all(std::chrono::milliseconds(10));
+
+  auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, true);
+  auto timer = node->create_wall_timer(std::chrono::milliseconds(1), [](){}, cbg);
+  executor.add_callback_group(cbg, node->get_node_base_interface());
+
+  executor.spin_all(std::chrono::milliseconds(10));
+
+  // Collect the entities
+  executor.collect();
+  EXPECT_EQ(1u, executor.collected_timers());
+
+  executor.wait();
+  auto next_executable = executor.next();
+  EXPECT_EQ(timer, next_executable.timer);
+  EXPECT_EQ(cbg, next_executable.callback_group);
+
+  EXPECT_EQ(nullptr, next_executable.client);
+  EXPECT_EQ(nullptr, next_executable.service);
+  EXPECT_EQ(nullptr, next_executable.subscription);
+  EXPECT_EQ(nullptr, next_executable.waitable);
+  EXPECT_EQ(nullptr, next_executable.data);
+
+  rclcpp::shutdown();
+}
+
+TEST(TestCallbackGroup, InvalidCbg)
+{
+  rclcpp::init(0, nullptr);
+
+  // Create a timer associated with a callback group
+  auto node = std::make_shared<rclcpp::Node>("node");
+
+  // Add the callback group to the executor
+  auto executor = CustomExecutor();
+  executor.add_node(node);
+  executor.spin_all(std::chrono::milliseconds(10));
+
+  auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  auto timer = node->create_wall_timer(std::chrono::milliseconds(1), [](){}, cbg);
+  executor.add_callback_group(cbg, nullptr);
+
+  executor.spin_all(std::chrono::milliseconds(10));
+
+  // Collect the entities
+  executor.collect();
+  EXPECT_EQ(1u, executor.collected_timers());
+
+  executor.wait();
+
+  cbg.reset();
+
+  // Since the callback group has been reset, this should not be allowed to
+  // be a valid executable (timer and cbg should both be nullptr).
+  // In the regression, timer == next_executable.timer whil
+  // next_executable.callback_group == nullptr, which was incorrect.
+  auto next_executable = executor.next();
+  EXPECT_EQ(nullptr, next_executable.timer);
+  EXPECT_EQ(nullptr, next_executable.callback_group);
+
+  EXPECT_EQ(nullptr, next_executable.client);
+  EXPECT_EQ(nullptr, next_executable.service);
+  EXPECT_EQ(nullptr, next_executable.subscription);
+  EXPECT_EQ(nullptr, next_executable.waitable);
+  EXPECT_EQ(nullptr, next_executable.data);
+
+  rclcpp::shutdown();
+}