import rosbag from std_msgs.msg import String bag = rosbag.Bag("objective.bag", "w") # QmevVxGMRbLgv6grkQxYYSy4NzYnkpojm48QD7ag15uaok - ipfs hash of log file bag.write("/log", String("QmevVxGMRbLgv6grkQxYYSy4NzYnkpojm48QD7ag15uaok")) bag.close()
def convert(self, inputfile, outputfile): #TODO : deal with exceptions try: inCompleteName = os.path.expanduser(inputfile) self.f = open(inCompleteName, "r") except (IOError, ValueError): rospy.logerr("Couldn't open %", inputfile) self.print_help() exit(-1) try: outCompleteName = os.path.expanduser(outputfile) self.bag = rosbag.Bag(outCompleteName, "w") except (IOError, ValueError): rospy.logerr("Couldn't open %", outputfile) self.print_help() exit(-1) rospy.loginfo("Reading data from : %s", inCompleteName) rospy.loginfo("Writing rosbag to : %s", outCompleteName) rospy.loginfo("Start convertion to rosbag") self.counter = 0 for line in self.f: words = line.split() if self.pause == True: while self.pause == True: rospy.sleep(1 / self.rate) if self.LASER_MESSAGE_DEFINED.count(words[0]): self.fillUpLaserMessage(words) topic = self.topics[words[0]] self.bag.write(topic, self.laser_msg, self.laser_msg.header.stamp) self.laser_msg.header.seq = self.laser_msg.header.seq + 1 elif self.OLD_LASER_MESSAGE_DEFINED.count(words[0]): self.fillUpOldLaserMessage(words) topic = self.topics[words[0]] if not self.laser_msg.header.stamp.secs == 0: self.bag.write(topic, self.laser_msg, self.laser_msg.header.stamp) if self.publish_corrected: topic = self.topics["TF"] self.bag.write(topic, self.tf2_msg, self.laser_msg.header.stamp) topic = self.topics["ODOM"] self.bag.write(topic, self.pose_msg, self.pose_msg.header.stamp) self.laser_msg.header.seq = self.laser_msg.header.seq + 1 self.pose_msg.header.seq = self.pose_msg.header.seq + 1 self.tf_odom_robot_msg.header.seq = self.tf_odom_robot_msg.header.seq + 1 elif self.ROBOT_LASER_MESSAGE_DEFINED.count(words[0]): self.fillUpRobotLaserMessage(words) topic = self.topics[words[0]] self.bag.write(topic, self.laser_msg, self.laser_msg.header.stamp) if self.publish_corrected: topic = self.topics["TF"] self.bag.write(topic, self.tf2_msg, self.laser_msg.header.stamp) topic = self.topics["ODOM"] self.bag.write(topic, self.pose_msg, self.pose_msg.header.stamp) self.pose_msg.header.seq = self.pose_msg.header.seq + 1 self.tf_laser_robot_msg.header.seq = self.tf_laser_robot_msg.header.seq + 1 self.tf_odom_robot_msg.header.seq = self.tf_odom_robot_msg.header.seq + 1 self.laser_msg.header.seq = self.laser_msg.header.seq + 1 elif self.ODOM_DEFINED == words[0]: self.fillUpOdomMessage(words) if not self.pose_msg.header.stamp.secs == 0 and not self.publish_corrected: topic = self.topics["ODOM"] self.bag.write(topic, self.pose_msg, self.pose_msg.header.stamp) topic = self.topics["TF"] self.bag.write(topic, self.tf2_msg, self.pose_msg.header.stamp) self.pose_msg.header.seq = self.pose_msg.header.seq + 1 self.tf_odom_robot_msg.header.seq = self.tf_odom_robot_msg.header.seq + 1 elif self.TRUEPOS_DEFINED == words[0]: self.fillUpTruePoseMessage(words) topic = self.topics[words[0]] self.bag.write(topic, self.true_pose_msg, self.true_pose_msg.header.stamp) topic = self.topics["TF"] self.bag.write(topic, self.tf2_msg, self.tf_msg.header.stamp) self.pose_msg.header.seq = self.true_pose_msg.header.seq + 1 elif self.PARAM_DEFINED == words[0]: self.param(words) else: if not self.unknown_entries.count(words[0]): self.unknown_entries.append(words[0]) if not words[0] == "#": rospy.logerr("Unknown entry %s !", words[0]) self.increment_stamp() self.tf2_msg = TFMessage()
def saveTopicVideo(bag_path, topic_name, start_time, end_time, msg_num, frame_width, frame_height, frame_rate, frame_encoding, save_path): if frame_encoding.__contains__("rgb8"): target_encoding = "bgr8" else: target_encoding = frame_encoding fourcc = cv2.VideoWriter_fourcc(*'XVID') out_video = cv2.VideoWriter(save_path, fourcc, frame_rate, (frame_width, frame_height)) counter = 0 if frame_encoding == "mono8": with rosbag.Bag(bag_path, 'r') as bag: bridge = CvBridge() for topic, msg, t in bag.read_messages(): cur_time = msg.header.stamp.to_sec() if start_time <= cur_time <= end_time: if topic == topic_name: try: cv_img = bridge.imgmsg_to_cv2(msg, target_encoding) out_video.write( cv2.cvtColor(cv_img, cv2.COLOR_GRAY2BGR)) timestr = "%.6f" % msg.header.stamp.to_sec() counter += 1 print( str(counter) + os.path.sep + str(msg_num) + ", " + timestr + os.path.sep + str(end_time)) except CvBridgeError as e: print(e) elif frame_encoding == "rgb8": with rosbag.Bag(bag_path, 'r') as bag: bridge = CvBridge() for topic, msg, t in bag.read_messages(): cur_time = msg.header.stamp.to_sec() if start_time <= cur_time <= end_time: if topic == topic_name: try: cv_img = bridge.imgmsg_to_cv2(msg, target_encoding) out_video.write(cv_img) timestr = "%.6f" % msg.header.stamp.to_sec() counter += 1 print( str(counter) + os.path.sep + str(msg_num) + ", " + timestr + os.path.sep + str(end_time)) except CvBridgeError as e: print(e) elif frame_encoding == "bgr8": with rosbag.Bag(bag_path, 'r') as bag: bridge = CvBridge() for topic, msg, t in bag.read_messages(): cur_time = msg.header.stamp.to_sec() if start_time <= cur_time <= end_time: if topic == topic_name: try: cv_img = bridge.imgmsg_to_cv2(msg, target_encoding) out_video.write( cv2.cvtColor(cv_img, cv2.COLOR_BGR2RGB)) timestr = "%.6f" % msg.header.stamp.to_sec() counter += 1 print( str(counter) + os.path.sep + str(msg_num) + ", " + timestr + os.path.sep + str(end_time)) except CvBridgeError as e: print(e) else: print("Usupported data format:" + frame_encoding) out_video.release() print("Video file has been saved at:" + save_path)
def makePlot(test, save): XSqrPts = [0, 14, 14, 0, 0, 14, 14, 0, 0, 17, 17, -1, -2] YSqrPts = [0, 0, 4, 4, 8, 8, 12, 12, 16, 16, -2, -2, -3] dataSet = test data = {'scircle_1_zero_1_fast': {'xoffset': 0,'yoffset':0,'scale':4.111672661,'file':"zeroDeg/scircle_1_zero_1_fast.bag"}, #scaling factor averaged #did not work. has a small straight line going in the wrong direction 'scircle_1_zero_2_fast': {'xoffset': 0,'yoffset':0,'scale':4.111672661,'file':"zeroDeg/scircle_1_zero_2_fast.bag"}, #scaling factor averaged #did not work. has a small straight line going in the wrong direction 'scircle_1_zero_3': {'xoffset': 0,'yoffset':0,'scale':4.111672661,'file':"zeroDeg/scircle_1_zero_3.bag"}, #scaling factor averaged #did not work. has a weird line that does a uturn 'scircle_3_zero_1': {'xoffset': 0.07,'yoffset':7,'scale':-5.329780147,'file':"zeroDeg/scircle_3_zero_1.bag"}, #worked farily well. only slight drift. 'scircle_3_zero_2': {'xoffset': 0.1,'yoffset':7.136,'scale':-5.294506949,'file':"zeroDeg/scircle_3_zero_2.bag"}, #worked farily well. only slight drift. 'scircle_3_zero_3': {'xoffset': 0,'yoffset':0,'scale':-5.312143548,'file':"zeroDeg/scircle_3_zero_3.bag"}, #scaling factor averaged #worked well until it completely failed. 'scorn_1_zero_1': {'xoffset': (14-12.65),'yoffset':0,'scale':-4.101963082,'file':"zeroDeg/scorn_1_zero_1.bag"}, 'scorn_1_zero_2': {'xoffset': (14 - 13.8388),'yoffset':(-0.0331184),'scale':-4.097161253,'file':"zeroDeg/scorn_1_zero_2.bag"}, 'scorn_1_zero_3': {'xoffset': (14 - 13.5169),'yoffset':(-0.023974),'scale':-4.135893648,'file':"zeroDeg/scorn_1_zero_3.bag"}, 'scorn_3_zero_1': {'xoffset': (14 - 11.6039),'yoffset':(-0.0548116),'scale':-6.021505376,'file':"zeroDeg/scorn_3_zero_1.bag"}, 'scorn_3_zero_2': {'xoffset': (14 - 13.613),'yoffset':(-0.0566209),'scale':-6.113537118,'file':"zeroDeg/scorn_3_zero_2.bag"}, 'scorn_3_zero_3': {'xoffset': (14 - 13.171),'yoffset':(-0.0503621),'scale':-6.024096386,'file':"zeroDeg/scorn_3_zero_3.bag"}, 'scircle_1_20deg_1': {'xoffset': 0,'yoffset':0,'scale':1,'file':"20Deg/scircle_1_20deg_1.bag"}, #did not complete and no scaling data 'scircle_1_20deg_2': {'xoffset': 0,'yoffset':0,'scale':1,'file':"20Deg/scircle_1_20deg_2.bag"}, #did not complete and no scaling data 'scircle_1_20deg_3': {'xoffset': 0,'yoffset':0,'scale':1,'file':"20Deg/scircle_1_20deg_3.bag"}, #did not complete and no scaling data 'scircle_3_20deg_1': {'xoffset': 0,'yoffset':7.4,'scale':-5.721212468,'file':"20Deg/scircle_3_20deg_1.bag"}, #worked pretty well 'scircle_3_20deg_2': {'xoffset': 0,'yoffset':7.4,'scale':-5.739210285,'file':"20Deg/scircle_3_20deg_2.bag"}, #worked pretty well 'scircle_3_20deg_3': {'xoffset': 0,'yoffset':7.45,'scale':-5.730211376,'file':"20Deg/scircle_3_20deg_3.bag"}, #scaling factor averaged #failed part wat through but looks desent before that 'scorn_1_20deg_1': {'xoffset': 0,'yoffset':0,'scale':1,'file':"20Deg/scorn_1_20deg_1.bag"}, 'scorn_1_20deg_2': {'xoffset': 0,'yoffset':0,'scale':1,'file':"20Deg/scorn_1_20deg_2.bag"}, 'scorn_1_20deg_3': {'xoffset': 0,'yoffset':0,'scale':1,'file':"20Deg/scorn_1_20deg_3.bag"}, 'scorn_3_20deg_1': {'xoffset': (14 - 13.56023),'yoffset':(-0.00795469),'scale':-6.636352755,'file':"20Deg/scorn_3_20deg_1.bag"}, 'scorn_3_20deg_2': {'xoffset': (14 - 13.9493),'yoffset':(-0.0753021),'scale':-6.675518818,'file':"20Deg/scorn_3_20deg_2.bag"}, 'scorn_3_20deg_3': {'xoffset': (14 - 13.103),'yoffset':(-0.028339),'scale':-6.706766744,'file':"20Deg/scorn_3_20deg_3.bag"}, } px = [] py = [] pz = [] posex = [] posey = [] posez = [] bag = rosbag.Bag(data[dataSet]['file']) for topic, msg, t in bag.read_messages(topics=['/svo/pose']): #print msg posex.append(msg.pose.pose.position.x) posey.append(msg.pose.pose.position.y) posez.append(msg.pose.pose.position.z) # for topic, msg, t in bag.read_messages(topics=['/svo/points']): # #print msg # px.append(msg.pose.position.x) # py.append(msg.pose.position.y) # pz.append(msg.pose.position.z) fig3 = plt.figure() ax = fig3.add_subplot(111, projection='3d') ax.scatter(posex, posey, posez, c='r', marker='o') plt.xlabel('x') plt.ylabel('y') fig4 = plt.figure() plt.plot( [x * abs(data[dataSet]['scale']) for x in posey], [x * abs(data[dataSet]['scale']) for x in posez], 'ro') offLineX = [3*2.747, -3*2.747] #x offLineY = [-3, 3] #y plt.plot(offLineX, offLineY, 'b') plt.xlabel('Inches') plt.ylabel('Inches') plt.axis('equal') if dataSet[-7:-2] == "20deg": print "adjusting angle" poseym = [x/cos(0.34906585) for x in posey] posey = poseym posezm = [a + b*sin(0.34906585) for a, b in zip(posez, posey)] posez = posezm #ax.scatter(posex, posey, c='b', marker='o') fig1 = plt.figure() plt.plot( [x * data[dataSet]['scale'] + data[dataSet]['xoffset'] for x in posex], [x * data[dataSet]['scale'] + data[dataSet]['yoffset'] for x in posey], 'ro') if dataSet[:3] == "sco" and data[dataSet]['scale'] != 1: plt.plot(XSqrPts, YSqrPts, 'b') if dataSet[:3] == "sci" and data[dataSet]['scale'] != 1: x = linspace(0,2*pi, 200) plt.plot(8*cos(x)+8, 8*sin(x)+8,'b') offLineX = [-1, 0] offLineY = [-1, 8] plt.plot(offLineX, offLineY, 'b') plt.title(dataSet) plt.xlabel('Inches') plt.ylabel('Inches') #plot( 3*cos(an), 3*sin(an) ) plt.axis('equal') fig2 = plt.figure() plt.plot( [x * abs(data[dataSet]['scale']) for x in posez], 'ro') plt.ylabel('Inches') #plt.zlabel('z') if save: fig1.savefig('Graphs/' + dataSet + '_xy.png', format="png") fig1.savefig('Graphs/' + dataSet + '_xy.eps', format="eps") fig2.savefig('Graphs/' + dataSet + '_z.png', format="png") fig2.savefig('Graphs/' + dataSet + '_z.eps', format="eps") print "saved" plt.show() bag.close()
def plotBag(bag_file, teach, failed): global repeat_counter, fig, ax quiver_counter = 0 x_ = [] y_ = [] yaw_ = [] avg_x = 0. avg_y = 0. avg_yaw = 0. temp_x = [] temp_y = [] temp_yaw = [] bag = rosbag.Bag(bag_file) for topic, msg, t in bag.read_messages( topics=['/vicon/bebop_vtr/bebop_vtr']): quaternion = (msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w) euler = tf.transformations.euler_from_quaternion(quaternion) temp_x.append(msg.transform.translation.x) temp_y.append(msg.transform.translation.y) yaw = euler[2] yaw = yaw - .4 # vicon offset if (len(temp_yaw) < 3 or abs(sum(temp_yaw) / float(len(temp_yaw)) - yaw) < 0.5): temp_yaw.append(yaw) if (quiver_counter < 1 and len(temp_x) > 10) or (quiver_counter >= 1 and len(temp_x) > 150): avg_x = sum(temp_x) / float(len(temp_x)) avg_y = sum(temp_y) / float(len(temp_y)) avg_yaw = sum(temp_yaw) / float(len(temp_yaw)) if (quiver_counter < 1): r = 0.4 u = r * cos(avg_yaw) v = r * sin(avg_yaw) # ax.quiver(avg_x, avg_y, u, v, angles='xy', scale_units='xy', scale=1.2) ax.quiver(avg_y, avg_x, v, u, angles='xy', scale_units='xy', scale=1.2) quiver_counter = quiver_counter + 1 else: x_.append(avg_x) y_.append(avg_y) yaw_.append(avg_yaw) # x_.append(avg_x) # y_.append(avg_y) # yaw_.append(avg_yaw) temp_x = [] temp_y = [] temp_yaw = [] bag.close() if teach == True: # plt.plot(x_, y_, label="Teaching Trail", linewidth=7.0) plt.plot(y_, x_, label="Teaching Trail", linewidth=7.0) else: # repeat_counter = repeat_counter + 1 if failed == True: plt.plot(y_, x_, linewidth=3.0) else: plt.plot(y_, x_, linewidth=1.0)
#!/bin/python import rosbag import sys, os import rospy import numpy as np import matplotlib.pyplot as plt bag = rosbag.Bag(sys.argv[1]) plot = True speed = 5.0 pre_time = 0.5 time = 1.0e5 for topic, msg, t in bag.read_messages(topics='/command_pose'): start_time = msg.header.stamp break d1x = [] d2x = [] d3x = [] d1y = [] d2y = [] d3y = [] d1z = [] d2z = [] d3z = [] t0 = [] x0 = [] y0 = [] z0 = []
def run(args): import sys from evo.core import metrics from evo.tools import file_interface, settings # manually check bins and tols arguments to allow them to be in config files if not args.bins or not args.tols: logging.error( "the following arguments are required: -b/--bins, -t/--tols") sys.exit(1) settings.configure_logging(args.verbose, args.silent, args.debug) if args.debug: import pprint logging.debug( "main_parser config:\n" + pprint.pformat({arg: getattr(args, arg) for arg in vars(args)}) + "\n") logging.debug(SEP) pose_relation = None if args.pose_relation == "trans_part": pose_relation = metrics.PoseRelation.translation_part elif args.pose_relation == "angle_deg": pose_relation = metrics.PoseRelation.rotation_angle_deg elif args.pose_relation == "angle_rad": pose_relation = metrics.PoseRelation.rotation_angle_rad traj_ref, traj_est, stamps_est = None, None, None ref_name, est_name = "", "" if args.subcommand == "tum": traj_ref, traj_est = file_interface.load_assoc_tum_trajectories( args.ref_file, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.ref_file, args.est_file elif args.subcommand == "kitti": traj_ref = file_interface.read_kitti_poses_file(args.ref_file) traj_est = file_interface.read_kitti_poses_file(args.est_file) ref_name, est_name = args.ref_file, args.est_file elif args.subcommand == "euroc": args.align = True logging.info( "forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)" ) logging.debug(SEP) traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories( args.state_gt_csv, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.state_gt_csv, args.est_file elif args.subcommand == "bag": import rosbag logging.debug("opening bag file " + args.bag) bag = rosbag.Bag(args.bag, 'r') try: traj_ref, traj_est = file_interface.load_assoc_bag_trajectories( bag, args.ref_topic, args.est_topic, args.t_max_diff, args.t_offset, ) finally: bag.close() ref_name, est_name = args.ref_topic, args.est_topic main_rpe_for_each(traj_ref, traj_est, pose_relation, args.mode, args.bins, args.tols, args.align, args.correct_scale, ref_name, est_name, args.plot, args.save_plot, args.save_results, args.no_warnings, serialize_plot=args.serialize_plot)
import argparse import rosbag parser = argparse.ArgumentParser(description='Extract mp3 file from audio topic in ros bag.') parser.add_argument('bagfile', help='ros bag file to use as input') parser.add_argument('outfile', help='destination of output') args = parser.parse_args() bag = rosbag.Bag(args.bagfile) output = open(args.outfile, 'w') for topic, msg, t in bag.read_messages(topics=['/audio/audio']): output.write(''.join(msg.data)) bag.close() output.close()
bag_file = '/home/mayank_sati/Documents/2011_09_30_drive_18.bag' # info_dict = yaml.load(Bag(bag_file, 'r')._get_yaml_info()) # # # Print the information contained in the info_dict # info_dict.keys() # for topic in info_dict["topics"]: # print("-"*50) # for k,v in topic.items(): # print(k.ljust(20), v) import rosbag import mayavi.mlab import sensor_msgs.point_cloud2 as pc2 bag = rosbag.Bag(bag_file) for topic, msg, t in bag.read_messages(topics=['velodyne_points']): # print (msg) # msgString = str(msg) # msgList = string.split(msgString, '\n') lidar = np.fromstring(msg.data, dtype=np.float32) lidar = lidar.reshape(-1, 4) a = pc2.read_points(msg) lidar = pc2.read_points(msg) lidar = np.array(list(lidar)) # import mayavi.mlab fig = mayavi.mlab.figure(bgcolor=(0, 0, 0), size=(800, 500)) mayavi.mlab.points3d(
def preprocess(path='./', new_thymio=False, distances=None): """Extracts data to bagfiles in a given path and converts them into dataframes and saves them into an HDF5 binary file. Args: path: the path in which bag files are. new_thymio: a string containing the number of the thymio for new thymios or False for the old version thymio. """ if new_thymio: cam = '/thymio' + new_thymio + '/camera/image_raw/compressed' prox = '/thymio' + new_thymio + '/proximity/laser' odom = '/thymio' + new_thymio + '/odom' else: cam = '/camera/image_rect_color/compressed' prox = '/proximity/laser' odom = '/odom' if distances is None: distances = [0.0, 3.3, 6.6, 10.0] extractors = { cam: lambda m: { 'camera': jpeg2np(m.data, (80, 64)) }, prox: lambda m: { 'prox_lx': m.intensities[4], 'prox_cl': m.intensities[3], 'prox_cx': m.intensities[2], 'prox_cr': m.intensities[1], 'prox_rx': m.intensities[0] }, odom: lambda m: { 'pos_x': m.pose.pose.position.x, 'pos_y': m.pose.pose.position.y, 'theta': quaternion2yaw(m.pose.pose.orientation) } } files = [file[:-4] for file in os.listdir(path) if file[-4:] == '.bag'] if not files: print('No bag files found!') return None h5f = h5py.File('data/' + str(datetime.now()) + '.h5', 'w') for index, file in enumerate(files): print('Found ' + path + file + '.bag') dfs = bag2dfs(rosbag.Bag(path + file + '.bag'), extractors) df = mergedfs(dfs) df = next_prox(df, distances) l = len(df) df.fillna(-1.0, inplace=True) df['camera'] = df['camera'].apply(lambda x: (x - x.mean()) / x.std()) Xs = h5f.create_dataset('bag' + str(index) + '_x', shape=(l, 64, 80, 3), maxshape=(None, 64, 80, 3), dtype=np.float, data=None, chunks=True) Ys = h5f.create_dataset('bag' + str(index) + '_y', shape=(l, 5 * len(distances)), maxshape=(None, 5 * len(distances)), dtype=np.float, data=None, chunks=True) cols = [ 't_%.1f_prox_%s' % (d, sensor) for d in distances for sensor in ['lx', 'cl', 'cx', 'cr', 'rx'] ] Xs[:] = np.stack(df['camera'].values) Ys[:] = df[cols].values h5f.close()
clusters[obj_count + 1] = [] for g in group: predicted_obj_id[g] = obj_count + 1 clusters[obj_count + 1].append(g) obj_count += 1 t = time.time() - t comp_time.append(t) print('Scan #%3d: cur:%4d/%3d agg:%5d/%3d acc %.2f time %.3f' % (count_msg, len(update_list), len(set(obj_id_list[len(obj_id_list) - len(update_list):])), len(point_id_map), len(set(obj_id_list)), acc, t)) count_msg += 1 bag = rosbag.Bag('data/%s_%s.bag' % (dataset, VAL_AREA), 'r') poses = [] for topic, msg, t in bag.read_messages(topics=['slam_out_pose']): poses.append([msg.pose.position.x, msg.pose.position.y]) i = 0 for topic, msg, t in bag.read_messages(topics=['laser_cloud_surround']): process_cloud(msg, poses[i]) i += 1 # if i==1: # break #ignore clutter in evaluation valid_mask = numpy.array(obj_id_list) > 0 gt_obj_id = numpy.array(obj_id_list)[valid_mask] predicted_obj_id = numpy.array(predicted_obj_id)[valid_mask] gt_cls_id = numpy.array(cls_id_list)[valid_mask]
class singleFrame: def __init__(self, lmsg, rmsg, vmsg, frameN): self.left = cvb.imgmsg_to_cv2(lmsg) self.right = cvb.imgmsg_to_cv2(rmsg) self.viso = copy.deepcopy(vmsg) self.nNumber = frameN def getHomography(self): return deserialHomography(self.viso.homography) inDir = sys.argv[1] print("Reading from " + inDir) inputBag = rosbag.Bag(inDir) leftImages = [] rightImages = [] outputData = [] print("extracting Topic Data") for topic, msg, t in inputBag.read_messages( topics=['/viso_extractor/output' ]): #,'/bumblebee/left/ROI','/bumblebee/right/ROI']): if (topic == "/viso_extractor/output"): outputData.append(msg) print(len(outputData)) # if(topic=="/bumblebee/left/ROI"): # leftImages.append(msg)
flip = False # Drone topics and laps used to dynamically change what topics the image writes to in rosbag # i.e. the first half ring of drone_1 is "drone_1_lap_1" in the rosbag, second half ring is "drone_1_lap_2" etc # simplifies the extraction process drone_topic = 'drone_' drone_lap = 1 drone_1_topic = 'drone_1' drone_2_topic = 'drone_2' drone_1_lap = 1 drone_2_lap = 1 drone_image = Image() drone_image_1 = Image() drone_image_2 = Image() bag = rosbag.Bag('image_bag', 'w') # Callbacks to set the drone's camera feed to a temp "drone_image" variable for one or double drones # in order to write to rosbag def image_callback(Image): global drone_image drone_image = Image def image1_callback(Image): global drone_image_1 drone_image_1 = Image
import rosbag from lfd import bag_proc bag = rosbag.Bag("/home/joschu/Data/bags/test_rope.bag") bag_proc.get_button_presses(bag)
if __name__ == '__main__': rospy.init_node('sensor_model', anonymous=True) # get map rospy.wait_for_service('static_map') static_map = rospy.ServiceProxy('/static_map', GetMap) rospy.logerr("Done waiting for map service.") map_resp = static_map() map_data_shaped = np.array(map_resp.map.data).reshape( (map_resp.map.info.height, map_resp.map.info.width)) map_data_shaped = map_data_shaped >= 0 # read laser scan from bag file bag_path = rospy.get_param("laser_scan_bag") scan_msg = list(rosbag.Bag(bag_path).read_messages())[0][1] # generate and plot heatmap heatmap = generate_heatmap(map_resp, scan_msg) plot_heatmap(map_data_shaped, heatmap) # plot discretization vs runtime #for theta in [3,5,7,10,15,20,50]: # HEATMAP_NUM_THETA = theta #for step in [200, 100, 50, 20, 10, 5, 3, 1]: # HEATMAP_XY_STEP = step # start = time.time() # heatmap = generate_heatmap(map_resp, scan_msg) # plot_heatmap(map_data_shaped, heatmap) # end = time.time()
return time def get_next_point(data, ptCnt): if ptCnt >= data.width: point3fi = struct.pack("4f", 0,0,0,0) else: point3fi = data.data[ptCnt*SIZE_OF_POINT+0:ptCnt*SIZE_OF_POINT+4] point3fi += data.data[ptCnt*SIZE_OF_POINT+4:ptCnt*SIZE_OF_POINT+8] point3fi += data.data[ptCnt*SIZE_OF_POINT+8:ptCnt*SIZE_OF_POINT+12] point3fi += data.data[ptCnt*SIZE_OF_POINT+16:ptCnt*SIZE_OF_POINT+20] ptCnt += 1 return point3fi, ptCnt # main outf = open(DSV_PATH, "wb") bag = rosbag.Bag(ROS_BAG_PATH) for topic, msg, ts in bag.read_messages(topics = [NAV_TOPIC, VELO_TOPIC]): # nav/Odometry messages if topic == NAV_TOPIC: navMsg = msg # sensor_msgs/PointCloud2 message if topic == VELO_TOPIC: gx, gy, gz, roll, pitch, yaw = parse_nav_msg(navMsg) millisec = int(get_millisec(ts)) sNav = struct.pack("6dq", roll, pitch, yaw, gx, gy, gz, millisec) print(millisec, roll, pitch, yaw) # parse velodyne points ptCnt = 0 for i in range(BLOCK_NUM_PER_FRAME / VELO_HZ_RATIO): outf.write(sNav)
sys.exit(2) info_dict = yaml.load( subprocess.Popen(['rosbag', 'info', '--yaml', infilename], stdout=subprocess.PIPE).communicate()[0]) msg_count = 0 for topic in info_dict["topics"]: msg_count += topic["messages"] print '\nSpeeding up bag file ' + infilename + ' consisting of ' + str( msg_count) + ' messages by a factor of ' + str(rate) gotFirstTimestamp = False msg_index = 0 outbag = rosbag.Bag(outfilename, 'w') try: for topic, msg, timestamp in rosbag.Bag(infilename).read_messages(): if not gotFirstTimestamp: firstTimestamp = timestamp gotFirstTimestamp = True # Calculate corrected timestamp correctedTimestamp = firstTimestamp + (timestamp - firstTimestamp) / rate # Fix header timestamps if msg._has_header: msg.header.stamp = correctedTimestamp # Fix TF transforms
break elif entry == 'n' or entry == 'N': sys.exit() print "\nFormatting: .rosbag -> .aedat (This should take a couple of minutes)\n" # open the file and write the headers file = open(aedatFile, "w") file.write('#!AER-DAT2.0\r\n') file.write('# This is a raw AE data file created by saveaerdat.m\r\n') file.write( '# Data format is int32 address, int32 timestamp (8 bytes total), repeated for each event\r\n' ) file.write('# Timestamps tick is 1 us\r\n') # open the rosbag file and process the events bag = rosbag.Bag(bagFile) for topic, msg, t in bag.read_messages(topics=['/dvs/events']): for e in msg.events: ts = int(e.ts.to_nsec() / 1000.0) x = '{0:07b}'.format(e.x) y = '{0:07b}'.format(e.y) p = '1' if e.polarity else '0' address = "0" + y + x + p # write the event using big endian format file.write("%s" % struct.pack('>I', int(address, 2))) file.write("%s" % struct.pack('>I', int(ts))) bag.close()
mingap = max(mingap, -mingap) for j in positions: gap = j.header.stamp.nsecs - nsec gap = max(gap, -gap) if gap < mingap: mingap = gap argmin = j img = bridge.imgmsg_to_cv2(msg, "bgr8") return [img, argmin.position, argmin.velocity, argmin.effort, nsec] if __name__ == "__main__": imageName = sys.argv[1] jointName = sys.argv[2] saveName = sys.argv[3] imageBag = rosbag.Bag(imageName) jointBag = rosbag.Bag(jointName) bridge = CvBridge() cur = 0 joints = [] data = [] for topic, msg, t in jointBag.read_messages( topics=['/robot/joint_states']): sectime = msg.header.stamp.secs if sectime > cur: cur = sectime joints.append([msg]) else: joints[-1].append(msg) firstT = joints[0][0].header.stamp.secs for topic, msg, t in imageBag.read_messages(topics=['image_raw']):
def bag_tf_poses(timed_pos_quat, bagpath, child_frame_id="vins", red=1.0, green=1.0, blue=0.0): """ :param timed_pos_quat: :param bagpath: :return: """ if os.path.isfile(bagpath): bag = rosbag.Bag(bagpath, "a") else: bag = rosbag.Bag(bagpath, "w") path = Path() for index, entry in enumerate(timed_pos_quat): odometry = Odometry() odometry.header.stamp = entry[0] odometry.header.seq = index odometry.header.frame_id = "world" odometry.child_frame_id = child_frame_id odometry.pose.pose.position.x = entry[1] odometry.pose.pose.position.y = entry[2] odometry.pose.pose.position.z = entry[3] odometry.pose.pose.orientation.x = entry[4] odometry.pose.pose.orientation.y = entry[5] odometry.pose.pose.orientation.z = entry[6] odometry.pose.pose.orientation.w = entry[7] odometry.twist.twist.linear.x = 0 odometry.twist.twist.linear.y = 0 odometry.twist.twist.linear.z = 0 pose_stamped = PoseStamped() pose_stamped.header.stamp = entry[0] pose_stamped.header.seq = index pose_stamped.header.frame_id = "world" pose_stamped.pose = odometry.pose.pose path.header.stamp = entry[0] path.header.seq = index path.header.frame_id = "world" path.poses.append(pose_stamped) if len(path.poses) > 2: path.poses.pop(0) marker = rviz_camera_frustum.generate_frustum_marker( entry[1:], 1.0, entry[0], red, green, blue) marker.header.frame_id = "world" marker.header.stamp = entry[0] marker.header.seq = index # see https://wiki.ros.org/rviz/DisplayTypes/Marker#Triangle_List_.28TRIANGLE_LIST.3D11.29_.5B1.1.2B-.5D for drawing complex markers bag.write("/path_{}".format(child_frame_id), path, entry[0]) bag.write("/odom_{}".format(child_frame_id), marker, entry[0]) bag.close()
try: width = sys.argv[sys.argv.index('-w') + 1] height = sys.argv[sys.argv.index('-h') + 1] output_size = [int(width), int(height)] except ValueError: print 'No output file specified' output_size = (64, 48) # Save the last read image for use as Y0 when command is read Y_last = 0 # Keep track of the actual zoom for the last command and the current zoom zoom_last = 250 zoom = 100 out_bag = rosbag.Bag(outfile, 'w') # Define state integers for image capturing states state_takeY0 = 0 # Waiting for first image state_wait_for_move = 1 # Wait for camera to start moving state_cam_moving = 2 # State for when camera is moving state_takeY1 = 3 # Waiting for last image next_state = -1 state_capture = 0 diff_treshold = 0.001 move_waits = 0 move_timeout = 9 move_time_timeout = 0.5 #time0 = rospy.get_time()
def create_plots( bagfile, output_pdf_file, output_csv_file="results.csv", groundtruth_bagfile=None, rmse_rel_start_time=0, rmse_rel_end_time=-1, ): bag = rosbag.Bag(bagfile) groundtruth_bag = rosbag.Bag( groundtruth_bagfile) if groundtruth_bagfile else bag bag_start_time = bag.get_start_time() has_imu_augmented_graph_localization_state = has_topic(bag, "/gnc/ekf") has_imu_bias_tester_poses = has_topic(bag, "/imu_bias_tester/pose") sparse_mapping_poses = poses.Poses("Sparse Mapping", "/sparse_mapping/pose") ar_tag_poses = poses.Poses("AR Tag", "/ar_tag/pose") imu_bias_tester_poses = poses.Poses("Imu Bias Tester", "/imu_bias_tester/pose") vec_of_poses = [ar_tag_poses, imu_bias_tester_poses] load_pose_msgs(vec_of_poses, bag, bag_start_time) has_depth_odom = has_topic(bag, "/loc/depth/odom") depth_odom_relative_poses = poses.Poses("Depth Odom", "/loc/depth/odom") load_odometry_msgs([depth_odom_relative_poses], bag, bag_start_time) groundtruth_vec_of_poses = [sparse_mapping_poses] load_pose_msgs(groundtruth_vec_of_poses, groundtruth_bag, bag_start_time) graph_localization_states = loc_states.LocStates("Graph Localization", "/graph_loc/state") imu_augmented_graph_localization_states = loc_states.LocStates( "Imu Augmented Graph Localization", "/gnc/ekf") vec_of_loc_states = [ graph_localization_states, imu_augmented_graph_localization_states, ] load_loc_state_msgs(vec_of_loc_states, bag, bag_start_time) imu_bias_tester_velocities = velocities.Velocities( "Imu Bias Tester", "/imu_bias_tester/velocity") load_velocity_msgs(imu_bias_tester_velocities, bag, bag_start_time) bag.close() with PdfPages(output_pdf_file) as pdf: add_graph_plots( pdf, sparse_mapping_poses, ar_tag_poses, graph_localization_states, imu_augmented_graph_localization_states, ) if has_imu_bias_tester_poses: add_imu_bias_tester_poses(pdf, imu_bias_tester_poses, sparse_mapping_poses) add_imu_bias_tester_velocities(pdf, imu_bias_tester_velocities) if has_imu_augmented_graph_localization_state: add_other_loc_plots( pdf, graph_localization_states, imu_augmented_graph_localization_states, sparse_mapping_poses, ar_tag_poses, ) else: add_other_loc_plots(pdf, graph_localization_states, graph_localization_states) if has_depth_odom: depth_odom_poses = utilities.make_absolute_poses_from_relative_poses( sparse_mapping_poses, depth_odom_relative_poses, "Depth Odometry") plot_poses(pdf, depth_odom_poses, sparse_mapping_poses, ar_tag_poses) # Note that for absolute time difference tolerance depth images and groudtruth use different sensor data # and therefore have less similar timestamps. This timestamp difference reduces the accuracy of depth odometry # groundtruth comparison. plot_loc_state_stats( pdf, depth_odom_poses, sparse_mapping_poses, output_csv_file, "depth_odometry_", 0.01, False, rmse_rel_start_time=rmse_rel_start_time, rmse_rel_end_time=rmse_rel_end_time, ) plot_covariances( pdf, depth_odom_relative_poses.times, depth_odom_relative_poses.covariances.position, "Depth Odometry Position", ) plot_covariances( pdf, depth_odom_relative_poses.times, depth_odom_relative_poses.covariances.orientation, "Depth Odometry Orientation", ) plot_loc_state_stats( pdf, graph_localization_states, sparse_mapping_poses, output_csv_file, rmse_rel_start_time=rmse_rel_start_time, rmse_rel_end_time=rmse_rel_end_time, ) plot_loc_state_stats( pdf, imu_augmented_graph_localization_states, sparse_mapping_poses, output_csv_file, "imu_augmented_", 0.01, rmse_rel_start_time=rmse_rel_start_time, rmse_rel_end_time=rmse_rel_end_time, ) if has_imu_bias_tester_poses: plot_loc_state_stats( pdf, imu_bias_tester_poses, sparse_mapping_poses, output_csv_file, "imu_bias_tester_", 0.01, False, rmse_rel_start_time=rmse_rel_start_time, rmse_rel_end_time=rmse_rel_end_time, )
# armLog.EEF_pos.y = pos[1] # armLog.EEF_pos.z = pos[2] # armLog.f_ext = right_arm_ctrl.force_measured # bag.write('right_arm_log',armLog) pos, ori = left_arm_ctrl.get_pose_arm() armLog.robot_error_pos = sqrt((np.transpose(left_arm_ctrl.pos_error) * left_arm_ctrl.pos_error)[0, 0]) armLog.robot_error_orient = sqrt( (np.transpose(left_arm_ctrl.orient_error) * left_arm_ctrl.orient_error)[0, 0]) armLog.EEF_pos.x = pos[0] armLog.EEF_pos.y = pos[1] armLog.EEF_pos.z = pos[2] armLog.f_ext = left_arm_ctrl.force_measured bag.write('left_arm_log', armLog) rate.sleep() if __name__ == '__main__': try: time_now = time.strftime("%H:%M:%S") date = time.strftime("%d-%m") bag = rosbag.Bag( 'Kinematic_Control_Bag_' + date + '_' + time_now + '.bag', 'w') main(bag) except rospy.ROSInterruptException: pass finally: bag.close()
def bag_to_dataframe(bag_name, include=None, exclude=None, parse_header=False, seconds=False): ''' Read in a rosbag file and create a pandas data frame that is indexed by the time the message was recorded in the bag. :bag_name: String name for the bag file :include: None, String, or List Topics to include in the dataframe if None all topics added, if string it is used as regular expression, if list that list is used. :exclude: None, String, or List Topics to be removed from those added using the include option using set difference. If None no topics removed. If String it is treated as a regular expression. A list removes those in the list. :seconds: time index is in seconds :returns: a pandas dataframe object ''' # get list of topics to parse yaml_info = get_bag_info(bag_name) bag_topics = get_topics(yaml_info) bag_topics = prune_topics(bag_topics, include, exclude) length = get_length(bag_topics, yaml_info) msgs_to_read, msg_type = get_msg_info(yaml_info, bag_topics, parse_header) bag = rosbag.Bag(bag_name) dmap = create_data_map(msgs_to_read) # create datastore datastore = {} for topic in dmap.keys(): for f, key in dmap[topic].iteritems(): t = msg_type[topic][f] if isinstance(t, int) or isinstance(t, float): arr = np.empty(length) arr.fill(np.NAN) elif isinstance(t, list): arr = np.empty(length) arr.fill(np.NAN) for i in range(len(t)): key_i = '{0}{1}'.format(key, i) datastore[key_i] = arr.copy() continue else: arr = np.empty(length, dtype=np.object) datastore[key] = arr # create the index index = np.empty(length) index.fill(np.NAN) # all of the data is loaded for idx, (topic, msg, mt) in enumerate(bag.read_messages(topics=bag_topics)): try: if seconds: index[idx] = msg.header.stamp.to_sec() else: index[idx] = msg.header.stamp.to_nsec() except: if seconds: index[idx] = mt.to_sec() else: index[idx] = mt.to_nsec() fields = dmap[topic] for f, key in fields.iteritems(): try: d = get_message_data(msg, f) if isinstance(d, tuple): for i, val in enumerate(d): key_i = '{0}{1}'.format(key, i) datastore[key_i][idx] = val else: datastore[key][idx] = d except: pass bag.close() # convert the index if not seconds: index = pd.to_datetime(index, unit='ns') # now we have read all of the messages its time to assemble the dataframe return pd.DataFrame(data=datastore, index=index)
if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("--bag-name", default="") parser.add_argument("-n", "--topic-list", nargs="+", default=default_topics) args = parser.parse_args() # Create topic dictionary freq_analyzer = Topic.initDictFromTopics(args.topic_list) # Analyse resource loads cpu_analyzer = CpuUsage() mem_analyzer = MemUsage() # Go through the messages in the bag file for topic_bag, msg, t in rosbag.Bag(args.bag_name).read_messages(): # Add message to cpu analyser cpu_analyzer.addCpuMessage(topic_bag, msg) # Add message to mem analyser mem_analyzer.addMemMessage(topic_bag, msg) # Check if the topic matches for topic in args.topic_list: if topic_bag == topic: # Calculate hz freq_analyzer[topic].addTimestamp(msg.header.stamp.to_sec()) # Plot the result cpu_analyzer.printResult() mem_analyzer.printResult() Topic.plotResults(freq_analyzer)
#!/usr/bin/env python2 """ Print total cumulative serialized msg size per topic in a ROS Bag file """ import rosbag import sys # Calculate size of messages in bag file topic_size_dict = {} for topic, msg, time in rosbag.Bag(sys.argv[1], 'r').read_messages(raw=True): topic_size_dict[topic] = topic_size_dict.get(topic, 0) + len(msg[1]) topic_size = list(topic_size_dict.items()) # Print results from smallest to largest topic_size.sort(key=lambda x: x[1]) print("TOPIC\t\t\tSIZE") for topic, size in topic_size: # For more human readable results, determine scale of each value (Kilo, Mega, Giga, Tera) order = 0 while size > 1000: size /= 1000 order += 1 order_str = ['', 'K', 'M', 'G', 'T'][order] print("%s %d%s" % (topic, size, order_str))
# j+=1 # # # print 'should be working' # # velocities.header.seq = data[i,2] # # velocities.vector.x = data[i,3] # # velocities.vector.y = data[i,4] # # velocities.vector.z = data[i,5] # # outbag.write('/velocities', velocities, velocities.header.stamp) # # tf_stat = TFMessage() # tf_stat = tf_data # for num in range(0,11): # tf_stat.transforms[num].header.stamp.secs=data[0,0] # tf_stat.transforms[num].header.stamp.nsecs=data[0,1] # print data[0,0] # print data[0,1] # # print tf_stat # outbag.write('/tf_static', tf_stat, tf_stat.transforms[0].header.stamp) if __name__ == '__main__': outbag = rosbag.Bag('/home/jacob/6_5_t1_new.bag', 'w') inbag = '/home/jacob/6_5_t1.bag' # refbag = '/home/jacob/bags/bag_test.bag' # [tf_data, cinfo_data, dinfo_data] = load_ref_data(refbag) # [data,data2] = load_data(inbag) # bagwriter(inbag, outbag, data, data2, tf_data, cinfo_data, dinfo_data) bagwriter(inbag, outbag) print "done" outbag.close()
# Now append the reconstructed images to the copied rosbag stamps = np.loadtxt( join(reconstructed_images_folder, 'timestamps.txt')) if len(stamps.shape) == 2: stamps = stamps[:, 1] # list all images in the folder images = [ f for f in glob.glob(join(reconstructed_images_folder, "*.png")) ] images = sorted(images) print('Found {} images'.format(len(images))) with rosbag.Bag(input_bag_filename, 'a') as outbag: for i, image_path in enumerate(images): stamp = stamps[i] img = cv2.imread( join(reconstructed_images_folder, image_path), 0) try: img_msg = bridge.cv2_to_imgmsg(img, encoding='mono8') stamp_ros = rospy.Time(stamp) print(img.shape, stamp_ros) img_msg.header.stamp = stamp_ros img_msg.header.seq = i outbag.write(args.image_topic, img_msg, img_msg.header.stamp)
def __init__(self, bagname, joints, individual_waypoints_recorded, seconds_for_each_waypoint, frequency_to_downsample, ignore_last_num_seconds=0.0, js_topic='/joint_states'): """Load gesture from the bag name given and remove jerkiness by downsampling""" # get bag info self.info_bag = yaml.load( subprocess.Popen(['rosbag', 'info', '--yaml', bagname], stdout=subprocess.PIPE).communicate()[0]) # Fill up traj with real trajectory points traj = [] # Also fill up downsampled traj downsampled_traj = [] bag = rosbag.Bag(bagname) first_point = True num_msgs = 0 num_downsampled_data_points = 0 for topic, msg, t in bag.read_messages(topics=[js_topic]): rospy.logdebug("Processing new message from rosbag") num_msgs += 1 # Process interesting joints here names, positions = self.getNamesAndMsgList(joints, msg) if individual_waypoints_recorded: # Take all waypoints num_downsampled_data_points += 1 downsampled_traj.append(positions) else: # Downsample the number of points if recorded using continuous mode if num_msgs % frequency_to_downsample == 0: num_downsampled_data_points += 1 downsampled_traj.append(positions) # Append interesting joints here traj.append(positions) if first_point: # Store first point self.gesture_x0 = positions first_point = False bag.close() # Store last point self.gesture_goal = positions print str(len(traj)) + " points in example traj" print "Downsampled traj has: " + str(len(downsampled_traj)) + " points" rospy.loginfo("Joints:" + str(joints)) rospy.loginfo("Initial pose:" + str(self.gesture_x0)) rospy.loginfo("Final pose: " + str(self.gesture_goal)) self.motion_joints = joints self.motion_traj = downsampled_traj if individual_waypoints_recorded: self.motion_duration = num_downsampled_data_points * seconds_for_each_waypoint else: self.motion_duration = self.info_bag['duration'] rospy.loginfo("Motion duration: " + str(self.motion_duration))
def run(args): from evo.core import metrics from evo.tools import file_interface, log log.configure_logging(args.verbose, args.silent, args.debug) if args.debug: import pprint logger.debug( "main_parser config:\n" + pprint.pformat({arg: getattr(args, arg) for arg in vars(args)}) + "\n") logger.debug(SEP) pose_relation = None if args.pose_relation == "full": pose_relation = metrics.PoseRelation.full_transformation elif args.pose_relation == "rot_part": pose_relation = metrics.PoseRelation.rotation_part elif args.pose_relation == "trans_part": pose_relation = metrics.PoseRelation.translation_part elif args.pose_relation == "angle_deg": pose_relation = metrics.PoseRelation.rotation_angle_deg elif args.pose_relation == "angle_rad": pose_relation = metrics.PoseRelation.rotation_angle_rad delta_unit = None if args.delta_unit == "f": delta_unit = metrics.Unit.frames elif args.delta_unit == "d": delta_unit = metrics.Unit.degrees elif args.delta_unit == "r": delta_unit = metrics.Unit.radians elif args.delta_unit == "m": delta_unit = metrics.Unit.meters traj_ref, traj_est, stamps_est = None, None, None ref_name, est_name = "", "" plot_mode = None # no plot imports unless really needed (slow) if args.subcommand == "tum": traj_ref, traj_est = file_interface.load_assoc_tum_trajectories( args.ref_file, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.ref_file, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "kitti": traj_ref = file_interface.read_kitti_poses_file(args.ref_file) traj_est = file_interface.read_kitti_poses_file(args.est_file) ref_name, est_name = args.ref_file, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "euroc": args.align = True logger.info( "forcing trajectory alignment implicitly (EuRoC ground truth is in IMU frame)" ) logger.debug(SEP) traj_ref, traj_est = file_interface.load_assoc_euroc_trajectories( args.state_gt_csv, args.est_file, args.t_max_diff, args.t_offset, ) ref_name, est_name = args.state_gt_csv, args.est_file if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xyz if not args.plot_mode else PlotMode[ args.plot_mode] elif args.subcommand == "bag": import rosbag logger.debug("opening bag file " + args.bag) bag = rosbag.Bag(args.bag, 'r') try: traj_ref, traj_est = file_interface.load_assoc_bag_trajectories( bag, args.ref_topic, args.est_topic, args.t_max_diff, args.t_offset, ) finally: bag.close() ref_name, est_name = args.ref_topic, args.est_topic if args.plot or args.save_plot: from evo.tools.plot import PlotMode plot_mode = PlotMode.xy if not args.plot_mode else PlotMode[ args.plot_mode] main_rpe(traj_ref, traj_est, pose_relation, args.delta, delta_unit, args.delta_tol, args.all_pairs, args.align, args.correct_scale, ref_name, est_name, args.plot, args.save_plot, plot_mode, args.save_results, args.no_warnings, serialize_plot=args.serialize_plot)