Merge pull request #2 from InfiniteSenseLab/main

Alex-Beh · web-flow · commit 0b4d4f867e06 · 2025-05-07T13:22:50.000+08:00
[07 May] Merge main
diff --git a/example/ZMQ/zmq_main.cpp b/example/ZMQ/zmq_main.cpp
@@ -1,19 +1,43 @@
 #include "infinite_sense.h"
-#include <thread>
 using namespace infinite_sense;
 int main() {
+
+  // 1.创建同步器
   Synchronizer synchronizer;
-  // use net link
-  // synchronizer.SetNetLink("192.168.1.188", 8888);
-  // use serial link
-  synchronizer.SetSerialLink("/dev/ttyACM0",460800);
-  // use mv camera
-  // synchronizer.UseMvCam();
+  /*
+   使用网口连接
+   synchronizer.SetNetLink("192.168.1.188", 8888);
+  */
+  // 使用USB连接
+  synchronizer.SetSerialLink("/dev/ttyACM0", 460800);
+  /*
+    如使用工业相机, 需要指定 相机名称 和 同步板的触发端口
+    std::map<std::string, TriggerDevice> params;
+    params["camera_1"] = TriggerDevice::CAM_1; //camera_1:表示设备的名称，TriggerDevice::CAM_1:使用同步板CAM_1端口触发
+    synchronizer.UseMvCam(params);
+  */
+
+  // 2.开启同步
   synchronizer.Start();
+
+  // 3.订阅数据
   Synchronizer::PrintSummary();
+  zmq::context_t context(1);
+  zmq::socket_t subscriber(context, zmq::socket_type::sub);
+  subscriber.connect("tcp://localhost:5555");
+
+  const std::string topic = "imu_1";   // 订阅特定主题（如,板载IMU数据： "imu_1"）
+  subscriber.setsockopt(ZMQ_SUBSCRIBE, topic.c_str(), topic.size());
+  zmq::message_t msg;
   while (true) {
-    std::this_thread::sleep_for(std::chrono::milliseconds{1000});
+    if (subscriber.recv(msg, zmq::recv_flags::dontwait)) {
+      if (subscriber.get(zmq::sockopt::rcvmore)) {
+        zmq::message_t dummy;
+        subscriber.recv(dummy);
+      }
+    }
   }
+  // 4.停止同步
   synchronizer.Stop();
   return 0;
 }
