fix(agnocastlib): use worker thread for ros2 publisher

src/agnocastlib/include/agnocast_publisher.hpp

@@ -16,8 +16,12 @@
 #include <sys/types.h>
 #include <unistd.h>
 
+#include <condition_variable>
 #include <cstdint>
 #include <cstring>
+#include <mutex>
+#include <queue>
+#include <utility>
 
 namespace agnocast
 {
@@ -33,6 +37,8 @@ void decrement_borrowed_publisher_num();
 
 extern int agnocast_fd;
 extern "C" uint32_t get_borrowed_publisher_num();
+extern std::vector<std::thread> threads;
+extern std::mutex threads_mtx;
 
 template <typename MessageT>
 class Publisher
@@ -46,6 +52,9 @@ class Publisher
   std::unordered_map<std::string, mqd_t> opened_mqs_;
   bool do_always_ros2_publish_;  // For transient local.
   typename rclcpp::Publisher<MessageT>::SharedPtr ros2_publisher_;
+  std::queue<ipc_shared_ptr<MessageT>> ros2_message_queue_ = std::queue<ipc_shared_ptr<MessageT>>();
+  std::mutex ros2_publish_mtx_ = std::mutex();
+  std::condition_variable ros2_publish_cv_ = std::condition_variable();
 
 public:
   using SharedPtr = std::shared_ptr<Publisher<MessageT>>;
@@ -72,6 +81,27 @@ class Publisher
       do_always_ros2_publish_ = false;
     }
 
+    auto th = std::thread([this]() {
+      while (true) {
+        std::unique_lock<std::mutex> lock(ros2_publish_mtx_);
+        ros2_publish_cv_.wait(lock, [this]() { return !ros2_message_queue_.empty(); });
+
+        while (!ros2_message_queue_.empty()) {
+          auto message = std::move(ros2_message_queue_.front());
+          ros2_message_queue_.pop();
+
+          lock.unlock();
+          ros2_publisher_->publish(*message.get());
+          lock.lock();
+        }
+      }
+    });
+
+    {
+      std::lock_guard<std::mutex> lock(threads_mtx);
+      threads.push_back(std::move(th));
+    }
+
     id_ = initialize_publisher(publisher_pid_, topic_name_);
   }
 
@@ -112,11 +142,13 @@ 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));
+      }
 
-    message.reset();
+      ros2_publish_cv_.notify_one();
+    }
   }
 
   uint32_t get_subscription_count() const

src/agnocastlib/src/agnocast.cpp

@@ -16,6 +16,7 @@ namespace agnocast
 int agnocast_fd = -1;
 std::atomic<bool> is_running = true;
 std::vector<std::thread> threads;
+std::mutex threads_mtx;
 extern mqd_t mq_new_publisher;
 
 std::vector<int> shm_fds;
@@ -159,8 +160,11 @@ static void shutdown_agnocast()
     }
   }
 
-  for (auto & th : threads) {
-    th.join();
+  {
+    std::lock_guard<std::mutex> lock(threads_mtx);
+    for (auto & th : threads) {
+      th.join();
+    }
   }
 
   printf("[INFO] [Agnocast]: shutdown_agnocast completed\n");

src/agnocastlib/src/agnocast_subscription.cpp

@@ -6,6 +6,7 @@ namespace agnocast
 mqd_t mq_new_publisher = -1;
 
 extern std::vector<std::thread> threads;
+extern std::mutex threads_mtx;
 
 SubscriptionBase::SubscriptionBase(
   rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos)
@@ -65,7 +66,10 @@ static void wait_for_new_publisher(const pid_t subscriber_pid)
     }
   });
 
-  threads.push_back(std::move(th));
+  {
+    std::lock_guard<std::mutex> lock(threads_mtx);
+    threads.push_back(std::move(th));
+  }
 }
 
 union ioctl_subscriber_args SubscriptionBase::initialize(bool is_take_sub)