diff --git a/rclcpp/include/rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp b/rclcpp/include/rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp index 31a2750662..8f8925ce50 100644 --- a/rclcpp/include/rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp +++ b/rclcpp/include/rclcpp/executors/events_cbg_executor/events_cbg_executor.hpp @@ -15,8 +15,10 @@ #pragma once #include +#include #include #include +#include #include #include @@ -36,6 +38,7 @@ class TimerManager; struct RegisteredEntityCache; class CBGScheduler; struct GlobalWeakExecutableCache; +struct DedicatedThreadPool; } class EventsCBGExecutor : public rclcpp::Executor @@ -120,6 +123,87 @@ class EventsCBGExecutor : public rclcpp::Executor // add a callback group to the executor, not bound to any node void add_callback_group_only(const rclcpp::CallbackGroup::SharedPtr & group_ptr); + /// Configuration of a dedicated worker thread. + /** + * All settings are optional. Defaults inherit the corresponding + * property from the process / parent thread. + */ + struct DedicatedThreadOptions + { + enum class SchedulingPolicy + { + /// Keep the scheduling policy of the process (default) + Inherit, + /// SCHED_OTHER, the standard time sharing policy + Other, + /// SCHED_RR, realtime round robin policy + RoundRobin, + /// SCHED_FIFO, realtime first in first out policy + Fifo, + }; + + /// Name of the dedicated thread, as shown by debugging and tracing + /// tools. Empty keeps the default thread name. Note, on Linux thread + /// names are limited to 15 characters, longer names are truncated. + std::string name; + + /// Scheduling policy of the dedicated thread. + SchedulingPolicy scheduling_policy = SchedulingPolicy::Inherit; + + /// Scheduling priority of the dedicated thread. Only used with the + /// RoundRobin and Fifo policies. Note, setting a realtime policy + /// usually requires elevated privileges (e.g. CAP_SYS_NICE or an + /// appropriate RLIMIT_RTPRIO). + int priority = 0; + + /// Indices of the cpu cores the dedicated thread may run on. + /// Empty keeps the affinity mask of the process. + std::vector cpu_affinity; + + /// Optional callback, executed once inside the dedicated thread after + /// the settings above were applied, and before any events are + /// processed. Escape hatch for settings not covered by this struct. + std::function thread_init_callback; + }; + + /// Assign a dedicated worker thread to the given callback group. + /** + * All events of the given callback group will be executed exclusively by + * a thread dedicated to this callback group, instead of the shared worker + * pool. This isolates the execution of the callback group from the load + * of the rest of the system, and allows the use of custom scheduling + * settings (priority, affinity) for the dedicated thread. + * + * If applying one of the requested thread settings fails (e.g. a + * realtime policy was requested without sufficient privileges), an + * error is logged and the thread continues with the inherited settings. + * + * Dedicated worker threads only process events while spin() or + * spin(exception_handler) is active. spin_once, spin_some and spin_all + * will NOT execute events of dedicated callback groups. + * + * Must be called before the callback group is added to this executor, + * either directly, or indirectly by adding its node. + * + * \param group_ptr the callback group that shall be executed by a + * dedicated worker thread + * \param options thread settings (name, scheduling policy, priority, + * cpu affinity) of the dedicated thread + * \throws std::runtime_error if the callback group was already added to + * this executor + */ + RCLCPP_PUBLIC + void + set_dedicated_thread_for_callback_group( + const rclcpp::CallbackGroup::SharedPtr & group_ptr, + DedicatedThreadOptions options); + + /// \sa set_dedicated_thread_for_callback_group, with default options + RCLCPP_PUBLIC + void + set_dedicated_thread_for_callback_group( + const rclcpp::CallbackGroup::SharedPtr & group_ptr); + /** * \sa rclcpp::Executor:spin() for more details * \throws std::runtime_error when spin() called while already spinning @@ -294,6 +378,23 @@ class EventsCBGExecutor : public rclcpp::Executor void sync_callback_groups(); + /** + * Spawns dedicated worker threads for all callback groups configured + * via set_dedicated_thread_for_callback_group, that do not have a + * running worker thread yet. + * + * No op, if no threaded spin is active. + */ + void start_dedicated_worker_threads(); + + /** + * Releases all dedicated worker threads and joins them. + * If called from within a dedicated worker thread (e.g. a callback + * initiated the shutdown), the calling thread is detached instead + * of joined. + */ + void stop_dedicated_worker_threads(); + /** * Either triggers a sync, or if not spinning, * syncs directly. @@ -352,6 +453,28 @@ class EventsCBGExecutor : public rclcpp::Executor /// Stores the executables for guard conditions of the nodes std::unique_ptr nodes_executable_cache; + + struct DedicatedThreadConfig + { + rclcpp::CallbackGroup::WeakPtr callback_group; + DedicatedThreadOptions options; + }; + + std::mutex dedicated_thread_configs_mutex_; + + /// Callback groups that shall be executed by a dedicated worker thread + std::vector dedicated_thread_configs_; + + /// The running dedicated worker threads, keyed by their scheduler handle + std::unique_ptr dedicated_threads_; + + /// True while a threaded spin (spin() / spin(exception_handler)) is active. + /// Dedicated worker threads are only spawned while this is set. + std::atomic_bool dedicated_workers_active_ = false; + + /// Exception handler passed to the dedicated worker threads, + /// set by spin(exception_handler) + std::function dedicated_exception_handler_; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp index 5cfaada3f7..59935b2f0f 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/events_cbg_executor.cpp @@ -12,12 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifdef __linux__ +#include +#include +#endif + #include +#include #include #include #include +#include +#include +#include #include +#include "rcutils/logging_macros.h" + #include "rcpputils/scope_exit.hpp" #include "rclcpp/exceptions/exceptions.hpp" #include "rclcpp/node.hpp" @@ -74,6 +85,15 @@ struct GlobalWeakExecutableCache guard_conditions.clear(); } }; + +struct DedicatedThreadPool +{ + std::mutex mutex; + + /// The running dedicated worker threads, keyed by the scheduler + /// handle of their callback group + std::unordered_map threads; +}; } // namespace cbg_executor EventsCBGExecutor::EventsCBGExecutor( @@ -90,7 +110,8 @@ EventsCBGExecutor::EventsCBGExecutor( context_(options.context), timer_manager(std::make_unique(context_)), global_executable_cache(std::make_unique() ), - nodes_executable_cache(std::make_unique() ) + nodes_executable_cache(std::make_unique() ), + dedicated_threads_(std::make_unique() ) { global_executable_cache->add_guard_condition_event ( interrupt_guard_condition_, @@ -139,6 +160,9 @@ void EventsCBGExecutor::shutdown() scheduler->release_all_worker_threads(); } + // release and join all dedicated worker threads + stop_dedicated_worker_threads(); + remove_all_nodes_and_callback_groups(); { @@ -243,7 +267,7 @@ void EventsCBGExecutor::sync_callback_groups() return; } - std::scoped_lock l(callback_groups_mutex); + std::unique_lock l(callback_groups_mutex); std::vector> cur_group_data; @@ -279,9 +303,20 @@ void EventsCBGExecutor::sync_callback_groups() } } + bool dedicated_worker = false; + { + std::lock_guard cfg_lock{dedicated_thread_configs_mutex_}; + for (const auto & config : dedicated_thread_configs_) { + if (config.callback_group.lock() == cbg) { + dedicated_worker = true; + break; + } + } + } + CallbackGroupData new_entry{.callback_group = cbg, .registered_entities = std::make_unique(*scheduler, - *timer_manager, cbg), .origin = origin}; + *timer_manager, cbg, dedicated_worker), .origin = origin}; new_entry.registered_entities->regenerate_events(); next_group_data.push_back(std::move(new_entry) ); }; @@ -335,6 +370,30 @@ void EventsCBGExecutor::sync_callback_groups() // FIXME inform scheduler about remove cbgs callback_groups.swap(next_group_data); + + // entries that were not moved into callback_groups belong to callback + // groups that were removed from the executor. We move them out, so that + // their destructors (which unregister the entity ready callbacks) run + // after callback_groups_mutex was released. + std::vector dropped_group_data; + for (auto & old_entry : next_group_data) { + if (old_entry.registered_entities) { + dropped_group_data.push_back(std::move(old_entry) ); + } + } + + l.unlock(); + + // release the dedicated workers of removed callback groups. The threads + // terminate on their own, and are joined on spin exit or shutdown. + for (const auto & dropped : dropped_group_data) { + auto & handle = dropped.registered_entities->scheduler_cbg_handle; + if (handle.has_dedicated_worker() ) { + handle.release_dedicated_worker(); + } + } + + start_dedicated_worker_threads(); } void @@ -502,11 +561,20 @@ EventsCBGExecutor::spin() throw std::runtime_error("spin() called while already spinning"); } RCPPUTILS_SCOPE_EXIT( + this->stop_dedicated_worker_threads(); this->spinning.store(false); this->cancel_requested_.store(false);); if (cancel_requested_.load()) { return; } + + dedicated_exception_handler_ = std::function(); + dedicated_workers_active_.store(true); + // make sure the dedicated worker threads of already + // known callback groups are running + sync_callback_groups(); + start_dedicated_worker_threads(); + std::vector threads; size_t thread_id = 0; for ( ; thread_id < number_of_threads_ - 1; ++thread_id) { @@ -529,11 +597,20 @@ void EventsCBGExecutor::spin( throw std::runtime_error("spin() called while already spinning"); } RCPPUTILS_SCOPE_EXIT( + this->stop_dedicated_worker_threads(); this->spinning.store(false); this->cancel_requested_.store(false);); if (cancel_requested_.load()) { return; } + + dedicated_exception_handler_ = exception_handler; + dedicated_workers_active_.store(true); + // make sure the dedicated worker threads of already + // known callback groups are running + sync_callback_groups(); + start_dedicated_worker_threads(); + std::vector threads; size_t thread_id = 0; for ( ; thread_id < number_of_threads_ - 1; ++thread_id) { @@ -821,4 +898,226 @@ void EventsCBGExecutor::add_callback_group_only(const rclcpp::CallbackGroup::Sha { add_callback_group(group_ptr, nullptr, true); } + +namespace +{ +/** + * Applies the given thread settings to the calling thread. + * Failures are logged, and do not prevent the thread from running + * with the inherited settings. + */ +void apply_dedicated_thread_options( + const EventsCBGExecutor::DedicatedThreadOptions & options) +{ + using SchedulingPolicy = EventsCBGExecutor::DedicatedThreadOptions::SchedulingPolicy; + +#ifdef __linux__ + if (!options.name.empty() ) { + // linux limits thread names to 15 characters plus null terminator + const std::string truncated_name = options.name.substr(0, 15); + const int ret = pthread_setname_np(pthread_self(), truncated_name.c_str() ); + if (ret != 0) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", "Failed to set name '%s' of dedicated thread: %s", + truncated_name.c_str(), strerror(ret) ); + } + } + + if (options.scheduling_policy != SchedulingPolicy::Inherit) { + int policy = SCHED_OTHER; + switch (options.scheduling_policy) { + case SchedulingPolicy::Inherit: + case SchedulingPolicy::Other: + policy = SCHED_OTHER; + break; + case SchedulingPolicy::RoundRobin: + policy = SCHED_RR; + break; + case SchedulingPolicy::Fifo: + policy = SCHED_FIFO; + break; + } + + sched_param param{}; + param.sched_priority = options.priority; + const int ret = pthread_setschedparam(pthread_self(), policy, ¶m); + if (ret != 0) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Failed to set scheduling policy / priority %d of dedicated thread: %s " + "(realtime policies usually require CAP_SYS_NICE or RLIMIT_RTPRIO)", + options.priority, strerror(ret) ); + } + } + + if (!options.cpu_affinity.empty() ) { + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + for (const size_t core : options.cpu_affinity) { + if (core >= CPU_SETSIZE) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", "Ignoring out of range cpu core %zu in cpu_affinity", core); + continue; + } + CPU_SET(core, &cpuset); + } + const int ret = pthread_setaffinity_np(pthread_self(), sizeof(cpuset), &cpuset); + if (ret != 0) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", "Failed to set cpu affinity of dedicated thread: %s", strerror(ret) ); + } + } +#else + if (!options.name.empty() || + options.scheduling_policy != SchedulingPolicy::Inherit || + !options.cpu_affinity.empty() ) + { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "The name, scheduling_policy and cpu_affinity settings of " + "DedicatedThreadOptions are only supported on Linux"); + } +#endif + + if (options.thread_init_callback) { + options.thread_init_callback(); + } +} +} // namespace + +void EventsCBGExecutor::set_dedicated_thread_for_callback_group( + const rclcpp::CallbackGroup::SharedPtr & group_ptr, + DedicatedThreadOptions options) +{ + { + std::lock_guard lock{callback_groups_mutex}; + for (const auto & cbg_data : callback_groups) { + if (cbg_data.callback_group.lock() == group_ptr) { + throw std::runtime_error( + "set_dedicated_thread_for_callback_group must be called before " + "the callback group is added to the executor"); + } + } + } + + std::lock_guard lock{dedicated_thread_configs_mutex_}; + + // drop expired entries + dedicated_thread_configs_.erase( + std::remove_if( + dedicated_thread_configs_.begin(), dedicated_thread_configs_.end(), + [](const DedicatedThreadConfig & config) { + return config.callback_group.expired(); + }), dedicated_thread_configs_.end() ); + + for (DedicatedThreadConfig & config : dedicated_thread_configs_) { + if (config.callback_group.lock() == group_ptr) { + config.options = std::move(options); + return; + } + } + + dedicated_thread_configs_.push_back({group_ptr, std::move(options)}); +} + +void EventsCBGExecutor::set_dedicated_thread_for_callback_group( + const rclcpp::CallbackGroup::SharedPtr & group_ptr) +{ + set_dedicated_thread_for_callback_group(group_ptr, DedicatedThreadOptions()); +} + +void EventsCBGExecutor::start_dedicated_worker_threads() +{ + if (!dedicated_workers_active_.load() ) { + return; + } + + std::scoped_lock l(callback_groups_mutex, dedicated_threads_->mutex); + + for (const CallbackGroupData & cbg_data : callback_groups) { + auto & handle = cbg_data.registered_entities->scheduler_cbg_handle; + if (!handle.has_dedicated_worker() ) { + continue; + } + if (dedicated_threads_->threads.count(&handle) > 0) { + continue; + } + + DedicatedThreadOptions thread_options; + { + const auto group = cbg_data.callback_group.lock(); + if (!group) { + continue; + } + std::lock_guard cfg_lock{dedicated_thread_configs_mutex_}; + for (const auto & config : dedicated_thread_configs_) { + if (config.callback_group.lock() == group) { + thread_options = config.options; + break; + } + } + } + + // allow the worker to block again, in case this is + // a new spin cycle after a cancel + handle.reset_dedicated_worker_release(); + + cbg_executor::CBGScheduler::CallbackGroupHandle * handle_ptr = &handle; + dedicated_threads_->threads.emplace( + handle_ptr, + std::thread( + [this, handle_ptr, thread_options = std::move(thread_options), + exception_handler = dedicated_exception_handler_]() { + apply_dedicated_thread_options(thread_options); + + while (rclcpp::ok(this->context_) && !cancel_requested_.load() ) { + auto ready_entity = handle_ptr->get_next_ready_entity(); + if (!ready_entity) { + if (!handle_ptr->dedicated_worker_wait_for_work() ) { + return; + } + continue; + } + + if (exception_handler) { + try { + ready_entity->execute_function(); + } catch (const std::exception & e) { + exception_handler(e); + } + } else { + ready_entity->execute_function(); + } + + scheduler->mark_entity_as_executed(*ready_entity); + } + }) ); + } +} + +void EventsCBGExecutor::stop_dedicated_worker_threads() +{ + dedicated_workers_active_.store(false); + + std::unordered_map threads; + { + std::lock_guard l(dedicated_threads_->mutex); + threads.swap(dedicated_threads_->threads); + } + + for (auto & entry : threads) { + entry.first->release_dedicated_worker(); + } + + const auto this_thread_id = std::this_thread::get_id(); + for (auto & entry : threads) { + if (entry.second.get_id() == this_thread_id) { + // stop was initiated from inside a dedicated worker callback, + // (e.g. the callback triggered the shutdown) we can not join ourself + entry.second.detach(); + continue; + } + entry.second.join(); + } +} } // namespace rclcpp::executors diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.hpp index df55e22f29..6f3a72a653 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/first_in_first_out_scheduler.hpp @@ -50,7 +50,7 @@ struct FirstInFirstOutCallbackGroupHandle final : public CBGScheduler::CallbackG std::function get_ready_callback_for_entity( const CBGScheduler::CallbackEventType & entity) final; - std::optional get_next_ready_entity(); + std::optional get_next_ready_entity() final; std::optional get_next_ready_entity( GlobalEventIdProvider::MonotonicId max_id); diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/registered_entity_cache.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/registered_entity_cache.hpp index 7109ba647c..eae53ff055 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/registered_entity_cache.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/registered_entity_cache.hpp @@ -114,11 +114,18 @@ struct RegisteredEntityCache { RegisteredEntityCache( CBGScheduler & scheduler, TimerManager & timer_manager, - const rclcpp::CallbackGroup::SharedPtr & callback_group) + const rclcpp::CallbackGroup::SharedPtr & callback_group, + bool dedicated_worker = false) : callback_group_weak_ptr(callback_group), scheduler_cbg_handle(*scheduler.add_callback_group(callback_group)), timer_manager(timer_manager) { + if (dedicated_worker) { + // must happen before any ready callbacks are registered, + // so that all events are routed to the dedicated worker + scheduler_cbg_handle.enable_dedicated_worker(); + } + auto cbg_gc = callback_group->get_notify_guard_condition(); if(cbg_gc) { diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp index 6fe052bec0..88bf8f9ffc 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/scheduler.hpp @@ -14,10 +14,13 @@ #pragma once #include +#include #include #include #include #include +#include +#include #include #include @@ -58,6 +61,16 @@ class CBGScheduler } }; + struct CallbackGroupHandle; + + struct ExecutableEntity + { + // if called executes the entity + std::function execute_function; + // The callback group associated with the entity. Can be nullptr. + CallbackGroupHandle *callback_handle = nullptr; + }; + struct CallbackGroupHandle { explicit CallbackGroupHandle(CBGScheduler & scheduler, CallbackGroupType type) @@ -86,6 +99,76 @@ class CBGScheduler virtual std::function get_ready_callback_for_entity( const CallbackEventType & entity) = 0; + /** + * Removes and returns the next ready entity of this callback group. + * In contrast to CBGScheduler::get_next_ready_entity, this operates + * only on the entities of this callback group, and does not interact + * with the scheduler wide ready queue. Used by dedicated worker threads. + */ + virtual std::optional get_next_ready_entity() = 0; + + /** + * Puts this handle into dedicated worker mode. Ready events of this + * callback group will not be announced via the scheduler wide ready + * queue. Instead the dedicated worker is woken up, and is expected to + * pop the events using get_next_ready_entity(). + * + * Must be called before any ready callbacks were registered for + * entities of this callback group. + */ + void enable_dedicated_worker() + { + dedicated_worker_state = std::make_unique(); + } + + bool has_dedicated_worker() const + { + return static_cast(dedicated_worker_state); + } + + void wake_dedicated_worker() + { + { + std::lock_guard l(dedicated_worker_state->mutex); + dedicated_worker_state->work_signaled = true; + } + dedicated_worker_state->condition_variable.notify_one(); + } + + /** + * Blocks until new work is signaled, or the worker is released. + * @return false if the dedicated worker shall terminate + */ + bool dedicated_worker_wait_for_work() + { + std::unique_lock lk(dedicated_worker_state->mutex); + dedicated_worker_state->condition_variable.wait( + lk, [this]() { + return dedicated_worker_state->work_signaled || dedicated_worker_state->released; + }); + dedicated_worker_state->work_signaled = false; + return !dedicated_worker_state->released; + } + + void release_dedicated_worker() + { + { + std::lock_guard l(dedicated_worker_state->mutex); + dedicated_worker_state->released = true; + } + dedicated_worker_state->condition_variable.notify_all(); + } + + /** + * Allows a newly spawned dedicated worker to block again, + * after a previous release. (spin cycle after cancel) + */ + void reset_dedicated_worker_release() + { + std::lock_guard l(dedicated_worker_state->mutex); + dedicated_worker_state->released = false; + } + /** * Marks the last removed ready entity as executed. */ @@ -169,6 +252,17 @@ class CBGScheduler std::mutex ready_mutex; private: + struct DedicatedWorkerState + { + std::mutex mutex; + std::condition_variable condition_variable; + bool work_signaled = false; + bool released = false; + }; + + // only set if this callback group is executed by a dedicated worker thread + std::unique_ptr dedicated_worker_state; + // will be set if cbg is mutual exclusive and something is executing bool not_ready = false; @@ -179,14 +273,6 @@ class CBGScheduler CallbackGroupType type; }; - struct ExecutableEntity - { - // if called executes the entity - std::function execute_function; - // The callback group associated with the entity. Can be nullptr. - CallbackGroupHandle *callback_handle = nullptr; - }; - /** * @param sync_function A special purpose sync function, that shall be * executed with high priority if triggered by @@ -235,6 +321,15 @@ class CBGScheduler */ void callback_group_ready(CallbackGroupHandle *handle, bool callback_group_was_idle) { + if (handle->has_dedicated_worker()) { + // dedicated callback groups are not scheduled via the ready queue, + // their worker pops the events directly from the handle + if (callback_group_was_idle) { + handle->wake_dedicated_worker(); + } + return; + } + { std::lock_guard l(ready_callback_groups_mutex); @@ -366,6 +461,12 @@ class CBGScheduler { std::lock_guard lk(ready_callback_groups_mutex); release_workers = true; + + for (const auto & handle : callback_groups) { + if (handle->has_dedicated_worker()) { + handle->release_dedicated_worker(); + } + } } work_ready_conditional.notify_all(); } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 83d827fc6e..d90a001d58 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -541,6 +541,14 @@ if(TARGET test_events_cbg_executor_reentrant) target_link_libraries(test_events_cbg_executor_reentrant ${PROJECT_NAME} test_msgs::test_msgs) endif() +ament_add_ros_isolated_gtest(test_events_cbg_executor_dedicated_thread + executors/test_events_cbg_executor_dedicated_thread.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_events_cbg_executor_dedicated_thread) + target_link_libraries(test_events_cbg_executor_dedicated_thread + ${PROJECT_NAME} test_msgs::test_msgs) +endif() + ament_add_ros_isolated_gtest(test_entities_collector executors/test_entities_collector.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_entities_collector) diff --git a/rclcpp/test/rclcpp/executors/test_events_cbg_executor_dedicated_thread.cpp b/rclcpp/test/rclcpp/executors/test_events_cbg_executor_dedicated_thread.cpp new file mode 100644 index 0000000000..3a5578bccc --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_events_cbg_executor_dedicated_thread.cpp @@ -0,0 +1,282 @@ +// Copyright 2026 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. + +#include + +#ifdef __linux__ +#include +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/empty.hpp" + +using namespace std::chrono_literals; + +class TestEventsCBGExecutorDedicatedThread : public testing::Test +{ +protected: + static void SetUpTestCase() {rclcpp::init(0, nullptr);} + + static void TearDownTestCase() {rclcpp::shutdown();} +}; + +/* + * Test that all callbacks of a callback group with a dedicated thread are + * executed by exactly one thread, that this thread is the one the + * thread_init_callback was executed on, that this thread executes no + * callbacks of other callback groups, and that the name and cpu_affinity + * settings of DedicatedThreadOptions are applied. + */ +TEST_F(TestEventsCBGExecutorDedicatedThread, dedicated_group_runs_on_own_thread) +{ + auto node = std::make_shared("test_dedicated_thread"); + + auto dedicated_cbg = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + std::mutex ids_mutex; + std::set dedicated_ids; + std::set pool_ids; + std::atomic dedicated_count{0}; + std::atomic pool_count{0}; + + std::string observed_thread_name; + std::atomic observed_cpu{-1}; + + rclcpp::SubscriptionOptions dedicated_opts; + dedicated_opts.callback_group = dedicated_cbg; + + auto sub_dedicated = node->create_subscription( + "dedicated_topic", 10, + [&](test_msgs::msg::Empty) { + { + std::lock_guard lock(ids_mutex); + dedicated_ids.insert(std::this_thread::get_id()); +#ifdef __linux__ + char thread_name[16] = {}; + pthread_getname_np(pthread_self(), thread_name, sizeof(thread_name)); + observed_thread_name = thread_name; + observed_cpu.store(sched_getcpu()); +#endif + } + dedicated_count++; + }, + dedicated_opts); + + // uses the default callback group of the node, executed by the worker pool + auto sub_pool = node->create_subscription( + "pool_topic", 10, + [&](test_msgs::msg::Empty) { + { + std::lock_guard lock(ids_mutex); + pool_ids.insert(std::this_thread::get_id()); + } + pool_count++; + }); + + auto pub_dedicated = node->create_publisher("dedicated_topic", 10); + auto pub_pool = node->create_publisher("pool_topic", 10); + + std::atomic init_thread_id{}; + std::atomic init_calls{0}; + + rclcpp::executors::EventsCBGExecutor executor(rclcpp::ExecutorOptions(), 2u); + + rclcpp::executors::EventsCBGExecutor::DedicatedThreadOptions thread_options; + thread_options.name = "dedicated_test"; + thread_options.cpu_affinity = {0}; + thread_options.thread_init_callback = [&]() { + init_thread_id.store(std::this_thread::get_id()); + init_calls++; + }; + executor.set_dedicated_thread_for_callback_group(dedicated_cbg, thread_options); + executor.add_node(node); + + std::thread spin_thread([&executor]() {executor.spin();}); + + test_msgs::msg::Empty msg; + auto deadline = std::chrono::steady_clock::now() + 10s; + while (std::chrono::steady_clock::now() < deadline && + (dedicated_count.load() < 5 || pool_count.load() < 5)) + { + pub_dedicated->publish(msg); + pub_pool->publish(msg); + std::this_thread::sleep_for(50ms); + } + + executor.cancel(); + spin_thread.join(); + + EXPECT_GE(dedicated_count.load(), 5u) << "dedicated subscription barely ran"; + EXPECT_GE(pool_count.load(), 5u) << "pool subscription barely ran"; + EXPECT_EQ(init_calls.load(), 1u); + + std::lock_guard lock(ids_mutex); + ASSERT_EQ(dedicated_ids.size(), 1u) << + "callbacks of the dedicated callback group ran on more than one thread"; + const std::thread::id dedicated_id = *dedicated_ids.begin(); + EXPECT_EQ(dedicated_id, init_thread_id.load()) << + "thread_init_callback did not run on the dedicated thread"; + EXPECT_EQ(pool_ids.count(dedicated_id), 0u) << + "the dedicated thread also executed callbacks of other callback groups"; + EXPECT_NE(dedicated_id, std::this_thread::get_id()); + +#ifdef __linux__ + EXPECT_EQ(observed_thread_name, "dedicated_test") << + "the name of DedicatedThreadOptions was not applied"; + EXPECT_EQ(observed_cpu.load(), 0) << + "the cpu_affinity of DedicatedThreadOptions was not applied"; +#endif +} + +/* + * Test that timers of a dedicated callback group are executed on the + * dedicated thread. + */ +TEST_F(TestEventsCBGExecutorDedicatedThread, timer_runs_on_dedicated_thread) +{ + auto node = std::make_shared("test_dedicated_thread_timer"); + + auto dedicated_cbg = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + std::mutex ids_mutex; + std::set timer_ids; + std::atomic timer_count{0}; + + auto timer = node->create_wall_timer( + 10ms, + [&]() { + { + std::lock_guard lock(ids_mutex); + timer_ids.insert(std::this_thread::get_id()); + } + timer_count++; + }, + dedicated_cbg); + + std::atomic init_thread_id{}; + + rclcpp::executors::EventsCBGExecutor executor(rclcpp::ExecutorOptions(), 2u); + + rclcpp::executors::EventsCBGExecutor::DedicatedThreadOptions thread_options; + thread_options.thread_init_callback = [&]() { + init_thread_id.store(std::this_thread::get_id()); + }; + executor.set_dedicated_thread_for_callback_group(dedicated_cbg, thread_options); + executor.add_node(node); + + std::thread spin_thread([&executor]() {executor.spin();}); + + auto deadline = std::chrono::steady_clock::now() + 10s; + while (std::chrono::steady_clock::now() < deadline && timer_count.load() < 5) { + std::this_thread::sleep_for(10ms); + } + + executor.cancel(); + spin_thread.join(); + + EXPECT_GE(timer_count.load(), 5u) << "timer barely fired"; + + std::lock_guard lock(ids_mutex); + ASSERT_EQ(timer_ids.size(), 1u) << + "timer callbacks of the dedicated callback group ran on more than one thread"; + EXPECT_EQ(*timer_ids.begin(), init_thread_id.load()) << + "timer did not run on the dedicated thread"; +} + +/* + * Test that configuring a dedicated thread for a callback group that is + * already known to the executor throws. + */ +TEST_F(TestEventsCBGExecutorDedicatedThread, set_dedicated_thread_after_add_throws) +{ + auto node = std::make_shared("test_dedicated_thread_throws"); + + auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + rclcpp::executors::EventsCBGExecutor executor(rclcpp::ExecutorOptions(), 2u); + executor.add_node(node); + + EXPECT_THROW( + executor.set_dedicated_thread_for_callback_group(cbg), + std::runtime_error); +} + +/* + * Test that an exception thrown in a callback of a dedicated callback group + * is passed to the exception handler of spin(exception_handler), and that + * the dedicated thread continues processing events afterwards. + */ +TEST_F(TestEventsCBGExecutorDedicatedThread, exception_handler_used_for_dedicated_thread) +{ + auto node = std::make_shared("test_dedicated_thread_exception"); + + auto dedicated_cbg = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + std::atomic callback_count{0}; + + rclcpp::SubscriptionOptions dedicated_opts; + dedicated_opts.callback_group = dedicated_cbg; + + auto sub = node->create_subscription( + "dedicated_topic", 10, + [&](test_msgs::msg::Empty) { + if (callback_count.fetch_add(1) == 0) { + throw std::runtime_error("first callback throws"); + } + }, + dedicated_opts); + + auto pub = node->create_publisher("dedicated_topic", 10); + + std::atomic handled_exceptions{0}; + + rclcpp::executors::EventsCBGExecutor executor(rclcpp::ExecutorOptions(), 2u); + executor.set_dedicated_thread_for_callback_group(dedicated_cbg); + executor.add_node(node); + + std::thread spin_thread( + [&executor, &handled_exceptions]() { + executor.spin( + [&handled_exceptions](const std::exception &) { + handled_exceptions++; + }); + }); + + test_msgs::msg::Empty msg; + auto deadline = std::chrono::steady_clock::now() + 10s; + while (std::chrono::steady_clock::now() < deadline && callback_count.load() < 3) { + pub->publish(msg); + std::this_thread::sleep_for(50ms); + } + + executor.cancel(); + spin_thread.join(); + + EXPECT_GE(callback_count.load(), 3u) << + "dedicated thread stopped processing events after the exception"; + EXPECT_EQ(handled_exceptions.load(), 1u); +}