fix(thread_configurator): apply scheduling to all reentrant callback group threads

README.md

@@ -245,12 +245,8 @@ After successful validation, the configurator will print the settings and then w
 In this state, when you launch the target ROS 2 application, the configurator node will receive callback group information from the application.
 The entries in the configurator window show the callback group ID and OS thread ID information received from the ROS 2 application.
 
-If your configuration file contains callback groups with the `SCHED_DEADLINE` policy, the configurator node's window will display the message `Apply sched deadline?` and wait as shown below. If no `SCHED_DEADLINE` configurations are present, this prompt will be skipped automatically.
-
-<img width="1346" height="160" alt="cie_image2" src="https://github.com/user-attachments/assets/934ab8d5-4894-4549-aa57-d9e7ef8f22c9" />
-
-At this stage, settings with policies other than `SCHED_DEADLINE` have already been applied, while the application of settings including the `SCHED_DEADLINE` policy is postponed.
-To apply settings that include the `SCHED_DEADLINE` policy, press the enter key in the window where `Apply sched deadline?` is displayed.
+Settings with policies other than `SCHED_DEADLINE` are applied immediately as each callback group is received.
+Once all callback groups have been registered, `SCHED_DEADLINE` settings are automatically applied together.
 
 <details>
 <summary>Why delayed configuration of the SCHED_DEADLINE policy?</summary>
@@ -268,11 +264,9 @@ Therefore, applying the `SCHED_DEADLINE` policy after the system has started, su
 You need to apply the `SCHED_DEADLINE` policy only after the system has fully started up and no further creation of new child threads is expected.
 </details>
 
-<img width="1348" height="223" alt="cie_image3" src="https://github.com/user-attachments/assets/0ce5f16c-af94-4d0e-976d-a09604c14367" />
-
-While the target ROS 2 application is running, the configurator node's window should not be closed and must remain open.
-After the execution of the target ROS 2 application has ended, press the enter key in the window displaying `Press enter to exit and remove cgroups...`.
-This will terminate the execution of the configurator node and simultaneously delete the cgroup that was created for setting the affinity of tasks with the `SCHED_DEADLINE` policy.
+The configurator node runs persistently and will automatically re-apply configurations if the target ROS 2 application is restarted.
+To terminate the configurator node, press Ctrl+C.
+If `SCHED_DEADLINE` policies were used, the configurator node will attempt to remove the cpuset cgroups it created for affinity settings when it exits. This cleanup will only succeed if the target application has already stopped and no threads remain attached to those cgroups; otherwise, you may need to stop the application first and/or remove the remaining cgroups manually.
 
 ## Notes on Adoption
 

cie_thread_configurator/include/cie_thread_configurator/thread_configurator_node.hpp

@@ -26,13 +26,10 @@ class ThreadConfiguratorNode : public rclcpp::Node {
   };
 
 public:
-  ThreadConfiguratorNode(const YAML::Node &yaml);
+  explicit ThreadConfiguratorNode(const YAML::Node &yaml);
   ~ThreadConfiguratorNode();
-  bool all_applied();
   void print_all_unapplied();
-
-  bool exist_deadline_config();
-  bool apply_deadline_configs();
+  bool has_configured_once() const;
 
 private:
   bool set_affinity_by_cgroup(int64_t thread_id, const std::vector<int> &cpus);
@@ -41,6 +38,7 @@ class ThreadConfiguratorNode : public rclcpp::Node {
       const cie_config_msgs::msg::CallbackGroupInfo::SharedPtr msg);
   void non_ros_thread_callback(
       const cie_config_msgs::msg::NonRosThreadInfo::SharedPtr msg);
+  void apply_deadline_configs();
 
   rclcpp::Subscription<cie_config_msgs::msg::CallbackGroupInfo>::SharedPtr
       subscription_;
@@ -51,6 +49,7 @@ class ThreadConfiguratorNode : public rclcpp::Node {
   std::unordered_map<std::string, ThreadConfig *> id_to_thread_config_;
   int unapplied_num_;
   int cgroup_num_;
+  bool configured_at_least_once_ = false;
 
-  std::vector<ThreadConfig *> deadline_configs_;
+  std::vector<ThreadConfig> deadline_configs_;
 };

cie_thread_configurator/src/main.cpp

@@ -80,26 +80,9 @@ static void spin_thread_configurator_node(const std::string &config_filename) {
 
   executor->add_node(node);
 
-  while (rclcpp::ok() && !node->all_applied()) {
-    executor->spin_once();
-  }
-
-  if (node->all_applied()) {
-    if (node->exist_deadline_config()) {
-      RCLCPP_INFO(node->get_logger(), "Apply sched deadline?");
-      std::cin.get();
-
-      node->apply_deadline_configs();
+  executor->spin();
 
-      RCLCPP_INFO(node->get_logger(),
-                  "Success: All of the configurations are applied."
-                  "\nPress enter to exit and remove cgroups, if there are "
-                  "SCHED_DEADLINE tasks:");
-      std::cin.get();
-    }
-    RCLCPP_INFO(node->get_logger(),
-                "Success: All of the configurations are applied.");
-  } else {
+  if (!node->has_configured_once()) {
     node->print_all_unapplied();
   }
 }

cie_thread_configurator/src/thread_configurator_node.cpp

@@ -100,8 +100,6 @@ ThreadConfiguratorNode::~ThreadConfiguratorNode() {
   }
 }
 
-bool ThreadConfiguratorNode::all_applied() { return unapplied_num_ == 0; }
-
 void ThreadConfiguratorNode::print_all_unapplied() {
   RCLCPP_WARN(this->get_logger(), "Following threads are not yet configured");
 
@@ -112,6 +110,10 @@ void ThreadConfiguratorNode::print_all_unapplied() {
   }
 }
 