diff --git a/infinite_sense_core/include/cam.h b/infinite_sense_core/include/cam.h
@@ -20,7 +20,7 @@ class CamManger {
   void Restart();
 
  private:
-  void Receive(void* handle, const std::string&) const;
+  void Receive(void* handle, const std::string&);
   bool is_running_{false};
   std::vector<int> rets_;
   std::vector<void*> handles_;
diff --git a/infinite_sense_core/include/data.h b/infinite_sense_core/include/data.h
@@ -3,43 +3,43 @@
 #include <string>
 #include "image.h"
 namespace infinite_sense {
-    struct ImuData {
-        uint64_t time_stamp_us;
-        float temperature;
-        std::string name;
-        float a[3];
-        float g[3];
-        float q[4];
-    };
+struct ImuData {
+  uint64_t time_stamp_us;
+  float temperature;
+  std::string name;
+  float a[3];
+  float g[3];
+  float q[4];
+};
 
-    struct CamData {
-        uint64_t time_stamp_us;
-        std::string name;
-        GMat image;
-    };
+struct CamData {
+  uint64_t time_stamp_us;
+  std::string name;
+  GMat image;
+};
 
-    struct LaserData {
-        uint64_t time_stamp_us;
-        std::string name;
-    };
+struct LaserData {
+  uint64_t time_stamp_us;
+  std::string name;
+};
 
-    struct GPSData {
-        uint64_t time_stamp_us;
-        uint64_t gps_stamp_us;
-        uint64_t gps_stamp_us_trigger;
-        std::string name;
-        float latitude;
-        float longitude;
-    };
+struct GPSData {
+  uint64_t time_stamp_us;
+  uint64_t gps_stamp_us;
+  uint64_t gps_stamp_us_trigger;
+  std::string name;
+  float latitude;
+  float longitude;
+};
 
-    enum TriggerDevice {
-        IMU_1 = 0,    // internal imu
-        IMU_2 = 1,    // external imu
-        CAM_1 = 2,    // camera 1
-        CAM_2 = 3,    // camera 2
-        CAM_3 = 4,    // camera 3
-        CAM_4 = 5,    // camera 4
-        LASER = 6,    // laser pps
-        GPS = 7,    // gps pps
-    };
-}
+enum TriggerDevice {
+  IMU_1 = 0,  // internal imu
+  IMU_2 = 1,  // external imu
+  CAM_1 = 2,  // camera 1
+  CAM_2 = 3,  // camera 2
+  CAM_3 = 4,  // camera 3
+  CAM_4 = 5,  // camera 4
+  LASER = 6,  // laser pps
+  GPS = 7,    // gps pps
+};
+}  // namespace infinite_sense
diff --git a/infinite_sense_core/include/image.h b/infinite_sense_core/include/image.h
@@ -104,7 +104,8 @@ class GMat {
  public:
   GMat();
 
-  GMat(int rows_, int cols_, int type = GMatType<>::Type, uchar* src = nullptr, bool copy = false, int image_align = 16);
+  GMat(int rows_, int cols_, int type = GMatType<>::Type, uchar* src = nullptr, bool copy = false,
+       int image_align = 16);
 
   GMat(const GMat& ref);
 
diff --git a/infinite_sense_core/include/infinite_sense.h b/infinite_sense_core/include/infinite_sense.h
@@ -10,25 +10,96 @@ class SerialManager;
 class CamManger;
 class TriggerManger;
 class Messenger;
+
+/**
+ * @class Synchronizer
+ * @brief 同步器类，负责统一协调网络、串口、相机等模块的启动与配置，管理多传感器系统的时间同步。
+ */
 class Synchronizer {
  public:
+  /**
+   * @brief 构造函数，初始化成员变量。
+   */
   Synchronizer();
+
+  /// 默认析构函数。
   ~Synchronizer() = default;
+
+  /**
+   * @brief 启动同步器，初始化并启动各组件（网络、串口、相机等）。
+   */
   void Start() const;
+
+  /**
+   * @brief 停止同步器，释放资源。
+   */
   void Stop() const;
-  static void SetLogPath(const std::string &path);
+
+  /**
+   * @brief 设置日志输出路径（静态方法）。
+   *
+   * @param path 日志保存路径。
+   */
+  static void SetLogPath(const std::string& path);
+
+  /**
+   * @brief 配置串口连接参数。
+   *
+   * @param serial_dev 串口设备名称（如 "/dev/ttyUSB0"）。
+   * @param serial_baud_rate 波特率（如 115200）。
+   */
   void SetSerialLink(std::string serial_dev, int serial_baud_rate);
+
+  /**
+   * @brief 配置网络连接参数。
+   *
+   * @param net_dev 目标网络 IP 地址。
+   * @param port 网络端口号。
+   */
   void SetNetLink(std::string net_dev, unsigned int port);
+
+  /**
+   * @brief 使用 MV（迈德威视）相机，并配置其对应的触发设备。
+   *
+   * @param params 可选参数：映射相机名称到 TriggerDevice 枚举。
+   */
   void UseMvCam(const std::map<std::string, TriggerDevice>& params = std::map<std::string, TriggerDevice>());
+
+  /**
+   * @brief 获取指定设备最近一次的触发时间（静态方法）。
+   *
+   * @param dev 查询的设备类型。
+   * @param time 返回对应的触发时间戳。
+   * @return true 如果查询成功（设备有触发记录）。
+   * @return false 如果设备无记录。
+   */
   static bool GetLastTriggerTime(TriggerDevice dev, uint64_t time);
-  static void PrintSummary() ;
+
+  /**
+   * @brief 打印当前系统的配置与状态摘要信息。
+   */
+  static void PrintSummary();
+
  private:
+  /// 网络地址
   std::string net_ip_;
+
+  /// 网络端口号
   unsigned short net_port_{};
+
+  /// 串口设备
   std::string serial_dev_;
+
+  /// 串口波特率
   int serial_baud_rate_{};
+
+  /// 网络管理器
   std::shared_ptr<NetManager> net_manager_{nullptr};
+
+  /// 串口管理器
   std::shared_ptr<SerialManager> serial_manager_{nullptr};
+
+  /// 相机管理器
   std::shared_ptr<CamManger> cam_manager_{nullptr};
 };
 
diff --git a/infinite_sense_core/include/trigger.h b/infinite_sense_core/include/trigger.h
@@ -3,28 +3,99 @@
 #include <mutex>
 #include <infinite_sense.h>
 #include <map>
+
 namespace infinite_sense {
+
+/**
+ * @brief 宏定义：设置所有设备的最新触发状态。
+ *
+ * @param timestamp 触发时间戳。
+ * @param status 触发状态位掩码（每一位对应一个设备）。
+ */
 #define SET_LAST_TRIGGER_STATUS(timestamp, status) TriggerManger::GetInstance().SetLastTriggerStatus(timestamp, status)
+
+/**
+ * @brief 宏定义：获取指定设备的最新触发状态时间。
+ *
+ * @param device 要查询的设备。
+ * @param timestamp 返回设备的最后触发时间戳。
+ */
 #define GET_LAST_TRIGGER_STATUS(device, timestamp) TriggerManger::GetInstance().GetLastTriggerStatus(device, timestamp)
+
+/**
+ * @class TriggerManger
+ * @brief 管理各传感器或设备的触发状态，并通过消息机制发布状态信息。
+ *
+ * 使用单例模式管理系统中所有的触发设备。支持位掩码形式更新所有设备的状态，
+ * 并可查询特定设备的最后一次有效触发时间。
+ */
 class TriggerManger {
  public:
+  /**
+   * @brief 获取 TriggerManger 单例对象。
+   * @return TriggerManger& 单例引用。
+   */
   static TriggerManger &GetInstance() {
     static TriggerManger instance;
     return instance;
   }
+
+  // 删除拷贝构造函数和赋值运算符，确保单例语义。
   TriggerManger(const TriggerManger &) = delete;
   TriggerManger &operator=(const TriggerManger &) = delete;
-  void SetLastTriggerStatus(const uint64_t &, const uint8_t &);
-  bool GetLastTriggerStatus(TriggerDevice, uint64_t &);
+
+  /**
+   * @brief 设置所有设备的最新触发状态。
+   *
+   * @param time 当前触发的时间戳。
+   * @param status 状态掩码，每一位表示一个设备是否触发。
+   */
+  void SetLastTriggerStatus(const uint64_t &time, const uint8_t &status);
+
+  /**
+   * @brief 获取指定设备的最后一次触发状态及时间。
+   *
+   * @param dev 设备枚举。
+   * @param time 返回该设备最后一次触发的时间戳。
+   * @return true 如果该设备有有效的触发状态。
+   * @return false 如果该设备无记录。
+   */
+  bool GetLastTriggerStatus(TriggerDevice dev, uint64_t &time);
+
  private:
+  /**
+   * @brief 从位掩码中获取对应设备的状态。
+   *
+   * @param data 状态字节。
+   * @param index 目标设备在位掩码中的索引。
+   * @return true 对应位置为1，表示已触发。
+   * @return false 对应位置为0，表示未触发。
+   */
   static bool GetBool(const uint8_t data, const int index) { return (data >> index) & 1; }
+
+  /**
+   * @brief 更新设备状态，并发布触发信息。
+   *
+   * @param dev 目标设备。
+   * @param bit_index 设备对应的位掩码索引。
+   * @param time 触发时间戳。
+   */
   void UpdateAndPublishDevice(TriggerDevice dev, int bit_index, uint64_t time);
+
+  /**
+   * @brief 发布指定设备的状态信息。
+   *
+   * @param dev 目标设备。
+   * @param time 触发时间戳。
+   * @param status 当前触发状态。
+   */
   void PublishDeviceStatus(TriggerDevice dev, uint64_t time, bool status);
   TriggerManger();
   ~TriggerManger() = default;
   uint8_t status_{0};
   std::mutex lock_{};
   std::map<TriggerDevice, std::tuple<bool, uint64_t>> status_map_{};
-  std::map<TriggerDevice, std::string> device_topics_; // 设备-topic映射
+  std::map<TriggerDevice, std::string> device_topics_;
 };
+
 }  // namespace infinite_sense
diff --git a/infinite_sense_core/src/cam.cpp b/infinite_sense_core/src/cam.cpp
@@ -2,7 +2,7 @@
 #include "infinite_sense.h"
 #include "MvCameraControl.h"
 #include "log.h"
-
+#include "trigger.h"
 namespace infinite_sense {
 bool IsColor(const MvGvspPixelType type) {
   switch (type) {
@@ -190,7 +190,7 @@ void CamManger::Stop() {
     LOG(INFO) << "Exit  " << i << "  cam ";
   }
 }
-void CamManger::Receive(void *handle, const std::string &name) const {
+void CamManger::Receive(void *handle, const std::string &name)  {
   unsigned int last_count = 0;
   MV_FRAME_OUT st_out_frame;
   CamData cam_data;
@@ -204,10 +204,18 @@ void CamManger::Receive(void *handle, const std::string &name) const {
       if (n_ret != MV_OK) {
         LOG(ERROR) << "Get ExposureTime fail! n_ret [0x" << std::hex << n_ret << "]";
       }
-      // {
-      //   cam_data.time_stamp_us =
-      //       DataManger::GetInstance().GetLastTiggerTime() + static_cast<uint64_t>(expose_time.fCurValue / 2.);
-      // }
+      // 这里的time_stamp_us是相机触发时间，需要加上曝光时间的一半，以获得相机拍摄的时间
+      if (params_.find(name) == params_.end()) {
+        LOG(ERROR) << "cam " << name << " not found!";
+      }
+      else {
+        if (uint64_t time; GET_LAST_TRIGGER_STATUS(params_[name], time)) {
+          cam_data.time_stamp_us = time + static_cast<uint64_t>(expose_time.fCurValue / 2.);
+        }
+        else {
+          LOG(ERROR) << "cam " << name << " not found!";
+        }
+      }
       MvGvspPixelType en_dst_pixel_type = PixelType_Gvsp_Undefined;
       unsigned int n_channel_num = 0;
       // 如果是彩色则转成RGB8
diff --git a/infinite_sense_core/src/infinite_sense.cpp b/infinite_sense_core/src/infinite_sense.cpp
diff --git a/infinite_sense_core/src/ser.cpp b/infinite_sense_core/src/ser.cpp
diff --git a/infinite_sense_core/src/trigger.cpp b/infinite_sense_core/src/trigger.cpp
