diff --git a/selfdrive/car/cruise_state.py b/selfdrive/car/cruise_state.py new file mode 100644 index 000000000..5355300c9 --- /dev/null +++ b/selfdrive/car/cruise_state.py @@ -0,0 +1,22 @@ +from opendbc.car import structs + + +def hyundai_openpilot_longitudinal_acc_req_feedback(CP: structs.CarParams) -> bool: + return CP.brand == 'hyundai' and CP.openpilotLongitudinalControl and not CP.pcmCruise + + +def should_cancel_stock_cruise(CP: structs.CarParams, cruise_enabled: bool, controls_enabled: bool) -> bool: + if not cruise_enabled: + return False + if not controls_enabled: + return True + return not CP.pcmCruise and not hyundai_openpilot_longitudinal_acc_req_feedback(CP) + + +def should_flag_cruise_mismatch(CP: structs.CarParams, cruise_enabled: bool, controls_enabled: bool, + effective_pcm_cruise: bool) -> bool: + if not cruise_enabled: + return False + if not controls_enabled: + return True + return not effective_pcm_cruise and not hyundai_openpilot_longitudinal_acc_req_feedback(CP) diff --git a/selfdrive/car/tests/test_car_specific.py b/selfdrive/car/tests/test_car_specific.py new file mode 100644 index 000000000..7b3eb01e1 --- /dev/null +++ b/selfdrive/car/tests/test_car_specific.py @@ -0,0 +1,41 @@ +from cereal import car + +from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise, should_flag_cruise_mismatch + + +def make_cp(brand="hyundai", op_long=True, pcm_cruise=False): + cp = car.CarParams.new_message() + cp.brand = brand + cp.openpilotLongitudinalControl = op_long + cp.pcmCruise = pcm_cruise + return cp + + +def test_hyundai_openpilot_long_does_not_cancel_active_acc_req_feedback(): + cp = make_cp() + + assert not should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=True) + assert not should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=True, effective_pcm_cruise=False) + + +def test_hyundai_openpilot_long_still_flags_cruise_when_controls_disabled(): + cp = make_cp() + + assert should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=False) + assert should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=False, effective_pcm_cruise=False) + + +def test_non_hyundai_openpilot_long_behavior_is_unchanged(): + cp = make_cp(brand="toyota") + + assert should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=True) + assert should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=True, effective_pcm_cruise=False) + + +def test_pcm_cruise_behavior_is_unchanged(): + cp = make_cp(op_long=False, pcm_cruise=True) + + assert not should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=True) + assert should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=False) + assert not should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=True, effective_pcm_cruise=True) + assert should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=False, effective_pcm_cruise=True) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index accad7d05..c49626c06 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -23,6 +23,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_bolt_2017_steer_ratio_scale, ) from openpilot.selfdrive.controls.lib.longcontrol import LongControl +from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose @@ -275,7 +276,7 @@ class Controls: CC.enabled, self.sm['starpilotCarState'].alwaysOnLateralEnabled, ) - cancel_requested = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise) + cancel_requested = should_cancel_stock_cruise(self.CP, CS.cruiseState.enabled, CC.enabled) CC.cruiseControl.cancel = cancel_requested and not pacifica_hybrid_aol legacy_resume_hack = False diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 4c1412ef7..55019c2d4 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -484,9 +484,9 @@ IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04 IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60 IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.88 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 9.3 -IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 0.9 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.96 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.0 +IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.4 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 0ce61cfe2..450038a24 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -476,6 +476,8 @@ class TestLatControl: assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 6.0) assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 6.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 9.0) assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 9.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 20.0) + assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 6.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0) + assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 20.0) def test_ioniq_6_output_taper_curve(self): assert get_ioniq_6_output_taper_scale(0.0, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.0, 0.0, 8.0) <= 1.0 diff --git a/selfdrive/modeld/compile_modeld.py b/selfdrive/modeld/compile_modeld.py index 5dec1084a..88e29967c 100644 --- a/selfdrive/modeld/compile_modeld.py +++ b/selfdrive/modeld/compile_modeld.py @@ -4,114 +4,89 @@ import atexit import os import pickle import time -from collections import defaultdict, namedtuple from functools import partial +from collections import namedtuple import numpy as np - def _patch_tinygrad_fetch_fw(): import hashlib import pathlib - import zstandard from tinygrad import helpers - - original_fetch_fw = getattr(helpers, "fetch_fw", None) - if original_fetch_fw is None: + _orig = getattr(helpers, "fetch_fw", None) + if _orig is None: return - def fetch_fw(path, name, sha256): - firmware_path = pathlib.Path(f"/lib/firmware/{path}/{name}.zst") - if firmware_path.is_file(): - blob = zstandard.ZstdDecompressor().stream_reader(firmware_path.read_bytes()).read() + p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst") + if p.is_file(): + blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read() if hashlib.sha256(blob).hexdigest() == sha256: return blob - return original_fetch_fw(path, name, sha256) - + return _orig(path, name, sha256) helpers.fetch_fw = fetch_fw - - _patch_tinygrad_fetch_fw() +from tinygrad.tensor import Tensor +from tinygrad.helpers import Context from tinygrad.device import Device from tinygrad.engine.jit import TinyJit -from tinygrad.helpers import Context -from tinygrad.tensor import Tensor -NV12Frame = namedtuple("NV12Frame", ["width", "height", "stride", "y_height", "uv_height", "size"]) -WARP_INPUTS = ["img_q", "big_img_q", "tfm", "big_tfm"] -POLICY_INPUTS = ["feat_q", "desire_q", "desire", "traffic_convention", "action_t"] +NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size']) +WARP_INPUTS = ['img_q', 'big_img_q', 'tfm', 'big_tfm'] +POLICY_INPUTS = ['feat_q', 'desire_q', 'desire', 'traffic_convention', 'action_t'] -WARP_DEV = os.getenv("WARP_DEV") +UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32) +UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX) + +WARP_DEV = os.getenv('WARP_DEV') def make_random_images(keys, shape, device=None): - return {key: Tensor.randint(shape, low=0, high=256, dtype="uint8", device=device).realize() for key in keys} + return {k: Tensor.randint(shape, low=0, high=256, dtype='uint8', device=device).realize() for k in keys} -class _BlobTensorInputs(dict): - _backing_arrays: dict[str, np.ndarray] +def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None): + w_dst, h_dst = dst_shape + h_src, w_src = src_shape + x = Tensor.arange(w_dst, device=WARP_DEV).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1) + y = Tensor.arange(h_dst, device=WARP_DEV).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1) -def make_random_blob_images(keys, shape, device=None): - blob_shape = shape if isinstance(shape, tuple) else (shape,) - backing_arrays = { - key: np.random.randint(0, 256, size=blob_shape, dtype=np.uint8) - for key in keys - } - inputs = _BlobTensorInputs({ - key: Tensor.from_blob(array.ctypes.data, array.shape, dtype="uint8", device=device).realize() - for key, array in backing_arrays.items() - }) - # Keep the numpy storage alive for the duration of the JIT capture/replay call. - inputs._backing_arrays = backing_arrays - return inputs - - -def warp_perspective_tinygrad(src_flat, matrix_inverse, dst_shape, src_shape, stride_pad, border_fill_val=None): - width_dst, height_dst = dst_shape - height_src, width_src = src_shape - - x = Tensor.arange(width_dst, device=WARP_DEV).reshape(1, width_dst).expand(height_dst, width_dst).reshape(-1) - y = Tensor.arange(height_dst, device=WARP_DEV).reshape(height_dst, 1).expand(height_dst, width_dst).reshape(-1) - - # Inline 3x3 matmul as elementwise to avoid reduce ops and enable fusion with gather. - src_x = matrix_inverse[0, 0] * x + matrix_inverse[0, 1] * y + matrix_inverse[0, 2] - src_y = matrix_inverse[1, 0] * x + matrix_inverse[1, 1] * y + matrix_inverse[1, 2] - src_w = matrix_inverse[2, 0] * x + matrix_inverse[2, 1] * y + matrix_inverse[2, 2] + # inline 3x3 matmul as elementwise to avoid reduce op (enables fusion with gather) + src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2] + src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2] + src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2] src_x = src_x / src_w src_y = src_y / src_w x_round = Tensor.round(src_x) y_round = Tensor.round(src_y) - x_nn_clipped = x_round.clip(0, width_src - 1).cast("int") - y_nn_clipped = y_round.clip(0, height_src - 1).cast("int") - idx = y_nn_clipped * (width_src + stride_pad) + x_nn_clipped + x_nn_clipped = x_round.clip(0, w_src - 1).cast('int') + y_nn_clipped = y_round.clip(0, h_src - 1).cast('int') + idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped sampled = src_flat[idx] if border_fill_val is None: return sampled - in_bounds = ((x_round >= 0) & (x_round <= width_src - 1) & - (y_round >= 0) & (y_round <= height_src - 1)).cast(sampled.dtype) + in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) & + (y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype) return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds) def frames_to_tensor(frames): - height = (frames.shape[0] * 2) // 3 - width = frames.shape[1] - return Tensor.cat( - frames[0:height:2, 0::2], - frames[1:height:2, 0::2], - frames[0:height:2, 1::2], - frames[1:height:2, 1::2], - frames[height:height + height // 4].reshape((height // 2, width // 2)), - frames[height + height // 4:height + height // 2].reshape((height // 2, width // 2)), - dim=0, - ).reshape((6, height // 2, width // 2)) + H = (frames.shape[0] * 2) // 3 + W = frames.shape[1] + in_img1 = Tensor.cat(frames[0:H:2, 0::2], + frames[1:H:2, 0::2], + frames[0:H:2, 1::2], + frames[1:H:2, 1::2], + frames[H:H+H//4].reshape((H//2, W//2)), + frames[H+H//4:H+H//2].reshape((H//2, W//2)), dim=0).reshape((6, H//2, W//2)) + return in_img1 def make_frame_prepare(nv12: NV12Frame, model_w, model_h): @@ -119,81 +94,65 @@ def make_frame_prepare(nv12: NV12Frame, model_w, model_h): uv_offset = stride * y_height stride_pad = stride - cam_w - def frame_prepare_tinygrad(input_frame, matrix_inverse): - # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling. - matrix_inverse_uv = matrix_inverse * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV) - # Deinterleave NV12 UV plane (UVUV... -> separate U, V). + def frame_prepare_tinygrad(input_frame, M_inv): + # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling + M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV) + # deinterleave NV12 UV plane (UVUV... -> separate U, V) uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride) with Context(SPLIT_REDUCEOP=0): - y = warp_perspective_tinygrad( - input_frame[:cam_h * stride], - matrix_inverse, - (model_w, model_h), - (cam_h, cam_w), - stride_pad, - ).realize() - u = warp_perspective_tinygrad( - uv[:cam_h // 2, :cam_w:2].flatten(), - matrix_inverse_uv, - (model_w // 2, model_h // 2), - (cam_h // 2, cam_w // 2), - 0, - ).realize() - v = warp_perspective_tinygrad( - uv[:cam_h // 2, 1:cam_w:2].flatten(), - matrix_inverse_uv, - (model_w // 2, model_h // 2), - (cam_h // 2, cam_w // 2), - 0, - ).realize() + y = warp_perspective_tinygrad(input_frame[:cam_h*stride], + M_inv, (model_w, model_h), + (cam_h, cam_w), stride_pad).realize() + u = warp_perspective_tinygrad(uv[:cam_h//2, :cam_w:2].flatten(), + M_inv_uv, (model_w//2, model_h//2), + (cam_h//2, cam_w//2), 0).realize() + v = warp_perspective_tinygrad(uv[:cam_h//2, 1:cam_w:2].flatten(), + M_inv_uv, (model_w//2, model_h//2), + (cam_h//2, cam_w//2), 0).realize() yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w)) - return frames_to_tensor(yuv) - + tensor = frames_to_tensor(yuv) + return tensor return frame_prepare_tinygrad -def make_tensor_inputs(vision_input_shapes, policy_input_shapes, frame_skip, device): - img = vision_input_shapes["img"] +def make_warp_input_queues(vision_input_shapes, frame_skip, device): + img = vision_input_shapes['img'] # (1, 12, 128, 256) n_frames = img[1] // 6 img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3]) - features_buffer = policy_input_shapes["features_buffer"] - desire_pulse = policy_input_shapes["desire_pulse"] - - return { - "img_q": Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(), - "big_img_q": Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(), - "feat_q": Tensor( - np.zeros((frame_skip * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]), dtype=np.float32), - device=device, - ).contiguous().realize(), - "desire_q": Tensor( - np.zeros((frame_skip * desire_pulse[1], desire_pulse[0], desire_pulse[2]), dtype=np.float32), - device=device, - ).contiguous().realize(), - } - - -def make_npy_inputs(policy_input_shapes): - desire_pulse = policy_input_shapes["desire_pulse"] - traffic_convention = policy_input_shapes["traffic_convention"] - npy = { - "desire": np.zeros(desire_pulse[2], dtype=np.float32), - "traffic_convention": np.zeros(traffic_convention, dtype=np.float32), - "tfm": np.zeros((3, 3), dtype=np.float32), - "big_tfm": np.zeros((3, 3), dtype=np.float32), + 'tfm': np.zeros((3, 3), dtype=np.float32), + 'big_tfm': np.zeros((3, 3), dtype=np.float32), } - if "action_t" in policy_input_shapes: - npy["action_t"] = np.zeros(policy_input_shapes["action_t"], dtype=np.float32) - npy_tensors = {key: Tensor(value, device="NPY").realize() for key, value in npy.items()} - return npy, npy_tensors + input_queues = { + 'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(), + 'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(), + **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()}, + } + return input_queues, npy def make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device): - tensor_inputs = make_tensor_inputs(vision_input_shapes, policy_input_shapes, frame_skip, device) - npy, npy_tensors = make_npy_inputs(policy_input_shapes) - return {**tensor_inputs, **npy_tensors}, npy + input_queues, npy = make_warp_input_queues(vision_input_shapes, frame_skip, device) + + fb = policy_input_shapes['features_buffer'] # (1, 25, 512) + dp = policy_input_shapes['desire_pulse'] # (1, 25, 8) + tc = policy_input_shapes['traffic_convention'] # (1, 2) + #TODO action_t is hardcoded to match tc for future compatibility + at = tc + + policy_npy = { + 'desire': np.zeros(dp[2], dtype=np.float32), + 'traffic_convention': np.zeros(tc, dtype=np.float32), + 'action_t': np.zeros(at, dtype=np.float32), + } + npy.update(policy_npy) + input_queues.update({ + 'feat_q': Tensor(np.zeros((frame_skip * (fb[1] - 1) + 1, fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(), + 'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(), + **{k: Tensor(v, device='NPY').realize() for k, v in policy_npy.items()}, + }) + return input_queues, npy def shift_and_sample(buf, new_val, sample_fn): @@ -223,44 +182,39 @@ def make_warp(nv12, model_w, model_h, frame_skip): img = shift_and_sample(img_q, warped_frame, sample_skip_fn) big_img = shift_and_sample(big_img_q, warped_big_frame, sample_skip_fn) return img, big_img - return warp_enqueue -def make_run_policy(vision_runner, off_policy_runner, on_policy_runner, vision_features_slice, frame_skip): +def make_run_policy(model_runners, model_metadata, frame_skip): sample_desire_fn = partial(sample_desire, frame_skip=frame_skip) sample_skip_fn = partial(sample_skip, frame_skip=frame_skip) + vision_features_slice = model_metadata['vision']['output_slices']['hidden_state'] def run_policy(img, big_img, feat_q, desire_q, desire, traffic_convention, action_t): desire = desire.to(Device.DEFAULT) traffic_convention = traffic_convention.to(Device.DEFAULT) action_t = action_t.to(Device.DEFAULT) Tensor.realize(desire, traffic_convention, action_t) - desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn) - vision_out = next(iter(vision_runner({"img": img, "big_img": big_img}).values())).cast("float32") + vision_out = next(iter(model_runners['vision']({'img': img, 'big_img': big_img}).values())).cast('float32') new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0) feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn) inputs = { - "features_buffer": feat_buf, - "desire_pulse": desire_buf, - "traffic_convention": traffic_convention, - "action_t": action_t, + 'features_buffer': feat_buf, + 'desire_pulse': desire_buf, + 'traffic_convention': traffic_convention, + 'action_t': action_t, } - on_policy_out = next(iter(on_policy_runner(inputs).values())).cast("float32") - off_policy_out = next(iter(off_policy_runner(inputs).values())).cast("float32") + on_policy_out = next(iter(model_runners['on_policy'](inputs).values())).cast('float32') + off_policy_out = next(iter(model_runners['off_policy'](inputs).values())).cast('float32') return vision_out, on_policy_out, off_policy_out - return run_policy -def compile_jit(jit, make_random_inputs, input_keys, frame_skip, vision_metadata, policy_metadata): - vision_input_shapes = vision_metadata["input_shapes"] - policy_input_shapes = policy_metadata["input_shapes"] - - seed = 42 +def compile_jit(jit, make_random_inputs, input_keys, make_queues): + SEED = 42 validation_rtol = 5e-3 if Device.DEFAULT == "QCOM" else 0.0 validation_atol = 5e-3 if Device.DEFAULT == "QCOM" else 0.0 @@ -271,118 +225,104 @@ def compile_jit(jit, make_random_inputs, input_keys, frame_skip, vision_metadata return np.allclose(lhs, rhs, rtol=validation_rtol, atol=validation_atol, equal_nan=True) return np.array_equal(lhs, rhs) - def random_inputs_run(fn, current_seed, test_val=None, test_buffers=None, expect_match=True): - input_queues, npy = make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT) - np.random.seed(current_seed) - Tensor.manual_seed(current_seed) + def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True): + input_queues, npy = make_queues(Device.DEFAULT) + np.random.seed(seed) + Tensor.manual_seed(seed) testing = test_val is not None or test_buffers is not None n_runs = 1 if testing else 3 - for idx in range(n_runs): - for value in npy.values(): - value[:] = np.random.randn(*value.shape).astype(value.dtype) + for i in range(n_runs): + for v in npy.values(): + v[:] = np.random.randn(*v.shape).astype(v.dtype) Device.default.synchronize() random_inputs = make_random_inputs() - start = time.perf_counter() - outs = fn(**{key: input_queues[key] for key in input_keys}, **random_inputs) - mid = time.perf_counter() + st = time.perf_counter() + outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs) + mt = time.perf_counter() Device.default.synchronize() - end = time.perf_counter() - print(f" [{idx + 1}/{n_runs}] enqueue {(mid - start) * 1e3:6.2f} ms -- total {(end - start) * 1e3:6.2f} ms") + et = time.perf_counter() + print(f" [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms") - if idx == 0: - val = [np.copy(value.numpy()) for value in outs] - buffers = [np.copy(value.numpy().copy()) for value in input_queues.values()] + if i == 0: + val = [np.copy(v.numpy()) for v in outs] + buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()] if Device.DEFAULT != "QCOM": if test_val is not None: - match = all(arrays_match(lhs, rhs) for lhs, rhs in zip(val, test_val, strict=True)) - assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={current_seed})" + match = all(arrays_match(a, b) for a, b in zip(val, test_val, strict=True)) + assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})" if test_buffers is not None: - match = all(arrays_match(lhs, rhs) for lhs, rhs in zip(buffers, test_buffers, strict=True)) - assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={current_seed})" + match = all(arrays_match(a, b) for a, b in zip(buffers, test_buffers, strict=True)) + assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})" return val, buffers - print("capture + replay") - test_val, test_buffers = random_inputs_run(jit, seed) - print("pickle round trip") + print('capture + replay') + test_val, test_buffers = random_inputs_run(jit, SEED) + print('pickle round trip') jit = pickle.loads(pickle.dumps(jit)) - random_inputs_run(jit, seed, test_val, test_buffers, expect_match=True) - random_inputs_run(jit, seed + 1, test_val, test_buffers, expect_match=False) + random_inputs_run(jit, SEED, test_val, test_buffers, expect_match=True) + random_inputs_run(jit, SEED+1, test_val, test_buffers, expect_match=False) return jit -def _parse_size(size): - width, height = size.lower().split("x") - return int(width), int(height) +def _parse_size(s): + w, h = s.lower().split('x') + return int(w), int(h) def read_file_chunked_to_shm(path): from openpilot.common.file_chunker import read_file_chunked from openpilot.system.hardware.hw import Paths - shm_path = os.path.join(Paths.shm_path(), os.path.basename(path)) atexit.register(lambda: os.path.exists(shm_path) and os.remove(shm_path)) - with open(shm_path, "wb") as f: + with open(shm_path, 'wb') as f: f.write(read_file_chunked(path)) return shm_path if __name__ == "__main__": from tinygrad.nn.onnx import OnnxRunner - from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict from openpilot.system.camerad.cameras.nv12_info import get_nv12_info + from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict + p = argparse.ArgumentParser() + p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH') + p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True, + help='camera resolutions WxH (one or more)') + p.add_argument('--vision-onnx', required=True) + p.add_argument('--off-policy-onnx', required=True) + p.add_argument('--on-policy-onnx', required=True) + p.add_argument('--output', required=True) + p.add_argument('--frame-skip', type=int, required=True) + args = p.parse_args() - parser = argparse.ArgumentParser() - parser.add_argument("--model-size", type=_parse_size, required=True, help="model input WxH") - parser.add_argument("--camera-resolutions", type=_parse_size, nargs="+", required=True, help="camera resolutions WxH (one or more)") - parser.add_argument("--vision-onnx", required=True) - parser.add_argument("--off-policy-onnx", required=True) - parser.add_argument("--on-policy-onnx", required=True) - parser.add_argument("--output", required=True) - parser.add_argument("--frame-skip", type=int, required=True) - args = parser.parse_args() - - out = defaultdict(dict) - vision_path = read_file_chunked_to_shm(args.vision_onnx) - off_policy_path = read_file_chunked_to_shm(args.off_policy_onnx) - on_policy_path = read_file_chunked_to_shm(args.on_policy_onnx) + model_paths = { + 'vision': read_file_chunked_to_shm(args.vision_onnx), + 'off_policy': read_file_chunked_to_shm(args.off_policy_onnx), + 'on_policy': read_file_chunked_to_shm(args.on_policy_onnx), + } model_w, model_h = args.model_size - vision_runner = OnnxRunner(vision_path) - off_policy_runner = OnnxRunner(off_policy_path) - on_policy_runner = OnnxRunner(on_policy_path) - vision_metadata = make_metadata_dict(vision_path) - off_policy_metadata = make_metadata_dict(off_policy_path) - on_policy_metadata = make_metadata_dict(on_policy_path) - assert off_policy_metadata["input_shapes"] == on_policy_metadata["input_shapes"] + model_runners = {name: OnnxRunner(path) for name, path in model_paths.items()} + out = {'metadata': {name: make_metadata_dict(path) for name, path in model_paths.items()}} - run_policy_jit = TinyJit( - make_run_policy( - vision_runner, - off_policy_runner, - on_policy_runner, - vision_metadata["output_slices"]["hidden_state"], - args.frame_skip, - ), - prune=True, - ) + assert out['metadata']['off_policy']['input_shapes'] == out['metadata']['on_policy']['input_shapes'] - out["metadata"]["vision"] = vision_metadata - out["metadata"]["off_policy"] = off_policy_metadata - out["metadata"]["on_policy"] = on_policy_metadata - out["tensor_inputs"] = make_tensor_inputs(vision_metadata["input_shapes"], on_policy_metadata["input_shapes"], args.frame_skip, Device.DEFAULT) + run_policy_jit = TinyJit(make_run_policy(model_runners, out['metadata'], args.frame_skip), prune=True) - make_random_model_inputs = partial(make_random_images, keys=["img", "big_img"], shape=vision_metadata["input_shapes"]["img"]) - out["run_policy"] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata) + make_policy_queues = partial(make_input_queues, out['metadata']['vision']['input_shapes'], + out['metadata']['on_policy']['input_shapes'], args.frame_skip) + make_random_model_inputs = partial(make_random_images, keys=['img', 'big_img'], shape=out['metadata']['vision']['input_shapes']['img']) + out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, + make_policy_queues) for cam_w, cam_h in args.camera_resolutions: nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)) - # Capture warp against blob-backed frames so the JIT ABI matches runtime VisionBuf inputs. - make_random_warp_inputs = partial(make_random_blob_images, keys=["frame", "big_frame"], shape=nv12.size, device=WARP_DEV) + make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV) warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True) - out[(cam_w, cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata) + make_warp_queues = partial(make_warp_input_queues, out['metadata']['vision']['input_shapes'], args.frame_skip) + out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, make_warp_queues) with open(args.output, "wb") as f: pickle.dump(out, f) diff --git a/selfdrive/modeld/modeld_v16.py b/selfdrive/modeld/modeld_v16.py index 05a069eb2..434a71927 100644 --- a/selfdrive/modeld/modeld_v16.py +++ b/selfdrive/modeld/modeld_v16.py @@ -27,9 +27,9 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper -from openpilot.selfdrive.controls.lib.drive_helpers import smooth_value -from openpilot.selfdrive.modeld.compile_modeld import POLICY_INPUTS, WARP_INPUTS, make_npy_inputs, make_tensor_inputs -from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan, smooth_value +from openpilot.selfdrive.modeld.compile_modeld import POLICY_INPUTS, WARP_INPUTS, make_input_queues +from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.fill_model_msg import PublishState, fill_model_msg, fill_pose_msg from openpilot.selfdrive.modeld.helpers import get_tg_input_devices from openpilot.selfdrive.modeld.parse_model_outputs import Parser @@ -113,9 +113,25 @@ def _combined_model_path(model_id: str, use_builtin_model: bool) -> Path: def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, v_ego: float) -> log.ModelDataV2.Action: - desired_curv_unscaled, desired_accel = model_output["action"][0] - desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2 - should_stop = (v_ego < 0.3 and desired_accel < 0.1) + if "action" in model_output: + desired_curv_unscaled, desired_accel = model_output["action"][0] + desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2 + should_stop = (v_ego < 0.3 and desired_accel < 0.1) + else: + plan = model_output["plan"][0] + desired_accel, should_stop = get_accel_from_plan( + plan[:, Plan.VELOCITY][:, 0], + plan[:, Plan.ACCELERATION][:, 0], + ModelConstants.T_IDXS, + action_t=DT_MDL, + ) + desired_curvature = get_curvature_from_plan( + plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], + plan[:, Plan.ORIENTATION_RATE][:, 2], + ModelConstants.T_IDXS, + v_ego, + DT_MDL, + ) desired_accel = smooth_value(float(desired_accel), prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) if v_ego > MIN_LAT_CONTROL_SPEED: @@ -184,25 +200,16 @@ class ModelState: self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ input_devices = get_tg_input_devices(PROCESS_NAME, usbgpu) self.WARP_DEV, self.QUEUE_DEV = input_devices["WARP_DEV"], input_devices["QUEUE_DEV"] - tensor_inputs = jits.get("tensor_inputs") - if tensor_inputs is None: - tensor_inputs = make_tensor_inputs(self.vision_input_shapes, self.policy_input_shapes, self.frame_skip, device=self.QUEUE_DEV) - self.npy, npy_tensors = make_npy_inputs(self.policy_input_shapes) - self.input_queues = {**tensor_inputs, **npy_tensors} + self.input_queues, self.npy = make_input_queues( + self.vision_input_shapes, self.policy_input_shapes, self.frame_skip, device=self.QUEUE_DEV + ) self.full_frames: dict[str, Tensor] = {} self._blob_cache: dict[tuple[str, int], Tensor] = {} self.parser = Parser() self.frame_buf_params = {key: get_nv12_info(cam_w, cam_h) for key in ("img", "big_img")} self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - - camera_jit = jits[(cam_w, cam_h)] - self.split_warp_layout = "run_policy" in jits and not isinstance(camera_jit, dict) - if self.split_warp_layout: - self.run_policy = jits["run_policy"] - self.warp_enqueue = camera_jit - else: - self.run_policy = camera_jit["run_policy"] - self.warp_enqueue = camera_jit["warp_enqueue"] + self.run_policy = jits["run_policy"] + self.warp_enqueue = jits[(cam_w, cam_h)] def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]: return {key: model_outputs[np.newaxis, value] for key, value in output_slices.items()} @@ -225,25 +232,20 @@ class ModelState: self.npy["tfm"][:, :] = transforms["img"][:, :] self.npy["big_tfm"][:, :] = transforms["big_img"][:, :] - if self.split_warp_layout: - img, big_img = self.warp_enqueue( - **{key: self.input_queues[key] for key in WARP_INPUTS}, - frame=self.full_frames["img"], - big_frame=self.full_frames["big_img"], - ) - if prepare_only: - return None - policy_inputs = {key: self.input_queues[key] for key in POLICY_INPUTS if key in self.input_queues} - vision_output, policy_output, off_policy_output = self.run_policy(**policy_inputs, img=img, big_img=big_img) - else: - if prepare_only: - self.warp_enqueue(**self.input_queues, frame=self.full_frames["img"], big_frame=self.full_frames["big_img"]) - return None - vision_output, policy_output, off_policy_output = self.run_policy( - **self.input_queues, - frame=self.full_frames["img"], - big_frame=self.full_frames["big_img"], - ) + img, big_img = self.warp_enqueue( + **{key: self.input_queues[key] for key in WARP_INPUTS}, + frame=self.full_frames["img"], + big_frame=self.full_frames["big_img"], + ) + + if prepare_only: + return None + + vision_output, policy_output, off_policy_output = self.run_policy( + **{key: self.input_queues[key] for key in POLICY_INPUTS if key in self.input_queues}, + img=img, + big_img=big_img, + ) vision_output = vision_output.numpy().flatten() policy_output = policy_output.numpy().flatten() diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 1bdadf047..510e671c4 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -18,6 +18,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.gps import get_gps_location_service from openpilot.selfdrive.car.car_specific import CarSpecificEvents +from openpilot.selfdrive.car.cruise_state import should_flag_cruise_mismatch from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.selfdrive.selfdrived.events import Events, ET from openpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck @@ -463,7 +464,8 @@ class SelfdriveD: self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise ) effective_pcm_cruise = self.CP.pcmCruise or preap_software_cruise - cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not effective_pcm_cruise) and not pacifica_hybrid_aol + cruise_mismatch = should_flag_cruise_mismatch(self.CP, CS.cruiseState.enabled, self.enabled, + effective_pcm_cruise) and not pacifica_hybrid_aol self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0 if self.cruise_mismatch_counter > int(6. / DT_CTRL): self.events.add(EventName.cruiseMismatch)