Update CPU affinity parameter to be able to set multiple CPUs (#1915)

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 <https://access.redhat.com/solutions/61334>`_
   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.

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<int>("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<int> 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<int>(cpu)); });
+    }
+    const auto affinity_result = realtime_tools::set_current_thread_affinity(cpus);
     if (!affinity_result.first)
     {
       RCLCPP_WARN(

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 <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>`_).
 * 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 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/1915>`_).
 
 hardware_interface
 ******************