fix(cie): take 'nice' instead of 'priority' for CFS policies

cie_thread_configurator/CMakeLists.txt

@@ -14,13 +14,13 @@ find_package(yaml-cpp REQUIRED)
 add_library(thread_configurator SHARED src/util.cpp)
 ament_target_dependencies(thread_configurator rclcpp cie_config_msgs)
 
-add_executable(thread_configurator_node src/thread_configurator_node_main.cpp src/thread_configurator_node.cpp)
-ament_target_dependencies(thread_configurator_node rclcpp cie_config_msgs)
-target_link_libraries(thread_configurator_node yaml-cpp thread_configurator)
+add_library(thread_configurator_node_core STATIC src/thread_configurator_node.cpp)
+ament_target_dependencies(thread_configurator_node_core rclcpp cie_config_msgs)
+target_link_libraries(thread_configurator_node_core yaml-cpp thread_configurator)
 
-target_include_directories(thread_configurator_node PRIVATE
-  ${CMAKE_CURRENT_SOURCE_DIR}/include
-)
+add_executable(thread_configurator_node src/thread_configurator_node_main.cpp)
+ament_target_dependencies(thread_configurator_node rclcpp cie_config_msgs)
+target_link_libraries(thread_configurator_node thread_configurator_node_core)
 
 add_executable(prerun_node src/prerun_node_main.cpp src/prerun_node.cpp)
 ament_target_dependencies(prerun_node rclcpp cie_config_msgs)
@@ -73,6 +73,12 @@ if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_util test/test_util.cpp)
   target_link_libraries(test_util thread_configurator)
   ament_target_dependencies(test_util rclcpp std_msgs std_srvs)
+
+  ament_add_ros_isolated_gtest(test_thread_configurator_node
+    test/test_thread_configurator_node.cpp
+  )
+  target_link_libraries(test_thread_configurator_node thread_configurator_node_core)
+  ament_target_dependencies(test_thread_configurator_node rclcpp cie_config_msgs)
 endif()
 
 ament_package()

cie_thread_configurator/README.md

@@ -16,7 +16,7 @@ callback_groups:
       - 0
       - 1
     policy: SCHED_OTHER
-    priority: -10
+    nice: -10
 
   - id: yyyyy
     affinity:
@@ -30,7 +30,7 @@ non_ros_threads:
     affinity:
       - 4
     policy: SCHED_OTHER
-    priority: 0
+    nice: 0
 
 ...
 ```
@@ -42,11 +42,12 @@ Leaving this array empty means allowing operation on all cores.
 In the policy field, you can specify one of the following scheduling policies:
 - `SCHED_OTHER`
 - `SCHED_BATCH`
+- `SCHED_IDLE`
 - `SCHED_FIFO`
 - `SCHED_RR`
 - `SCHED_DEADLINE`
 
-When specifying the task scheduling policy as `SCHED_OTHER` or `SCHED_BATCH`, it is executed on the CFS (Completely Fair Scheduler).
+When specifying the task scheduling policy as `SCHED_OTHER`, `SCHED_BATCH`, or `SCHED_IDLE`, it is executed on the CFS (Completely Fair Scheduler).
 When set to `SCHED_FIFO` or `SCHED_RR`, it runs on the FIFO scheduler.
 When specified as `SCHED_DEADLINE`, it is scheduled on the EDF (Earliest Deadline First) scheduler.
 
@@ -55,16 +56,17 @@ Below, the configurable items for each scheduler are described.
 
 ### CFS
 For threads operating on the CFS, you can specify the nice value.
-In the YAML file, this is specified under the entry name  `priority`.
+In the YAML file, this is specified under the entry name `nice`.
 The values can range from `-20` (highest priority) to `19` (lowest priority).
+Values outside this range are rejected at startup.
 
 ```yaml
   - id: xxxxx
     affinity:
       - 0
       - 1
     policy: SCHED_OTHER
