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
