def regen_and_save(route, sidx, upload=False, use_route_meta=False):
if use_route_meta:
r = Route(args.route)
lr = LogReader(r.log_paths()[args.seg])
fr = FrameReader(r.camera_paths()[args.seg])
else:
lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2")
fr = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc")
rpath = regen_segment(lr, {'roadCameraState': fr})
# compress raw rlog before uploading
with open(os.path.join(rpath, "rlog"), "rb") as f:
data = bz2.compress(f.read())
with open(os.path.join(rpath, "rlog.bz2"), "wb") as f:
f.write(data)
os.remove(os.path.join(rpath, "rlog"))
lr = LogReader(os.path.join(rpath, 'rlog.bz2'))
controls_state_active = [m.controlsState.active for m in lr if m.which() == 'controlsState']
assert any(controls_state_active), "Segment did not engage"
relr = os.path.relpath(rpath)
print("\n\n", "*"*30, "\n\n")
print("New route:", relr, "\n")
if upload:
upload_route(relr)
return relr
def test_framereader(self):
def _check_data(f):
self.assertEqual(f.frame_count, 1200)
self.assertEqual(f.w, 1164)
self.assertEqual(f.h, 874)
frame_first_30 = f.get(0, 30)
self.assertEqual(len(frame_first_30), 30)
print(frame_first_30[15])
print("frame_0")
frame_0 = f.get(0, 1)
frame_15 = f.get(15, 1)
print(frame_15[0])
assert np.all(frame_first_30[0] == frame_0[0])
assert np.all(frame_first_30[15] == frame_15[0])
with tempfile.NamedTemporaryFile(suffix=".hevc") as fp:
r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/video.hevc?raw=true")
fp.write(r.content)
fp.flush()
fr_file = FrameReader(fp.name)
_check_data(fr_file)
fr_url = FrameReader("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/video.hevc?raw=true")
_check_data(fr_url)
def __init__(self, main_dir, transform=None):
self.main_dir = main_dir
self.frame_reader = FrameReader(main_dir + 'video.hevc')
self.gps_times = np.load(main_dir + 'global_pose/frame_gps_times')
self.orientations = np.load(main_dir +
'global_pose/frame_orientations')
self.positions = np.load(main_dir + 'global_pose/frame_positions')
self.times = np.load(main_dir + 'global_pose/frame_times')
self.velocities = np.load(main_dir + 'global_pose/frame_velocities')
self.transform = transform
def regen_and_save(route, sidx, upload=False, use_route_meta=True):
if use_route_meta:
r = Route(args.route)
lr = LogReader(r.log_paths()[args.seg])
fr = FrameReader(r.camera_paths()[args.seg])
else:
lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2")
fr = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc")
rpath = regen_segment(lr, {'roadCameraState': fr})
relr = os.path.relpath(rpath)
print("\n\n", "*"*30, "\n\n")
print("New route:", relr, "\n")
if upload:
upload_route(relr)
return relr
class CommaDataset(Dataset):
def __init__(self, main_dir, transform=None):
self.main_dir = main_dir
self.frame_reader = FrameReader(main_dir + 'video.hevc')
self.gps_times = np.load(main_dir + 'global_pose/frame_gps_times')
self.orientations = np.load(main_dir +
'global_pose/frame_orientations')
self.positions = np.load(main_dir + 'global_pose/frame_positions')
self.times = np.load(main_dir + 'global_pose/frame_times')
self.velocities = np.load(main_dir + 'global_pose/frame_velocities')
self.transform = transform
def __len__(self):
return len(self.velocities)
def __getitem__(self, idx):
image = np.array(self.frame_reader.get(idx, pix_fmt='rgb24')[0],
dtype=np.float64)
sample = {
'image': image,
'gps_times': self.gps_times,
'orientations': self.orientations,
'positions': self.positions,
'times': self.times,
'velocities': self.velocities[idx]
}
if self.transform:
sample = self.transform(sample)
return sample
def __missing__(self, key):
if self._camera_paths.get(key, None) is not None:
frame_reader = FrameReader(self._camera_paths[key],
self._cache_paths.get(key), **self._framereader_kwargs)
self[key] = frame_reader
return frame_reader
else:
raise KeyError("Segment index out of bounds: {}".format(key))
def inject_model(msgs, segment_name):
if segment_name.count('--') == 2:
segment_name = rreplace(segment_name, '--', '/', 1)
frame_reader = FrameReader('cd:/' + segment_name.replace("|", "/") +
"/fcamera.hevc")
manager.start_managed_process('camerad')
manager.start_managed_process('modeld')
# TODO do better than just wait for modeld to boot
time.sleep(5)
pm = PubMaster(['liveCalibration', 'roadCameraState'])
model_sock = sub_sock('model')
try:
out_msgs = regen_model(msgs, pm, frame_reader, model_sock)
except (KeyboardInterrupt, SystemExit, Exception) as e:
manager.kill_managed_process('modeld')
time.sleep(2)
manager.kill_managed_process('camerad')
raise e
manager.kill_managed_process('modeld')
time.sleep(2)
manager.kill_managed_process('camerad')
new_msgs = []
midx = 0
for msg in msgs:
if (msg.which() == 'model') and (midx < len(out_msgs)):
model = out_msgs[midx].as_builder()
model.logMonoTime = msg.logMonoTime
model = model.as_reader()
new_msgs.append(model)
midx += 1
else:
new_msgs.append(msg)
print(len(new_msgs), len(list(msgs)))
assert abs(len(new_msgs) - len(list(msgs))) < 2
return new_msgs
def replay(route, loop):
route = route.replace('|', '/')
lr = LogReader(f"cd:/{route}/rlog.bz2")
fr = FrameReader(f"cd:/{route}/fcamera.hevc", readahead=True)
# Build mapping from frameId to segmentId from roadEncodeIdx, type == fullHEVC
msgs = [m for m in lr if m.which() not in IGNORE]
msgs = sorted(msgs, key=lambda m: m.logMonoTime)
times = [m.logMonoTime for m in msgs]
frame_idx = {m.roadEncodeIdx.frameId: m.roadEncodeIdx.segmentId for m in msgs if m.which() == 'roadEncodeIdx' and m.roadEncodeIdx.type == 'fullHEVC'}
socks = {}
lag = 0
i = 0
max_i = len(msgs) - 2
while True:
msg = msgs[i].as_builder()
next_msg = msgs[i + 1]
start_time = time.time()
w = msg.which()
if w == 'roadCameraState':
try:
img = fr.get(frame_idx[msg.roadCameraState.frameId], pix_fmt="rgb24")
img = img[0][:, :, ::-1] # Convert RGB to BGR, which is what the camera outputs
msg.roadCameraState.image = img.flatten().tobytes()
except (KeyError, ValueError):
pass
if w not in socks:
socks[w] = messaging.pub_sock(w)
try:
if socks[w]:
socks[w].send(msg.to_bytes())
except messaging.messaging_pyx.MultiplePublishersError:
socks[w] = None
lag += (next_msg.logMonoTime - msg.logMonoTime) / 1e9
lag -= time.time() - start_time
dt = max(lag, 0)
lag -= dt
time.sleep(dt)
if lag < -1.0 and i % 1000 == 0:
print(f"{-lag:.2f} s behind")
if input_ready():
key = sys.stdin.read(1)
# Handle pause
if key == " ":
while True:
if input_ready() and sys.stdin.read(1) == " ":
break
time.sleep(0.01)
# Handle seek
dt = defaultdict(int, s=10, S=-10)[key]
new_time = msgs[i].logMonoTime + dt * 1e9
i = bisect.bisect_left(times, new_time)
i = (i + 1) % max_i if loop else min(i + 1, max_i)
managed_processes['dmonitoringmodeld'].stop()
return log_msgs
if __name__ == "__main__":
update = "--update" in sys.argv
replay_dir = os.path.dirname(os.path.abspath(__file__))
ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit")
# load logs
lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT)))
frs = {
'roadCameraState':
FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="fcamera")),
'driverCameraState':
FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="dcamera")),
}
if TICI:
frs['wideRoadCameraState'] = FrameReader(
get_url(TEST_ROUTE, SEGMENT, log_type="ecamera"))
# run replay
log_msgs = model_replay(lr, frs)
# get diff
failed = False
if not update:
with open(ref_commit_fn) as f:
ref_commit = f.read().strip()
managed_processes['dmonitoringmodeld'].stop()
return log_msgs
if __name__ == "__main__":
update = "--update" in sys.argv
replay_dir = os.path.dirname(os.path.abspath(__file__))
ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit")
# load logs
lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT)))
frs = {
'roadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="fcamera")),
'driverCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="dcamera")),
}
# run replay
log_msgs = model_replay(lr, frs)
# get diff
failed = False
if not update:
with open(ref_commit_fn) as f:
ref_commit = f.read().strip()
log_fn = get_log_fn(ref_commit)
cmp_log = LogReader(BASE_URL + log_fn)
ignore = [
managed_processes['dmonitoringmodeld'].stop()
return log_msgs
if __name__ == "__main__":
update = "--update" in sys.argv
replay_dir = os.path.dirname(os.path.abspath(__file__))
ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit")
# load logs
lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT)))
frs = {
'roadCameraState':
FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="fcamera")),
'driverCameraState':
FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="dcamera")),
'wideRoadCameraState':
FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="ecamera"))
}
# run replay
log_msgs = model_replay(lr, frs)
# get diff
failed = False
if not update:
with open(ref_commit_fn) as f:
ref_commit = f.read().strip()
log_fn = get_log_fn(ref_commit, TEST_ROUTE)
#!/usr/bin/env python3
import os
import numpy as np
from tools.lib.logreader import LogReader
from tools.lib.framereader import FrameReader
from tools.lib.cache import cache_path_for_file_path
from selfdrive.test.process_replay.camera_replay import camera_replay
if __name__ == "__main__":
lr = LogReader(os.path.expanduser('~/rlog.bz2'))
fr = FrameReader(os.path.expanduser('~/fcamera.hevc'))
desire = np.load(os.path.expanduser('~/desire.npy'))
calib = np.load(os.path.expanduser('~/calib.npy'))
try:
msgs = camera_replay(list(lr), fr, desire=desire, calib=calib)
finally:
cache_path = cache_path_for_file_path(
os.path.expanduser('~/fcamera.hevc'))
if os.path.isfile(cache_path):
os.remove(cache_path)
output_size = len(np.frombuffer(msgs[0].model.rawPred, dtype=np.float32))
output_data = np.zeros((len(msgs), output_size), dtype=np.float32)
for i, msg in enumerate(msgs):
output_data[i] = np.frombuffer(msg.model.rawPred, dtype=np.float32)
np.save(os.path.expanduser('~/modeldata.npy'), output_data)
print("Finished replay")
if TICI:
os.system(
'sudo mount -o remount,size=200M /tmp') # c3 hevcs are 75M each
replay_dir = os.path.dirname(os.path.abspath(__file__))
ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit")
segnum = 0
frs = {}
if CACHE_DIR:
lr = LogReader(
os.path.join(
CACHE_DIR,
'%s--%d--rlog.bz2' % (TEST_ROUTE.replace('|', '_'), segnum)))
frs['roadCameraState'] = FrameReader(
os.path.join(
CACHE_DIR, '%s--%d--fcamera.hevc' %
(TEST_ROUTE.replace('|', '_'), segnum)))
frs['driverCameraState'] = FrameReader(
os.path.join(
CACHE_DIR, '%s--%d--dcamera.hevc' %
(TEST_ROUTE.replace('|', '_'), segnum)))
else:
lr = LogReader(get_url(TEST_ROUTE, segnum))
frs['roadCameraState'] = FrameReader(
get_url(TEST_ROUTE, segnum, log_type="fcamera"))
frs['driverCameraState'] = FrameReader(
get_url(TEST_ROUTE, segnum, log_type="dcamera"))
lr_list = list(lr)
log_msgs = model_replay(lr_list, frs)
import requests
from PIL import Image
from tools.lib.auth_config import get_token
from tools.lib.framereader import FrameReader
jwt = get_token()
route = sys.argv[1]
segment = int(sys.argv[2])
frame = int(sys.argv[3])
url = 'https://api.commadotai.com/v1/route/'+sys.argv[1]+"/files"
r = requests.get(url, headers={"Authorization": "JWT "+jwt})
assert r.status_code == 200
print("got api response")
cameras = r.json()['cameras']
if segment >= len(cameras):
raise Exception("segment %d not found, got %d segments" % (segment, len(cameras)))
fr = FrameReader(cameras[segment])
if frame >= fr.frame_count:
raise Exception("frame %d not found, got %d frames" % (frame, fr.frame_count))
im = Image.fromarray(fr.get(frame, count=1, pix_fmt="rgb24")[0])
fn = "uxxx_"+route.replace("|", "_")+"_%d_%d.png" % (segment, frame)
im.save(fn)
print(f"saved {fn}")
from common.transformations.camera import transform_img, eon_intrinsics
from common.transformations.model import medmodel_intrinsics
import numpy as np
from tqdm import tqdm
from tools.lib.framereader import FrameReader
import matplotlib
import matplotlib.pyplot as plt
# imgs just a list of images as in YUV format
fr = FrameReader("leon.hevc")
imgs = []
for i in tqdm(range(100)):
imgs.append(fr.get(i, pix_fmt='yuv420p')[0].reshape((874 * 3 // 2, 1164)))
def frames_to_tensor(frames):
H = (frames.shape[1] * 2) // 3
W = frames.shape[2]
in_img1 = np.zeros((frames.shape[0], 6, H // 2, W // 2), dtype=np.uint8)
in_img1[:, 0] = frames[:, 0:H:2, 0::2]
in_img1[:, 1] = frames[:, 1:H:2, 0::2]
in_img1[:, 2] = frames[:, 0:H:2, 1::2]
in_img1[:, 3] = frames[:, 1:H:2, 1::2]
in_img1[:, 4] = frames[:, H:H + H // 4].reshape((-1, H // 2, W // 2))
in_img1[:, 5] = frames[:, H + H // 4:H + H // 2].reshape(
(-1, H // 2, W // 2))
return in_img1
imgs_med_model = np.zeros((len(imgs), 384, 512), dtype=np.uint8)
spinner.close()
managed_processes['modeld'].stop()
time.sleep(2)
managed_processes['camerad'].stop()
return log_msgs
if __name__ == "__main__":
update = "--update" in sys.argv
replay_dir = os.path.dirname(os.path.abspath(__file__))
ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit")
lr = LogReader(get_url(TEST_ROUTE, 0))
fr = FrameReader(get_url(TEST_ROUTE, 0, log_type="fcamera"))
log_msgs = camera_replay(list(lr), fr)
failed = False
if not update:
ref_commit = open(ref_commit_fn).read().strip()
log_fn = "%s_%s_%s.bz2" % (TEST_ROUTE, "model", ref_commit)
cmp_log = LogReader(BASE_URL + log_fn)
ignore = [
'logMonoTime', 'valid', 'modelV2.frameDropPerc',
'modelV2.modelExecutionTime'
]
results: Any = {TEST_ROUTE: {}}
results[TEST_ROUTE]["modeld"] = compare_logs(cmp_log,
from tensorflow.keras.models import load_model
from tools.lib.parser import parser
import cv2
import sys
camerafile = sys.argv[1]
supercombo = load_model('supercombo.keras')
MAX_DISTANCE = 140.
LANE_OFFSET = 1.8
MAX_REL_V = 10.
LEAD_X_SCALE = 10
LEAD_Y_SCALE = 10
fr = FrameReader(camerafile)
cap = cv2.VideoCapture(camerafile)
imgs = []
raw_imgs = []
for i in tqdm(range(1000)):
imgs.append(fr.get(i, pix_fmt='yuv420p')[0].reshape((874 * 3 // 2, 1164)))
ret, frame = cap.read()
raw_imgs.append(frame)
raw_imgs = np.array(raw_imgs)
print(raw_imgs.shape)
def frames_to_tensor(frames):
H = (frames.shape[1] * 2) // 3