-    priority: -10
+    nice: -10
 ```
 
 ### FIFO Scheduler
@@ -73,6 +75,7 @@ In the YAML file, this is specified under the entry name  `priority`.
 The values can range from `99` (highest priority) to `1` (lowest priority).
 This range corresponds to the return values of `sched_get_priority_max(2)` and `sched_get_priority_min(2)`.
 On Linux, these values are `99` and `1`.
+Values outside this range are rejected at startup.
 
 ```yaml
   - id: xxxxx
@@ -107,22 +110,22 @@ callback_groups:
   - id: /sample_node@Subscription(/parameter_events)@Service(/sample_node/get_parameters)@Service(/sample_node/get_parameter_types)@Service(/sample_node/set_parameters)@Service(/sample_node/set_parameters_atomically)@Service(/sample_node/describe_parameters)@Service(/sample_node/list_parameters)@Waitable@Waitable@Waitable@Waitable
     affinity: ~
     policy: SCHED_OTHER
-    priority: 0
+    nice: 0
 
   - id: /sample_node@Subscription(/topic_in)@Waitable
     affinity: ~
     policy: SCHED_OTHER
-    priority: 0
+    nice: 0
 
   - id: /sample_node@Timer(1333000000)
     affinity: ~
     policy: SCHED_OTHER
-    priority: 0
+    nice: 0
 
   - id: /sample_node@Timer(3000000000)
     affinity: ~
     policy: SCHED_OTHER
