ros 多传感器同步及topic下采样重发

我们在使用ros过程中，经常要对某个或多个topic进行下采样后重发，用于ros数据采集。

非同步下采样重发

rosrun topic_tools throttle messages /rgb_n_f/img/compressed 5 /rgb_n_f/img_throttle/compressed

参考：topic_tools throttle

throttle工具能实现单个topic的制定频率的重发，但是对于多个传感器的下采样就无能为力了，只能运行多个throttle实例导致多传感器同步问题

同步下采样重发

基于上述多传感器下采样不同步的问题，我们自然想到同步接收多个传感器的数据，然后下采样后逐个转发。需要用到的关键代码为：

message_filters.ApproximateTimeSynchronizer

具体代码如下

#!/usr/bin/env python

import rospy
import message_filters
from rostopic import get_topic_class

global syn_cnt
syn_cnt=0

def callback(*msgs):
    global syn_cnt
    global down_rate
    if syn_cnt%down_rate==0:
        stamp = None
        for msg, pub in zip(msgs, pubs):
            if stamp is None:
                stamp = msg.header.stamp
            else:
                msg.header.stamp = stamp
            pub.publish(msg)
    if syn_cnt>down_rate*1000:
        syn_cnt=0
    else:
        syn_cnt+=1

if __name__ == '__main__':
    rospy.init_node('synchrnoze_republish')
    topics = rospy.get_param('/repub_my_topics')
    use_async = rospy.get_param('~approximate_sync', True)
    queue_size = rospy.get_param('~queue_size', 100)
    slop = rospy.get_param('~slop', 0.5)
    global down_rate
    down_rate = rospy.get_param('/repub_down_rate', 2)

    pubs = []
    subs = []
    for i, topic in enumerate(topics):
        topic = rospy.resolve_name(topic)
        msg_class = get_topic_class(topic, blocking=True)[0]
        # pub_topic_name='~pub_{0:0>2}/compressed'.format(i)
        if 'img/compressed' in topic:
            pub_topic_name=topic.replace('img/compressed','img_throttle/compressed')
        else:
            pub_topic_name=topic+'/repub'
        pub = rospy.Publisher(
            pub_topic_name, msg_class, queue_size=1)
        pubs.append(pub)
        sub = message_filters.Subscriber(topic, msg_class)
        subs.append(sub)
    if use_async:
        sync = message_filters.ApproximateTimeSynchronizer(
            subs, queue_size=queue_size, slop=slop)
    else:
        sync = message_filters.TimeSynchronizer(subs, queue_size=queue_size)
    sync.registerCallback(callback)
    rospy.spin()

首先通过命令行设置全局参数：

rosparam set repub_my_topics \[/rgb_n_f/img_throttle/compressed,/rgb_w_f/img_throttle/compressed,/rgb_w_l/img_throttle/compressed,/rgb_w_r/img_throttle/compressed,/rgb_w_b/img_throttle/compressed,/rslidar_points,/rslidar_points_2,/livox/lidar_5,/livox/lidar_6\]
rosparam set repub_down_rate 4
python synchronize_republish.py

官方说最多支持９个传感器的数据同步，我实测５路图像，４路激光可行。

代码参考：
jsk_tools