diff --git a/.vscode/launch.json b/.vscode/launch.json index f090061c42..151b757dab 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -52,6 +52,9 @@ "type": "lldb", "request": "attach", "pid": "${command:pickMyProcess}", + "sourceMap": { + ".": "${workspaceFolder}/opendbc/safety" + }, "initCommands": [ "script import time; time.sleep(3)" ] diff --git a/RELEASES.md b/RELEASES.md index a3a95ae6a8..55141710d0 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,7 +1,8 @@ -Version 0.11.1 (2026-04-08) +Version 0.11.1 (2026-04-22) ======================== * New driver monitoring model * Improved image processing pipeline for driver camera +* Rivian R1S and R1T 2025 support thanks to lukasloetkolben! Version 0.11.0 (2026-03-17) ======================== diff --git a/cereal/log.capnp b/cereal/log.capnp index d8e9f56316..7e47ab171d 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -613,6 +613,11 @@ struct PandaState @0xa7649e2575e4591e { voltage @0 :UInt32; current @1 :UInt32; + # these fields are not used by openpilot, but they're + # reserved for forks building alternate experiences. + controlsAllowedRESERVED1 @38 :Bool; + controlsAllowedRESERVED2 @39 :Bool; + enum FaultStatus { none @0; faultTemp @1; @@ -2176,12 +2181,14 @@ struct DriverStateV2 { facePosition @2 :List(Float32); facePositionStd @3 :List(Float32); faceProb @4 :Float32; - leftEyeProb @5 :Float32; - rightEyeProb @6 :Float32; - leftBlinkProb @7 :Float32; - rightBlinkProb @8 :Float32; - sunglassesProb @9 :Float32; + eyesVisibleProb @14 :Float32; + eyesClosedProb @15 :Float32; phoneProb @13 :Float32; + leftEyeProbDEPRECATED @5 :Float32; + rightEyeProbDEPRECATED @6 :Float32; + leftBlinkProbDEPRECATED @7 :Float32; + rightBlinkProbDEPRECATED @8 :Float32; + sunglassesProbDEPRECATED @9 :Float32; notReadyProbDEPRECATED @12 :List(Float32); occludedProbDEPRECATED @10 :Float32; readyProbDEPRECATED @11 :List(Float32); diff --git a/opendbc_repo b/opendbc_repo index d0c76cb17e..c59bdd3f33 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit d0c76cb17e8e141d1bbae41480b03b2c8f64fe26 +Subproject commit c59bdd3f33fff239054c33e32abaa0b73cc73799 diff --git a/scripts/reporter.py b/scripts/reporter.py index d894b8af48..64f6cb99b8 100755 --- a/scripts/reporter.py +++ b/scripts/reporter.py @@ -38,6 +38,11 @@ if __name__ == "__main__": continue fn = os.path.basename(f) - master = get_checkpoint(MASTER_PATH + MODEL_PATH + fn) + master_path = MASTER_PATH + MODEL_PATH + fn + if os.path.exists(master_path): + master = get_checkpoint(master_path) + master_col = f"[{master}](https://reporter.comma.life/experiment/{master})" + else: + master_col = "N/A (new model)" pr = get_checkpoint(BASEDIR + MODEL_PATH + fn) - print("|", fn, "|", f"[{master}](https://reporter.comma.life/experiment/{master})", "|", f"[{pr}](https://reporter.comma.life/experiment/{pr})", "|") + print("|", fn, "|", master_col, "|", f"[{pr}](https://reporter.comma.life/experiment/{pr})", "|") diff --git a/selfdrive/assets/icons_mici/onroad/glasses.png b/selfdrive/assets/icons_mici/onroad/glasses.png deleted file mode 100644 index 006972fd39..0000000000 --- a/selfdrive/assets/icons_mici/onroad/glasses.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:56de402482b5987ed9a0ff3f793a1c89f857304b34fbb8a3deb5b5d4a332be1c -size 3688 diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 3f38324a6e..bad1cdd500 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -18,10 +18,10 @@ def estimate_pickle_max_size(onnx_size): tg_flags = { 'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0', 'Darwin': f'DEV=CPU THREADS=0 HOME={os.path.expanduser("~")}', # tinygrad calls brew which needs a $HOME in the env -}.get(arch, 'DEV=CPU CPU_LLVM=1 THREADS=0') +}.get(arch, 'DEV=CPU:LLVM THREADS=0') # Get model metadata -for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']: +for model_name in ['driving_vision', 'driving_off_policy', 'driving_on_policy', 'dmonitoring_model']: fn = File(f"models/{model_name}").abspath script_files = [File(Dir("#selfdrive/modeld").File("get_model_metadata.py").abspath)] cmd = f'{tg_flags} python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx' @@ -59,19 +59,5 @@ def tg_compile(flags, model_name): ) # Compile small models -for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']: +for model_name in ['driving_vision', 'driving_off_policy', 'driving_on_policy', 'dmonitoring_model']: tg_compile(tg_flags, model_name) - -# Compile BIG model if USB GPU is available -if "USBGPU" in os.environ: - import subprocess - # because tg doesn't support multi-process - devs = subprocess.check_output('python3 -c "from tinygrad import Device; print(list(Device.get_available_devices()))"', shell=True, cwd=env.Dir('#').abspath) - if b"AMD" in devs: - print("USB GPU detected... building") - flags = "DEV=AMD AMD_IFACE=USB AMD_LLVM=1 NOLOCALS=0 IMAGE=0" - bp = tg_compile(flags, "big_driving_policy") - bv = tg_compile(flags, "big_driving_vision") - lenv.SideEffect('lock', [bp, bv]) # tg doesn't support multi-process so build serially - else: - print("USB GPU not detected... skipping") diff --git a/selfdrive/modeld/compile_warp.py b/selfdrive/modeld/compile_warp.py index 75cc65f84c..47511f2a2b 100755 --- a/selfdrive/modeld/compile_warp.py +++ b/selfdrive/modeld/compile_warp.py @@ -94,11 +94,11 @@ def make_frame_prepare(cam_w, cam_h, model_w, model_h): def make_update_img_input(frame_prepare, model_w, model_h): - def update_img_input_tinygrad(tensor, frame, M_inv): + def update_img_input_tinygrad(frame_buffer, frame, M_inv): M_inv = M_inv.to(Device.DEFAULT) new_img = frame_prepare(frame, M_inv) - full_buffer = tensor[6:].cat(new_img, dim=0).contiguous() - return full_buffer, Tensor.cat(full_buffer[:6], full_buffer[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2) + frame_buffer.assign(frame_buffer[6:].cat(new_img, dim=0).contiguous()) + return Tensor.cat(frame_buffer[:6], frame_buffer[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2) return update_img_input_tinygrad @@ -107,9 +107,9 @@ def make_update_both_imgs(frame_prepare, model_w, model_h): def update_both_imgs_tinygrad(calib_img_buffer, new_img, M_inv, calib_big_img_buffer, new_big_img, M_inv_big): - calib_img_buffer, calib_img_pair = update_img(calib_img_buffer, new_img, M_inv) - calib_big_img_buffer, calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big) - return calib_img_buffer, calib_img_pair, calib_big_img_buffer, calib_big_img_pair + calib_img_pair = update_img(calib_img_buffer, new_img, M_inv) + calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big) + return calib_img_pair, calib_big_img_pair return update_both_imgs_tinygrad @@ -136,29 +136,18 @@ def compile_modeld_warp(cam_w, cam_h): full_buffer = Tensor.zeros(IMG_BUFFER_SHAPE, dtype='uint8').contiguous().realize() big_full_buffer = Tensor.zeros(IMG_BUFFER_SHAPE, dtype='uint8').contiguous().realize() - full_buffer_np = np.zeros(IMG_BUFFER_SHAPE, dtype=np.uint8) - big_full_buffer_np = np.zeros(IMG_BUFFER_SHAPE, dtype=np.uint8) - for i in range(10): - new_frame_np = (32 * np.random.randn(yuv_size).astype(np.float32) + 128).clip(0, 255).astype(np.uint8) img_inputs = [full_buffer, - Tensor.from_blob(new_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(), + Tensor(np.random.randint(0, 256, yuv_size, dtype=np.uint8)).realize(), Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')] - new_big_frame_np = (32 * np.random.randn(yuv_size).astype(np.float32) + 128).clip(0, 255).astype(np.uint8) big_img_inputs = [big_full_buffer, - Tensor.from_blob(new_big_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(), + Tensor(np.random.randint(0, 256, yuv_size, dtype=np.uint8)).realize(), Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')] inputs = img_inputs + big_img_inputs Device.default.synchronize() - inputs_np = [x.numpy() for x in inputs] - inputs_np[0] = full_buffer_np - inputs_np[3] = big_full_buffer_np - st = time.perf_counter() - out = update_img_jit(*inputs) - full_buffer = out[0].contiguous().realize().clone() - big_full_buffer = out[2].contiguous().realize().clone() + _ = update_img_jit(*inputs) mt = time.perf_counter() Device.default.synchronize() et = time.perf_counter() @@ -183,7 +172,7 @@ def compile_dm_warp(cam_w, cam_h): warp_dm_jit = TinyJit(warp_dm, prune=True) for i in range(10): - inputs = [Tensor.from_blob((32 * Tensor.randn(yuv_size,) + 128).cast(dtype='uint8').realize().numpy().ctypes.data, (yuv_size,), dtype='uint8'), + inputs = [Tensor(np.random.randint(0, 256, yuv_size, dtype=np.uint8)).realize(), Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')] Device.default.synchronize() st = time.perf_counter() diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 28190db3e6..efd8214b9f 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -80,7 +80,7 @@ def parse_model_output(model_output): face_descs = model_output[f'face_descs_{ds_suffix}'] parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6] parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:]) - for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']: + for key in ['face_prob', 'eyes_visible_prob', 'eyes_closed_prob', 'using_phone_prob']: parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}']) return parsed @@ -90,11 +90,8 @@ def fill_driver_data(msg, model_output, ds_suffix): msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist() msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist() msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item() - msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item() - msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item() - msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item() - msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item() - msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item() + msg.eyesVisibleProb = model_output[f'eyes_visible_prob_{ds_suffix}'][0, 0].item() + msg.eyesClosedProb = model_output[f'eyes_closed_prob_{ds_suffix}'][0, 0].item() msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item() def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float): diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index bff59366d6..82e750cf8b 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -34,11 +34,13 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') -VISION_PKL_PATH = Path(__file__).parent / 'models/driving_vision_tinygrad.pkl' -POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl' -VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl' -POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl' MODELS_DIR = Path(__file__).parent / 'models' +VISION_PKL_PATH = MODELS_DIR / 'driving_vision_tinygrad.pkl' +VISION_METADATA_PATH = MODELS_DIR / 'driving_vision_metadata.pkl' +ON_POLICY_PKL_PATH = MODELS_DIR / 'driving_on_policy_tinygrad.pkl' +ON_POLICY_METADATA_PATH = MODELS_DIR / 'driving_on_policy_metadata.pkl' +OFF_POLICY_PKL_PATH = MODELS_DIR / 'driving_off_policy_tinygrad.pkl' +OFF_POLICY_METADATA_PATH = MODELS_DIR / 'driving_off_policy_metadata.pkl' LAT_SMOOTH_SECONDS = 0.0 LONG_SMOOTH_SECONDS = 0.3 @@ -151,7 +153,13 @@ class ModelState: self.vision_output_slices = vision_metadata['output_slices'] vision_output_size = vision_metadata['output_shapes']['outputs'][1] - with open(POLICY_METADATA_PATH, 'rb') as f: + with open(OFF_POLICY_METADATA_PATH, 'rb') as f: + off_policy_metadata = pickle.load(f) + self.off_policy_input_shapes = off_policy_metadata['input_shapes'] + self.off_policy_output_slices = off_policy_metadata['output_slices'] + off_policy_output_size = off_policy_metadata['output_shapes']['outputs'][1] + + with open(ON_POLICY_METADATA_PATH, 'rb') as f: policy_metadata = pickle.load(f) self.policy_input_shapes = policy_metadata['input_shapes'] self.policy_output_slices = policy_metadata['output_slices'] @@ -175,11 +183,13 @@ class ModelState: self.vision_output = np.zeros(vision_output_size, dtype=np.float32) self.policy_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} self.policy_output = np.zeros(policy_output_size, dtype=np.float32) + self.off_policy_output = np.zeros(off_policy_output_size, dtype=np.float32) self.parser = Parser() self.frame_buf_params : dict[str, tuple[int, int, int, int]] = {} self.update_imgs = None self.vision_run = pickle.loads(read_file_chunked(str(VISION_PKL_PATH))) - self.policy_run = pickle.loads(read_file_chunked(str(POLICY_PKL_PATH))) + self.policy_run = pickle.loads(read_file_chunked(str(ON_POLICY_PKL_PATH))) + self.off_policy_run = pickle.loads(read_file_chunked(str(OFF_POLICY_PKL_PATH))) def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]: parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()} @@ -212,8 +222,7 @@ class ModelState: out = self.update_imgs(self.img_queues['img'], self.full_frames['img'], self.transforms['img'], self.img_queues['big_img'], self.full_frames['big_img'], self.transforms['big_img']) - self.img_queues['img'], self.img_queues['big_img'] = out[0].realize(), out[2].realize() - vision_inputs = {'img': out[1], 'big_img': out[3]} + vision_inputs = {'img': out[0], 'big_img': out[1]} if prepare_only: return None @@ -228,9 +237,17 @@ class ModelState: self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy().flatten() policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices)) - combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict} + + self.off_policy_output = self.off_policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy() + off_policy_outputs_dict = self.parser.parse_off_policy_outputs(self.slice_outputs(self.off_policy_output, self.off_policy_output_slices)) + off_policy_outputs_dict.pop('plan') + + + combined_outputs_dict = {**vision_outputs_dict, **off_policy_outputs_dict, **policy_outputs_dict} + if 'planplus' in combined_outputs_dict and 'plan' in combined_outputs_dict: + combined_outputs_dict['plan'] = combined_outputs_dict['plan'] + combined_outputs_dict['planplus'] if SEND_RAW_PRED: - combined_outputs_dict['raw_pred'] = np.concatenate([self.vision_output.copy(), self.policy_output.copy()]) + combined_outputs_dict['raw_pred'] = np.concatenate([self.vision_output.copy(), self.policy_output.copy(), self.off_policy_output.copy()]) return combined_outputs_dict @@ -388,7 +405,9 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) + frame_delay = DT_MDL # compensate for time passed since the frame was captured: current_time - timestamp_eof is 50ms on average + action_delay = DT_MDL / 2 # middle of the interval between model output (current state) and next frame (expected state) + action = get_action_from_model(model_output, prev_action, lat_delay + frame_delay + action_delay, long_delay + frame_delay + action_delay, v_ego) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, diff --git a/selfdrive/modeld/models/big_driving_policy.onnx b/selfdrive/modeld/models/big_driving_policy.onnx deleted file mode 120000 index e1b653a14a..0000000000 --- a/selfdrive/modeld/models/big_driving_policy.onnx +++ /dev/null @@ -1 +0,0 @@ -driving_policy.onnx \ No newline at end of file diff --git a/selfdrive/modeld/models/big_driving_vision.onnx b/selfdrive/modeld/models/big_driving_vision.onnx deleted file mode 120000 index 28ee71dd74..0000000000 --- a/selfdrive/modeld/models/big_driving_vision.onnx +++ /dev/null @@ -1 +0,0 @@ -driving_vision.onnx \ No newline at end of file diff --git a/selfdrive/modeld/models/dmonitoring_model.onnx b/selfdrive/modeld/models/dmonitoring_model.onnx index dc621bed03..628f385796 100644 --- a/selfdrive/modeld/models/dmonitoring_model.onnx +++ b/selfdrive/modeld/models/dmonitoring_model.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:7aff7ff1dc08bbaf562a8f77380ab5e5914f8557dba2f760d87e4d953f5604b0 -size 7307246 +oid sha256:2fd471febb6e973313ac0d0c6755f6410c1937ba92230b58a433761e8c883072 +size 7364290 diff --git a/selfdrive/modeld/models/driving_off_policy.onnx b/selfdrive/modeld/models/driving_off_policy.onnx new file mode 100644 index 0000000000..5b0effc100 --- /dev/null +++ b/selfdrive/modeld/models/driving_off_policy.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e53f4e0527766082ba7bde38e275def0fe3c14f6c59ae2854439e239884d3ecc +size 13393365 diff --git a/selfdrive/modeld/models/driving_on_policy.onnx b/selfdrive/modeld/models/driving_on_policy.onnx new file mode 100644 index 0000000000..bfe10ca185 --- /dev/null +++ b/selfdrive/modeld/models/driving_on_policy.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ea89c50da3a16e710da292f97c81b083a982cfdee5c28eca0d37ed2fb99af6c5 +size 13022642 diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx deleted file mode 100644 index 7c71bc9471..0000000000 --- a/selfdrive/modeld/models/driving_policy.onnx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:853c6634746ff439a848349d00e4d5581cd941f13f7c1862c31b72a31cc24858 -size 14061595 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index afd617667c..335e9dbcf8 100644 --- a/selfdrive/modeld/models/driving_vision.onnx +++ b/selfdrive/modeld/models/driving_vision.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:940e9006a25f27f0b6e85da798e6a8fd1f6dd492dd7d0b9ff1a9436460f46129 -size 46887794 +oid sha256:6263aa3fbb44cde6c68a34cdb7cd8c389789dbc02b15c1911afdac4e018281ae +size 23267151 diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index 5c11e8ca18..802f0ad859 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -96,11 +96,17 @@ class Parser: self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,)) self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) + self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH)) + self.parse_binary_crossentropy('meta', outs) + return outs + + def parse_off_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: + plan_mhp = self.is_mhp(outs, 'plan', ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH) + plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0) + self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)) self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) self.parse_binary_crossentropy('lane_lines_prob', outs) - self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH)) - self.parse_binary_crossentropy('meta', outs) self.parse_binary_crossentropy('lead_prob', outs) lead_mhp = self.is_mhp(outs, 'lead', ModelConstants.LEAD_MHP_SELECTION * ModelConstants.LEAD_TRAJ_LEN * ModelConstants.LEAD_WIDTH) lead_in_N, lead_out_N = (ModelConstants.LEAD_MHP_N, ModelConstants.LEAD_MHP_SELECTION) if lead_mhp else (0, 0) @@ -120,5 +126,6 @@ class Parser: def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: outs = self.parse_vision_outputs(outs) + outs = self.parse_off_policy_outputs(outs) outs = self.parse_policy_outputs(outs) return outs diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 0b54504b64..90cc565802 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -1,4 +1,4 @@ -from math import atan2 +from math import atan2, radians import numpy as np from cereal import car, log @@ -32,9 +32,8 @@ class DRIVER_MONITOR_SETTINGS: self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. self._FACE_THRESHOLD = 0.7 - self._EYE_THRESHOLD = 0.65 - self._SG_THRESHOLD = 0.9 - self._BLINK_THRESHOLD = 0.865 + self._EYE_THRESHOLD = 0.5 + self._BLINK_THRESHOLD = 0.5 self._PHONE_THRESH = 0.5 self._POSE_PITCH_THRESHOLD = 0.3133 @@ -43,6 +42,9 @@ class DRIVER_MONITOR_SETTINGS: self._POSE_YAW_THRESHOLD = 0.4020 self._POSE_YAW_THRESHOLD_SLACK = 0.5042 self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD + self._POSE_YAW_MIN_STEER_DEG = 30 + self._POSE_YAW_STEER_FACTOR = 0.15 + self._POSE_YAW_STEER_MAX_OFFSET = 0.3927 self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned self._PITCH_NATURAL_THRESHOLD = 0.449 self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned @@ -59,7 +61,6 @@ class DRIVER_MONITOR_SETTINGS: self._POSESTD_THRESHOLD = 0.3 self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz - self._ALWAYS_ON_ALERT_MIN_SPEED = 11 self._POSE_CALIB_MIN_SPEED = 13 # 30 mph self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative @@ -101,6 +102,7 @@ class DriverPose: self.low_std = True self.cfactor_pitch = 1. self.cfactor_yaw = 1. + self.steer_yaw_offset = 0. class DriverProb: def __init__(self, raw_priors, max_trackable): @@ -108,11 +110,6 @@ class DriverProb: self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable) self.prob_calibrated = False -class DriverBlink: - def __init__(self): - self.left = 0. - self.right = 0. - # model output refers to center of undistorted+leveled image EFL = 598.0 # focal length in K @@ -147,7 +144,7 @@ class DriverMonitoring: wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2) self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT) self.pose = DriverPose(settings=self.settings) - self.blink = DriverBlink() + self.blink_prob = 0. self.phone_prob = 0. self.always_on = always_on @@ -238,7 +235,11 @@ class DriverMonitoring: yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(), self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET) pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit - yaw_error = abs(yaw_error) + + if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional + yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.) + else: + yaw_error = abs(yaw_error) pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw @@ -246,7 +247,7 @@ class DriverMonitoring: if pitch_error > pitch_threshold or yaw_error > yaw_threshold: distracted_types.append(DistractedType.DISTRACTED_POSE) - if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD: + if self.blink_prob > self.settings._BLINK_THRESHOLD: distracted_types.append(DistractedType.DISTRACTED_BLINK) if self.phone_prob > self.settings._PHONE_THRESH: @@ -254,7 +255,7 @@ class DriverMonitoring: return distracted_types - def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False): + def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False, steering_angle_deg=0.): rhd_pred = driver_state.wheelOnRightProb # calibrates only when there's movement and either face detected if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or @@ -277,17 +278,17 @@ class DriverMonitoring: self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) + steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.) + self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR if self.wheel_on_right: self.pose.yaw *= -1 + self.pose.steer_yaw_offset *= -1 self.wheel_on_right_last = self.wheel_on_right self.pose.pitch_std = driver_data.faceOrientationStd[0] self.pose.yaw_std = driver_data.faceOrientationStd[1] model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD - self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \ - * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ - * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) + self.blink_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_THRESHOLD) self.phone_prob = driver_data.phoneProb self.distracted_types = self._get_distracted_types() @@ -360,19 +361,19 @@ class DriverMonitoring: if self.awareness > self.threshold_prompt: return + _reaching_pre = self.awareness - self.step_change <= self.threshold_pre _reaching_audible = self.awareness - self.step_change <= self.threshold_prompt _reaching_terminal = self.awareness - self.step_change <= 0 - standstill_orange_exemption = standstill and _reaching_audible + standstill_exemption = standstill and _reaching_pre always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal - always_on_lowspeed_exemption = always_on_valid and not op_engaged and car_speed < self.settings._ALWAYS_ON_ALERT_MIN_SPEED certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected if certainly_distracted or maybe_distracted: - # should always be counting if distracted unless at standstill (lowspeed for always-on) and reaching orange + # should always be counting if distracted unless at standstill and reaching green # also will not be reaching 0 if DM is active when not engaged - if not (standstill_orange_exemption or always_on_red_exemption or (always_on_lowspeed_exemption and _reaching_audible)): + if not (standstill_exemption or always_on_red_exemption): self.awareness = max(self.awareness - self.step_change, -0.1) alert = None @@ -385,7 +386,7 @@ class DriverMonitoring: elif self.awareness <= self.threshold_prompt: # prompt orange alert alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive - elif self.awareness <= self.threshold_pre and not always_on_lowspeed_exemption: + elif self.awareness <= self.threshold_pre: # pre green alert alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive @@ -451,6 +452,7 @@ class DriverMonitoring: op_engaged=enabled, standstill=standstill, demo_mode=demo, + steering_angle_deg=sm['carState'].steeringAngleDeg, ) # Update distraction events diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 6ea9b80283..733ea85bc0 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -19,10 +19,8 @@ def make_msg(face_detected, distracted=False, model_uncertain=False): ds.leftDriverData.faceOrientation = [0., 0., 0.] ds.leftDriverData.facePosition = [0., 0.] ds.leftDriverData.faceProb = 1. * face_detected - ds.leftDriverData.leftEyeProb = 1. - ds.leftDriverData.rightEyeProb = 1. - ds.leftDriverData.leftBlinkProb = 1. * distracted - ds.leftDriverData.rightBlinkProb = 1. * distracted + ds.leftDriverData.eyesVisibleProb = 1. + ds.leftDriverData.eyesClosedProb = 1. * distracted ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] # TODO: test both separately when e2e is used @@ -186,10 +184,10 @@ class TestMonitoring: standstill_vector = always_true[:] standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) - assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0] == \ - EventName.preDriverDistracted - assert events[int((_redlight_time-0.1)/DT_DMON)].names[0] == EventName.preDriverDistracted - assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert len(events[int((_redlight_time-0.1)/DT_DMON)]) == 0 + _pre_to_prompt = d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL - d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.preDriverDistracted + assert events[int((_redlight_time+_pre_to_prompt+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted # engaged, model is somehow uncertain and driver is distracted # - should fall back to wheel touch after uncertain alert diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index a6ccaa1047..eb7cdbe34a 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -76,7 +76,7 @@ def generate_report(proposed, master, tmp, commit): (lambda x: get_idx_if_non_empty(x.wheelOnRightProb), "wheelOnRightProb"), (lambda x: get_idx_if_non_empty(x.leftDriverData.faceProb), "leftDriverData.faceProb"), (lambda x: get_idx_if_non_empty(x.leftDriverData.faceOrientation, 0), "leftDriverData.faceOrientation0"), - (lambda x: get_idx_if_non_empty(x.leftDriverData.leftBlinkProb), "leftDriverData.leftBlinkProb"), + (lambda x: get_idx_if_non_empty(x.leftDriverData.eyesClosedProb), "leftDriverData.eyesClosedProb"), (lambda x: get_idx_if_non_empty(x.leftDriverData.phoneProb), "leftDriverData.phoneProb"), (lambda x: get_idx_if_non_empty(x.rightDriverData.faceProb), "rightDriverData.faceProb"), ], "driverStateV2") diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py index e8321b099c..e8b8abb7f5 100644 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ b/selfdrive/ui/mici/onroad/driver_camera_dialog.py @@ -39,8 +39,6 @@ class BaseDriverCameraDialog(Widget): self._eye_fill_texture = None self._eye_orange_texture = None self._eye_size = 74 - self._glasses_texture = None - self._glasses_size = 171 self._load_eye_textures() @@ -154,8 +152,6 @@ class BaseDriverCameraDialog(Widget): self._eye_fill_texture = gui_app.texture("icons_mici/onroad/eye_fill.png", self._eye_size, self._eye_size) if self._eye_orange_texture is None: self._eye_orange_texture = gui_app.texture("icons_mici/onroad/eye_orange.png", self._eye_size, self._eye_size) - if self._glasses_texture is None: - self._glasses_texture = gui_app.texture("icons_mici/onroad/glasses.png", self._glasses_size, self._glasses_size) def _draw_face_detection(self, rect: rl.Rectangle): dm_state = ui_state.sm["driverMonitoringState"] @@ -202,31 +198,21 @@ class BaseDriverCameraDialog(Widget): eye_offset_x = 10 eye_offset_y = 10 eye_spacing = self._eye_size + 15 + eyes_prob = driver_data.eyesVisibleProb left_eye_x = rect.x + eye_offset_x left_eye_y = rect.y + eye_offset_y - left_eye_prob = driver_data.leftEyeProb right_eye_x = rect.x + eye_offset_x + eye_spacing right_eye_y = rect.y + eye_offset_y - right_eye_prob = driver_data.rightEyeProb # Draw eyes with opacity based on probability - for eye_x, eye_y, eye_prob in [(left_eye_x, left_eye_y, left_eye_prob), (right_eye_x, right_eye_y, right_eye_prob)]: - fill_opacity = eye_prob - orange_opacity = 1.0 - eye_prob - + fill_opacity = eyes_prob + orange_opacity = 1.0 - eyes_prob + for eye_x, eye_y in [(left_eye_x, left_eye_y), (right_eye_x, right_eye_y)]: rl.draw_texture_v(self._eye_orange_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * orange_opacity))) rl.draw_texture_v(self._eye_fill_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * fill_opacity))) - # Draw sunglasses indicator based on sunglasses probability - # Position glasses centered between the two eyes at top left - glasses_x = rect.x + eye_offset_x - 4 - glasses_y = rect.y - glasses_pos = rl.Vector2(glasses_x, glasses_y) - glasses_prob = driver_data.sunglassesProb - rl.draw_texture_v(self._glasses_texture, glasses_pos, rl.Color(70, 80, 161, int(255 * glasses_prob))) - class DriverCameraDialog(NavWidget, BaseDriverCameraDialog): def __init__(self): diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 35d04fe702..56d83992ff 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -120,7 +120,7 @@ class HudRenderer(Widget): self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50) self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50) - self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 44, 44) + self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 9, 44) self._wheel_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps) self._wheel_y_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) diff --git a/tinygrad_repo b/tinygrad_repo index 2f55005ad9..1aa04eab08 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 2f55005ad93c777cca69b20dddc28c7f02f0eb01 +Subproject commit 1aa04eab086d9c22855cfe50f746235200d28867 diff --git a/tools/jotpluggler/plot.cc b/tools/jotpluggler/plot.cc index a3c68ddcef..5d9d644808 100644 --- a/tools/jotpluggler/plot.cc +++ b/tools/jotpluggler/plot.cc @@ -6,7 +6,6 @@ #include #include #include -#include constexpr double PLOT_Y_PAD_FRACTION = 0.4; @@ -74,10 +73,6 @@ struct StateBlock { std::string label; }; -struct PaneEnumContext { - std::vector enums; -}; - struct PaneValueFormatContext { SeriesFormat format; bool valid = false; @@ -106,38 +101,6 @@ bool curves_are_bool_like(const std::vector &prepared_curves) { return true; } -bool curve_is_state_like(const PreparedCurve &curve) { - if (!curve.display_info.integer_like || curve.xs.size() < 2 || curve.xs.size() != curve.ys.size()) { - return false; - } - if (curve.enum_info != nullptr) { - return true; - } - std::unordered_set distinct_values; - for (double value : curve.ys) { - if (!std::isfinite(value)) { - continue; - } - distinct_values.insert(static_cast(std::llround(value))); - if (distinct_values.size() > 12) { - return false; - } - } - return !distinct_values.empty(); -} - -bool curves_use_state_blocks(const std::vector &prepared_curves) { - if (prepared_curves.empty()) { - return false; - } - for (const PreparedCurve &curve : prepared_curves) { - if (!curve_is_state_like(curve)) { - return false; - } - } - return true; -} - ImU32 state_block_color(int value, float alpha = 1.0f) { static constexpr std::array, 8> kPalette = {{ {{111, 143, 175}}, @@ -307,36 +270,6 @@ std::optional app_sample_xy_value_at_time(const std::vector &xs, return y0 + (y1 - y0) * alpha; } -int format_enum_axis_tick(double value, char *buf, int size, void *user_data) { - const auto *ctx = static_cast(user_data); - const int idx = static_cast(std::llround(value)); - if (ctx != nullptr && idx >= 0 && std::abs(value - static_cast(idx)) < 0.01) { - std::vector names; - names.reserve(ctx->enums.size()); - for (const EnumInfo *info : ctx->enums) { - if (info == nullptr || static_cast(idx) >= info->names.size()) { - continue; - } - const std::string &name = info->names[static_cast(idx)]; - if (name.empty()) continue; - if (std::find(names.begin(), names.end(), std::string_view(name)) == names.end()) { - names.emplace_back(name); - } - } - if (!names.empty()) { - std::string joined; - for (size_t i = 0; i < names.size(); ++i) { - if (i != 0) { - joined += ", "; - } - joined += names[i]; - } - return std::snprintf(buf, size, "%d (%s)", idx, joined.c_str()); - } - } - return std::snprintf(buf, size, "%.6g", value); -} - int format_numeric_axis_tick(double value, char *buf, int size, void *user_data) { const auto *ctx = static_cast(user_data); if (ctx == nullptr || !ctx->valid) { @@ -831,23 +764,16 @@ void draw_plot(const AppSession &session, Pane *pane, UiState *state) { } const PlotBounds bounds = compute_plot_bounds(*pane, prepared_curves, *state); - PaneEnumContext enum_context; PaneValueFormatContext pane_value_format; - const bool state_block_mode = curves_use_state_blocks(prepared_curves); - bool all_enum_curves = !prepared_curves.empty(); + bool state_block_mode = !prepared_curves.empty(); size_t max_legend_label_width = 0; for (const PreparedCurve &curve : prepared_curves) { max_legend_label_width = std::max(max_legend_label_width, curve.label.size()); - if (curve.enum_info != nullptr) { - enum_context.enums.push_back(curve.enum_info); - } else { - all_enum_curves = false; + if (curve.enum_info == nullptr) { + state_block_mode = false; merge_pane_value_format(&pane_value_format, curve.display_info); } } - if (prepared_curves.empty()) { - all_enum_curves = false; - } const int supported_count = static_cast(prepared_curves.size()); const ImVec2 plot_size = ImGui::GetContentRegionAvail(); const bool has_cursor_time = state->has_tracker_time; @@ -895,8 +821,6 @@ void draw_plot(const AppSession &session, Pane *pane, UiState *state) { ImPlot::SetupAxisFormat(ImAxis_X1, "%.1f"); if (state_block_mode) { ImPlot::SetupAxisLimits(ImAxis_Y1, 0.0, 1.0, ImPlotCond_Always); - } else if (all_enum_curves && !enum_context.enums.empty()) { - ImPlot::SetupAxisFormat(ImAxis_Y1, format_enum_axis_tick, &enum_context); } else if (pane_value_format.valid) { ImPlot::SetupAxisFormat(ImAxis_Y1, format_numeric_axis_tick, &pane_value_format); } else { diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py index 7c95e9cad4..039dd4f235 100644 --- a/tools/replay/lib/ui_helpers.py +++ b/tools/replay/lib/ui_helpers.py @@ -5,6 +5,7 @@ import numpy as np import pyray as rl from matplotlib.backends.backend_agg import FigureCanvasAgg +from matplotlib.offsetbox import AnchoredOffsetbox, HPacker, TextArea from openpilot.common.transformations.camera import get_view_frame_from_calib_frame from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA @@ -94,6 +95,7 @@ def draw_path(path, color, img, calibration, top_down, lid_color=None, z_off=0): def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles): color_palette = {"r": (1, 0, 0), "g": (0, 1, 0), "b": (0, 0, 1), "k": (0, 0, 0), "y": (1, 1, 0), "p": (0, 1, 1), "m": (1, 0, 1)} + label_palette = {**color_palette, "b": (43/255, 114/255, 1.0)} dpi = 90 fig = plt.figure(figsize=(575 / dpi, 600 / dpi), dpi=dpi) @@ -116,10 +118,18 @@ def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_co plots.append(plot) idxs.append(name_to_arr_idx[item]) plot_select.append(i) - axs[i].set_title(", ".join(f"{nm} ({cl})" for (nm, cl) in zip(pl_list, plot_colors[i], strict=False)), fontsize=10) + # Build colored title: each label colored to match its plot line + title_texts = [] + for j2, (nm, cl) in enumerate(zip(pl_list, plot_colors[i], strict=False)): + if j2 > 0: + title_texts.append(TextArea(", ", textprops=dict(color="white", fontsize=10))) + title_texts.append(TextArea(nm, textprops=dict(color=label_palette[cl], fontsize=10))) + packed = HPacker(children=title_texts, pad=0, sep=0) + ab = AnchoredOffsetbox(loc='lower center', child=packed, bbox_to_anchor=(0.5, 1.0), + bbox_transform=axs[i].transAxes, frameon=False, pad=0) + axs[i].add_artist(ab) axs[i].tick_params(axis="x", colors="white") axs[i].tick_params(axis="y", colors="white") - axs[i].title.set_color("white") if i < len(plot_ylims) - 1: axs[i].set_xticks([]) diff --git a/tools/replay/ui.py b/tools/replay/ui.py index 7fe2f405a1..405c79f425 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -3,7 +3,6 @@ import argparse import os import sys -import cv2 import numpy as np import pyray as rl @@ -22,7 +21,8 @@ from openpilot.tools.replay.lib.ui_helpers import ( plot_lead, plot_model, ) -from msgq.visionipc import VisionIpcClient, VisionStreamType +from msgq.visionipc import VisionStreamType +from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView os.environ['BASEDIR'] = BASEDIR @@ -30,8 +30,6 @@ ANGLE_SCALE = 5.0 def ui_thread(addr): - cv2.setNumThreads(1) - # Get monitor info before creating window rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT) rl.init_window(1, 1, "") @@ -59,14 +57,15 @@ def ui_thread(addr): font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf") font = rl.load_font_ex(font_path, 32, None, 0) - # Create textures for camera and top-down view - camera_image = rl.gen_image_color(640, 480, rl.BLACK) - camera_texture = rl.load_texture_from_image(camera_image) - rl.unload_image(camera_image) + camera_view = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD) + + # Overlay texture for model/lane line drawing + overlay_img = np.zeros((480, 640, 4), dtype='uint8') + overlay_image = rl.gen_image_color(640, 480, rl.BLANK) + overlay_texture = rl.load_texture_from_image(overlay_image) + rl.unload_image(overlay_image) # lid_overlay array is (lidar_x, lidar_y) = (384, 960) - # pygame treats first axis as width, so texture is 384 wide x 960 tall - # For raylib, we need to transpose to get (height, width) = (960, 384) for the RGBA array top_down_image = rl.gen_image_color(UP.lidar_x, UP.lidar_y, rl.BLACK) top_down_texture = rl.load_texture_from_image(top_down_image) rl.unload_image(top_down_image) @@ -89,7 +88,6 @@ def ui_thread(addr): ) img = np.zeros((480, 640, 3), dtype='uint8') - imgff = None num_px = 0 calibration = None @@ -116,7 +114,7 @@ def ui_thread(addr): plot_arr = np.zeros((100, len(name_to_arr_idx.values()))) plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])] - plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0.0, 75.0), (-3.0, 2.0)] + plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0.0, 75.0), (-3.5, 2.0)] plot_names = [ ["gas", "computer_gas", "user_brake", "computer_brake"], ["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"], @@ -138,20 +136,16 @@ def ui_thread(addr): palette[110] = [110, 110, 110, 255] # car_color (gray) palette[255] = [255, 255, 255, 255] # WHITE - vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True) while not rl.window_should_close(): - # ***** frame ***** - if not vipc_client.is_connected(): - vipc_client.connect(False) - rl.begin_drawing() rl.clear_background(rl.Color(64, 64, 64, 255)) - yuv_img_raw = vipc_client.recv() - if yuv_img_raw is None or not yuv_img_raw.data.any(): - rl.draw_text_ex(font, "waiting for frames", rl.Vector2(200, 200), 30, 0, rl.WHITE) - rl.end_drawing() - continue + # Render camera (NV12->RGB on GPU via shader) + if camera_view.frame: + cam_h = 640.0 * camera_view.frame.height / camera_view.frame.width + else: + cam_h = 480.0 + camera_view.render(rl.Rectangle(0, 0, 640, cam_h)) lid_overlay = lid_overlay_blank.copy() top_down = top_down_texture, lid_overlay @@ -159,19 +153,10 @@ def ui_thread(addr): sm.update(0) camera = DEVICE_CAMERAS[("tici", str(sm['roadCameraState'].sensor))] - - # Use received buffer dimensions (full HEVC can have stride != buffer_len/rows due to VENUS padding) - h, w, stride = yuv_img_raw.height, yuv_img_raw.width, yuv_img_raw.stride - nv12_size = h * 3 // 2 * stride - imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8, count=nv12_size).reshape((h * 3 // 2, stride)) - num_px = w * h - rgb = cv2.cvtColor(imgff[: h * 3 // 2, : w], cv2.COLOR_YUV2RGB_NV12) - - qcam = "QCAM" in os.environ - bb_scale = (528 if qcam else camera.fcam.width) / 640.0 calib_scale = camera.fcam.width / 640.0 - zoom_matrix = np.asarray([[bb_scale, 0.0, 0.0], [0.0, bb_scale, 0.0], [0.0, 0.0, 1.0]]) - cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP) + + if camera_view.frame: + num_px = camera_view.frame.width * camera_view.frame.height intrinsic_matrix = camera.fcam.intrinsics @@ -183,7 +168,8 @@ def ui_thread(addr): else: angle_steers_k = np.inf - plot_arr[:-1] = plot_arr[1:] + if sm.updated['carState']: + plot_arr[:-1] = plot_arr[1:] plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k @@ -198,9 +184,10 @@ def ui_thread(addr): plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo - if len(sm['longitudinalPlan'].accels): - plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].accels[0] + plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].aTarget + # Draw model overlays onto img, then blit as transparent overlay + img[:] = 0 if sm.recv_frame['modelV2']: plot_model(sm['modelV2'], img, calibration, top_down) @@ -214,11 +201,12 @@ def ui_thread(addr): rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib) calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale) - # *** blits *** - # Update camera texture from numpy array - img_rgba = cv2.cvtColor(img, cv2.COLOR_RGB2RGBA) - rl.update_texture(camera_texture, rl.ffi.cast("void *", img_rgba.ctypes.data)) - rl.draw_texture(camera_texture, 0, 0, rl.WHITE) # noqa: TID251 + # Update overlay texture (RGB img -> RGBA with non-black pixels visible) + mask = np.any(img > 0, axis=2) + overlay_img[:, :, :3] = img + overlay_img[:, :, 3] = mask * 255 + rl.update_texture(overlay_texture, rl.ffi.cast("void *", overlay_img.ctypes.data)) + rl.draw_texture(overlay_texture, 0, 0, rl.WHITE) # noqa: TID251 # display alerts rl.draw_text_ex(font, sm['selfdriveState'].alertText1, rl.Vector2(180, 150), 30, 0, rl.RED) @@ -257,9 +245,10 @@ def ui_thread(addr): rl.end_drawing() - rl.unload_texture(camera_texture) + rl.unload_texture(overlay_texture) rl.unload_texture(top_down_texture) rl.unload_font(font) + camera_view.close() rl.close_window()