diff --git a/README.md b/README.md index 60a0f97..0fd4027 100644 --- a/README.md +++ b/README.md @@ -253,12 +253,11 @@ After successful validation, the configurator will print the settings and then w cie_image1 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, and all configured policies are applied immediately upon receipt. +The entries in the configurator window show the callback group ID and OS thread ID information received from the ROS 2 application, and all configured policies are applied immediately upon receipt, including `SCHED_DEADLINE` (which uses `SCHED_FLAG_RESET_ON_FORK` so that forked children reset to `SCHED_OTHER`). -While the target ROS 2 application is running, the configurator node's window should not be closed and must remain open. -If your configuration includes `SCHED_DEADLINE` threads with CPU affinity (configured via cgroup), after all configurations are applied the configurator will display `Press enter to exit and remove cgroups...`. -Press enter to terminate the configurator and clean up the cgroup directories. -If no cgroup-based affinity was used, the configurator exits automatically once all configurations have been applied (that is, after it receives the required thread information from the target application). +The configurator node keeps running after all configurations have been applied. +This allows it to re-apply configurations automatically when a target application restarts (the OS may reuse thread IDs, so thread ID equality cannot be used to skip reconfiguration). +If your configuration includes `SCHED_DEADLINE` threads with CPU affinity (configured via cgroup), the cgroup directories are cleaned up when the configurator node is terminated. ## Notes on Adoption diff --git a/cie_thread_configurator/include/cie_thread_configurator/thread_configurator_node.hpp b/cie_thread_configurator/include/cie_thread_configurator/thread_configurator_node.hpp index 0cd31d6..bcd4fc3 100644 --- a/cie_thread_configurator/include/cie_thread_configurator/thread_configurator_node.hpp +++ b/cie_thread_configurator/include/cie_thread_configurator/thread_configurator_node.hpp @@ -35,7 +35,6 @@ class ThreadConfiguratorNode : public rclcpp::Node { explicit ThreadConfiguratorNode( const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); ~ThreadConfiguratorNode(); - bool all_applied(); void print_all_unapplied(); bool has_cgroup() const; @@ -48,6 +47,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 on_all_configured(); rclcpp::Subscription::SharedPtr subscription_; @@ -58,4 +58,5 @@ class ThreadConfiguratorNode : public rclcpp::Node { std::unordered_map id_to_thread_config_; int unapplied_num_; int cgroup_num_; + bool configured_at_least_once_ = false; }; diff --git a/cie_thread_configurator/src/thread_configurator_node.cpp b/cie_thread_configurator/src/thread_configurator_node.cpp index dcf56d5..e9973ab 100644 --- a/cie_thread_configurator/src/thread_configurator_node.cpp +++ b/cie_thread_configurator/src/thread_configurator_node.cpp @@ -119,6 +119,10 @@ ThreadConfiguratorNode::ThreadConfiguratorNode( "/cie_thread_configurator/non_ros_thread_info", non_ros_thread_qos, std::bind(&ThreadConfiguratorNode::non_ros_thread_callback, this, std::placeholders::_1)); + + if (unapplied_num_ == 0) { + on_all_configured(); + } } void ThreadConfiguratorNode::validate_hardware_info(const YAML::Node &yaml) { @@ -170,9 +174,11 @@ ThreadConfiguratorNode::~ThreadConfiguratorNode() { } } -bool ThreadConfiguratorNode::all_applied() { return unapplied_num_ == 0; } - void ThreadConfiguratorNode::print_all_unapplied() { + if (unapplied_num_ == 0) { + return; + } + RCLCPP_WARN(this->get_logger(), "Following threads are not yet configured"); for (auto &config : thread_configs_) { @@ -325,11 +331,14 @@ void ThreadConfiguratorNode::callback_group_callback( ThreadConfig *config = it->second; if (config->applied) { + // Always re-apply: the OS may reuse the same thread IDs after an + // application restarts, so we cannot use thread_id equality to skip + // reconfiguration. RCLCPP_INFO( this->get_logger(), - "This callback group is already configured. skip (id=%s, tid=%ld)", + "Re-applying configuration for already configured callback group " + "(id=%s, tid=%ld)", msg->callback_group_id.c_str(), msg->thread_id); - return; } RCLCPP_INFO(this->get_logger(), "Received CallbackGroupInfo: tid=%ld | %s", @@ -344,8 +353,14 @@ void ThreadConfiguratorNode::callback_group_callback( return; } + if (!config->applied) { + unapplied_num_--; + } config->applied = true; - unapplied_num_--; + + if (unapplied_num_ == 0 && !configured_at_least_once_) { + on_all_configured(); + } } void ThreadConfiguratorNode::non_ros_thread_callback( @@ -361,10 +376,14 @@ void ThreadConfiguratorNode::non_ros_thread_callback( ThreadConfig *config = it->second; if (config->applied) { - RCLCPP_INFO(this->get_logger(), - "This thread is already configured. skip (name=%s, tid=%ld)", - msg->thread_name.c_str(), msg->thread_id); - return; + // Always re-apply: the OS may reuse the same thread IDs after an + // application restarts, so we cannot use thread_id equality to skip + // reconfiguration. + RCLCPP_INFO( + this->get_logger(), + "Re-applying configuration for already configured non-ROS thread " + "(name=%s, tid=%ld)", + msg->thread_name.c_str(), msg->thread_id); } RCLCPP_INFO(this->get_logger(), "Received NonRosThreadInfo: tid=%ld | %s", @@ -379,8 +398,21 @@ void ThreadConfiguratorNode::non_ros_thread_callback( return; } + if (!config->applied) { + unapplied_num_--; + } config->applied = true; - unapplied_num_--; + + if (unapplied_num_ == 0 && !configured_at_least_once_) { + on_all_configured(); + } +} + +void ThreadConfiguratorNode::on_all_configured() { + RCLCPP_INFO(this->get_logger(), + "Success: All of the configurations are applied."); + + configured_at_least_once_ = true; } bool ThreadConfiguratorNode::has_cgroup() const { return cgroup_num_ > 0; } diff --git a/cie_thread_configurator/src/thread_configurator_node_main.cpp b/cie_thread_configurator/src/thread_configurator_node_main.cpp index 85ccfa4..430c4d5 100644 --- a/cie_thread_configurator/src/thread_configurator_node_main.cpp +++ b/cie_thread_configurator/src/thread_configurator_node_main.cpp @@ -15,23 +15,9 @@ int main(int argc, char *argv[]) { std::make_shared(); executor->add_node(node); + executor->spin(); - while (rclcpp::ok() && !node->all_applied()) { - executor->spin_once(); - } - - if (node->all_applied()) { - RCLCPP_INFO(node->get_logger(), - "Success: All of the configurations are applied."); - if (node->has_cgroup()) { - RCLCPP_INFO(node->get_logger(), - "Press enter to exit and remove the cgroups created for " - "thread affinity:"); - std::cin.get(); - } - } else { - node->print_all_unapplied(); - } + node->print_all_unapplied(); } catch (const std::exception &e) { std::cerr << "[ERROR] " << e.what() << std::endl; rclcpp::shutdown();