def test_large_blocks_with_time_overflow(self): with ExitStack() as at_exit: with patch('osgar.logger.datetime.datetime'): osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1)) with osgar.logger.LogWriter( prefix='tmpA', note='test_time_overflow with large blocks') as log: at_exit.callback(os.remove, log.filename) t1 = log.write(1, b'\x01' * 100000) self.assertEqual(t1, timedelta(0)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1, 1)) t2 = log.write(1, b'\x02' * 100000) self.assertEqual(t2, timedelta(hours=1)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1, 2)) t3 = log.write(1, b'\x03' * 100000) self.assertEqual(t3, timedelta(hours=2)) with LogIndexedReader(log.filename) as log: dt, channel, data = log[1] self.assertEqual(dt, timedelta(hours=0)) dt, channel, data = log[2] self.assertEqual(dt, timedelta(hours=1)) dt, channel, data = log[3] self.assertEqual(dt, timedelta(hours=2))
def test_time_overflow(self): with ExitStack() as at_exit: with patch('osgar.logger.datetime.datetime'): osgar.logger.datetime.datetime = TimeStandsStill( datetime(2020, 1, 21)) with osgar.logger.LogWriter(prefix='tmp9', note='test_time_overflow') as log: at_exit.callback(os.remove, log.filename) t1 = log.write(1, b'\x01') self.assertEqual(t1, timedelta(0)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2020, 1, 21, 1)) t2 = log.write(1, b'\x02') self.assertEqual(t2, timedelta(hours=1)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2020, 1, 21, 2)) t3 = log.write(1, b'\x03') self.assertEqual(t3, timedelta(hours=2)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2020, 1, 21, 4)) # TODO this write should rise exception as the time gap is too big to track! t4 = log.write(1, b'\x04') self.assertEqual(t4, timedelta(hours=4)) with LogIndexedReader(log.filename) as log: dt, channel, data = log[1] self.assertEqual(dt, timedelta(hours=0)) dt, channel, data = log[2] self.assertEqual(dt, timedelta(hours=1)) dt, channel, data = log[3] self.assertEqual(dt, timedelta(hours=2))
def test_large_blocks_with_growing_file(self): block_size = 100000 # fits into 2 packets, so 16 bytes overhead with ExitStack() as at_exit: with patch('osgar.logger.datetime.datetime'): osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1)) with osgar.logger.LogWriter(prefix='tmpA', note='') as log: at_exit.callback(os.remove, log.filename) t1 = log.write(1, b'\x01' * block_size) self.assertEqual(t1, timedelta(0)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1, 1)) t2 = log.write(1, b'\x02' * block_size) self.assertEqual(t2, timedelta(hours=1)) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1, 2)) t3 = log.write(1, b'\x03' * block_size) self.assertEqual(t3, timedelta(hours=2)) partial = log.filename + '.part' with open(log.filename, 'rb') as f_in, open(partial, 'wb') as f_out: at_exit.callback(os.remove, partial) # log file starts with 16 byte header f_out.write(f_in.read(16)) f_out.write(f_in.read(100)) f_out.flush() with LogIndexedReader(partial) as log: with self.assertRaises(IndexError): log[0] # add whole block plus 2 headers minus 100 already there f_out.write(f_in.read(block_size - 100 + 16)) f_out.flush() log.grow() self.assertEqual(len(log), 1) # add a partial header only f_out.write(f_in.read(4)) f_out.flush() log.grow() self.assertEqual(len(log), 1) # add another block f_out.write(f_in.read(block_size + 16)) f_out.flush() log.grow() dt, channel, data = log[0] self.assertEqual(dt, timedelta(hours=0)) dt, channel, data = log[1] self.assertEqual(dt, timedelta(hours=1)) f_out.write(f_in.read()) f_out.flush() log.grow() dt, channel, data = log[2] self.assertEqual(dt, timedelta(hours=2))
def __init__(self, filepath, lidar_name=None, lidar2_name=None, pose2d_name=None, pose3d_name=None, camera_name=None, camera2_name=None, bbox_name=None, joint_name=None, keyframes_name=None, title_name=None): self.log = LogIndexedReader(filepath) self.current = 0 self.pose = [0, 0, 0] self.pose2d = [0, 0, 0] self.pose3d = [[0, 0, 0],[1, 0, 0, 0]] self.scan = [] self.scan2 = [] self.image = None self.image2 = None self.bbox = None self.joint = None self.keyframe = None self.title = None self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None self.lidar2_id = None self.camera2_id = None self.bbox_id = None self.joint_id = None self.keyframes_id = None self.title_id = None names = lookup_stream_names(filepath) if lidar_name is not None: self.lidar_id = names.index(lidar_name) + 1 if lidar2_name is not None: self.lidar2_id = names.index(lidar2_name) + 1 if pose2d_name is not None: self.pose2d_id = names.index(pose2d_name) + 1 if pose3d_name is not None: self.pose3d_id = names.index(pose3d_name) + 1 if camera_name is not None: self.camera_id = names.index(camera_name) + 1 if camera2_name is not None: self.camera2_id = names.index(camera2_name) + 1 if bbox_name is not None: self.bbox_id = names.index(bbox_name) + 1 if joint_name is not None: self.joint_id = names.index(joint_name) + 1 if keyframes_name is not None: self.keyframes_id = names.index(keyframes_name) + 1 if title_name is not None: self.title_id = names.index(title_name) + 1
def test_incomplete(self): header = osgar.logger.format_header(datetime(2019, 1, 1, 1)) packet = osgar.logger.format_packet(1, b"\x00"*10, timedelta()) logdata = b"".join(header + packet) with tempfile.TemporaryDirectory(dir=".") as d: filename = Path(".") / d / "log" with open(filename, "wb") as f: f.write(logdata[:-1]) with LogIndexedReader(filename) as l: with self.assertRaises(IndexError): l[0]
def test_no_eof(self): with patch('osgar.logger.datetime.datetime'): osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1)) with LogWriter(prefix='tmpEof', note='test_EOF') as log: filename = log.filename osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1, 1)) t1 = log.write(1, b'\x01' * 100) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1, 2)) t2 = log.write(1, b'\x02' * 100) osgar.logger.datetime.datetime = TimeStandsStill( datetime(2019, 1, 1, 3)) t3 = log.write(1, b'\x03' * 100000) partial = filename + '.part' with open(filename, 'rb') as f_in, open(partial, 'wb') as f_out: f_out.write(f_in.read(100)) f_out.flush() with LogIndexedReader(partial) as log: self.assertEqual(len(log), 1) dt, channel, data = log[0] with self.assertRaises(IndexError): log[1] f_out.write(f_in.read(100)) f_out.flush() log.grow() self.assertEqual(len(log), 2) dt, channel, data = log[1] self.assertEqual(data, b'\x01' * 100) self.assertEqual(dt, timedelta(hours=1)) with self.assertRaises(IndexError): log[2] f_out.write(f_in.read()) f_out.flush() log.grow() dt, channel, data = log[2] self.assertEqual(dt, timedelta(hours=2)) self.assertEqual(data, b'\x02' * 100) dt, channel, data = log[3] self.assertEqual(dt, timedelta(hours=3)) self.assertEqual(len(log), 4) self.assertEqual(data, b'\x03' * 100000) os.remove(partial) os.remove(filename)
def __init__(self, filepath, lidar_name=None, pose2d_name=None, pose3d_name=None, camera_name=None): self.log = LogIndexedReader(filepath) self.current = 0 self.pose = [0, 0, 0] self.pose2d = [0, 0, 0] self.pose3d = [[0, 0, 0], [1, 0, 0, 0]] self.scan = [] self.image = None self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None names = lookup_stream_names(filepath) if lidar_name is not None: self.lidar_id = names.index(lidar_name) + 1 if pose2d_name is not None: self.pose2d_id = names.index(pose2d_name) + 1 if pose3d_name is not None: self.pose3d_id = names.index(pose3d_name) + 1 if camera_name is not None: self.camera_id = names.index(camera_name) + 1
def test_large_invalid(self): header = osgar.logger.format_header(datetime(2019, 1, 1, 1)) packet = osgar.logger.format_packet(1, b"\x00"*(2**16), timedelta()) invalid = b"\xFF"*len(packet[-2]) self.assertNotEqual(packet[-2], invalid) packet[-2] = invalid logdata = b"".join(header + packet) with tempfile.TemporaryDirectory(dir=".") as d: filename = Path(".") / d / "log" with open(filename, "wb") as f: f.write(logdata[:-1]) with self.assertLogs(logger=osgar.logger.__name__, level=logging.ERROR) as log: with LogIndexedReader(filename) as l: with self.assertRaises(IndexError): l[0]
def test_indexed_large_block(self): data = bytes([x for x in range(100)]*1000) self.assertEqual(len(data), 100000) with ExitStack() as at_exit: with LogWriter(prefix='tmpIndexedLarge', note='test_large_block') as log: at_exit.callback(os.remove, log.filename) t1 = log.write(1, data) t2 = log.write(1, data[:0xFFFF]) t3 = log.write(1, b'') t4 = log.write(1, b'ABC') t5 = log.write(1, data+data) # multiple split with LogIndexedReader(log.filename) as log: self.assertEqual(len(log[1][2]), 100000) self.assertEqual(len(log[2][2]), 65535) self.assertEqual(len(log[3][2]), 0) self.assertEqual(len(log[4][2]), 3) self.assertEqual(len(log[5][2]), 200000)
def test_indexed_reader(self): note = 'test_indexed_reader' sample = [b'\x01', b'\x02', b'\x03'] times = [] with ExitStack() as at_exit: with LogWriter(prefix='tmpIndexed', note=note) as log: at_exit.callback(os.remove, log.filename) for a in sample: t = log.write(1, a) times.append(t) time.sleep(0.001) with LogIndexedReader(log.filename) as log: dt, channel, data = log[0] assert data.decode('utf-8') == note, data for i in range(len(sample)): dt, channel, data = log[i + 1] assert data == sample[i], data assert dt == times[i], (dt, times[i]) assert len(log) == len(sample) + 1, len(log)
class Framer: """Creates frames from log entries. Packs together closest scan, pose and camera picture.""" def __init__(self, filepath, lidar_name=None, lidar2_name=None, pose2d_name=None, pose3d_name=None, camera_name=None, camera2_name=None, bbox_name=None, joint_name=None, keyframes_name=None, title_name=None): self.log = LogIndexedReader(filepath) self.current = 0 self.pose = [0, 0, 0] self.pose2d = [0, 0, 0] self.pose3d = [[0, 0, 0],[1, 0, 0, 0]] self.scan = [] self.scan2 = [] self.image = None self.image2 = None self.bbox = None self.joint = None self.keyframe = None self.title = None self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None self.lidar2_id = None self.camera2_id = None self.bbox_id = None self.joint_id = None self.keyframes_id = None self.title_id = None names = lookup_stream_names(filepath) if lidar_name is not None: self.lidar_id = names.index(lidar_name) + 1 if lidar2_name is not None: self.lidar2_id = names.index(lidar2_name) + 1 if pose2d_name is not None: self.pose2d_id = names.index(pose2d_name) + 1 if pose3d_name is not None: self.pose3d_id = names.index(pose3d_name) + 1 if camera_name is not None: self.camera_id = names.index(camera_name) + 1 if camera2_name is not None: self.camera2_id = names.index(camera2_name) + 1 if bbox_name is not None: self.bbox_id = names.index(bbox_name) + 1 if joint_name is not None: self.joint_id = names.index(joint_name) + 1 if keyframes_name is not None: self.keyframes_id = names.index(keyframes_name) + 1 if title_name is not None: self.title_id = names.index(title_name) + 1 def __enter__(self): self.log.__enter__() return self def __exit__(self, *args): self.log.__exit__() def prev(self): return self._step(-1) def next(self): return self._step(1) def seek(self, desired_timestamp): print('Seek:', desired_timestamp) timestamp, stream_id, data = self.log[self.current] print('current', timestamp) if timestamp > desired_timestamp: return timestamp start = self.current end = len(self.log) - 1 timestamp, stream_id, data = self.log[end] print('end', timestamp) while timestamp < desired_timestamp: self.log.grow() end = len(self.log) - 1 timestamp, stream_id, data = self.log[end] print('end', timestamp) while start + 1 < end: mid = (start + end)//2 timestamp, stream_id, data = self.log[mid] if timestamp < desired_timestamp: start = mid else: end = mid print('selected', timestamp) self.current = start def _step(self, direction): self.bbox = None if (self.current + direction) >= len(self.log): self.log.grow() while self.current + direction >= 0 and self.current + direction < len(self.log): self.current += direction timestamp, stream_id, data = self.log[self.current] if stream_id == self.keyframes_id: self.keyframe = True if stream_id == self.title_id: self.title = deserialize(data) if stream_id == self.bbox_id: self.bbox = deserialize(data) if stream_id == self.lidar_id: self.scan = deserialize(data) keyframe = self.keyframe self.keyframe = False return timestamp, self.pose, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False if stream_id == self.lidar2_id: self.scan2 = deserialize(data) elif stream_id == self.camera_id: self.image = get_image(deserialize(data)) if self.lidar_id is None: keyframe = self.keyframe self.keyframe = False return timestamp, self.pose, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False elif stream_id == self.camera2_id: self.image2 = get_image(deserialize(data)) elif stream_id == self.joint_id: self.joint = deserialize(data) elif stream_id == self.pose3d_id: pose3d, orientation = deserialize(data) assert len(pose3d) == 3 assert len(orientation) == 4 self.pose = [pose3d[0], pose3d[1], quaternion.heading(orientation)] self.pose3d = [pose3d, orientation] elif stream_id == self.pose2d_id: arr = deserialize(data) assert len(arr) == 3 self.pose = (arr[0]/1000.0, arr[1]/1000.0, math.radians(arr[2]/100.0)) x, y, heading = self.pose self.pose = (x * math.cos(g_rotation_offset_rad) - y * math.sin(g_rotation_offset_rad), x * math.sin(g_rotation_offset_rad) + y * math.cos(g_rotation_offset_rad), heading + g_rotation_offset_rad) if self.lidar_id is None and self.camera_id is None: keyframe = self.keyframe self.keyframe = False return timestamp, self.pose, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False return timedelta(), self.pose, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, self.keyframe, self.title, True
class Framer: """Creates frames from log entries. Packs together closest scan, pose and camera picture.""" def __init__(self, filepath, lidar_name=None, pose2d_name=None, pose3d_name=None, camera_name=None): self.log = LogIndexedReader(filepath) self.current = 0 self.pose = [0, 0, 0] self.pose2d = [0, 0, 0] self.pose3d = [[0, 0, 0], [1, 0, 0, 0]] self.scan = [] self.image = None self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None names = lookup_stream_names(filepath) if lidar_name is not None: self.lidar_id = names.index(lidar_name) + 1 if pose2d_name is not None: self.pose2d_id = names.index(pose2d_name) + 1 if pose3d_name is not None: self.pose3d_id = names.index(pose3d_name) + 1 if camera_name is not None: self.camera_id = names.index(camera_name) + 1 def __enter__(self): self.log.__enter__() return self def __exit__(self, *args): self.log.__exit__() def prev(self): return self._step(-1) def next(self): return self._step(1) def _step(self, direction): if (self.current + direction) >= len(self.log): self.log.grow() while self.current + direction >= 0 and self.current + direction < len( self.log): self.current += direction timestamp, stream_id, data = self.log[self.current] if stream_id == self.lidar_id: self.scan = deserialize(data) return timestamp, self.pose, self.scan, self.image, False elif stream_id == self.camera_id: jpeg = deserialize(data) self.image = pygame.image.load(io.BytesIO(jpeg), 'JPG').convert() if self.lidar_id is None: return timestamp, self.pose, self.scan, self.image, False elif stream_id == self.pose3d_id: pose3d, orientation = deserialize(data) assert len(pose3d) == 3 assert len(orientation) == 4 self.pose = [ pose3d[0], pose3d[1], quaternion.heading(orientation) ] self.pose3d = [pose3d, orientation] elif stream_id == self.pose2d_id: arr = deserialize(data) assert len(arr) == 3 self.pose = (arr[0] / 1000.0, arr[1] / 1000.0, math.radians(arr[2] / 100.0)) x, y, heading = self.pose self.pose = (x * math.cos(g_rotation_offset_rad) - y * math.sin(g_rotation_offset_rad), x * math.sin(g_rotation_offset_rad) + y * math.cos(g_rotation_offset_rad), heading + g_rotation_offset_rad) if self.lidar_id is None and self.camera_id is None: return timestamp, self.pose, self.scan, self.image, False return timedelta(), self.pose, self.scan, self.image, True
def __init__(self, filepath, lidar_name=None, lidar2_name=None, pose2d_name=None, pose3d_name=None, camera_name=None, camera2_name=None, bbox_name=None, rgbd_name=None, joint_name=None, keyframes_name=None, title_name=None, lidar_up_name=None, lidar_down_name=None): self.log = LogIndexedReader(filepath) self.current = 0 self.frame = Frame() self.pose = [0, 0, 0] self.pose2d = [0, 0, 0] self.pose3d = [[0, 0, 0], [1, 0, 0, 0]] self.scan = [] self.scan2 = [] self.image = None self.image2 = None self.bbox = None self.joint = None self.keyframe = None self.title = None self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None self.lidar2_id = None self.camera2_id = None self.bbox_id = None self.rgbd_id = None self.joint_id = None self.keyframes_id = None self.title_id = [] self.lidar_up_id = None self.lidar_down_id = None names = lookup_stream_names(filepath) if lidar_name is not None: self.lidar_id = names.index(lidar_name) + 1 if lidar2_name is not None: self.lidar2_id = names.index(lidar2_name) + 1 if pose2d_name is not None: self.pose2d_id = names.index(pose2d_name) + 1 if pose3d_name is not None: self.pose3d_id = names.index(pose3d_name) + 1 if camera_name is not None: self.camera_id = names.index(camera_name) + 1 if camera2_name is not None: self.camera2_id = names.index(camera2_name) + 1 if bbox_name is not None: self.bbox_id = names.index(bbox_name) + 1 if rgbd_name is not None: self.rgbd_id = names.index(rgbd_name) + 1 if joint_name is not None: self.joint_id = names.index(joint_name) + 1 if keyframes_name is not None: self.keyframes_id = names.index(keyframes_name) + 1 if title_name is not None: title_list = title_name.split(",") for ttn in title_list: self.title_id.append(names.index(ttn) + 1) if lidar_up_name is not None: self.lidar_up_id = names.index(lidar_up_name) + 1 if lidar_down_name is not None: self.lidar_down_id = names.index(lidar_down_name) + 1
class Framer: """Creates frames from log entries. Packs together closest scan, pose and camera picture.""" def __init__(self, filepath, lidar_name=None, lidar2_name=None, pose2d_name=None, pose3d_name=None, camera_name=None, camera2_name=None, bbox_name=None, rgbd_name=None, joint_name=None, keyframes_name=None, title_name=None, lidar_up_name=None, lidar_down_name=None): self.log = LogIndexedReader(filepath) self.current = 0 self.frame = Frame() self.pose = [0, 0, 0] self.pose2d = [0, 0, 0] self.pose3d = [[0, 0, 0], [1, 0, 0, 0]] self.scan = [] self.scan2 = [] self.image = None self.image2 = None self.bbox = None self.joint = None self.keyframe = None self.title = None self.lidar_id, self.pose2d_id, self.pose3d_id, self.camera_id = None, None, None, None self.lidar2_id = None self.camera2_id = None self.bbox_id = None self.rgbd_id = None self.joint_id = None self.keyframes_id = None self.title_id = [] self.lidar_up_id = None self.lidar_down_id = None names = lookup_stream_names(filepath) if lidar_name is not None: self.lidar_id = names.index(lidar_name) + 1 if lidar2_name is not None: self.lidar2_id = names.index(lidar2_name) + 1 if pose2d_name is not None: self.pose2d_id = names.index(pose2d_name) + 1 if pose3d_name is not None: self.pose3d_id = names.index(pose3d_name) + 1 if camera_name is not None: self.camera_id = names.index(camera_name) + 1 if camera2_name is not None: self.camera2_id = names.index(camera2_name) + 1 if bbox_name is not None: self.bbox_id = names.index(bbox_name) + 1 if rgbd_name is not None: self.rgbd_id = names.index(rgbd_name) + 1 if joint_name is not None: self.joint_id = names.index(joint_name) + 1 if keyframes_name is not None: self.keyframes_id = names.index(keyframes_name) + 1 if title_name is not None: title_list = title_name.split(",") for ttn in title_list: self.title_id.append(names.index(ttn) + 1) if lidar_up_name is not None: self.lidar_up_id = names.index(lidar_up_name) + 1 if lidar_down_name is not None: self.lidar_down_id = names.index(lidar_down_name) + 1 def __enter__(self): self.log.__enter__() return self def __exit__(self, *args): self.log.__exit__() def prev(self): return self._step(-1) def next(self): return self._step(1) def seek(self, desired_timestamp): print('Seek:', desired_timestamp) timestamp, stream_id, data = self.log[self.current] print('current', timestamp) if timestamp > desired_timestamp: return timestamp start = self.current end = len(self.log) - 1 timestamp, stream_id, data = self.log[end] print('end', timestamp) while timestamp < desired_timestamp: self.log.grow() end = len(self.log) - 1 timestamp, stream_id, data = self.log[end] print('end', timestamp) while start + 1 < end: mid = (start + end) // 2 timestamp, stream_id, data = self.log[mid] if timestamp < desired_timestamp: start = mid else: end = mid print('selected', timestamp) self.current = start def _step(self, direction): self.bbox = [] self.title = [] if (self.current + direction) >= len(self.log): self.log.grow() while self.current + direction >= 0 and self.current + direction < len( self.log): self.current += direction timestamp, stream_id, data = self.log[self.current] if stream_id == self.keyframes_id: self.keyframe = True if stream_id in self.title_id and stream_id != self.bbox_id: self.title.append(deserialize(data)) if stream_id == self.lidar_id: self.scan = deserialize(data) keyframe = self.keyframe self.keyframe = False return timestamp, self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False if stream_id == self.lidar2_id: self.scan2 = deserialize(data) if stream_id == self.lidar_up_id: self.frame.lidar_up = deserialize(data) if stream_id == self.lidar_down_id: self.frame.lidar_down = deserialize(data) elif stream_id == self.camera_id: self.image, _ = get_image(deserialize(data)) # bounding boxes associated with an image are stored after the image in the log # therefore, we need to continue reading the log past the image in order to gathering its bounding box data current = self.current while current + direction >= 0 and current + direction < len( self.log): current += direction _, new_stream_id, new_data = self.log[current] if new_stream_id == self.bbox_id: self.bbox.append(deserialize(new_data)) if new_stream_id in self.title_id: self.title.append(deserialize(new_data)) if new_stream_id == self.camera_id: break if self.lidar_id is None: keyframe = self.keyframe self.keyframe = False return timestamp, self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False elif stream_id == self.camera2_id: self.image2, _ = get_image(deserialize(data)) elif stream_id == self.rgbd_id: _, _, img_data, depth_data = deserialize(data) self.image, self.image2 = get_image((img_data, depth_data)) if self.lidar_id is None: keyframe = self.keyframe self.keyframe = False return timestamp, self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False elif stream_id == self.joint_id: self.joint = deserialize(data) elif stream_id == self.pose3d_id: pose3d, orientation = deserialize(data) assert len(pose3d) == 3 assert len(orientation) == 4 self.pose = [ pose3d[0], pose3d[1], quaternion.heading(orientation) ] self.pose3d = [pose3d, orientation] elif stream_id == self.pose2d_id: arr = deserialize(data) assert len(arr) == 3 self.pose = (arr[0] / 1000.0, arr[1] / 1000.0, math.radians(arr[2] / 100.0)) x, y, heading = self.pose self.pose = (x * math.cos(g_rotation_offset_rad) - y * math.sin(g_rotation_offset_rad), x * math.sin(g_rotation_offset_rad) + y * math.cos(g_rotation_offset_rad), heading + g_rotation_offset_rad) if self.lidar_id is None and self.camera_id is None: keyframe = self.keyframe self.keyframe = False return timestamp, self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, keyframe, self.title, False return timedelta( ), self.frame, self.pose, self.pose3d, self.scan, self.scan2, self.image, self.image2, self.bbox, self.joint, self.keyframe, self.title, True