-    priority: 0
+    nice: 0
 ```
 
 As shown in this example, the CallbackGroup ID consists of multiple strings separated by `@`.

cie_thread_configurator/include/cie_thread_configurator/thread_configurator_node.hpp

@@ -15,7 +15,8 @@ class ThreadConfiguratorNode : public rclcpp::Node {
     int64_t thread_id = -1;
     std::vector<int> affinity;
     std::string policy;
-    int priority = 0;
+    int nice = 0;     // SCHED_OTHER/BATCH/IDLE only (-20..19)
+    int priority = 0; // rt_priority; SCHED_FIFO/RR only (1..99)
 
     // For SCHED_DEADLINE
     unsigned int runtime = 0;
@@ -30,8 +31,10 @@ class ThreadConfiguratorNode : public rclcpp::Node {
   /// YAML, and performs hardware validation when a 'hardware_info' section
   /// is present in the configuration.
   /// @throws std::runtime_error if the 'config_file' parameter is empty, the
-  ///         YAML file cannot be loaded, or a present hardware_info section
-  ///         does not match the current system.
+  ///         YAML file cannot be loaded, a present hardware_info section does
+  ///         not match the current system, or an entry has an unknown 'policy'
+  ///         or a missing / non-integer / out-of-range scheduling parameter
+  ///         ('nice' for CFS policies, 'priority' for SCHED_FIFO/SCHED_RR).
   explicit ThreadConfiguratorNode(
       const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
   ~ThreadConfiguratorNode();

cie_thread_configurator/src/prerun_node.cpp

@@ -88,7 +88,7 @@ void PrerunNode::dump_yaml_config(std::filesystem::path path) {
     out << YAML::Key << "id" << YAML::Value << callback_group_id;
     out << YAML::Key << "affinity" << YAML::Value << YAML::Null;
     out << YAML::Key << "policy" << YAML::Value << "SCHED_OTHER";
-    out << YAML::Key << "priority" << YAML::Value << 0;
+    out << YAML::Key << "nice" << YAML::Value << 0;
     out << YAML::EndMap;
     out << YAML::Newline;
   }
@@ -104,7 +104,7 @@ void PrerunNode::dump_yaml_config(std::filesystem::path path) {
     out << YAML::Key << "id" << YAML::Value << thread_name;
     out << YAML::Key << "affinity" << YAML::Value << YAML::Null;
     out << YAML::Key << "policy" << YAML::Value << "SCHED_OTHER";
-    out << YAML::Key << "priority" << YAML::Value << 0;
+    out << YAML::Key << "nice" << YAML::Value << 0;
     out << YAML::EndMap;
     out << YAML::Newline;
   }

cie_thread_configurator/src/thread_configurator_node.cpp

@@ -21,6 +21,68 @@
 #include "cie_thread_configurator/sched_deadline.hpp"
 #include "cie_thread_configurator/thread_configurator_node.hpp"
 
+namespace {
+
+constexpr int k_nice_min = -20;
+constexpr int k_nice_max = 19;
+constexpr int k_rt_priority_min = 1;
+constexpr int k_rt_priority_max = 99;
+
+bool is_cfs_policy(const std::string &policy) {
+  return policy == "SCHED_OTHER" || policy == "SCHED_BATCH" ||
+         policy == "SCHED_IDLE";
+}
+
+// 'nice' is required for the CFS policies (SCHED_OTHER/BATCH/IDLE);
+// parse_rt_priority is the mirror image for SCHED_FIFO/SCHED_RR. `entry_desc`
+// is the "id=..." fragment used in messages.
+int parse_nice(const YAML::Node &entry, const std::string &policy,
+               const std::string &entry_desc) {
+  const YAML::Node nice = entry["nice"];
+  // A key with an empty value ("nice:") is a defined null node, so `!nice`
+  // alone would pass it on to as<int>()'s context-free BadConversion.
+  if (!nice || nice.IsNull()) {
+    throw std::runtime_error("Policy '" + policy + "' requires 'nice' for " +
+                             entry_desc);
+  }
+  int value = 0;
+  try {
+    value = nice.as<int>();
+  } catch (const YAML::Exception &) {
+    throw std::runtime_error("'nice' must be an integer for " + entry_desc);
+  }
+  if (value < k_nice_min || value > k_nice_max) {
+    // setpriority(2) would silently clamp an out-of-range value to
+    // [-20, 19]; reject it here so a misunderstanding of the scale
+    // (e.g. an rt_priority-style 50) fails loudly instead.
+    throw std::runtime_error("'nice' must be in [-20, 19] for " + entry_desc +
+                             ", got " + std::to_string(value));
+  }
+  return value;
+}
+
+int parse_rt_priority(const YAML::Node &entry, const std::string &policy,
+                      const std::string &entry_desc) {
+  const YAML::Node priority = entry["priority"];
+  if (!priority || priority.IsNull()) {
+    throw std::runtime_error("Policy '" + policy +
+                             "' requires 'priority' for " + entry_desc);
+  }
+  int value = 0;
+  try {
+    value = priority.as<int>();
+  } catch (const YAML::Exception &) {
+    throw std::runtime_error("'priority' must be an integer for " + entry_desc);
+  }
+  if (value < k_rt_priority_min || value > k_rt_priority_max) {
+    throw std::runtime_error("'priority' must be in [1, 99] for " + entry_desc +
+                             ", got " + std::to_string(value));
+  }
+  return value;
+}
+
+} // namespace
+
 ThreadConfiguratorNode::ThreadConfiguratorNode(
     const rclcpp::NodeOptions &options)
     : Node("thread_configurator_node", options), unapplied_num_(0),
@@ -93,8 +155,11 @@ ThreadConfiguratorNode::ThreadConfiguratorNode(
       config.runtime = node["runtime"].as<unsigned int>();
       config.period = node["period"].as<unsigned int>();
       config.deadline = node["deadline"].as<unsigned int>();
+    } else if (is_cfs_policy(config.policy)) {
+      config.nice = parse_nice(node, config.policy, "id=" + config.thread_str);
     } else {
-      config.priority = node["priority"].as<int>();
+      config.priority =
+          parse_rt_priority(node, config.policy, "id=" + config.thread_str);
     }
   };
 
@@ -254,7 +319,7 @@ bool ThreadConfiguratorNode::issue_syscalls(const ThreadConfig &config) {
     }
 
     // Specify nice value
-    if (setpriority(PRIO_PROCESS, config.thread_id, config.priority) == -1) {
+    if (setpriority(PRIO_PROCESS, config.thread_id, config.nice) == -1) {
       RCLCPP_ERROR(this->get_logger(),
                    "Failed to configure nice value (id=%s, tid=%ld): %s",
                    config.thread_str.c_str(), config.thread_id,