前言

最近在做激光+視覺的建圖，參照fast-livo做一套時間硬同步的設備，但fast-livo開源出來的驅動并不能真正做到時間同步
所以自己啃了下livox和海康相機的驅動，整理出來一些坑，給大家參考下，歡迎指正

硬件用的：livox mid70 + MV-CA013-A0UC + esp32

修改好的驅動：

https://github.com/vell001/livox_ros_driver

https://github.com/vell001/mvs_ros_pkg

arduino pps輸出代碼：

#include #define CAMERA_PWM_PIN 23#define LIDAR_PWM_PIN 13void setup() {
    pinMode(CAMERA_PWM_PIN, OUTPUT);
    pinMode(LIDAR_PWM_PIN, OUTPUT);}void loop() {
    for (int i = 0; i < 10; i++) {
        if (i == 0) {
            digitalWrite(LIDAR_PWM_PIN, HIGH);
        }
        digitalWrite(CAMERA_PWM_PIN, HIGH);
        delay(50);

        if (i == 0) {
            digitalWrite(LIDAR_PWM_PIN, LOW);
        }
        digitalWrite(CAMERA_PWM_PIN, LOW);
        delay(50);
    }}

原理：

1. livox+camera都接入同源pps，livox 1hz，camera 10hz【參照fast-livo，不過我使用esp32代替stm32】

image

1. livox當pps觸發時，雷達基準時間會歸零，所以可以通過這個事件得知pps對應的實際觸發時間

2. 在livox驅動里，監聽livox原始時間戳，如果當前時間戳小于上一幀時間戳，則代表觸發了pps，記錄下當前utc時間當作基準時間base_time

image

1. 將base_time信息寫入共享內存：/dev/shm/shm_timer，給camera驅動使用

2. 在livox驅動里，將packet的timebase改為共享內存中的基準時間base_time

image

1. 在camera驅動里讀取共享內存的基準時間base_time

2. 當base_time變更時，也就是lidar pps觸發時，查找接收到的圖像里，哪幀距離base_time最近，認為最近的一幀就是和lidar pps觸發為同一時機的數據，將時間對齊到base_time上，從數據上看，一般都是base_time變更比camera接收到數據早幾十ms左右，所以幾乎永遠是當前幀最近，也符合邏輯，camera傳輸比lidar慢一些

image

1. livox publish數據是按絕對時間來的，不是相對的間隔100ms，會把絕對時間之前的數據都給丟掉，如果這個邏輯存在則永遠無法和pps觸發時機對齊，所以需要屏蔽下這段邏輯，邏輯在Lddc::GetPublishStartTime函數內，跳過過濾邏輯，直接return就好了

image

1. livox 是接收數據放到隊列，然后從隊列里取出數據來publish，按一秒內總數據量除以10，來計算每個包的數據量，所以不能保證每次pps觸發時剛好是publish時間，所以在publish組裝數據時可以加一個判斷，當發現當前數據是pps觸發時機時，則不再組裝當前數據，留給下一次publish時進行組裝，這樣就能保證每次pps觸發時機剛好是publish時間

image

image

1. livox在組裝數據進行publish的時候是按固定的packet數量來組裝，pps觸發時不一定剛好時packet數量，也就是當packet數量滿了就會pub，pps觸發也會pub，如果packet數量滿的時機和pps很近，就會pub兩幀時間戳很近的數據，為了解決這個問題，則需要去除它使用固定的packet數量來組裝的邏輯

1. 由于livox驅動邏輯是先將雷達數據放入隊列，再從隊列里取出數據進行publish，所以接收到lidar數據的時間要比header.stamp要晚一些，從bag包上看會有對不齊的情況，可以通過python腳本將bag包的寫入時間改為header.stamp

import rosbagimport sysif __name__ == '__main__':
    if len(sys.argv) < 3:
        print('args: input.bag output.bag')
        sys.exit()
    input_bag_path = sys.argv[1]
    output_bag_path = sys.argv[2]
    print('input: {} output: {}'.format(input_bag_path,output_bag_path))
    with rosbag.Bag(output_bag_path,'w') as out_bag:
        for topic,msg,t in rosbag.Bag(input_bag_path).read_messages():
            out_bag.write(topic,msg,msg.header.stamp)
    print('finished')

1. 最終時間同步效果：