rosbag在存储时间时,接收消息的时间和生成消息的时间可能会有很大的差别。例如:程序处理速度慢导致的消息延后; 消息在慢速播放时被记录;Synchronizer的msg传入过晚…
因此,使用每个topic内的header时间作为播放时的时间戳(rosbag record时间使用的是当前的电脑时间)不失为一种比较奈斯的方法:
#!/usr/bin/env python3
import rospy
import rosbag
import sys
if sys.getdefaultencoding() != 'utf-8':
reload(sys)
sys.setdefaultencoding('utf-8')
bag_name = '2020-08-12-12-48-42.bag' #被修改的bag名
out_bag_name = 'out_2020-08-12-12-48-42.bag' #修改后的bag名
dst_dir = '/home/yinji/sda/bag_2020/' #使用路径
with rosbag.Bag(dst_dir+out_bag_name, 'w') as outbag:
stamp = None
#topic:就是发布的topic msg:该topic在当前时间点下的message t:消息记录时间(非header)
##read_messages内可以指定的某个topic
for topic, msg, t in rosbag.Bag(dst_dir+bag_name).read_messages():
if topic == '/gps':
stamp = msg.header.stamp
#针对没有header的文件,使用上面帧数最高的topic(即:/gps)的时间戳
##因为read_messages是逐时间读取,所以该方法可以使用
elif topic == '/image_stamp' and stamp is not None:
outbag.write(topic, msg, stamp)
continue
#针对格式为Header的topic
elif topic == '/image_time':
outbag.write(topic, msg, msg.stamp)
continue
#针对一般topic
outbag.write(topic, msg, msg.header.stamp)
print("finished")