def write_gyro_accel_to_rosbag(bag, imufiles, timerange=None, buffertime=5, topic="/imu0", shift_secs=0.0): gyro_file = imufiles[0] accel_file = imufiles[1] for filename in imufiles: utility_functions.check_file_exists(filename) time_gyro_array = utility_functions.load_advio_imu_data(gyro_file) time_accel_array = utility_functions.load_advio_imu_data(accel_file) time_imu_array = utility_functions.interpolate_imu_data( time_gyro_array, time_accel_array) bag_imu_count = 0 for row in time_imu_array: timestamp = rospy.Time.from_sec(row[0]) + rospy.Duration.from_sec(shift_secs) imumsg = create_imu_message(timestamp, row[1:4], row[4:7]) timestampinsec = timestamp.to_sec() # check below conditions when video and imu use different clocks # and their lengths differ much if timerange and \ (timestampinsec < timerange[0] - buffertime or timestampinsec > timerange[1] + buffertime): continue bag_imu_count += 1 bag.write(topic, imumsg, timestamp) print('Saved {} out of {} inertial messages to the rosbag'.format( bag_imu_count, time_imu_array.shape[0]))
def main(): args = parseArgs() video_from_to = [args.start_seconds, args.finish_seconds] utility_functions.check_file_exists(args.video) print('Frame time range within the video: {}'.format(video_from_to)) frame_timestamps = np.loadtxt(args.time_file) rostimestamps = [] for row in frame_timestamps: rostimestamps.append(rospy.Time(row[0])) print('Loaded {} timestamps for frames'.format(len(rostimestamps))) first_frame_imu_time = rostimestamps[0].to_sec() bag = rosbag.Bag(args.output_bag, 'a') videotimerange = kalibr_bagcreater.write_video_to_rosbag( bag, args.video, video_from_to, first_frame_imu_time, rostimestamps, frame_remote_timestamps=None, max_video_frame_height=100000, shift_in_time=0.0, topic=args.image_topic, ratio=args.ratio) bag.close() print('Saved to bag file {}'.format(args.output_bag))
def write_imufile_remotetime_to_rosbag(bag, imufile, timerange=None, buffertime=5, topic="/imu0"): """ :param bag: output bag stream. :param imufile: each line: time(sec), ax, ay, az, gx, gy, gz, device-time(sec), and others :param timerange: only write imu data within this local time range to the bag, in seconds. :param buffertime: time to pad the timerange, in seconds. :param topic: imu topic :return: """ utility_functions.check_file_exists(imufile) with open(imufile, 'r') as csvfile: reader = csv.reader(csvfile, delimiter=',') rowcount = 0 imucount = 0 for row in reader: # note a row is a list of strings. if utility_functions.is_header_line(row[0]): continue local_timestamp = rospy.Time.from_sec(float(row[0])) remote_timestamp = rospy.Time.from_sec(float(row[7])) imumsg = create_imu_message(remote_timestamp, row[4:7], row[1:4]) timestampinsec = local_timestamp.to_sec() rowcount += 1 if timerange and \ (timestampinsec < timerange[0] - buffertime or timestampinsec > timerange[1] + buffertime): continue imucount += 1 bag.write(topic, imumsg, local_timestamp) print('Saved {} out of {} inertial messages to the rosbag'.format( imucount, rowcount))
def write_imufile_to_rosbag(bag, imufile, timerange=None, buffertime=5, topic="/imu0"): """ :param bag: output bag stream. :param imufile: each line: time(ns), gx, gy, gz, ax, ay, az :param timerange: only write imu data within this range to the bag, in seconds. :param buffertime: time to pad the timerange, in seconds. :param topic: imu topic :return: """ utility_functions.check_file_exists(imufile) with open(imufile, 'r') as stream: rowcount = 0 imucount = 0 for line in stream: row = re.split(' |,|[|]', line) # note a row is a list of strings. if utility_functions.is_header_line(row[0]): continue imumsg, timestamp = create_imu_message_time_string( row[0], row[1:4], row[4:7]) timestampinsec = timestamp.to_sec() rowcount += 1 if timerange and \ (timestampinsec < timerange[0] - buffertime or timestampinsec > timerange[1] + buffertime): continue imucount += 1 bag.write(topic, imumsg, timestamp) print('Saved {} out of {} inertial messages to the rosbag'.format( imucount, rowcount))
def main(): parsed = parse_args() bag = rosbag.Bag(parsed.output_bag, 'w') # write video utility_functions.check_file_exists(parsed.video_file) if parsed.video_from_to: print('Frame time range within the video: {}'.format( parsed.video_from_to)) frame_local_timestamps = None frame_remote_timestamps = None if parsed.video_time_file: frame_timestamps = kalibr_bagcreater.load_local_and_remote_times( parsed.video_time_file) aligned_local_timestamps = [ time[0] + rospy.Duration.from_sec(parsed.video_file_time_offset) for time in frame_timestamps ] frame_local_timestamps = aligned_local_timestamps frame_remote_timestamps = [time[1] for time in frame_timestamps] print('Loaded {} timestamps for frames'.format(len(frame_timestamps))) first_frame_imu_time = frame_local_timestamps[0].to_sec() videotimerange = kalibr_bagcreater.write_video_to_rosbag( bag, parsed.video_file, parsed.video_from_to, first_frame_imu_time, frame_local_timestamps, frame_remote_timestamps, parsed.max_video_frame_height, shift_in_time=0.0, topic="/cam0/image_raw") print('video time range {}'.format(videotimerange)) # write IMU data that falling into the unix time range kalibr_bagcreater.write_imufile_remotetime_to_rosbag( bag, parsed.imu_file, videotimerange, 5, "/imu0") bag.close() print('Saved to bag file {}'.format(parsed.output_bag))
def main(): if len(sys.argv) < 5: print("Usage: {} <video avi> " "<output bag fullpath> <start seconds> <finish seconds> [frame select ratio, (0-1)]".format(sys.argv[0])) sys.exit(1) ratio = 1.0 if len(sys.argv) > 5: ratio = float(sys.argv[5]) video = sys.argv[1] potential_exts = [".txt", ".csv"] video_time_file = None for ext in potential_exts: video_time_file = video.replace('.avi', ext) if os.path.isfile(video_time_file): break if video_time_file is None: print("Failed to find timestamp file for {}".format(video)) output_bag = sys.argv[2] video_from_to = [float(sys.argv[3]), float(sys.argv[4])] bag = rosbag.Bag(output_bag, 'w') utility_functions.check_file_exists(video) print('Frame time range within the video: {}'.format(video_from_to)) frame_timestamps = kalibr_bagcreater.loadtimestamps(video_time_file) print('Loaded {} timestamps for frames'.format(len(frame_timestamps))) first_frame_imu_time = frame_timestamps[0].to_sec() videotimerange = kalibr_bagcreater.write_video_to_rosbag( bag, video, video_from_to, first_frame_imu_time, frame_timestamps, frame_remote_timestamps=None, max_video_frame_height=100000, shift_in_time=0.0, topic="/cam0/image_raw", ratio=ratio) bag.close() print('Saved to bag file {}'.format(output_bag))
def main(): parsed = parse_args() bag = rosbag.Bag(parsed.output_bag, 'w') videotimerange = None # time range of video frames in IMU clock if parsed.video is not None: utility_functions.check_file_exists(parsed.video) print('given video time offset {}'.format(parsed.video_time_offset)) if parsed.video_from_to: print('video_from_to: {}'.format(parsed.video_from_to)) video_time_offset = parsed.video_time_offset frame_timestamps = list() if parsed.video_time_file: frame_timestamps = loadtimestamps(parsed.video_time_file) aligned_timestamps = [ time + rospy.Duration.from_sec(parsed.video_file_time_offset) for time in frame_timestamps ] frame_timestamps = aligned_timestamps print('Loaded {} timestamps for frames'.format( len(frame_timestamps))) video_time_offset = frame_timestamps[0].to_sec() videotimerange = write_video_to_rosbag( bag, parsed.video, parsed.video_from_to, video_time_offset, frame_timestamps=frame_timestamps, max_video_frame_height=parsed.max_video_frame_height, shift_in_time=parsed.shift_secs) elif parsed.folder is not None: # write images camfolders = get_cam_folders_from_dir(parsed.folder) for camfolder in camfolders: camdir = parsed.folder + "/{0}".format(camfolder) image_files = get_image_files_from_dir(camdir) for image_filename in image_files: image_msg, timestamp = load_image_to_ros_msg(image_filename) bag.write("/{0}/image_raw".format(camfolder), image_msg, timestamp) print("Saved #images {} of {} to bag".format( len(image_files), camfolder)) else: raise Exception('Invalid/Empty video file and image folder') # write imu data if (not parsed.imu) and parsed.folder is None: print("Neither a folder nor any imu file is provided. " "Rosbag will have only visual data") elif not parsed.imu: imufiles = get_imu_csv_files(parsed.folder) for imufile in imufiles: topic = os.path.splitext(os.path.basename(imufile))[0] with open(imufile, 'r') as csvfile: reader = csv.reader(csvfile, delimiter=',') for row in reader: if utility_functions.is_header_line(row[0]): continue imumsg, timestamp = create_imu_message_time_string( row[0], row[1:4], row[4:7]) bag.write("/{0}".format(topic), imumsg, timestamp) elif len(parsed.imu) == 1: imufile = parsed.imu[0] topic = 'imu0' utility_functions.check_file_exists(imufile) with open(imufile, 'r') as csvfile: reader = csv.reader(csvfile, delimiter=',') rowcount = 0 imucount = 0 for row in reader: # note row has many strings. if utility_functions.is_header_line(row[0]): continue imumsg, timestamp = create_imu_message_time_string( row[0], row[1:4], row[4:7]) timestampinsec = timestamp.to_sec() rowcount += 1 # check below conditions when video and imu use different # clocks and their lengths differ much if videotimerange and \ (timestampinsec < videotimerange[0] - MARGIN_TIME or timestampinsec > videotimerange[1] + MARGIN_TIME): continue imucount += 1 bag.write("/{0}".format(topic), imumsg, timestamp) print('Saved {} out of {} inertial messages to the rosbag'.format( imucount, rowcount)) else: gyro_file = parsed.imu[0] accel_file = parsed.imu[1] topic = 'imu0' for filename in parsed.imu: utility_functions.check_file_exists(filename) time_gyro_array = utility_functions.load_advio_imu_data(gyro_file) time_accel_array = utility_functions.load_advio_imu_data(accel_file) time_imu_array = utility_functions.interpolate_imu_data( time_gyro_array, time_accel_array) bag_imu_count = 0 for row in time_imu_array: timestamp = rospy.Time.from_sec(row[0]) + rospy.Duration.from_sec( parsed.shift_secs) imumsg = create_imu_message(timestamp, row[1:4], row[4:7]) timestampinsec = timestamp.to_sec() # check below conditions when video and imu use different clocks # and their lengths differ much if videotimerange and \ (timestampinsec < videotimerange[0] - MARGIN_TIME or timestampinsec > videotimerange[1] + MARGIN_TIME): continue bag_imu_count += 1 bag.write("/{0}".format(topic), imumsg, timestamp) print('Saved {} out of {} inertial messages to the rosbag'.format( bag_imu_count, time_imu_array.shape[0])) bag.close() print('Saved to bag file {}'.format(parsed.output_bag))
def main(): parsed = parse_args() bag = rosbag.Bag(parsed.output_bag, 'w') videotimerange = None # time range of video frames in IMU clock if parsed.video is not None: utility_functions.check_file_exists(parsed.video) print('Given video time offset {}'.format(parsed.first_frame_imu_time)) if parsed.video_from_to: print('Frame time range within the video: {}'.format(parsed.video_from_to)) first_frame_imu_time = parsed.first_frame_imu_time frame_timestamps = list() if parsed.video_time_file: frame_timestamps = loadtimestamps(parsed.video_time_file) aligned_timestamps = [time + rospy.Duration.from_sec(parsed.video_file_time_offset) for time in frame_timestamps] frame_timestamps = aligned_timestamps print('Loaded {} timestamps for frames'.format( len(frame_timestamps))) first_frame_imu_time = frame_timestamps[0].to_sec() videotimerange = write_video_to_rosbag( bag, parsed.video, parsed.video_from_to, first_frame_imu_time, frame_timestamps, frame_remote_timestamps=None, max_video_frame_height=parsed.max_video_frame_height, shift_in_time=parsed.shift_secs, topic="/cam0/image_raw") elif parsed.folder is not None: # write images camfolders = get_cam_folders_from_dir(parsed.folder) for camfolder in camfolders: camdir = parsed.folder + "/{0}".format(camfolder) image_files = get_image_files_from_dir(camdir) for image_filename in image_files: image_msg, timestamp = load_image_to_ros_msg(image_filename) bag.write("/{0}/image_raw".format(camfolder), image_msg, timestamp) print("Saved #images {} of {} to bag".format( len(image_files), camfolder)) else: raise Exception('Invalid/Empty video file and image folder') # write imu data if (not parsed.imu) and parsed.folder is None: print("Neither a folder nor any imu file is provided. " "Rosbag will have only visual data") elif not parsed.imu: imufiles = get_imu_csv_files(parsed.folder) for imufile in imufiles: topic = os.path.splitext(os.path.basename(imufile))[0] with open(imufile, 'r') as csvfile: reader = csv.reader(csvfile, delimiter=',') for row in reader: if utility_functions.is_header_line(row[0]): continue imumsg, timestamp = create_imu_message_time_string( row[0], row[1:4], row[4:7]) bag.write("/{0}".format(topic), imumsg, timestamp) elif len(parsed.imu) == 1: write_imufile_to_rosbag(bag, parsed.imu[0], videotimerange, 5, "/imu0") else: write_gyro_accel_to_rosbag(bag, parsed.imu, videotimerange, 5, "/imu0", parsed.shift_secs) bag.close() print('Saved to bag file {}'.format(parsed.output_bag))