我们在使用ros过程中,经常要对某个或多个topic进行下采样后重发,用于ros数据采集。
非同步下采样重发
rosrun topic_tools throttle messages /rgb_n_f/img/compressed 5 /rgb_n_f/img_throttle/compressed
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
官方说最多支持9个传感器的数据同步,我实测5路图像,4路激光可行。
代码参考:
jsk_tools