fix(agnocastlib): use worker thread for ros2 publisher

src/agnocastlib/include/agnocast_mq.hpp

@@ -7,4 +7,9 @@ struct MqMsgAgnocast
 {
 };
 
+struct MqMsgPublishNotification
+{
+  bool should_terminate;
+};
+
 }  // namespace agnocast

src/agnocastlib/include/agnocast_publisher.hpp

@@ -18,6 +18,9 @@
 
 #include <cstdint>
 #include <cstring>
+#include <mutex>
+#include <queue>
+#include <thread>
 
 namespace agnocast
 {
@@ -45,8 +48,15 @@ class Publisher
   // TODO(Koichi98): The mq should be closed when a subscriber unsubscribes the topic, but this is
   // not currently implemented.
   std::unordered_map<std::string, mqd_t> opened_mqs_;
+
+  // ROS2 publish related variables
   bool do_always_ros2_publish_;  // For transient local.
   typename rclcpp::Publisher<MessageT>::SharedPtr ros2_publisher_;
+  mqd_t ros2_publish_mq_;
+  std::string ros2_publish_mq_name_;
+  std::queue<ipc_shared_ptr<MessageT>> ros2_message_queue_;
+  std::thread ros2_publish_thread_;
+  std::mutex ros2_publish_mtx_;
 
 public:
   using SharedPtr = std::shared_ptr<Publisher<MessageT>>;
@@ -74,6 +84,85 @@ class Publisher
     }
 
     id_ = initialize_publisher(publisher_pid_, topic_name_);
+
+    ros2_publish_mq_name_ = create_mq_name_for_ros2_publish(topic_name_, id_);
+
+    create_ros2_publish_thread();
+
+    ros2_publish_mq_ = mq_open(ros2_publish_mq_name_.c_str(), O_WRONLY | O_NONBLOCK);
+    if (ros2_publish_mq_ == -1) {
+      RCLCPP_ERROR(logger, "mq_open failed: %s", strerror(errno));
+      close(agnocast_fd);
+      exit(EXIT_FAILURE);
+    }
+  }
+
+  ~Publisher()
+  {
+    MqMsgPublishNotification mq_msg = {};
+    mq_msg.should_terminate = true;
+    if (mq_send(ros2_publish_mq_, reinterpret_cast<char *>(&mq_msg), sizeof(mq_msg), 0) == -1) {
+      RCLCPP_ERROR(logger, "mq_send failed: %s", strerror(errno));
+    }
+
+    ros2_publish_thread_.join();
+  }
+
+  void create_ros2_publish_thread()
+  {
+    const int mq_mode = 0666;
+    struct mq_attr attr = {};
+    attr.mq_flags = 0;
+    attr.mq_maxmsg = 1;
+    attr.mq_msgsize = sizeof(MqMsgPublishNotification);
+    attr.mq_curmsgs = 0;
+    mqd_t mq = mq_open(ros2_publish_mq_name_.c_str(), O_CREAT | O_RDONLY, mq_mode, &attr);
+    if (mq == -1) {
+      RCLCPP_ERROR(logger, "mq_open failed: %s", strerror(errno));
+      close(agnocast_fd);
+      exit(EXIT_FAILURE);
+    }
+
+    ros2_publish_thread_ = std::thread([this, mq]() {
+      while (true) {
+        MqMsgPublishNotification mq_msg = {};
+        auto ret = mq_receive(mq, reinterpret_cast<char *>(&mq_msg), sizeof(mq_msg), nullptr);
+        if (ret == -1) {
+          RCLCPP_ERROR(
+            logger, "mq_receive for ROS 2 publish notification failed: %s", strerror(errno));
+          close(agnocast_fd);
+          exit(EXIT_FAILURE);
+        }
+
+        if (mq_msg.should_terminate) {
+          break;
+        }
+
+        while (true) {
+          ipc_shared_ptr<MessageT> message;
+
+          {
+            std::scoped_lock lock(ros2_publish_mtx_);
+            if (ros2_message_queue_.empty()) {
+              break;
+            }
+
+            message = std::move(ros2_message_queue_.front());
+            ros2_message_queue_.pop();
+          }
+
+          ros2_publisher_->publish(*message.get());
+        }
+      }
+
+      if (mq_close(mq) == -1) {
+        RCLCPP_ERROR(logger, "mq_close failed: %s", strerror(errno));
+      }
+
+      if (mq_unlink(ros2_publish_mq_name_.c_str()) == -1) {
+        RCLCPP_ERROR(logger, "mq_unlink failed: %s", strerror(errno));
+      }
+    });
   }
 
   ipc_shared_ptr<MessageT> borrow_loaned_message()
@@ -105,11 +194,24 @@ class Publisher
     }
 
     if (do_always_ros2_publish_ || ros2_publisher_->get_subscription_count() > 0) {
-      const MessageT * raw = message.get();
-      ros2_publisher_->publish(*raw);
+      {
+        std::lock_guard<std::mutex> lock(ros2_publish_mtx_);
+        ros2_message_queue_.push(std::move(message));
+      }
+
+      MqMsgPublishNotification mq_msg = {};
+      mq_msg.should_terminate = false;
+      if (mq_send(ros2_publish_mq_, reinterpret_cast<char *>(&mq_msg), sizeof(mq_msg), 0) == -1) {
+        // If it returns EAGAIN, it means mq_send has already been executed, but the ros2 publish
+        // thread hasn't received it yet. Thus, there's no need to send it again since the
+        // notification has already been sent.
+        if (errno != EAGAIN) {
+          RCLCPP_ERROR(logger, "mq_send failed: %s", strerror(errno));
+        }
+      }
+    } else {
+      message.reset();
     }
