From 711d60239a63e434a5f10f14a101b120e17c479b Mon Sep 17 00:00:00 2001 From: Mitchell Goff Date: Tue, 8 Dec 2020 19:01:59 -0800 Subject: [PATCH] Added model_replay and fixed some bugs in camera_replay (#2679) * Added model_replay and fixed some bugs in camera_replay * Unpack capnp logs on device * add sync to device * GPU now also works on PC * update model ref * update refs * dont change this one * Use pipeline calib instead of rlog calib * remove that * update refs Co-authored-by: Willem Melching Co-authored-by: Adeeb Shihadeh old-commit-hash: bbe9accd4d1436b02281a3e8af2427f1cca5af49 --- selfdrive/modeld/modeld.cc | 9 +--- selfdrive/test/model_replay.py | 30 +++++++++++++ .../test/process_replay/camera_replay.py | 43 +++++++++++++++---- selfdrive/test/process_replay/inject_model.py | 2 +- .../process_replay/model_replay_ref_commit | 2 +- 5 files changed, 68 insertions(+), 18 deletions(-) create mode 100755 selfdrive/test/model_replay.py diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 992473bbc6..9a28a070f1 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -114,14 +114,8 @@ int main(int argc, char **argv) { PubMaster pm({"modelV2", "model", "cameraOdometry"}); SubMaster sm({"pathPlan", "frame"}); -#if defined(QCOM) || defined(QCOM2) - cl_device_type device_type = CL_DEVICE_TYPE_DEFAULT; -#else - cl_device_type device_type = CL_DEVICE_TYPE_CPU; -#endif - // cl init - cl_device_id device_id = cl_get_device_id(device_type); + cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); cl_command_queue q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); @@ -188,6 +182,7 @@ int main(int argc, char **argv) { // TODO: don't make copies! memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len); + visionbuf_sync(&yuv_ion, VISIONBUF_SYNC_TO_DEVICE); ModelDataRaw model_buf = model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height, diff --git a/selfdrive/test/model_replay.py b/selfdrive/test/model_replay.py new file mode 100755 index 0000000000..6e3ab67924 --- /dev/null +++ b/selfdrive/test/model_replay.py @@ -0,0 +1,30 @@ +#!/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") diff --git a/selfdrive/test/process_replay/camera_replay.py b/selfdrive/test/process_replay/camera_replay.py index 0a50be404d..853470c97b 100755 --- a/selfdrive/test/process_replay/camera_replay.py +++ b/selfdrive/test/process_replay/camera_replay.py @@ -10,10 +10,11 @@ os.environ['CI'] = "1" if ANDROID: os.environ['QCOM_REPLAY'] = "1" +from common.spinner import Spinner from common.timeout import Timeout import selfdrive.manager as manager -from common.spinner import Spinner +from cereal import log import cereal.messaging as messaging from tools.lib.framereader import FrameReader from tools.lib.logreader import LogReader @@ -21,14 +22,21 @@ from selfdrive.test.openpilotci import BASE_URL, get_url from selfdrive.test.process_replay.compare_logs import compare_logs, save_log from selfdrive.test.process_replay.test_processes import format_diff from selfdrive.version import get_git_commit +from common.transformations.camera import get_view_frame_from_road_frame TEST_ROUTE = "99c94dc769b5d96e|2019-08-03--14-19-59" -def camera_replay(lr, fr): +def replace_calib(msg, calib): + msg = msg.as_builder() + if calib is not None: + msg.liveCalibration.extrinsicMatrix = get_view_frame_from_road_frame(*calib, 1.22).flatten().tolist() + return msg + +def camera_replay(lr, fr, desire=None, calib=None): spinner = Spinner() - pm = messaging.PubMaster(['frame', 'liveCalibration']) + pm = messaging.PubMaster(['frame', 'liveCalibration', 'pathPlan']) sm = messaging.SubMaster(['model']) # TODO: add dmonitoringmodeld @@ -42,18 +50,26 @@ def camera_replay(lr, fr): time.sleep(5) print("procs started") + desires_by_index = {v:k for k,v in log.PathPlan.Desire.schema.enumerants.items()} + cal = [msg for msg in lr if msg.which() == "liveCalibration"] for msg in cal[:5]: - pm.send(msg.which(), msg.as_builder()) + pm.send(msg.which(), replace_calib(msg, calib)) log_msgs = [] frame_idx = 0 for msg in tqdm(lr): - if msg.which() == "liveCalibrationd": - pm.send(msg.which(), msg.as_builder()) + if msg.which() == "liveCalibration": + pm.send(msg.which(), replace_calib(msg, calib)) elif msg.which() == "frame": + if desire is not None: + for i in desire[frame_idx].nonzero()[0]: + dat = messaging.new_message('pathPlan') + dat.pathPlan.desire = desires_by_index[i] + pm.send('pathPlan', dat) + f = msg.as_builder() - img = fr.get(frame_idx, pix_fmt="rgb24")[0][:, ::, -1] + img = fr.get(frame_idx, pix_fmt="rgb24")[0][:,:,::-1] f.frame.image = img.flatten().tobytes() frame_idx += 1 @@ -79,6 +95,9 @@ 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")) @@ -86,7 +105,7 @@ if __name__ == "__main__": failed = False if not update: - ref_commit = open("model_replay_ref_commit").read().strip() + 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) results: Any = {TEST_ROUTE: {}} @@ -105,7 +124,13 @@ if __name__ == "__main__": new_commit = get_git_commit() log_fn = "%s_%s_%s.bz2" % (TEST_ROUTE, "model", new_commit) save_log(log_fn, log_msgs) - upload_file(log_fn, os.path.basename(log_fn)) + try: + upload_file(log_fn, os.path.basename(log_fn)) + except Exception as e: + print("failed to upload", e) + + with open(ref_commit_fn, 'w') as f: + f.write(str(new_commit)) print("\n\nNew ref commit: ", new_commit) diff --git a/selfdrive/test/process_replay/inject_model.py b/selfdrive/test/process_replay/inject_model.py index ee16846751..1ebc0f8e50 100755 --- a/selfdrive/test/process_replay/inject_model.py +++ b/selfdrive/test/process_replay/inject_model.py @@ -28,7 +28,7 @@ def regen_model(msgs, pm, frame_reader, model_sock): if w == 'frame': msg = msg.as_builder() - img = frame_reader.get(fidx, pix_fmt="rgb24")[0][:, ::-1] + img = frame_reader.get(fidx, pix_fmt="rgb24")[0][:,:,::-1] msg.frame.image = img.flatten().tobytes() diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 1aab831e20..19f83517b5 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -0d5ecd64a10debf2f62e27fc23738ccce9d1cdb3 \ No newline at end of file +f040d87da71d8d9b29000b0eeea43d4f932137dd \ No newline at end of file