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..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 @@ -1,6 +1,7 @@ #pragma once #include "rclcpp/rclcpp.hpp" +#include #include #include #include @@ -57,15 +58,41 @@ 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)); - 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); + 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)); }); diff --git a/cie_thread_configurator/src/util.cpp b/cie_thread_configurator/src/util.cpp index ce5a125..4d81a74 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;