def csv2bag(ground_truth_file,odometry_file,laser_file,output_folder): output_folder = sys.argv[4] # infer name from ground truth dataset_name = sys.argv[1].split('/')[-1] savename = join(output_folder,dataset_name.split('.')[0])+'.bag' print savename # we need to read these 3 data elements together # idea: we build a dictionary were keys are timestamps dataset = [] dataset_items = {'ground_truth': sys.argv[1],'odometry': sys.argv[2], 'laser': sys.argv[3]} for key,value in dataset_items.iteritems(): dataset_item = open(value) # skip the csv header first = True for line in dataset_item.readlines(): if first: first = False else: line = line.strip() tokens = line.split(',') timestamp = float(tokens[0]) tokens = [elem.strip() for elem in tokens] data = (key, ','.join(tokens[1:])) dataset.append((timestamp, data)) dataset_item.close() # probably useful to sort here dataset = sorted(dataset, key=lambda x: x[0]) bag = rosbag.Bag(savename, 'w') iteration = 0 odom_x, odom_y, odom_theta = 0,0,0 for timestamp, data in dataset: # we need to decide how to interpret this data if True:#iteration%20==0: if data[0] == 'ground_truth': line = data[1].strip() tokens = line.split(',') msg = Odometry() msg.header.frame_id = 'odom' t = rospy.Time(timestamp) msg.header.stamp = t msg.pose.pose.position.x = float(tokens[0]) msg.pose.pose.position.y = float(tokens[1]) msg.pose.pose.position.z = 0.0 odom_quat = tf.transformations.quaternion_from_euler(0, 0, float(tokens[2])) msg.pose.pose.orientation = Quaternion(*odom_quat) msg.child_frame_id = "base_link" # I have to figure out if this data is available msg.twist.twist.linear.x = 0.0 msg.twist.twist.linear.y = 0.0 msg.twist.twist.angular.z = 0.0 bag.write('base_pose_ground_truth', msg, t) elif data[0] == 'odometry': line = data[1].strip() tokens = line.split(',') msg = Odometry() msg.header.frame_id = 'odom' t = rospy.Time(timestamp) msg.header.stamp = t msg.pose.pose.position.x = float(tokens[3]) msg.pose.pose.position.y = float(tokens[4]) msg.pose.pose.position.z = 0.0 odom_quat = tf.transformations.quaternion_from_euler(0, 0, float(tokens[5])) msg.pose.pose.orientation = Quaternion(*odom_quat) msg.child_frame_id = "base_link" # I have to figure out if this data is available msg.twist.twist.linear.x = 0.0 msg.twist.twist.linear.y = 0.0 msg.twist.twist.angular.z = 0.0 bag.write('odom', msg, t) odom_trans = TransformStamped() odom_trans.header.stamp = rospy.Time(timestamp) odom_trans.header.frame_id = "/odom" odom_trans.child_frame_id = "/base_link" odom_trans.transform.translation.x = float(tokens[3]) odom_trans.transform.translation.y = float(tokens[4]) odom_trans.transform.translation.z = 0.0 odom_trans.transform.rotation = Quaternion(*odom_quat) tf_msg = TFMessage() tf_msg.transforms.append(odom_trans) bag.write('tf', tf_msg, t) odom_x, odom_y, odom_theta = [float(t) for t in tokens[3:6]] t = rospy.Time(timestamp) tf_msg = make_tf_msg(0.08, 0.0, 0.0, t) bag.write('tf', tf_msg, t) elif data[0] == 'laser': line = data[1].strip() tokens = line.split(',') msg = LaserScan() num_scans = len(tokens)-2 msg.header.frame_id = 'base_laser_link' t = rospy.Time(timestamp) msg.header.stamp = t msg.angle_min = -90.0 / 180.0 * pi msg.angle_max = 90.0 / 180.0 * pi msg.angle_increment = pi / num_scans msg.time_increment = 0.2 / 360.0 msg.scan_time = 0.2 msg.range_min = 0.001 msg.range_max = 50.0 msg.ranges = [float(r) for r in tokens[2:(num_scans + 1)]] bag.write('base_scan', msg, t) # experimental: reuse immediately preceding odometry data as pose #t = rospy.Time(timestamp) #tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t) #bag.write('tf', tf_msg, t) iteration+=1