深入浅出ros2时间同步

多传感器时间同步中提到，
ROS2提供了message_filters来进行时间同步，message_filters类似一个消息缓存，分别订阅不同的需要融合的传感器的topic，当消息到达message_filters的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出，message_filters并不会修改各个topic传过来的数据（也就是缓冲区中的数据），而是将各个缓冲区建立时间线，然后根据时间线上的各个时间点来判断是否需要输出。

理论部分

ros2时间同步有两种策略，ExactTime Policy和ApproximateTime Policy，前者需要严格的时间匹配，而ApproximateTime Policy则提供了时间窗口的匹配逻辑。WIKI上描述如下：

令 S 为最后发布的集合，T 为下一个正在创建的集合。其工作原理如下

1. 发布集合S
   当一个集合S被发布时，对于每一个topic，时间戳小于 m a x t { S } max_{t}\{S\} maxt​{ S}的数据都将被丢弃
2. 更新消息队列
   每一个topic生成一个指定长度的queue, 新的消息按照到达的顺序插入到各自topic的队列中。
3. 寻找pivot消息
   一旦每个topic的队列中至少有一个消息，就会找到队首消息中最新的那个，称为pivot消息。pivot消息是创建集合T的基石，时间匹配机制都依赖于pivot消息，记pivot消息对应的时间戳为$t_{pivot}$
4. 构建集合Tm
   对于所有topic中每比$t_{pivot}$要大的消息m，找到一个最小的集合Tm，按时间顺序检查消息m，如果下一个消息还不能确定，就等待直到队列达到指定大小。
5. 发布最小集合Tm
   一旦到达pivot消息，就意味着所有的Tm都已经被枚举出来，或者当可以使用各种界限证明已经找到了最小的Tm时，就发布最小的Tm。

代码部分

说实话，WIKI的描述有点云里雾里，不结合代码的话很难理解，Talk is cheap, show me the code

ApproximateTimeSynchronizer集成自TimeSynchronizer和SimpleFilter
SimpleFilter <- TimeSynchronizer <- ApproximateTimeSynchronizer

初始化

通过TimeSynchronizer.connectInput初始化队列，每个ros topic初始化一个队列，注册add回调函数, 添加到input_connections列表。队列实际上是字典的形式。消息的毫秒时间戳作为key，消息体为value，消息先进先出

def __init__(self, fs, queue_size):
    SimpleFilter.__init__(self)
    self.connectInput(fs)
    self.queue_size = queue_size
    self.lock = threading.Lock()

def connectInput(self, fs