From 5e9b4b6f14ab33e9ccc0b487d5c649894188ab00 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Tue, 5 Nov 2024 11:08:17 +0100
Subject: [PATCH] [ros2_control_node] Add option to set the CPU affinity 
 (#1852)

---
 controller_manager/src/ros2_control_node.cpp | 11 +++++++++++
 doc/release_notes.rst                        |  1 +
 2 files changed, 12 insertions(+)

diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index 263d0c48cb..904f9af08e 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -51,6 +51,17 @@ int main(int argc, char ** argv)
     RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str());
   }
 
+  const int cpu_affinity = cm->get_parameter_or<int>("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());
+    }
+  }
+
   RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
   const int thread_priority = cm->get_parameter_or<int>("thread_priority", kSchedPriority);
   RCLCPP_INFO(
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
index 9dbc08436b..f9e1dcc858 100644
--- a/doc/release_notes.rst
+++ b/doc/release_notes.rst
@@ -20,3 +20,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 <https://github.com/ros-controls/ros2_control/pull/1820>`_).
 * Added support for the wildcard entries for the controller configuration files (`#1724 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/1852>`_).