add topic asker

Author: KingRichord

infinite_sense_core/include/messenger.h

@@ -19,12 +19,15 @@ class Messenger {
   std::string GetPubEndpoint() const;
   void Pub(const std::string& topic, const std::string& metadata);
   void PubStruct(const std::string& topic, const void* data, size_t size);
-
+  [[noreturn]] void WaitAsk();
  private:
   Messenger();
   ~Messenger();
   zmq::context_t context_{};
   zmq::socket_t publisher_{};
   std::string endpoint_{};
+ private:
+  zmq::socket_t asker_{};
+  std::thread ask_thread_;
 };
 }  // namespace infinite_sense

infinite_sense_core/src/messenger.cpp

@@ -11,14 +11,22 @@ Messenger::Messenger() {
   try {
     publisher_.bind("tcp://*:0");
     endpoint_ = publisher_.get(zmq::sockopt::last_endpoint);
-    LOG(INFO) << "ZMQ PUB: " << endpoint_;
+    LOG(INFO) << "Publisher: " << endpoint_;
   } catch (const zmq::error_t &e) {
     LOG(ERROR) << "Failed to bind ZMQ publisher: " << e.what();
     context_.close();
     publisher_.close();
   }
+  asker_ = zmq::socket_t(context_, zmq::socket_type::rep);
+  asker_.bind("tcp://*:4565");
+  ask_thread_ = std::thread([this] { WaitAsk(); });
+}
+Messenger::~Messenger() {
+  publisher_.close();
+  while (ask_thread_.joinable()) {
+    ask_thread_.join();
+  }
 }
-Messenger::~Messenger() { publisher_.close(); }
 void Messenger::Pub(const std::string &topic, const std::string &metadata) {
   try {
     zmq::message_t topic_msg(topic.size());
@@ -47,4 +55,18 @@ void Messenger::PubStruct(const std::string &topic, const void *data, const size
 }
 std::string Messenger::GetPubEndpoint() const { return endpoint_; }
 
+[[noreturn]] void Messenger::WaitAsk() {
+  while (true) {
+    zmq::message_t request;
+    asker_.recv(request, zmq::recv_flags::none);
+    std::string received_msg(static_cast<char *>(request.data()), request.size());
+    if (received_msg == "ask_endpoint") {
+      std::string reply_msg = GetPubEndpoint();
+      zmq::message_t reply(reply_msg.size());
+      memcpy(reply.data(), reply_msg.data(), reply_msg.size());
+      asker_.send(reply, zmq::send_flags::none);
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+}
 }  // namespace infinite_sense

tools/topic_monitor/monitor.cpp

@@ -22,6 +22,7 @@ void TopicMonitor::Stop() {
   }
 }
 TopicMonitor::TopicMonitor() : context_(1), subscriber_(context_, ZMQ_SUB), should_run_(false) {
+
   try {
     subscriber_.connect(infinite_sense::Messenger::GetInstance().GetPubEndpoint());
     subscriber_.set(zmq::sockopt::subscribe, "");