From 04a479a08479d46870c95addac72b5a0c51f97c2 Mon Sep 17 00:00:00 2001 From: HarunTeper Date: Wed, 11 Nov 2026 02:07:37 +0100 Subject: [PATCH] Fix starvation in mutually exclusive callback groups of the MultiThreadedExecutor A handle that is ready while its mutually exclusive callback group is in use is skipped and dropped before the next wait. Its callback group is left out of the wait set while it is in use, so once the group is free again all of its ready handles are collected at the same time and the first one in the list is picked every time. With callbacks that take longer than their period the other entities of the group are never executed. Let the default memory strategy keep the skipped handles and execute them first, in the order in which they were skipped, when their callback group can be taken from again. This is done in a class that is local to the library, so the public classes keep their layout. Executors that are given another memory strategy behave as before. Signed-off-by: HarunTeper --- .../strategies/allocator_memory_strategy.hpp | 2 +- rclcpp/src/rclcpp/memory_strategies.cpp | 219 +++++++++++++++++- .../test_multi_threaded_executor.cpp | 34 +++ 3 files changed, 253 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 6d30d1e804..45e3c37132 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -490,7 +490,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return waitable_handles_.size(); } -private: +protected: template using VectorRebind = std::vector::template rebind_alloc>; diff --git a/rclcpp/src/rclcpp/memory_strategies.cpp b/rclcpp/src/rclcpp/memory_strategies.cpp index 4c82dbbe7f..fd882461c4 100644 --- a/rclcpp/src/rclcpp/memory_strategies.cpp +++ b/rclcpp/src/rclcpp/memory_strategies.cpp @@ -14,14 +14,231 @@ #include "rclcpp/memory_strategies.hpp" +#include +#include #include #include "rclcpp/strategies/allocator_memory_strategy.hpp" using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; +namespace +{ + +/// Keeps the handles that were ready while their callback group was in use. +/** + * These handles are not waited on again, they are executed first, in the order in which + * they were blocked, as soon as their callback group can be taken from. + */ +class DefaultMemoryStrategy : public AllocatorMemoryStrategy<> +{ +public: + void clear_handles() override + { + if (waited_) { + // The handles that are left were ready, but their callback group was in use. + keep_blocked(EntityType::Timer, timer_handles_); + keep_blocked(EntityType::Subscription, subscription_handles_); + keep_blocked(EntityType::Service, service_handles_); + keep_blocked(EntityType::Client, client_handles_); + keep_blocked(EntityType::Waitable, waitable_handles_); + waited_ = false; + } + AllocatorMemoryStrategy<>::clear_handles(); + } + + void remove_null_handles(rcl_wait_set_t * wait_set) override + { + AllocatorMemoryStrategy<>::remove_null_handles(wait_set); + // A handle that is already blocked must not be executed a second time. + for (const auto & blocked : blocked_handles_) { + auto handle = blocked.handle.lock(); + remove_handle(timer_handles_, handle); + remove_handle(subscription_handles_, handle); + remove_handle(service_handles_, handle); + remove_handle(client_handles_, handle); + remove_handle(waitable_handles_, handle); + } + waited_ = true; + } + + void + get_next_timer( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override + { + if (restore_blocked(EntityType::Timer, weak_groups_to_nodes)) { + AllocatorMemoryStrategy<>::get_next_timer(any_exec, weak_groups_to_nodes); + } + } + + void + get_next_subscription( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override + { + if (restore_blocked(EntityType::Subscription, weak_groups_to_nodes)) { + AllocatorMemoryStrategy<>::get_next_subscription(any_exec, weak_groups_to_nodes); + } + } + + void + get_next_service( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override + { + if (restore_blocked(EntityType::Service, weak_groups_to_nodes)) { + AllocatorMemoryStrategy<>::get_next_service(any_exec, weak_groups_to_nodes); + } + } + + void + get_next_client( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override + { + if (restore_blocked(EntityType::Client, weak_groups_to_nodes)) { + AllocatorMemoryStrategy<>::get_next_client(any_exec, weak_groups_to_nodes); + } + } + + void + get_next_waitable( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override + { + if (restore_blocked(EntityType::Waitable, weak_groups_to_nodes)) { + AllocatorMemoryStrategy<>::get_next_waitable(any_exec, weak_groups_to_nodes); + } + } + +private: + enum class EntityType {Timer, Subscription, Service, Client, Waitable}; + + struct BlockedHandle + { + EntityType type; + std::weak_ptr handle; + }; + + template + void keep_blocked(EntityType type, const HandlesT & handles) + { + for (const auto & handle : handles) { + blocked_handles_.push_back({type, handle}); + } + } + + template + static void remove_handle(HandlesT & handles, const std::shared_ptr & handle) + { + handles.erase(std::remove(handles.begin(), handles.end(), handle), handles.end()); + } + + rclcpp::CallbackGroup::SharedPtr + get_group( + const BlockedHandle & blocked, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) + { + auto handle = blocked.handle.lock(); + if (!handle) { + return nullptr; + } + switch (blocked.type) { + case EntityType::Timer: + { + auto timer = get_timer_by_handle( + std::static_pointer_cast(handle), weak_groups_to_nodes); + // The timer may have been canceled or reset in the meantime. + if (!timer || !timer->is_ready()) { + return nullptr; + } + return get_group_by_timer(timer, weak_groups_to_nodes); + } + case EntityType::Subscription: + return get_group_by_subscription( + get_subscription_by_handle( + std::static_pointer_cast(handle), weak_groups_to_nodes), + weak_groups_to_nodes); + case EntityType::Service: + return get_group_by_service( + get_service_by_handle( + std::static_pointer_cast(handle), weak_groups_to_nodes), + weak_groups_to_nodes); + case EntityType::Client: + return get_group_by_client( + get_client_by_handle( + std::static_pointer_cast(handle), weak_groups_to_nodes), + weak_groups_to_nodes); + case EntityType::Waitable: + return get_group_by_waitable(to_waitable(handle), weak_groups_to_nodes); + } + return nullptr; + } + + static rclcpp::Waitable::SharedPtr to_waitable(const std::shared_ptr & handle) + { + return std::const_pointer_cast( + std::static_pointer_cast(handle)); + } + + /// Put the oldest blocked handle whose callback group can be taken from back in front. + /** + * \return false if that handle is of another type, nothing of this type may be taken then. + */ + bool + restore_blocked(EntityType type, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) + { + for (auto it = blocked_handles_.begin(); it != blocked_handles_.end(); ) { + auto group = get_group(*it, weak_groups_to_nodes); + if (!group) { + it = blocked_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + ++it; + continue; + } + if (it->type != type) { + return false; + } + auto handle = it->handle.lock(); + switch (type) { + case EntityType::Timer: + timer_handles_.insert( + timer_handles_.begin(), std::static_pointer_cast(handle)); + break; + case EntityType::Subscription: + subscription_handles_.insert( + subscription_handles_.begin(), + std::static_pointer_cast(handle)); + break; + case EntityType::Service: + service_handles_.insert( + service_handles_.begin(), std::static_pointer_cast(handle)); + break; + case EntityType::Client: + client_handles_.insert( + client_handles_.begin(), std::static_pointer_cast(handle)); + break; + case EntityType::Waitable: + waitable_handles_.insert(waitable_handles_.begin(), to_waitable(handle)); + break; + } + blocked_handles_.erase(it); + return true; + } + return true; + } + + std::list blocked_handles_; + bool waited_ = false; +}; + +} // namespace + rclcpp::memory_strategy::MemoryStrategy::SharedPtr rclcpp::memory_strategies::create_default_strategy() { - return std::make_shared>(); + return std::make_shared(); } diff --git a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp index b6fd2c3fda..2dcadf169a 100644 --- a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp @@ -14,9 +14,11 @@ #include +#include #include #include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/node.hpp" @@ -97,3 +99,35 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) { executor.add_node(node); executor.spin(); } + +/* + Test that two timers in the same mutually exclusive callback group are both executed + if their callbacks take longer than the timer period. + */ +TEST_F(TestMultiThreadedExecutor, mutually_exclusive_timers_starvation) { + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 2u); + + std::shared_ptr node = + std::make_shared("test_multi_threaded_executor_timer_starvation"); + + auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + std::atomic_int count_one {0}; + std::atomic_int count_two {0}; + + auto timer_callback = [&executor, &count_one, &count_two](std::atomic_int & count) { + std::this_thread::sleep_for(20ms); + count++; + if (count_one + count_two >= 20) { + executor.cancel(); + } + }; + + auto timer_one = node->create_wall_timer(5ms, [&]() {timer_callback(count_one);}, cbg); + auto timer_two = node->create_wall_timer(5ms, [&]() {timer_callback(count_two);}, cbg); + executor.add_node(node); + executor.spin(); + + EXPECT_GE(count_one, 5); + EXPECT_GE(count_two, 5); +}