From f854dd055f6fd33d95cf1a466d9f2dc599edcdf9 Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa Date: Tue, 10 Jun 2025 16:13:35 +0900 Subject: [PATCH] fix: improve thread management and CPU affinity handling in SensorTrigger --- src/sensor_trigger.cpp | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/src/sensor_trigger.cpp b/src/sensor_trigger.cpp index 1364d4a..214775b 100644 --- a/src/sensor_trigger.cpp +++ b/src/sensor_trigger.cpp @@ -65,29 +65,28 @@ SensorTrigger::SensorTrigger(const rclcpp::NodeOptions & node_options) << gpio_chip_ << "line number " << gpio_line_ << "."); } - // Set CPU affinity + // Create thread + trigger_time_publisher_ = create_publisher("trigger_time", 1000); + trigger_thread_ = std::make_unique(&SensorTrigger::run, this); + + // Set CPU affinity for trigger_thread_ cpu_set_t cpuset; CPU_ZERO(&cpuset); CPU_SET(cpu_, &cpuset); - if (pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset)) { - RCLCPP_WARN_STREAM(get_logger(), "Failed to set CPU affinity: " << strerror(errno) << "."); + if (pthread_setaffinity_np(trigger_thread_->native_handle(), sizeof(cpu_set_t), &cpuset)) { + RCLCPP_WARN_STREAM(get_logger(), "Failed to set thread CPU affinity: " << strerror(errno) << "."); } - if (pthread_getaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset)) { - RCLCPP_WARN_STREAM(get_logger(), "Failed to check CPU affinity: " << strerror(errno) << "."); + if (pthread_getaffinity_np(trigger_thread_->native_handle(), sizeof(cpu_set_t), &cpuset)) { + RCLCPP_WARN_STREAM(get_logger(), "Failed to check thread CPU affinity: " << strerror(errno) << "."); } - // Create thread - trigger_time_publisher_ = create_publisher("trigger_time", 1000); - trigger_thread_ = std::make_unique(&SensorTrigger::run, this); - // Set thread priority sched_param sch; int policy; pthread_getschedparam(trigger_thread_->native_handle(), &policy, &sch); - sch.sched_priority = 30; + sch.sched_priority = 80; if (pthread_setschedparam(trigger_thread_->native_handle(), SCHED_FIFO, &sch)) { - RCLCPP_WARN_STREAM( - get_logger(), "Failed to set schedule parameters: " << strerror(errno) << "."); + RCLCPP_WARN_STREAM(get_logger(), "Failed to set schedule parameters: " << strerror(errno) << "."); } }