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; diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index c2e6b2bfe4..15d99337c1 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/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 diff --git a/rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp new file mode 100644 index 0000000000..1b33c1fbb4 --- /dev/null +++ b/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 + +#include +#include +#include +#include + +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("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("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(); +}