From 16236e997508a02a6ff8c27b3bfe1c7507572324 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Wed, 28 Jan 2026 11:26:15 +0900 Subject: [PATCH 1/2] fix(cie_thread_configurator): wait for subscriber before publishing thread info - Add subscriber discovery wait loop in spawn_non_ros2_thread with 1 second timeout to prevent message loss when publishing NonRosThreadInfo - Add warning when no subscriber is connected for CallbackGroupInfo to guide users to start thread_configurator_node - Backport from tier4/agnocast#967 --- .../cie_thread_configurator.hpp | 30 +++++++++++++++---- cie_thread_configurator/src/util.cpp | 8 +++++ 2 files changed, 32 insertions(+), 6 deletions(-) diff --git a/cie_thread_configurator/include/cie_thread_configurator/cie_thread_configurator.hpp b/cie_thread_configurator/include/cie_thread_configurator/cie_thread_configurator.hpp index dee0639..bf36eba 100644 --- a/cie_thread_configurator/include/cie_thread_configurator/cie_thread_configurator.hpp +++ b/cie_thread_configurator/include/cie_thread_configurator/cie_thread_configurator.hpp @@ -1,6 +1,7 @@ #pragma once #include "rclcpp/rclcpp.hpp" +#include #include #include #include @@ -60,12 +61,29 @@ std::thread spawn_non_ros2_thread(const char *thread_name, F &&f, rclcpp::QoS(1000).keep_all()); auto tid = static_cast(syscall(SYS_gettid)); - auto message = std::make_shared(); - 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(); + message->thread_id = tid; + message->thread_name = thread_name; + publisher->publish(*message); + } 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)); }); diff --git a/cie_thread_configurator/src/util.cpp b/cie_thread_configurator/src/util.cpp index c56015b..193b482 100644 --- a/cie_thread_configurator/src/util.cpp +++ b/cie_thread_configurator/src/util.cpp @@ -77,6 +77,14 @@ void publish_callback_group_info( const rclcpp::Publisher::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(); message->thread_id = tid; From 07c9a5d7312841739354737f4a3c51a4e5dd8f89 Mon Sep 17 00:00:00 2001 From: atsushi421 Date: Thu, 29 Jan 2026 10:47:40 +0900 Subject: [PATCH 2/2] fix(cie_thread_configurator): ensure NonRosThreadInfo message delivery before shutdown Use reliable QoS and wait_for_all_acked() to ensure the message is delivered to subscribers before shutting down the context, fixing flaky test failures in CI environments. --- .../cie_thread_configurator.hpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/cie_thread_configurator/include/cie_thread_configurator/cie_thread_configurator.hpp b/cie_thread_configurator/include/cie_thread_configurator/cie_thread_configurator.hpp index bf36eba..317f421 100644 --- a/cie_thread_configurator/include/cie_thread_configurator/cie_thread_configurator.hpp +++ b/cie_thread_configurator/include/cie_thread_configurator/cie_thread_configurator.hpp @@ -58,7 +58,7 @@ std::thread spawn_non_ros2_thread(const char *thread_name, F &&f, auto publisher = node->create_publisher( "/cie_thread_configurator/non_ros_thread_info", - rclcpp::QoS(1000).keep_all()); + rclcpp::QoS(1000).reliable()); auto tid = static_cast(syscall(SYS_gettid)); // Wait for subscriber to connect before publishing (timeout: 1 second) @@ -75,6 +75,15 @@ std::thread spawn_non_ros2_thread(const char *thread_name, F &&f, 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'). "