rosbag的使用：同步不同bag包不同话题的时间戳

# -*- coding:UTF-8 -*-
import rosbag
import rospy
import math

def merge():
    bag1 = rosbag.Bag('/rosbag_merge/2022-09-08-07-28-10.bag','r') #包源1
    bag2 = rosbag.Bag('/rosbag_merge/2022-09-16-16-38-57.bag','r') #包源2
    # bagn = rosbag.Bag('xxx/n.bag','r') #可推广到包源包n
    bag_merge = rosbag.Bag('/rosbag_merge/merge.bag','w')  #新包merge.bag
    
    #包源2 操作： 时间戳长度 + 保存时间戳
    gps_tmp = []                                # 定义GPS时间戳列表
    for topic, msg, t in bag2:                  # 注意topic、msg、t顺序，从包中解析依次是话题、该话题消息内容、该话题的时间戳，推荐使用所示顺序
        gps_tmp.append(t)                       # 将GPS的时间戳添加到列表
    print('old gps data length: ',len(gps_tmp)) # 打印老的GPS的时间戳长度

    imu_tmp = []                                # 定义IMU时间戳列表
    i = 0
    gpu_time_idx = 0                            # 定义GPS时间戳索引    如果GPS起止时间早于IMU n秒时，gpu_time_idx=-n，若晚于n秒 gpu_time_idx=+n
    new_gps_tmp = []                            # 定义新的GPS时间戳
    
    #包源1 操作： 将IMU每秒的第一数据的时间戳拎出来，作为后面新的GPS时间戳 存在这个new_gps_tmp[]列表里面
    for topic, msg, t in bag1:      
        if topic == '/livox/imu_hap':
            print('imu data load: ', i)
            tmp = t
            tmp = float(str(tmp)) / 1e+9
            if i == 0:
                tmp0 = tmp
            if (int(tmp)-int(tmp0)) >= 1 :
                gpu_time_idx += 1
            if len(new_gps_tmp) == gpu_time_idx:
                    new_gps_tmp.append(tmp0)

            # print('a',new_gps_tmp[0])
            # d = rospy.Duration.from_sec(new_gps_tmp[0]) # a minute and change
            # zs,xs=str(1662593290.1156225252).split('.')

            # print(type(msg.header.stamp.secs))
            # seconds = d.to_sec() #floating point
            # nanoseconds = d.to_nsec()
            # x,y= repr(new_gps_tmp[i1]).split('.')
            # print(int(x),int(y))
            # # print(str(rospy.Time.from_sec(new_gps_tmp[0])))

            tmp0 = tmp
            i += 1
    print('imu sec data length: ',len(new_gps_tmp))  # 获取imu数据中整数秒个数 

    if len(new_gps_tmp) >= len(gps_tmp):             # 去除imu多余秒数数据
        new_gps_tmp = new_gps_tmp[:len(gps_tmp)]

    for i1,(topic, msg, t) in enumerate(bag2):    
        tmp = t
        tmp = float(str(tmp)) / 1e+9

        print('================================')
        print(i1)
        print('old gps time: ',tmp)
        print('new gps time: ',new_gps_tmp[i1])
        print('================================')

        new_t = rospy.Time.from_sec(new_gps_tmp[i1])
        x,y= repr(new_gps_tmp[i1]).split('.')
        msg.header.stamp.secs = int(x)
        msg.header.stamp.nsecs = int(y)
        bag_merge.write(topic, msg, new_t)

    bag1.close()
    bag2.close()
    bag_merge.close()   #注意，所有打开过的包，都得关闭，否则下次访问可能会失败

if __name__== "__main__":
    merge()