Create Publisher in RealtimePublisher

realtime_tools/include/realtime_tools/realtime_publisher.hpp

@@ -47,6 +47,7 @@
 #include <utility>
 
 #include "rclcpp/publisher.hpp"
+#include "rclcpp/node.hpp"
 
 namespace realtime_tools
 {
@@ -86,6 +87,24 @@ class RealtimePublisher
     }
   }
 
+  explicit RealtimePublisher(std::shared_ptr<rclcpp::Node> node, const std::string & topic_name,
+    const rclcpp::QoS & qos = rclcpp::SystemDefaultsQoS())
+  : is_running_(false), keep_running_(true), turn_(State::LOOP_NOT_STARTED)
+  {
+    publisher_= node->create_publisher<MessageT>(
+      topic_name,
+      qos);
+    thread_ = std::thread(&RealtimePublisher::publishingLoop, this);
+
+    // Wait for the thread to be ready before proceeding
+    // This is important to ensure that the thread is properly initialized and ready to handle
+    // messages before any other operations are performed on the RealtimePublisher instance.
+    while (!thread_.joinable() ||
+           turn_.load(std::memory_order_acquire) == State::LOOP_NOT_STARTED) {
+      std::this_thread::sleep_for(std::chrono::microseconds(100));
+    }
+  }
+
   /// Destructor
   ~RealtimePublisher()
   {

realtime_tools/test/realtime_publisher_tests.cpp

@@ -29,6 +29,7 @@
 #include <gmock/gmock.h>
 
 #include <chrono>
+#include <future>
 #include <memory>
 #include <mutex>
 #include <thread>
@@ -98,3 +99,79 @@ TEST(RealtimePublisher, rt_can_try_publish)
   EXPECT_STREQ(expected_msg, str_callback.msg_.string_value.c_str());
   rclcpp::shutdown();
 }
+
+class RealtimePublisherTest : public ::testing::Test {
+protected:
+  void SetUp() override {
+    rclcpp::init(0, nullptr);
+
+    node_ = std::make_shared<rclcpp::Node>("rt_test_node");
+
+    exec_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
+    exec_->add_node(node_);
+
+    spin_thread_ = std::thread([this]() { exec_->spin(); });
+  }
+
+  void TearDown() override {
+    if (exec_) {
+      exec_->cancel();
+    }
+    if (spin_thread_.joinable()) {
+      spin_thread_.join();
+    }
+    node_.reset();
+    rclcpp::shutdown();
+  }
+
+  std::shared_ptr<rclcpp::Node> node_ = nullptr;
+  std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> exec_ = nullptr;
+  std::thread spin_thread_;
+  const std::string TOPIC_NAME_ = "/rt_publish";
+  const std::chrono::duration<int64_t> WAIT_ON_PUBLISH_TIMEOUT_ = std::chrono::seconds(1);
+};
+
+TEST_F(RealtimePublisherTest, node_based_init_works)
+{
+  rclcpp::QoS qos = rclcpp::QoS(10);
+  qos.reliable().transient_local();
+  RealtimePublisher<StringMsg> rt_pub(node_, TOPIC_NAME_, qos);
+  ASSERT_TRUE(rt_pub.can_publish());
+}
+
+TEST_F(RealtimePublisherTest, publish_latched_message)
+{
+  const char * expected_msg = "Hello World";
+  StringMsg msg;
+  msg.string_value = expected_msg;
+  rclcpp::QoS qos = rclcpp::QoS(10);
+  qos.reliable().transient_local();
+  RealtimePublisher<StringMsg> rt_pub(node_, TOPIC_NAME_, qos);
+
+  ASSERT_TRUE(rt_pub.can_publish());
+  ASSERT_TRUE(rt_pub.try_publish(msg));
+
+  // create another node and subscriber to verify message received
+  StringCallback str_callback;
+  std::promise<StringMsg> promise;
+  std::future<StringMsg> future = promise.get_future();
+  auto node = std::make_shared<rclcpp::Node>("rt_msg_sub_node");
+  auto sub = node->create_subscription<StringMsg>(
+    TOPIC_NAME_, qos,
+    [&promise](const StringMsg::SharedPtr incoming_msg) {
+      promise.set_value(*incoming_msg);
+    }
+  );
+
+  exec_->add_node(node);
+
+  auto start = std::chrono::steady_clock::now();
+  while (future.wait_for(std::chrono::milliseconds(100)) != std::future_status::ready) {
+    if (std::chrono::steady_clock::now() - start > WAIT_ON_PUBLISH_TIMEOUT_) {
+      FAIL() << "Timed out while waiting for latched message";
+    }
+  }
+  // Validate received message
+  auto received = future.get();
+  EXPECT_STREQ(expected_msg, received.string_value.c_str());
+}