diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 263d0c48cb..146f2e359c 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -44,6 +44,16 @@ int main(int argc, char ** argv) auto cm = std::make_shared(executor, manager_node_name); + const int cpu_affinity = cm->get_parameter_or("cpu_affinity", -1); + if (cpu_affinity >= 0) + { + const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity); + if (!affinity_result.first) + { + RCLCPP_WARN( + cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str()); + } + } const bool lock_memory = cm->get_parameter_or("lock_memory", true); std::string message; if (lock_memory && !realtime_tools::lock_memory(message)) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index ca100178b6..baf340336f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -19,3 +19,4 @@ controller_manager * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). +* The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_).