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