多传感器软同步的方法——ROS下实现

问题背景

现在有三种不同的传感器：
（1）双目相机，输出其左右目的RGB图，已知相机内部做过硬同步，频率已被我调至10帧
（2）深度相机，输出其RGB图和深度图，没有硬同步，频率为30帧
（3）激光雷达，输出其点云，频率为10帧

问题：用软同步的方法数据存储为离线文件

ROS基础

（1）使用while循环，并设置循环频率为30Hz
（2）ROS中subscriber在后台起线程接收数据至队列中，但回调函数位于主线程中
（3）磁盘的读写速度不够快，需要起多线程进行读写，注意：对读/写所用的容器在读/写时需要上mutex锁
（4）实验中务必保证传感器接触良好，且电路正常工作

最基础要求

只要所有的消息在一个100ms内接收到，就算做同步
实现方案一：ROS的时间同步机制
实现方案二：回调函数分开写，但取每个100ms内的最新消息作为同步
两种方案下回调函数都直接将数据写入读/写容器
缺点：无视了硬同步

更高的要求

考虑硬同步，并希望将深度相机中相距较近的两个RGB和深度图视作一体

实现方案：
维护另一个中间容器，回调函数将数据写入中间容器。
先用两个时间间隔较短的ROS时间同步机制，再在每轮循环中添加判定1：若没有接受到所有消息，则继续;若接受到所有消息，则将各中间容器中的消息pop一个至各读/写容器，并清空中间容器。判定2：若中间容器的size()超过阈值，则clear()，原因是始终有消息没有收到。
实际中又有一个问题：激光雷达实际频率为9.999Hz，会产生漂移，久而久之与双目相机的“同步”信号会产生累计时间差，如何解决？答：将ROS的激光雷达和双目相机subscriber队列长度都置为1,让系统在每个循环中帮我们解决这个问题

缺点：不是最优解

所谓同步