+bool ThreadConfiguratorNode::has_configured_once() const {
+  return configured_at_least_once_;
+}
+
 bool ThreadConfiguratorNode::set_affinity_by_cgroup(
     int64_t thread_id, const std::vector<int> &cpus) {
   std::string cgroup_path =
@@ -250,28 +252,47 @@ void ThreadConfiguratorNode::callback_group_callback(
 
   ThreadConfig *config = it->second;
   if (config->applied) {
+    if (config->thread_id == msg->thread_id) {
+      RCLCPP_INFO(
+          this->get_logger(),
+          "This callback group is already configured. skip (id=%s, tid=%ld)",
+          msg->callback_group_id.c_str(), msg->thread_id);
+      return;
+    }
     RCLCPP_INFO(
         this->get_logger(),
-        "This callback group is already configured. skip (id=%s, tid=%ld)",
-        msg->callback_group_id.c_str(), msg->thread_id);
-    return;
+        "Detected a new thread for an already-configured callback group. "
+        "This is expected when the application was restarted or the group "
+        "is Reentrant with multiple threads. Applying configuration "
+        "(id=%s, old_tid=%ld, new_tid=%ld)",
+        msg->callback_group_id.c_str(), config->thread_id, msg->thread_id);
   }
 
   RCLCPP_INFO(this->get_logger(), "Received CallbackGroupInfo: tid=%ld | %s",
               msg->thread_id, msg->callback_group_id.c_str());
   config->thread_id = msg->thread_id;
 
   if (config->policy == "SCHED_DEADLINE") {
-    // delayed applying
-    deadline_configs_.push_back(config);
+    // Store a copy so that each thread_id is preserved independently
+    deadline_configs_.push_back(*config);
   } else {
-    bool success = issue_syscalls(*config);
-    if (!success)
+    if (!issue_syscalls(*config)) {
+      RCLCPP_WARN(this->get_logger(),
+                  "Skipping configuration for callback group (id=%s, tid=%ld) "
+                  "due to syscall failure.",
+                  msg->callback_group_id.c_str(), msg->thread_id);
       return;
+    }
   }
 
-  config->applied = true;
-  unapplied_num_--;
+  if (!config->applied) {
+    unapplied_num_--;
+    config->applied = true;
+  }
+
+  if (unapplied_num_ == 0) {
+    apply_deadline_configs();
+  }
 }
 
 void ThreadConfiguratorNode::non_ros_thread_callback(
@@ -287,38 +308,65 @@ void ThreadConfiguratorNode::non_ros_thread_callback(
 
   ThreadConfig *config = it->second;
   if (config->applied) {
+    if (config->thread_id == msg->thread_id) {
+      RCLCPP_INFO(
+          this->get_logger(),
+          "This non-ROS thread is already configured. skip (name=%s, tid=%ld)",
+          msg->thread_name.c_str(), msg->thread_id);
+      return;
+    }
     RCLCPP_INFO(this->get_logger(),
-                "This thread is already configured. skip (name=%s, tid=%ld)",
-                msg->thread_name.c_str(), msg->thread_id);
-    return;
+                "This non-ROS thread is already configured, but thread_id "
+                "changed. Re-applying configuration (name=%s, old_tid=%ld, "
+                "new_tid=%ld)",
+                msg->thread_name.c_str(), config->thread_id, msg->thread_id);
   }
 
   RCLCPP_INFO(this->get_logger(), "Received NonRosThreadInfo: tid=%ld | %s",
               msg->thread_id, msg->thread_name.c_str());
   config->thread_id = msg->thread_id;
 
   if (config->policy == "SCHED_DEADLINE") {
-    // delayed applying
-    deadline_configs_.push_back(config);
+    // Store a copy so that each thread_id is preserved independently
+    deadline_configs_.push_back(*config);
   } else {
-    bool success = issue_syscalls(*config);
-    if (!success)
+    if (!issue_syscalls(*config)) {
+      RCLCPP_WARN(this->get_logger(),
+                  "Skipping configuration for non-ROS thread (name=%s, "
+                  "tid=%ld) due to syscall failure.",
+                  msg->thread_name.c_str(), msg->thread_id);
       return;
+    }
   }
 
+  if (!config->applied) {
+    unapplied_num_--;
+  }
   config->applied = true;
-  unapplied_num_--;
+
+  if (unapplied_num_ == 0) {
+    apply_deadline_configs();
+  }
 }
 
-bool ThreadConfiguratorNode::apply_deadline_configs() {
-  for (auto config : deadline_configs_) {
-    if (!issue_syscalls(*config))
-      return false;
+void ThreadConfiguratorNode::apply_deadline_configs() {
+  for (const auto &config : deadline_configs_) {
+    if (!issue_syscalls(config)) {
+      RCLCPP_WARN(this->get_logger(),
+                  "Failed to apply SCHED_DEADLINE for tid=%ld",
+                  config.thread_id);
+    }
   }
+  deadline_configs_.clear();
 
-  return true;
-}
+  RCLCPP_INFO(this->get_logger(),
+              "Success: All of the configurations are applied.");
 
-bool ThreadConfiguratorNode::exist_deadline_config() {
-  return !deadline_configs_.empty();
+  configured_at_least_once_ = true;
+
+  // Reset for re-application when target applications restart
+  unapplied_num_ = thread_configs_.size();
+  for (auto &config : thread_configs_) {
+    config.applied = false;
+  }
 }