解决rosbag时间戳未对齐的解决方法

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")