def load_time_trans_quat(tffile):
"""
load a txt file
:param tffile:
:param line_format: TUM_RGBD format t[s] x y z qx qy qz qw
or KALIBR format t[ns] x y z qx qy qz qw
Because this function does not do normalization,
for better precision, qx qy qz qw should have 9 decimal digits
:return: a list of timed poses, each entry is
[rospy time, tx, ty, tz, qx, qy, qz, qw]
"""
chunk = []
delimiter = None
with open(tffile, "r") as stream:
for _, line in enumerate(stream):
if utility_functions.is_header_line(line):
continue
if delimiter is None:
delimiter = utility_functions.decide_delimiter(line)
if delimiter == " ":
time_pose_str = line.split()
else:
time_pose_str = line.split(delimiter)
secs, nsecs = utility_functions.parse_time(time_pose_str[0], 's')
timestamp = rospy.Time(secs, nsecs)
row = [timestamp] + [float(x) for x in time_pose_str[1:]]
chunk.append(row)
return chunk
def loadtimestamps(video_time_file):
'''Except for the header, each row has the timestamp in sec as its first component'''
frame_rostimes = list()
with open(video_time_file, 'rb') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
next(reader, None) # headers
for row in reader:
secs, nsecs = utility_functions.parse_time(row[0])
frame_rostimes.append(rospy.Time(secs, nsecs))
return frame_rostimes
def test_parse_time():
timestr_list = [
'0.0023', '0.983122', '0.99979', '1.016458', '1.033126',
'0.000000000000000000e+00', '1.665499999999997538e-02',
'1.316759999999999931e+00'
]
for timestr in timestr_list:
sec, nsec = utility_functions.parse_time(timestr, "s")
assert sec + nsec * 1e-9 - float(timestr) < 1e-8
def loadtimestamps(video_time_file):
"""Except for the header, each row has the timestamp in nanosec
as its first component"""
frame_rostimes = list()
with open(video_time_file, 'r') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
for row in reader:
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
frame_rostimes.append(rospy.Time(secs, nsecs))
return frame_rostimes
def createImuMessge(timestamp_str, omega, alpha):
secs, nsecs = utility_functions.parse_time(timestamp_str)
timestamp = rospy.Time(secs, nsecs)
rosimu = Imu()
rosimu.header.stamp = timestamp
rosimu.angular_velocity.x = float(omega[0])
rosimu.angular_velocity.y = float(omega[1])
rosimu.angular_velocity.z = float(omega[2])
rosimu.linear_acceleration.x = float(alpha[0])
rosimu.linear_acceleration.y = float(alpha[1])
rosimu.linear_acceleration.z = float(alpha[2])
return rosimu, timestamp
def loadtimestamps(time_file):
"""Except for the header, each row has the timestamp in nanosec
as its first component"""
frame_rostimes = list()
# https://stackoverflow.com/questions/41847146/multiple-delimiters-in-single-csv-file
with open(time_file, 'r') as stream:
for line in stream:
row = re.split(' |,|[|]', line)
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
frame_rostimes.append(rospy.Time(secs, nsecs))
return frame_rostimes
def load_local_and_remote_times(time_file):
"""
:param time_file: Except for the header lines, each line has the first column as the remote time in ns,
and the last column as the local time in secs.
:return: list of tuples (local time, remote time)
"""
frame_rostimes = list()
with open(time_file, 'r') as csvfile:
reader = csv.reader(csvfile, delimiter=',')
for row in reader:
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
remote_time = rospy.Time(secs, nsecs)
local_time = rospy.Time.from_sec(float(row[-1]))
frame_rostimes.append((local_time, remote_time))
return frame_rostimes
def load_local_and_remote_times(time_file):
"""
:param time_file: Except for the header lines, each line has the first column as the remote time in ns,
and the last column as the local time in secs.
:return: list of tuples (local time, remote time)
"""
frame_rostimes = list()
with open(time_file, 'r') as stream:
for line in stream:
row = re.split(' |,|[|]', line)
if utility_functions.is_header_line(row[0]):
continue
secs, nsecs = utility_functions.parse_time(row[0], 'ns')
remote_time = rospy.Time(secs, nsecs)
local_time = rospy.Time.from_sec(float(row[-1]))
frame_rostimes.append((local_time, remote_time))
return frame_rostimes
def create_imu_message_time_string(timestamp_str, omega, alpha):
secs, nsecs = utility_functions.parse_time(timestamp_str, 'ns')
timestamp = rospy.Time(secs, nsecs)
return create_imu_message(timestamp, omega, alpha), timestamp
def convert_pose_format(infile,
outfile,
in_time_unit=None,
in_quat_order="xyzw",
output_format="KALIBR",
output_delimiter=","):
# check infile line format
with open(infile, "r") as stream:
lines = []
max_lines = 2
read_next = True
line = None
while read_next:
line = stream.readline().rstrip('\n')
read_next = utility_functions.is_header_line(line)
if not read_next and len(lines) < max_lines:
lines.append(line)
read_next = True
in_delimiter = utility_functions.decide_delimiter(lines[-1])
time_index, time_unit, r_index = \
utility_functions.decide_time_index_and_unit(lines, in_delimiter)
if in_time_unit is not None:
time_unit = in_time_unit
print(
"The determined time index {} time unit {} and translation start index {}"
.format(time_index, time_unit, r_index))
q_index = r_index + 3
# set default outfile
if not outfile:
in_dir = os.path.dirname(infile)
in_base = os.path.basename(infile)
in_name, in_ext = os.path.splitext(in_base)
outfile = os.path.join(in_dir, in_name + "_canon" + in_ext)
# load infile and write outfile
with open(outfile, "w") as ostream:
with open(infile, "r") as istream:
for line in istream:
if utility_functions.is_header_line(line):
continue
rags = line.split(in_delimiter)
rags = [rag.strip() for rag in rags]
secs, nanos = utility_functions.parse_time(rags[time_index])
if output_format == "KALIBR":
if secs > 0:
ostream.write("{}{:09d}".format(secs, nanos))
else: # secs == 0
ostream.write("{}".format(nanos))
elif output_format == "TUM_RGBD":
if secs > 0:
ostream.write("{}.{:09d}".format(secs, nanos))
else: # secs == 0
ostream.write("{}".format(
float(nanos) / utility_functions.SECOND_TO_NANOS))
else:
raise ValueError(
"Unsupported output format {}!".format(output_format))
for j in range(3):
ostream.write("{}{}".format(output_delimiter,
rags[r_index + j]))
quat = utility_functions.normalize_quat_str(
rags[q_index:q_index + 4])
if in_quat_order == "xyzw":
for j in range(4):
ostream.write("{}{}".format(output_delimiter, quat[j]))
ostream.write("\n")
elif in_quat_order == "wxyz":
for j in range(3):
ostream.write("{}{}".format(output_delimiter,
quat[j + 1]))
ostream.write("{}{}\n".format(output_delimiter, quat[0]))
else:
raise ValueError(
"Unsupported input quaternion order {}!".format(
in_quat_order))
print("Successfully converted {}\ninto pose file: {}".format(
infile, outfile))
