fix(agnocastlib): use worker thread for ros2 publisher

src/agnocastlib/include/agnocast_mq.hpp

@@ -17,4 +17,9 @@ struct MqMsgAgnocast
   bool dummy;
 };
 
+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
 {
@@ -44,8 +47,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>>;
@@ -72,9 +82,91 @@ class Publisher
       do_always_ros2_publish_ = false;
     }
 
+    const std::thread::id tid = std::this_thread::get_id();
+    ros2_publish_mq_name_ = create_mq_name(topic_name_, tid);
+
+    create_ros2_publish_thread();
+
+    ros2_publish_mq_ = mq_open(ros2_publish_mq_name_.c_str(), O_WRONLY);
+    if (ros2_publish_mq_ == -1) {
+      RCLCPP_ERROR(logger, "mq_open for ROS 2 publish notification failed: %s", strerror(errno));
+      close(agnocast_fd);
+      exit(EXIT_FAILURE);
+    }
+
     id_ = initialize_publisher(publisher_pid_, topic_name_);
   }
 
+  ~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 for ROS 2 publish notification failed: %s", strerror(errno));
+      close(agnocast_fd);
+      exit(EXIT_FAILURE);
+    }
+
+    ros2_publish_thread_.join();
+  }
+
+  void create_ros2_publish_thread()
+  {
+    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, 0666, &attr);
+    if (mq == -1) {
+      RCLCPP_ERROR(logger, "mq_open for ROS 2 publish notification 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;
+        }
+
+        ros2_publish_mtx_.lock();
+        while (!ros2_message_queue_.empty()) {
+          auto message = std::move(ros2_message_queue_.front());
+          ros2_message_queue_.pop();
+          ros2_publish_mtx_.unlock();
+
+          ros2_publisher_->publish(*message.get());
+
+          ros2_publish_mtx_.lock();
+        }
+        ros2_publish_mtx_.unlock();
+      }
+
+      if (mq_close(mq) == -1) {
+        RCLCPP_ERROR(logger, "mq_close for ROS 2 publish notification failed: %s", strerror(errno));
+        close(agnocast_fd);
+        exit(EXIT_FAILURE);
+      }
+
+      if (mq_unlink(ros2_publish_mq_name_.c_str()) == -1) {
+        RCLCPP_ERROR(
+          logger, "mq_unlink for ROS 2 publish notification failed: %s", strerror(errno));
+        close(agnocast_fd);
+        exit(EXIT_FAILURE);
+      }
+    });
+  }
+
   ipc_shared_ptr<MessageT> borrow_loaned_message()
   {
     increment_borrowed_publisher_num();
@@ -112,11 +204,21 @@ 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) {
+        RCLCPP_ERROR(logger, "mq_send for ROS 2 publish notification failed: %s", strerror(errno));
+        close(agnocast_fd);
+        exit(EXIT_FAILURE);
+      }
+    } else {
+      message.reset();
     }
-
-    message.reset();
   }
 
   uint32_t get_subscription_count() const

src/agnocastlib/include/agnocast_utils.hpp

@@ -11,6 +11,7 @@ namespace agnocast
 extern rclcpp::Logger logger;
 
 void validate_ld_preload();
+std::string create_mq_name(const std::string & topic_name, const std::thread::id id);
 std::string create_mq_name(const std::string & topic_name, const topic_local_id_t id);
 std::string create_shm_name(const pid_t pid);
 std::string create_mq_name_new_publisher(const pid_t pid);

src/agnocastlib/src/agnocast_utils.cpp

@@ -1,6 +1,8 @@
 #include "agnocast_utils.hpp"
 
 #include <cstring>
+#include <sstream>
+#include <thread>
 
 namespace agnocast
 {
@@ -20,9 +22,9 @@ 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(const std::string & topic_name, const std::string & id)
 {
-  std::string mq_name = topic_name + "@" + std::to_string(id);
+  std::string mq_name = topic_name + "@" + id;
 
   if (mq_name[0] != '/') {
     RCLCPP_ERROR(logger, "create_mq_name failed");
@@ -40,6 +42,18 @@ std::string create_mq_name(const std::string & topic_name, const topic_local_id_
   return mq_name;
 }
 
+std::string create_mq_name(const std::string & topic_name, const std::thread::id tid)
+{
+  std::stringstream ss;
+  ss << tid;
+  return create_mq_name(topic_name, "thread_" + ss.str());
+}
+
+std::string create_mq_name(const std::string & topic_name, const topic_local_id_t id)
+{
+  return create_mq_name(topic_name, std::to_string(id));
+}
+
 std::string create_shm_name(const pid_t pid)
 {
   return "/agnocast@" + std::to_string(pid);