livox+camera 时间硬同步(mid70 + 海康相机)

前言

最近在做激光+视觉的建图，参照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 13

void 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

2. livox当pps触发时，雷达基准时间会归零，所以可以通过这个事件得知pps对应的实际触发时间

3. 在livox驱动里，监听livox原始时间戳，如果当前时间戳小于上一帧时间戳，则代表触发了pps，记录下当前utc时间当作基准时间base_time

image

4. 将base_time信息写入共享内存：/dev/shm/shm_timer，给camera驱动使用

5. 在livox驱动里，将packet的timebase改为共享内存中的基准时间base_time

image

6. 在camera驱动里读取共享内存的基准时间base_time

7. 当base_time变更时，也就是lidar pps触发时，查找接收到的图像里，哪帧距离base_time最近，认为最近的一帧就是和lidar pps触发为同一时机的数据，将时间对齐到base_time上，从数据上看，一般都是base_time变更比camera接收到数据早几十ms左右，所以几乎永远是当前帧最近，也符合逻辑，camera传输比lidar慢一些

image

8. livox publish数据是按绝对时间来的，不是相对的间隔100ms，会把绝对时间之前的数据都给丢掉，如果这个逻辑存在则永远无法和pps触发时机对齐，所以需要屏蔽下这段逻辑，逻辑在Lddc::GetPublishStartTime函数内，跳过过滤逻辑，直接return就好了

image

image

9. livox 是接收数据放到队列，然后从队列里取出数据来publish，按一秒内总数据量除以10，来计算每个包的数据量，所以不能保证每次pps触发时刚好是publish时间，所以在publish组装数据时可以加一个判断，当发现当前数据是pps触发时机时，则不再组装当前数据，留给下一次publish时进行组装，这样就能保证每次pps触发时机刚好是publish时间

image

image

10. livox在组装数据进行publish的时候是按固定的packet数量来组装，pps触发时不一定刚好时packet数量，也就是当packet数量满了就会pub，pps触发也会pub，如果packet数量满的时机和pps很近，就会pub两帧时间戳很近的数据，为了解决这个问题，则需要去除它使用固定的packet数量来组装的逻辑

11. 由于livox驱动逻辑是先将雷达数据放入队列，再从队列里取出数据进行publish，所以接收到lidar数据的时间要比header.stamp要晚一些，从bag包上看会有对不齐的情况，可以通过python脚本将bag包的写入时间改为header.stamp

import rosbag
import sys

if __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")

12. 最终时间同步效果：

image