# -*- 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()
版权声明:本文为qq_43511200原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。