diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index c18df5b7bc..0daa1f4680 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -22,6 +22,8 @@ #include #include +#include + #include "rclcpp/executor.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/memory_strategies.hpp" @@ -87,6 +89,9 @@ class MultiThreadedExecutor : public rclcpp::Executor std::chrono::nanoseconds next_exec_timeout_; std::set scheduled_timers_; + + struct sched_param sch_org, sch_mod; + int policy; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 0dfdc35467..226af65104 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -37,6 +37,9 @@ MultiThreadedExecutor::MultiThreadedExecutor( if (number_of_threads_ == 0) { number_of_threads_ = 1; } + + pthread_getschedparam(pthread_self(), &policy, &sch_org); + sch_mod.sched_priority = 1; // set lowest priority in SCHED_RR } MultiThreadedExecutor::~MultiThreadedExecutor() {} @@ -94,6 +97,7 @@ MultiThreadedExecutor::run(size_t) continue; } scheduled_timers_.insert(any_exec.timer); + pthread_setschedparam(pthread_self(), SCHED_RR, &sch_mod); } } if (yield_before_execute_) { @@ -103,11 +107,14 @@ MultiThreadedExecutor::run(size_t) execute_any_executable(any_exec); if (any_exec.timer) { - std::lock_guard wait_lock(wait_mutex_); - auto it = scheduled_timers_.find(any_exec.timer); - if (it != scheduled_timers_.end()) { - scheduled_timers_.erase(it); + { + std::lock_guard wait_lock(wait_mutex_); + auto it = scheduled_timers_.find(any_exec.timer); + if (it != scheduled_timers_.end()) { + scheduled_timers_.erase(it); + } } + pthread_setschedparam(pthread_self(), policy, &sch_org); } // Clear the callback_group to prevent the AnyExecutable destructor from // resetting the callback group `can_be_taken_from`