-
-    message.reset();
   }
 
   uint32_t get_subscription_count() const

src/agnocastlib/include/agnocast_utils.hpp

@@ -11,7 +11,10 @@ namespace agnocast
 extern rclcpp::Logger logger;
 
 void validate_ld_preload();
-std::string create_mq_name(const std::string & topic_name, const topic_local_id_t id);
+std::string create_mq_name_for_agnocast_publish(
+  const std::string & topic_name, const topic_local_id_t id);
+std::string create_mq_name_for_ros2_publish(
+  const std::string & topic_name, const topic_local_id_t id);
 std::string create_shm_name(const pid_t pid);
 uint64_t agnocast_get_timestamp();
 

src/agnocastlib/src/agnocast_publisher.cpp

@@ -69,7 +69,7 @@ union ioctl_publish_args publish_core(
   for (uint32_t i = 0; i < publish_args.ret_subscriber_num; i++) {
     const topic_local_id_t subscriber_id = publish_args.ret_subscriber_ids[i];
 
-    const std::string mq_name = create_mq_name(topic_name, subscriber_id);
+    const std::string mq_name = create_mq_name_for_agnocast_publish(topic_name, subscriber_id);
     mqd_t mq = 0;
     if (opened_mqs.find(mq_name) != opened_mqs.end()) {
       mq = opened_mqs[mq_name];

src/agnocastlib/src/agnocast_subscription.cpp

@@ -43,7 +43,7 @@ mqd_t open_mq_for_subscription(
   const std::string & topic_name, const topic_local_id_t subscriber_id,
   std::pair<mqd_t, std::string> & mq_subscription)
 {
-  std::string mq_name = create_mq_name(topic_name, subscriber_id);
+  std::string mq_name = create_mq_name_for_agnocast_publish(topic_name, subscriber_id);
   struct mq_attr attr = {};
   attr.mq_flags = 0;                        // Blocking queue
   attr.mq_msgsize = sizeof(MqMsgAgnocast);  // Maximum message size

src/agnocastlib/src/agnocast_utils.cpp

@@ -20,7 +20,8 @@ void validate_ld_preload()
   }
 }
 
-std::string create_mq_name(const std::string & topic_name, const topic_local_id_t id)
+static std::string create_mq_name(
+  const std::string & header, const std::string & topic_name, const topic_local_id_t id)
 {
   if (topic_name.length() == 0 || topic_name[0] != '/') {
     RCLCPP_ERROR(logger, "create_mq_name failed");
@@ -30,7 +31,7 @@ std::string create_mq_name(const std::string & topic_name, const topic_local_id_
 
   std::string mq_name = topic_name;
   mq_name[0] = '@';
-  mq_name = "/agnocast" + mq_name + "@" + std::to_string(id);
+  mq_name = header + mq_name + "@" + std::to_string(id);
 
   // As a mq_name, '/' cannot be used
   for (size_t i = 1; i < mq_name.size(); i++) {
@@ -42,6 +43,18 @@ std::string create_mq_name(const std::string & topic_name, const topic_local_id_
   return mq_name;
 }
 
+std::string create_mq_name_for_agnocast_publish(
+  const std::string & topic_name, const topic_local_id_t id)
+{
+  return create_mq_name("/agnocast", topic_name, id);
+}
+
+std::string create_mq_name_for_ros2_publish(
+  const std::string & topic_name, const topic_local_id_t id)
+{
+  return create_mq_name("/agnocast_to_ros2", topic_name, id);
+}
+
 std::string create_shm_name(const pid_t pid)
 {
   return "/agnocast@" + std::to_string(pid);

src/agnocastlib/test/test_agnocast_utils.cpp

@@ -4,17 +4,27 @@
 
 TEST(AgnocastUtilsTest, create_mq_name_normal)
 {
-  EXPECT_EQ(agnocast::create_mq_name("/dummy", 0), "/agnocast@dummy@0");
+  EXPECT_EQ(agnocast::create_mq_name_for_agnocast_publish("/dummy", 0), "/agnocast@dummy@0");
+  EXPECT_EQ(agnocast::create_mq_name_for_ros2_publish("/dummy", 0), "/agnocast_to_ros2@dummy@0");
 }
 
 TEST(AgnocastUtilsTest, create_mq_name_slash_included)
 {
-  EXPECT_EQ(agnocast::create_mq_name("/dummy/dummy", 0), "/agnocast@dummy_dummy@0");
+  EXPECT_EQ(
+    agnocast::create_mq_name_for_agnocast_publish("/dummy/dummy", 0), "/agnocast@dummy_dummy@0");
+  EXPECT_EQ(
+    agnocast::create_mq_name_for_ros2_publish("/dummy/dummy", 0),
+    "/agnocast_to_ros2@dummy_dummy@0");
 }
 
 TEST(AgnocastUtilsTest, create_mq_name_invalid_topic)
 {
-  EXPECT_EXIT(agnocast::create_mq_name("dummy", 0), ::testing::ExitedWithCode(EXIT_FAILURE), "");
+  EXPECT_EXIT(
+    agnocast::create_mq_name_for_agnocast_publish("dummy", 0),
+    ::testing::ExitedWithCode(EXIT_FAILURE), "");
+  EXPECT_EXIT(
+    agnocast::create_mq_name_for_ros2_publish("dummy", 0), ::testing::ExitedWithCode(EXIT_FAILURE),
+    "");
 }
 
 TEST(AgnocastUtilsTest, validate_ld_preload_normal)