mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-07 17:27:01 +08:00
Merge remote-tracking branch 'commaai/openpilot/master' into gv80-2025
# Conflicts: # opendbc_repo
This commit is contained in:
Jason Wen
Vendored
+3
@@ -52,6 +52,9 @@
|
||||
"type": "lldb",
|
||||
"request": "attach",
|
||||
"pid": "${command:pickMyProcess}",
|
||||
"sourceMap": {
|
||||
".": "${workspaceFolder}/opendbc/safety"
|
||||
},
|
||||
"initCommands": [
|
||||
"script import time; time.sleep(3)"
|
||||
]
|
||||
|
||||
+2
-1
@@ -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)
|
||||
========================
|
||||
|
||||
+12
-5
@@ -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);
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: d0c76cb17e...c59bdd3f33
+7
-2
@@ -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})", "|")
|
||||
|
||||
@@ -1,3 +0,0 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:56de402482b5987ed9a0ff3f793a1c89f857304b34fbb8a3deb5b5d4a332be1c
|
||||
size 3688
|
||||
@@ -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")
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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):
|
||||
|
||||
+30
-11
@@ -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,
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
driving_policy.onnx
|
||||
@@ -1 +0,0 @@
|
||||
driving_vision.onnx
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:7aff7ff1dc08bbaf562a8f77380ab5e5914f8557dba2f760d87e4d953f5604b0
|
||||
size 7307246
|
||||
oid sha256:2fd471febb6e973313ac0d0c6755f6410c1937ba92230b58a433761e8c883072
|
||||
size 7364290
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:e53f4e0527766082ba7bde38e275def0fe3c14f6c59ae2854439e239884d3ecc
|
||||
size 13393365
|
||||
@@ -0,0 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:ea89c50da3a16e710da292f97c81b083a982cfdee5c28eca0d37ed2fb99af6c5
|
||||
size 13022642
|
||||
@@ -1,3 +0,0 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:853c6634746ff439a848349d00e4d5581cd941f13f7c1862c31b72a31cc24858
|
||||
size 14061595
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:940e9006a25f27f0b6e85da798e6a8fd1f6dd492dd7d0b9ff1a9436460f46129
|
||||
size 46887794
|
||||
oid sha256:6263aa3fbb44cde6c68a34cdb7cd8c389789dbc02b15c1911afdac4e018281ae
|
||||
size 23267151
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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)
|
||||
|
||||
+1
-1
Submodule tinygrad_repo updated: 2f55005ad9...1aa04eab08
@@ -6,7 +6,6 @@
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <limits>
|
||||
#include <unordered_set>
|
||||
|
||||
constexpr double PLOT_Y_PAD_FRACTION = 0.4;
|
||||
|
||||
@@ -74,10 +73,6 @@ struct StateBlock {
|
||||
std::string label;
|
||||
};
|
||||
|
||||
struct PaneEnumContext {
|
||||
std::vector<const EnumInfo *> enums;
|
||||
};
|
||||
|
||||
struct PaneValueFormatContext {
|
||||
SeriesFormat format;
|
||||
bool valid = false;
|
||||
@@ -106,38 +101,6 @@ bool curves_are_bool_like(const std::vector<PreparedCurve> &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<int> distinct_values;
|
||||
for (double value : curve.ys) {
|
||||
if (!std::isfinite(value)) {
|
||||
continue;
|
||||
}
|
||||
distinct_values.insert(static_cast<int>(std::llround(value)));
|
||||
if (distinct_values.size() > 12) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return !distinct_values.empty();
|
||||
}
|
||||
|
||||
bool curves_use_state_blocks(const std::vector<PreparedCurve> &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<std::array<uint8_t, 3>, 8> kPalette = {{
|
||||
{{111, 143, 175}},
|
||||
@@ -307,36 +270,6 @@ std::optional<double> app_sample_xy_value_at_time(const std::vector<double> &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<const PaneEnumContext *>(user_data);
|
||||
const int idx = static_cast<int>(std::llround(value));
|
||||
if (ctx != nullptr && idx >= 0 && std::abs(value - static_cast<double>(idx)) < 0.01) {
|
||||
std::vector<std::string_view> names;
|
||||
names.reserve(ctx->enums.size());
|
||||
for (const EnumInfo *info : ctx->enums) {
|
||||
if (info == nullptr || static_cast<size_t>(idx) >= info->names.size()) {
|
||||
continue;
|
||||
}
|
||||
const std::string &name = info->names[static_cast<size_t>(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<const PaneValueFormatContext *>(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<int>(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 {
|
||||
|
||||
@@ -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([])
|
||||
|
||||
+32
-43
@@ -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()
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user