赞
踩
# -*- 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()
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。