From 79f18478ed747affb04f57369f75f37647d404bb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Dec 2024 23:13:53 +0100 Subject: [PATCH 1/3] Update CPU affinity parameter to be set multiple CPUs --- controller_manager/src/ros2_control_node.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index af8d26d8b1..4cec3368b7 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -67,10 +67,22 @@ 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("cpu_affinity", -1); - if (cpu_affinity >= 0) + rclcpp::Parameter cpu_affinity_param; + if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) { - const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity); + std::vector cpus = {}; + if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + cpus = {cpu_affinity_param.as_int()}; + } + else if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) + { + const auto cpu_affinity_param_array = cpu_affinity_param.as_integer_array(); + std::for_each( + cpu_affinity_param_array.begin(), cpu_affinity_param_array.end(), + [&cpus](int cpu) { cpus.push_back(static_cast(cpu)); }); + } + const auto affinity_result = realtime_tools::set_current_thread_affinity(cpus); if (!affinity_result.first) { RCLCPP_WARN( From 9b588e98d39d2cd5f75dc90c594adb72aa226ae8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 5 Dec 2024 09:21:45 +0100 Subject: [PATCH 2/3] update docs --- controller_manager/doc/userdoc.rst | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index fb69753ee2..3df3697dc7 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -331,9 +331,10 @@ lock_memory (optional; bool; default: false for a non-realtime kernel, true for Find more information about the setup for memory locking in the following link : `How to set ulimit values `_ The following command can be used to set the memory locking limit temporarily : ``ulimit -l unlimited``. -cpu_affinity (optional; int; default: -1) +cpu_affinity (optional; int (or) int_array;) Sets the CPU affinity of the ``controller_manager`` node to the specified CPU core. - The value of -1 means that the CPU affinity is not set. + If it is an integer, the node's affinity will be set to the specified CPU core. + If it is an array of integers, the node's affinity will be set to the specified set of CPU cores. thread_priority (optional; int; default: 50) Sets the thread priority of the ``controller_manager`` node to the specified value. The value must be between 0 and 99. From e3a7e8d0d4ab7822c05b1771a6c179e4335f501d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 6 Dec 2024 22:43:07 +0100 Subject: [PATCH 3/3] update release notes --- doc/release_notes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a20099a20a..a187e62437 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -83,6 +83,7 @@ controller_manager * 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 `_). * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). +* The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 `_). hardware_interface ******************