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))
