fix(cie_thread_configurator): ensure thread info message delivery

cie_thread_configurator/include/cie_thread_configurator/cie_thread_configurator.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "rclcpp/rclcpp.hpp"
+#include <chrono>
 #include <map>
 #include <memory>
 #include <string>
@@ -57,15 +58,41 @@ std::thread spawn_non_ros2_thread(const char *thread_name, F &&f,
     auto publisher =
         node->create_publisher<cie_config_msgs::msg::NonRosThreadInfo>(
             "/cie_thread_configurator/non_ros_thread_info",
-            rclcpp::QoS(1000).keep_all());
+            rclcpp::QoS(1000).reliable());
     auto tid = static_cast<pid_t>(syscall(SYS_gettid));
 
-    auto message = std::make_shared<cie_config_msgs::msg::NonRosThreadInfo>();
-    message->thread_id = tid;
-    message->thread_name = thread_name;
-    publisher->publish(*message);
-
-    context->shutdown("Publishing is finished.");
+    // Wait for subscriber to connect before publishing (timeout: 1 second)
+    constexpr int max_subscriber_wait_iterations = 100; // 100 * 10ms = 1 second
+    int wait_count = 0;
+    while (publisher->get_subscription_count() == 0 &&
+           wait_count < max_subscriber_wait_iterations) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      ++wait_count;
+    }
+
+    if (publisher->get_subscription_count() > 0) {
+      auto message = std::make_shared<cie_config_msgs::msg::NonRosThreadInfo>();
+      message->thread_id = tid;
+      message->thread_name = thread_name;
+      publisher->publish(*message);
+      const bool all_acked =
+          publisher->wait_for_all_acked(std::chrono::milliseconds(500));
+      if (!all_acked) {
+        RCLCPP_WARN(
+            node->get_logger(),
+            "Timed out waiting for NonRosThreadInfo acknowledgment (thread "
+            "'%s').",
+            thread_name);
+      }
+    } else {
+      RCLCPP_WARN(node->get_logger(),
+                  "No subscriber for NonRosThreadInfo (thread '%s'). "
+                  "Please run thread_configurator_node if you want to "
+                  "configure thread scheduling.",
+                  thread_name);
+    }
+
+    context->shutdown("cie_thread_client finished.");
 
     std::apply(std::move(func), std::move(captured_args));
   });

cie_thread_configurator/src/util.cpp

@@ -77,6 +77,14 @@ void publish_callback_group_info(
     const rclcpp::Publisher<cie_config_msgs::msg::CallbackGroupInfo>::SharedPtr
         &publisher,
     int64_t tid, const std::string &callback_group_id) {
+  if (publisher->get_subscription_count() == 0) {
+    RCLCPP_WARN(rclcpp::get_logger("cie_thread_configurator"),
+                "No subscriber for CallbackGroupInfo. "
+                "Please run thread_configurator_node if you want to configure "
+                "thread scheduling.");
+    return;
+  }
+
   auto message = std::make_shared<cie_config_msgs::msg::CallbackGroupInfo>();
 
   message->thread_id = tid;