From d7e5f3cf433cad8085a930dcd16aa8f503168e39 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 1 Feb 2026 10:12:28 -0800 Subject: [PATCH] init sunnymodel with wiped locationd and livepose for temporary sim xytz data --- common/params_keys.h | 2 + selfdrive/debug/uiview.py | 5 +- selfdrive/locationd/locationd.py | 5 +- selfdrive/selfdrived/selfdrived.py | 6 +- sunnypilot/SConscript | 1 + sunnypilot/modeld_sunny/SConscript | 32 ++ sunnypilot/modeld_sunny/__init__.py | 0 .../modeld_sunny/compile_split_tinygrad.py | 109 ++++++ sunnypilot/modeld_sunny/fill_model_msg.py | 161 ++++++++ sunnypilot/modeld_sunny/input_id_helper.py | 51 +++ sunnypilot/modeld_sunny/kinematic_model.py | 60 +++ sunnypilot/modeld_sunny/loader.py | 19 + sunnypilot/modeld_sunny/modeld.py | 344 ++++++++++++++++++ sunnypilot/modeld_sunny/tests/__init__.py | 0 .../modeld_sunny/tests/test_integration.py | 83 +++++ sunnypilot/modeld_v2/modeld.py | 2 +- sunnypilot/modeld_v2/models/commonmodel.cc | 6 +- sunnypilot/modeld_v2/models/commonmodel.h | 12 +- sunnypilot/modeld_v2/models/commonmodel.pxd | 2 +- .../modeld_v2/models/commonmodel_pyx.pyx | 4 +- system/manager/manager.py | 3 +- system/manager/process_config.py | 11 +- 22 files changed, 895 insertions(+), 23 deletions(-) create mode 100644 sunnypilot/modeld_sunny/SConscript create mode 100644 sunnypilot/modeld_sunny/__init__.py create mode 100644 sunnypilot/modeld_sunny/compile_split_tinygrad.py create mode 100644 sunnypilot/modeld_sunny/fill_model_msg.py create mode 100644 sunnypilot/modeld_sunny/input_id_helper.py create mode 100644 sunnypilot/modeld_sunny/kinematic_model.py create mode 100644 sunnypilot/modeld_sunny/loader.py create mode 100644 sunnypilot/modeld_sunny/modeld.py create mode 100644 sunnypilot/modeld_sunny/tests/__init__.py create mode 100644 sunnypilot/modeld_sunny/tests/test_integration.py diff --git a/common/params_keys.h b/common/params_keys.h index ecc656cc78..f6af16e898 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -228,6 +228,8 @@ inline static std::unordered_map keys = { {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}}, {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}}, {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}}, + {"ModeldSunny", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"AlpamayoDriveFast", {PERSISTENT | BACKUP, BOOL, "0"}}, // mapd {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}}, diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py index ad3ccea036..7e170648eb 100755 --- a/selfdrive/debug/uiview.py +++ b/selfdrive/debug/uiview.py @@ -3,14 +3,15 @@ import time from cereal import car, log, messaging from openpilot.common.params import Params -from openpilot.system.manager.process_config import managed_processes, is_snpe_model, is_tinygrad_model, is_stock_model +from openpilot.system.manager.process_config import managed_processes, is_snpe_model, is_tinygrad_model, is_stock_model, is_modeld_sunny from openpilot.system.hardware import HARDWARE if __name__ == "__main__": CP = car.CarParams(notCar=True, wheelbase=1, steerRatio=10) params = Params() params.put("CarParams", CP.to_bytes()) - + if is_modeld_sunny:= is_modeld_sunny(False, params, CP): + print("Using sunnypilot custom modeld") if use_snpe_modeld := is_snpe_model(False, params, CP): print("Using SNPE modeld") if use_tinygrad_modeld := is_tinygrad_model(False, params, CP): diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index f6a0935ed9..aef6a4f46a 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -270,7 +270,7 @@ def main(): estimator = LocationEstimator(DEBUG) filter_initialized = False - critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"] + critcal_services = ["accelerometer", "gyroscope"] observation_input_invalid = defaultdict(int) input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services} @@ -320,8 +320,7 @@ def main(): filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) if sm.updated["cameraOdometry"]: - critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services) - inputs_valid = sm.all_valid() and critical_service_inputs_valid + inputs_valid = True sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 7b546f3780..94d8d073b9 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -91,7 +91,7 @@ class SelfdriveD(CruiseHelper): ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP'] if SIMULATION: - ignore += ['driverCameraState', 'managerState'] + ignore += ['driverCameraState', 'managerState', 'livePose', 'liveCalibration', 'liveParameters', 'liveTorqueParameters', 'liveDelay', 'driverAssistance'] if REPLAY: # no vipc in replay will make them ignored anyways ignore += ['roadCameraState', 'wideRoadCameraState'] @@ -388,8 +388,8 @@ class SelfdriveD(CruiseHelper): if not self.CP.notCar: if not self.sm['livePose'].posenetOK: self.events.add(EventName.posenetInvalid) - if not self.sm['livePose'].inputsOK: - self.events.add(EventName.locationdTemporaryError) + # if not self.sm['livePose'].inputsOK: + # self.events.add(EventName.locationdTemporaryError) if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY): self.events.add(EventName.paramsdTemporaryError) diff --git a/sunnypilot/SConscript b/sunnypilot/SConscript index eb3698f9d0..51bdbb6abc 100644 --- a/sunnypilot/SConscript +++ b/sunnypilot/SConscript @@ -2,3 +2,4 @@ SConscript(['common/transformations/SConscript']) SConscript(['modeld/SConscript']) SConscript(['modeld_v2/SConscript']) SConscript(['selfdrive/locationd/SConscript']) +SConscript(['modeld_sunny/SConscript']) diff --git a/sunnypilot/modeld_sunny/SConscript b/sunnypilot/modeld_sunny/SConscript new file mode 100644 index 0000000000..894176c2b6 --- /dev/null +++ b/sunnypilot/modeld_sunny/SConscript @@ -0,0 +1,32 @@ +import os +import glob + +Import('env', 'arch') +lenv = env.Clone() +tinygrad_repo = env.Dir("#tinygrad_repo") +tinygrad_files = ["#"+x for x in glob.glob(tinygrad_repo.relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x] + +def mayo_compile(flags, model_name): + pythonpath_string = f'PYTHONPATH="${{PYTHONPATH}}:{tinygrad_repo.abspath}"' + onnx_fn = f"distilled_models/{model_name}.onnx" + pkl_fn = f"distilled_models/{model_name}_tinygrad.pkl" + if not os.path.exists("distilled_models"): + try: + os.makedirs("distilled_models") + except OSError: + pass + + if os.path.isfile(File(onnx_fn).abspath): + return lenv.Command( + pkl_fn, + [onnx_fn, "compile_split_tinygrad.py"] + tinygrad_files, + f'{pythonpath_string} {flags} python3 {File("compile_split_tinygrad.py").abspath} {File(onnx_fn).abspath} {File(pkl_fn).abspath}' + ) + +flags = { + 'larch64': 'DEV=QCOM', + 'Darwin': f'DEV=METAL HOME={os.path.expanduser("~")} IMAGE=0', +}.get(arch, 'DEV=CPU CPU_LLVM=1 IMAGE=0') + +for m in ["student_vision", "student_policy"]: + mayo_compile(flags, m) diff --git a/sunnypilot/modeld_sunny/__init__.py b/sunnypilot/modeld_sunny/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/modeld_sunny/compile_split_tinygrad.py b/sunnypilot/modeld_sunny/compile_split_tinygrad.py new file mode 100644 index 0000000000..a7d166b9c5 --- /dev/null +++ b/sunnypilot/modeld_sunny/compile_split_tinygrad.py @@ -0,0 +1,109 @@ +""" +The whole point of this module is to mimic compile3.py while adapting it for our buffers to prevent buffer explosion +""" +import os +import sys +import pickle + +from tinygrad import Tensor, TinyJit, Context, Device +from tinygrad.device import Buffer +# from tinygrad.nn.onnx import OnnxRunner +from tinygrad.frontend.onnx import OnnxRunner + +if "JIT_BATCH_SIZE" not in os.environ: + os.environ["JIT_BATCH_SIZE"] = "0" +if "FLOAT16" not in os.environ: + os.environ["FLOAT16"] = "1" +if "OPT" not in os.environ: + os.environ["OPT"] = "99" + + +KEEP_BUFFERS = set() +original_reduce = Buffer.__reduce__ +def stripped_reduce(self): + if id(self) in KEEP_BUFFERS: + return original_reduce(self) + return (self.__class__, (self.device, self.size, self.dtype)) +Buffer.__reduce__ = stripped_reduce + + +def compile_model(onnx_path, output_path, input_shapes=None, input_types=None): + print(f"Compiling {onnx_path} -> {output_path}") + run_onnx = OnnxRunner(onnx_path) + + if input_shapes is None: + input_shapes = {name: spec.shape for name, spec in run_onnx.graph_inputs.items()} + if input_types is None: + input_types = {name: spec.dtype for name, spec in run_onnx.graph_inputs.items()} + + Tensor.manual_seed(100) + inputs = {k: Tensor(Tensor.randn(*shp, dtype=input_types[k]).mul(8).realize().numpy(), device='NPY') for k, shp in sorted(input_shapes.items())} + inputs = {k: v.to(Device.DEFAULT).realize() for k, v in inputs.items()} + print(f"Realized all {len(inputs)} inputs on {Device.DEFAULT}") + + input_buf_ids = set() + for _, v in inputs.items(): + if hasattr(v, '_buffer'): + try: + b = v._buffer() + if b is not None: + input_buf_ids.add(id(b)) + except Exception: + pass + + if "vision" in onnx_path: + onnx_jit = TinyJit(lambda **kwargs: next(iter(run_onnx({k:v.to(Device.DEFAULT) for k,v in kwargs.items()}).values())).cast('float32'), prune=True) + else: + onnx_jit = TinyJit(lambda **kwargs: [x.cast('float32').contiguous().realize() for x in run_onnx({k:v.to(Device.DEFAULT) for k,v in kwargs.items()}).values()], prune=True) + + for i in range(3): + with Context(DEBUG=max(int(os.getenv("DEBUG", 0)), 2 if i == 2 else 1)): + res = onnx_jit(**inputs) + if isinstance(res, list): + for x in res: + x.numpy() + else: + res.numpy() + print(f"Captured {len(onnx_jit.captured.jit_cache)} kernels") + + all_read_ids = set() + all_written_ids = set() + + for ji in onnx_jit.captured.jit_cache: + if len(ji.bufs) > 0: + if ji.bufs[0] is not None: + all_written_ids.add(id(ji.bufs[0])) + + for b in ji.bufs[1:]: + if b is not None: + all_read_ids.add(id(b)) + + weight_candidates = all_read_ids - all_written_ids + weight_ids = weight_candidates - input_buf_ids + print(f"Identified {len(weight_ids)} weight candidates (Read-Only & Not Input).") + total_weight_size = 0 + + marked_count = 0 + for ji in onnx_jit.captured.jit_cache: + for b in ji.bufs: + if b is not None and id(b) in weight_ids: + if id(b) not in KEEP_BUFFERS: + KEEP_BUFFERS.add(id(b)) + total_weight_size += b.size * b.dtype.itemsize + marked_count += 1 + + print(f"Preserving {marked_count} unique weight buffers.") + print(f"Total Preserved Weight Data Size: {total_weight_size / 1e6:.2f} MB") + + with open(output_path, "wb") as f: + pickle.dump(onnx_jit, f) + print(f"Saved {output_path}, pkl size: {os.path.getsize(output_path)/1e6:.2f} MB") + +if __name__ == "__main__": + if len(sys.argv) < 3: + print("Usage: python compile_split_tinygrad.py ") + sys.exit(1) + + input_onnx = sys.argv[1] + output_pkl = sys.argv[2] + compile_model(input_onnx, output_pkl) diff --git a/sunnypilot/modeld_sunny/fill_model_msg.py b/sunnypilot/modeld_sunny/fill_model_msg.py new file mode 100644 index 0000000000..9b03ca9736 --- /dev/null +++ b/sunnypilot/modeld_sunny/fill_model_msg.py @@ -0,0 +1,161 @@ +import numpy as np + +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan + + +def interp_vec(t_out, t_in, vec): + # vec shape (N, 3), output (M, 3) + return np.stack([np.interp(t_out, t_in, vec[:, i]) for i in range(3)], axis=1) + + +def fill_alpamayo_msg(modelV2, net_outputs, frame_id, frame_drop_ratio, timestamp_eof, CP, lat_delay, v_ego): + modelV2.frameId = frame_id + modelV2.frameIdExtra = frame_id + modelV2.timestampEof = timestamp_eof + modelV2.frameDropPerc = frame_drop_ratio * 100.0 + + modelV2.init('laneLines', 4) + modelV2.init('roadEdges', 2) + modelV2.init('laneLineProbs', 4) + modelV2.init('roadEdgeStds', 2) + + for i in range(4): + l = modelV2.laneLines[i] + l.t = [0.0] + l.x = [0.0] + l.y = [0.0] + l.z = [0.0] + modelV2.laneLineProbs[i] = 0.0 + + for i in range(2): + e = modelV2.roadEdges[i] + e.t = [0.0] + e.x = [0.0] + e.y = [0.0] + e.z = [0.0] + modelV2.roadEdgeStds[i] = 1.0 + + + leads = modelV2.init('leadsV3', 1) + lead = leads[0] + pred_lead = net_outputs['pred_lead'][0] + prob_logit = float(pred_lead[0]) + dist_pred = float(pred_lead[1] * 100.0) + dist_sigma = float(np.exp(pred_lead[2])) + + v_rel_pred = float(pred_lead[3]) + v_sigma = float(np.exp(pred_lead[4])) + + a_rel_pred = float(pred_lead[5]) + a_sigma = float(np.exp(pred_lead[6])) + + prob = float(1.0 / (1.0 + np.exp(-prob_logit))) + + lead.prob = prob + lead.probTime = 0.0 + + # X(t) = X0 + V_rel*t + 0.5*A_rel*t^2 + T = ModelConstants.T_IDXS + lead.t = list(T) + lead.x = [float(dist_pred + v_rel_pred * t + 0.5 * a_rel_pred * t**2) for t in T] + lead.v = [float(v_ego + v_rel_pred + a_rel_pred * t) for t in T] + a_ego = net_outputs['acceleration'][0, 0, 0] # T=0 ego accel estimate (x component) + lead.a = [float(a_ego + a_rel_pred)] * len(T) + lead.y = [0.0] * len(T) + + lead.xStd = [max(0.5, dist_sigma * 100.0)] * len(T) + lead.yStd = [1.0] * len(T) + lead.vStd = [max(0.1, v_sigma)] * len(T) + lead.aStd = [max(0.1, a_sigma)] * len(T) + + modelV2.meta.engagedProb = 1.0 + desire_pred = [0.0] * 8 + + if 'pred_light' in net_outputs: + red_prob = float(1.0 / (1.0 + np.exp(-net_outputs['pred_light'][0, 1] + net_outputs['pred_light'][0, 0]))) + desire_pred[4] = red_prob + + modelV2.meta.desirePrediction = desire_pred + modelV2.meta.desireState = [0.0] * 8 + reasoning_error = net_outputs.get('consistency_error', 0.0) + + if reasoning_error < 0.5: + modelV2.confidence = "green" + elif reasoning_error < 1.5: + modelV2.confidence = "yellow" + else: + modelV2.confidence = "red" + + ALPAMAYO_T_IDXS = np.arange(1, 65) * 0.1 # 64 steps at .1s intervals + t_idxs = ModelConstants.T_IDXS + t_all = np.concatenate(([0.0], ALPAMAYO_T_IDXS)) # this model starts at t=0.1 so if we prepend 0.0 and interpolate for t=now it should match op + + pos_interp = interp_vec(t_idxs, t_all, np.vstack((np.zeros(3), net_outputs['position'][0]))) + pos_std_interp = interp_vec(t_idxs, t_all, np.vstack((np.zeros(3), net_outputs.get('position_std', np.ones((64, 3)) * 0.1)))) + vel_interp = interp_vec(t_idxs, t_all, np.vstack(([v_ego, 0.0, 0.0], net_outputs['velocity'][0]))) + acc_interp = interp_vec(t_idxs, t_all, np.vstack((net_outputs['acceleration'][0][0], net_outputs['acceleration'][0]))) + rot_interp = interp_vec(t_idxs, t_all, np.vstack((np.zeros(3), net_outputs['orientation'][0]))) + rate_interp = interp_vec(t_idxs, t_all, np.vstack((net_outputs['orientation_rate'][0][0], net_outputs['orientation_rate'][0]))) + + # https://www.mathworks.com/help/vdynblks/ug/coordinate-systems-in-vehicle-dynamics-blockset.html + # following SAE J670 and ISO 8855, for sunnymayo model: x is forward (f), y is left (lat), z is up/vert + # Openpilot Modelv2 and camerad expects SAE J670: x is forward, y is right, z is down + + modelV2.position.t = t_idxs # time, obviously + modelV2.position.x = pos_interp[:, 0].tolist() # f dist + modelV2.position.y = (-pos_interp[:, 1]).tolist() # lat offset (Flip L->R) + modelV2.position.z = (-pos_interp[:, 2]).tolist() # vert offset (Flip U->D) (elevation) + modelV2.position.xStd = pos_std_interp[:, 0].tolist() + modelV2.position.yStd = pos_std_interp[:, 1].tolist() + modelV2.position.zStd = pos_std_interp[:, 2].tolist() + + modelV2.velocity.t = t_idxs + modelV2.velocity.x = vel_interp[:, 0].tolist() # f vel (vego) + modelV2.velocity.y = (-vel_interp[:, 1]).tolist() # lat vel (curvature) + modelV2.velocity.z = (-vel_interp[:, 2]).tolist() # vert vel + + modelV2.acceleration.t = t_idxs + modelV2.acceleration.x = acc_interp[:, 0].tolist() # f accel (aego) + modelV2.acceleration.y = (-acc_interp[:, 1]).tolist() # lat accel + modelV2.acceleration.z = (-acc_interp[:, 2]).tolist() # vert accel + + modelV2.orientation.t = t_idxs + modelV2.orientation.x = rot_interp[:, 0].tolist() # roll (treated as 0) + modelV2.orientation.y = (-rot_interp[:, 1]).tolist() # pitch (from z-slope) + modelV2.orientation.z = (-rot_interp[:, 2]).tolist() # yaw (heading) + + modelV2.orientationRate.t = t_idxs + modelV2.orientationRate.x = rate_interp[:, 0].tolist() # roll rate + modelV2.orientationRate.y = (-rate_interp[:, 1]).tolist() # pitch rate (Flip U->D) + modelV2.orientationRate.z = (-rate_interp[:, 2]).tolist() # yaw rate (Flip L->R) + + long_action_t = CP.longitudinalActuatorDelay + DT_MDL + desired_accel, should_stop = get_accel_from_plan(vel_interp[:, 0], acc_interp[:, 0], t_idxs, action_t=long_action_t) + modelV2.action.desiredAcceleration = float(desired_accel) + modelV2.action.shouldStop = bool(should_stop) + + lat_action_t = lat_delay + DT_MDL + desired_curvature = get_curvature_from_plan(-rot_interp[:, 2], -rate_interp[:, 2], t_idxs, vego=v_ego, action_t=lat_action_t) + modelV2.action.desiredCurvature = float(desired_curvature) + + +def fill_pose_msg(camera_odometry, net_outputs, frame_id, timestamp_eof): + camera_odometry.frameId = frame_id + camera_odometry.timestampEof = timestamp_eof + + trans = net_outputs['velocity'][0, 0].copy() + trans[1] *= -1.0 + trans[2] *= -1.0 + camera_odometry.trans = trans.tolist() + + std_val = float(max(0.01, net_outputs.get('consistency_error', 0.1))) + camera_odometry.transStd = [std_val, std_val, std_val] + rot = net_outputs['orientation_rate'][0, 0].copy() + rot[1] *= -1.0 + rot[2] *= -1.0 + camera_odometry.rot = rot.tolist() + + rot_std = std_val * 0.1 + camera_odometry.rotStd = [rot_std, rot_std, rot_std] diff --git a/sunnypilot/modeld_sunny/input_id_helper.py b/sunnypilot/modeld_sunny/input_id_helper.py new file mode 100644 index 0000000000..db746a221a --- /dev/null +++ b/sunnypilot/modeld_sunny/input_id_helper.py @@ -0,0 +1,51 @@ +import numpy as np +from dataclasses import dataclass +from openpilot.common.params import Params + + +@dataclass +class AlpamayoDesire: + DRIVE_SAFELY = 0 + TURN_LEFT = 2 + TURN_RIGHT = 1 + DRIVE_FAST = 3 + STOP = 4 + + +class InputIDHelper: + def __init__(self): + self.current_ids = np.zeros((1, 16), dtype=np.int64) + self.desire = AlpamayoDesire.DRIVE_SAFELY + self.params = Params() + self.drive_fast = False + self.msg_count = -1 + + def update_params(self): + if self.msg_count % 60 == 0: + self.drive_fast = self.params.get_bool("AlpamayoDriveFast") + self.msg_count += 1 + + def update(self, sm): + self.update_params() + if sm is None: + return self.current_ids + + left_blinker = False + right_blinker = False + + if sm.seen['carState']: + left_blinker = sm['carState'].leftBlinker + right_blinker = sm['carState'].rightBlinker + + # Priority: STOP (TODO) > Turn > Drive Fast > Drive Safely + new_desire = AlpamayoDesire.DRIVE_SAFELY + if left_blinker: + new_desire = AlpamayoDesire.TURN_LEFT + elif right_blinker: + new_desire = AlpamayoDesire.TURN_RIGHT + elif self.drive_fast: + new_desire = AlpamayoDesire.DRIVE_FAST + + self.desire = new_desire + self.current_ids.fill(self.desire) + return self.current_ids diff --git a/sunnypilot/modeld_sunny/kinematic_model.py b/sunnypilot/modeld_sunny/kinematic_model.py new file mode 100644 index 0000000000..2f9982fa67 --- /dev/null +++ b/sunnypilot/modeld_sunny/kinematic_model.py @@ -0,0 +1,60 @@ +from tinygrad.tensor import Tensor + + +def action_to_traj(action: Tensor, v0: Tensor, dt: float = 0.1): + """ + This function is a lightweight tinygrad transformation of the unicycle accel physics model based on Nvidia's + unicycle model https://github.com/NVlabs/alpamayo/blob/main/src/alpamayo_r1/action_space/unicycle_accel_curvature.py + + Integrate action (accel, kappa) to trajectory (x, y, theta) + Args: + action: (B, T, 2) [accel, kappa] + v0: (B,) Initial velocity + dt: Time step + Returns: + res: Dict containing position, velocity, acceleration, orientation, orientation_rate + """ + B, T, _ = action.shape + ACCEL_MEAN = 0.02902695 + ACCEL_STD = 0.68104267 + CURV_MEAN = 0.00026922 + CURV_STD = 0.02614828 + + accel = action[..., 0] * ACCEL_STD + ACCEL_MEAN + kappa = action[..., 1] * CURV_STD + CURV_MEAN + + # v_{t+1} = v_t + a_t * dt + v_diff = accel * dt + v_seq = v_diff.cumsum(axis=1) + v0.reshape(B, 1) # cumulative sum over T dimension (axis 1) + velocity = v0.reshape(B, 1).cat(v_seq, dim=1) + + # theta_{t+1} = theta_t + kappa_t * (v_t * dt + 0.5 * a_t * dt^2) + dt_2_term = 0.5 * (dt**2) + dtheta = kappa * (velocity[:, :-1] * dt + accel * dt_2_term) + theta_seq = dtheta.cumsum(axis=1) + theta = Tensor.zeros(B, 1, device=action.device, dtype=action.dtype).cat(theta_seq, dim=1) + + # trapezoidal euler + half_dt = 0.5 * dt + v_cos = velocity * theta.cos() + v_sin = velocity * theta.sin() + + dx = (v_cos[:, :-1] + v_cos[:, 1:]) * half_dt + dy = (v_sin[:, :-1] + v_sin[:, 1:]) * half_dt + x = dx.cumsum(axis=1) + y = dy.cumsum(axis=1) + + res = {} + res['action'] = accel.stack(kappa, dim=-1) # raw model output + + # (x, y, 0) + res['position'] = x.stack(y, Tensor.zeros(B, T, device=action.device, dtype=action.dtype), dim=-1) + # (vx, vy, 0) + res['velocity'] = v_cos[:, 1:].stack(v_sin[:, 1:], Tensor.zeros(B, T, device=action.device, dtype=action.dtype), dim=-1) + # ax = accel * cos(theta), ay = accel * sin(theta), 0 + res['acceleration'] = (accel * theta[:, 1:].cos()).stack(accel * theta[:, 1:].sin(), Tensor.zeros(B, T, device=action.device, dtype=action.dtype), dim=-1) + # (0, 0, theta) + res['orientation'] = Tensor.zeros(B, T, device=action.device, dtype=action.dtype).stack(Tensor.zeros(B, T, device=action.device, dtype=action.dtype), theta[:, 1:], dim=-1) + # (0, 0, dtheta/dt) + res['orientation_rate'] = Tensor.zeros(B, T, device=action.device, dtype=action.dtype).stack(Tensor.zeros(B, T, device=action.device, dtype=action.dtype), dtheta / dt, dim=-1) + return res diff --git a/sunnypilot/modeld_sunny/loader.py b/sunnypilot/modeld_sunny/loader.py new file mode 100644 index 0000000000..cba2941b7b --- /dev/null +++ b/sunnypilot/modeld_sunny/loader.py @@ -0,0 +1,19 @@ +import pickle +from pathlib import Path +from openpilot.common.swaglog import cloudlog + + +def load_compiled_model(model_name: str = "student"): + pkl_path = Path(__file__).parent / "distilled_models" / f"{model_name}_tinygrad.pkl" + + if not pkl_path.exists(): + cloudlog.error(f"Compiled model not found at {pkl_path}") + return None + + try: + with open(pkl_path, "rb") as f: + model_run = pickle.load(f) + return model_run + except Exception as e: + cloudlog.error(f"Failed to load compiled Tinygrad model: {e}") + return None diff --git a/sunnypilot/modeld_sunny/modeld.py b/sunnypilot/modeld_sunny/modeld.py new file mode 100644 index 0000000000..d3bfab9ff4 --- /dev/null +++ b/sunnypilot/modeld_sunny/modeld.py @@ -0,0 +1,344 @@ +import time +import numpy as np +import os +import platform +from setproctitle import setproctitle +from openpilot.system.hardware import TICI + +if TICI: + os.environ['DEV'] = 'QCOM' +elif platform.system() == "Darwin": + os.environ['DEV'] = "METAL" +else: + os.environ['DEV'] = 'CPU' +USBGPU = "USBGPU" in os.environ +if USBGPU: + os.environ['DEV'] = 'AMD' + os.environ['AMD_IFACE'] = 'USB' + +from tinygrad.tensor import Tensor +from tinygrad.dtype import dtypes + +import cereal.messaging as messaging +from cereal import car +from cereal.messaging import PubMaster, SubMaster +from msgq.visionipc import VisionIpcClient, VisionStreamType +from openpilot.common.swaglog import cloudlog +from openpilot.common.params import Params +from openpilot.common.realtime import config_realtime_process, DT_MDL +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.transformations.model import bigmodel_frame_from_calib_frame +from openpilot.common.transformations.camera import DEVICE_CAMERAS, view_frame_from_device_frame +from openpilot.common.transformations.orientation import rot_from_euler +from openpilot.common.realtime import Ratekeeper +from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address + +from openpilot.sunnypilot.livedelay.helpers import get_lat_delay +from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLContext +from openpilot.sunnypilot.modeld_sunny.kinematic_model import action_to_traj +from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper +from openpilot.sunnypilot.modeld_sunny.loader import load_compiled_model +from openpilot.sunnypilot.modeld_sunny.input_id_helper import InputIDHelper +from openpilot.sunnypilot.modeld_sunny.fill_model_msg import fill_alpamayo_msg, fill_pose_msg + +PROCESS_NAME = "selfdrive.modeld.openpilot.sunnypilot.modeld_sunny" + + +class FrameMeta: + frame_id: int = 0 + timestamp_sof: int = 0 + timestamp_eof: int = 0 + + def __init__(self, vipc=None): + if vipc is not None: + self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof + + +def safe_exp(x): + return np.exp(np.clip(x, -np.inf, 11)) + + +def softmax(x, axis=-1): + x = x - np.max(x, axis=axis, keepdims=True) + x = safe_exp(x) + return x / np.sum(x, axis=axis, keepdims=True) + + +class AlpamayoModelD: + def __init__(self, context: CLContext): + self.params = Params() + self.context = context + + self.model_vision = load_compiled_model("student_vision") + self.model_policy = load_compiled_model("student_policy") + self.model_loaded = self.model_vision is not None and self.model_policy is not None + + self.vision_input_names = ['road', 'wide'] + self.vision_input_shapes = { + 'road': (1, 3, 512, 1024), + 'wide': (1, 3, 512, 1024) + } + + self.frames = {name: DrivingModelFrame(context, 1024, 512, buffer_length=4) for name in self.vision_input_names} + self.history_buffer = np.zeros((16, 3), dtype=np.float32) + self.logic_pulse = np.zeros((1, 2048), dtype=np.float32) + + def run(self, bufs, transforms, inputs, prepare_only): + if prepare_only: + return None + if not hasattr(self, 'vision_inputs'): + self.vision_inputs = {} + + imgs_cl = {n: self.frames[n].prepare(bufs[n], transforms[n].flatten()) for n in self.vision_input_names if bufs.get(n)} + + if TICI and not USBGPU: + for k, v in imgs_cl.items(): + if k not in self.vision_inputs: + self.vision_inputs[k] = qcom_tensor_from_opencl_address(v.mem_address, self.vision_input_shapes[k], dtype=dtypes.uint8) + else: + for k, v in imgs_cl.items(): + self.vision_inputs[k] = Tensor(self.frames[k].buffer_from_cl(v).reshape(self.vision_input_shapes[k]), dtype=dtypes.uint8).realize() + + img_t = Tensor.stack([self.vision_inputs['wide'].cast(dtypes.float32) / 255.0, + self.vision_inputs['road'].cast(dtypes.float32) / 255.0], dim=1).unsqueeze(0) + + vis_res = self.model_vision( + history=Tensor(inputs["history"]).contiguous().realize(), + img=img_t.contiguous().realize(), + input_ids=Tensor(inputs["input_ids"]).contiguous().realize(), + logic_pulse=Tensor(inputs["logic_pulse"]).contiguous().realize() + ) + context = vis_res.contiguous().realize() + + x_input = Tensor.zeros(1, 64, 2, device=os.environ.get("DEV"), dtype=dtypes.float32) + v_mu, v_std, pred_pulse, state_mu, state_std, pred_light, pred_lead, hypot_logits = self.model_policy( + context=context, + noisy_action=x_input.contiguous().realize(), + t=Tensor(np.array([[0.0]], dtype=np.float32)).contiguous().realize(), # t=0 + traffic=Tensor(inputs["traffic_convention"]).contiguous().realize() + ) + + weights = softmax(hypot_logits.numpy(), axis=1) # (B, M) + winner_idx = np.argmax(weights[0]) + + v_winner = v_mu[:, winner_idx] + state_winner = state_mu[:, winner_idx] + state_std_winner = state_std[:, winner_idx] + + outputs_tg = action_to_traj(v_winner, Tensor([inputs["v_ego"]], dtype=dtypes.float32), dt=0.1) + outputs = {k: v.numpy() for k, v in outputs_tg.items()} + outputs.update({ + "pred_pulse": pred_pulse.numpy(), + "pred_light": pred_light[0:1].numpy(), + "pred_lead": pred_lead[0:1].numpy(), + "weights": weights[0] + }) + + # Inject world positions for Z/Pitch + pos_world = state_winner[0].numpy() + pos_std = np.exp(state_std_winner[0].numpy()) + outputs["position"][0, :, 2] = pos_world[:, 2] + outputs["position_std"] = pos_std # log_sigma -> sigma + d_pos = np.diff(outputs["position"][0], axis=0, prepend=np.zeros((1, 3))) + d_dist = np.maximum(np.linalg.norm(d_pos[:, :2], axis=1), 1e-4) + pitch = np.arctan2(np.diff(pos_world[:, 2], prepend=0.0), d_dist) + outputs["orientation"][0, :, 1] = pitch + outputs["orientation_rate"][0, :, 1] = np.diff(pitch, prepend=0.0) / 0.1 + outputs["velocity"][0, :, 2] = np.linalg.norm(outputs["velocity"][0, :, :2], axis=1) * np.tan(pitch) + outputs["consistency_error"] = float(np.mean(np.linalg.norm(outputs["position"][0] - pos_world, axis=1))) + + return outputs + + +def main(): + setproctitle(PROCESS_NAME) + config_realtime_process(7, 54) + # Loop runs at 20Hz to match camera acquisition. + # Model inference runs at 10Hz via frame skipping. + rk = Ratekeeper(1.0 / DT_MDL) + cl_context = CLContext() + modeld = AlpamayoModelD(cl_context) + + # Load CarParams + cloudlog.warning("Modeld: Waiting for CarParams...") + CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams) + cloudlog.warning("Modeld: Got CarParams") + + camera_offset_helper = CameraOffsetHelper() + input_id_helper = InputIDHelper() + + if modeld.model_loaded: + cloudlog.warning("Modeld: Successfully loaded compiled student model.") + + pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "liveDelay", "livePose", "driverMonitoringState"]) + + # VisionIPC Clients + while True: + available_streams = VisionIpcClient.available_streams("camerad", block=False) + if available_streams: + use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams + main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams + break + time.sleep(.1) + + vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_ROAD + vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True, cl_context) + vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context) + cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}") + + while not vipc_client_main.connect(False): + time.sleep(0.1) + while use_extra_client and not vipc_client_extra.connect(False): + time.sleep(0.1) + + cloudlog.warning(f"connected main cam with buffer size: {vipc_client_main.buffer_len} ({vipc_client_main.width} x {vipc_client_main.height})") + if use_extra_client: + cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") + + model_transform_main = np.zeros((3, 3), dtype=np.float32) + model_transform_extra = np.zeros((3, 3), dtype=np.float32) + buf_main, buf_extra = None, None + meta_main = FrameMeta() + meta_extra = FrameMeta() + + # filter to track dropped frames + frame_dropped_filter = FirstOrderFilter(0., 10., 1. / 20.0) + last_vipc_frame_id = 0 + run_count = 0 + lat_delay = 0.0 + live_calib_seen = False + + while True: + # Keep receiving frames until we are at least 1 frame ahead of previous extra frame + while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: + buf_main = vipc_client_main.recv() + meta_main = FrameMeta(vipc_client_main) + if buf_main is None: + break + + if buf_main is None: + cloudlog.debug("vipc_client_main no frame") + continue + + if use_extra_client: + # Keep receiving extra frames until frame id matches main camera + while True: + buf_extra = vipc_client_extra.recv() + meta_extra = FrameMeta(vipc_client_extra) + if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: + break + + if buf_extra is None: + cloudlog.debug("vipc_client_extra no frame") + continue + + if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000: + cloudlog.error(f"frames out of sync! main: {meta_main.frame_id} ({meta_main.timestamp_sof / 1e9:.5f}),\ + extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})") + + else: + # Use single camera + buf_extra = buf_main + meta_extra = meta_main + + # 10Hz Execution Check (Skip odd frames) + # We use main camera frameId as the clock + if meta_main.frame_id % 2 != 0: + last_vipc_frame_id = meta_main.frame_id + continue + + sm.update(0) + v_ego = sm['carState'].vEgo if sm.seen['carState'] else 0.0 + + yaw_rate = 0.0 + if sm.seen['livePose'] and sm['livePose'].angularVelocityDevice.valid: + yaw_rate = sm['livePose'].angularVelocityDevice.z + + if sm.frame % 60 == 0: + lat_delay = get_lat_delay(modeld.params, sm["liveDelay"].lateralDelay) + camera_offset_helper.set_offset(modeld.params.get("CameraOffset", return_default=True)) + + if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: + device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) + dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] + calib_from_bigmodel = np.linalg.inv(bigmodel_frame_from_calib_frame[:, :3]) + device_from_calib = rot_from_euler(device_from_calib_euler) + camera_from_calib_main = (dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics) @ view_frame_from_device_frame @ device_from_calib + model_transform_main = camera_from_calib_main @ calib_from_bigmodel + camera_from_calib_extra = dc.ecam.intrinsics @ view_frame_from_device_frame @ device_from_calib + model_transform_extra = camera_from_calib_extra @ calib_from_bigmodel + + model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera) + live_calib_seen = True + + # Track dropped frames + vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1) + frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10)) + if run_count < 10: # let frame drops warm up + frame_dropped_filter.x = 0. + frames_dropped = 0. + run_count = run_count + 1 + + frame_drop_ratio = frames_dropped / (1 + frames_dropped) + prepare_only = vipc_dropped_frames > 0 + if prepare_only: + cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames") + + bufs = {'road': buf_main, 'wide': buf_extra} + transforms = {'road': model_transform_main, 'wide': model_transform_extra} + + dt = 0.1 + d_yaw = yaw_rate * dt + d_pos = v_ego * dt * np.array([np.cos(d_yaw/2), np.sin(d_yaw/2)]) + rot = np.array([[np.cos(-d_yaw), -np.sin(-d_yaw)], [np.sin(-d_yaw), np.cos(-d_yaw)]]) + modeld.history_buffer[:, :2] = (modeld.history_buffer[:, :2] - d_pos) @ rot.T + modeld.history_buffer[:, 2] -= d_yaw + modeld.history_buffer = np.roll(modeld.history_buffer, -1, axis=0) + modeld.history_buffer[-1] = 0. + + hist_input = modeld.history_buffer.copy() + hist_input[:, 1] *= -1.0 + hist_input[:, 2] *= -1.0 + yaws_fixed = hist_input[:, 2] + cos_y, sin_y = np.cos(yaws_fixed), np.sin(yaws_fixed) + zeros, ones = np.zeros_like(cos_y), np.ones_like(cos_y) + rot_flat = np.column_stack([cos_y, -sin_y, zeros, sin_y, cos_y, zeros, zeros, zeros, ones]) + + inputs = { + 'input_ids': input_id_helper.update(sm), + 'history': np.column_stack([hist_input[:, :2], zeros, rot_flat])[None, ...].astype(np.float32), + 'logic_pulse': modeld.logic_pulse, + 'traffic_convention': np.array([[0.0, 1.0]] if sm["driverMonitoringState"].isRHD else [[1.0, 0.0]], dtype=np.float32), + 'v_ego': v_ego + } + + t0 = time.monotonic() + outputs = modeld.run(bufs, transforms, inputs, prepare_only) + t1 = time.monotonic() + if not prepare_only: + cloudlog.warning(f"Modeld: Inference took {(t1-t0)*1000:.2f} ms") + last_vipc_frame_id = meta_main.frame_id + + if outputs is not None: + modeld.logic_pulse[:] = outputs["pred_pulse"] + model_msg = messaging.new_message('modelV2') + drivingdata_msg = messaging.new_message('drivingModelData') + posenet_msg = messaging.new_message('cameraOdometry') + + fill_alpamayo_msg(model_msg.modelV2, outputs, meta_main.frame_id, frame_drop_ratio, meta_main.timestamp_eof, CP, lat_delay, v_ego) + model_msg.valid = live_calib_seen and (vipc_dropped_frames < 1) + + fill_pose_msg(posenet_msg.cameraOdometry, outputs, meta_main.frame_id, meta_main.timestamp_eof) + posenet_msg.valid = live_calib_seen and (vipc_dropped_frames < 1) + + drivingdata_msg.drivingModelData.frameId = meta_main.frame_id + + pm.send('drivingModelData', drivingdata_msg) + pm.send('cameraOdometry', posenet_msg) + pm.send('modelV2', model_msg) + rk.keep_time() + + +if __name__ == "__main__": + main() diff --git a/sunnypilot/modeld_sunny/tests/__init__.py b/sunnypilot/modeld_sunny/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/modeld_sunny/tests/test_integration.py b/sunnypilot/modeld_sunny/tests/test_integration.py new file mode 100644 index 0000000000..ff2449f927 --- /dev/null +++ b/sunnypilot/modeld_sunny/tests/test_integration.py @@ -0,0 +1,83 @@ +import pytest +import numpy as np +from cereal import car +import cereal.messaging as messaging +from msgq.visionipc import VisionIpcServer, VisionIpcClient, VisionStreamType +from sunnypilot.modeld_sunny.modeld import AlpamayoModelD +from sunnypilot.modeld_v2.models.commonmodel_pyx import CLContext +from sunnypilot.modeld_sunny.fill_model_msg import fill_alpamayo_msg + + +@pytest.fixture(scope="module") +def cl_context(): + return CLContext() + + +@pytest.fixture(scope="module") +def modeld(cl_context): + print("Initializing AlpamayoModelD...") + return AlpamayoModelD(cl_context) + + +@pytest.fixture(scope="function") +def vipc_server(): + server_name = "camerad_test" + server = VisionIpcServer(server_name) + server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 1, 1024, 512) + server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 1, 1024, 512) + server.start_listener() + yield server + + +def test_modeld(cl_context, modeld, vipc_server): + v_ego = 20.0 + inputs = { + 'input_ids': np.zeros((1, 16), dtype=np.int64), + 'history': np.zeros((1, 16, 12), dtype=np.float32), + 'logic_pulse': modeld.logic_pulse, + 'traffic_convention': np.array([[1.0, 0.0]], dtype=np.float32), + 'v_ego': v_ego + } + + server_name = "camerad_test" + client_road = VisionIpcClient(server_name, VisionStreamType.VISION_STREAM_ROAD, False, cl_context) + assert client_road.connect(True), "Road client failed to connect" + client_wide = VisionIpcClient(server_name, VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context) + assert client_wide.connect(True), "Wide client failed to connect" + + # NV12 size for 1024x512 = 1024*512 * 1.5 = 786432 + yuv_data = b'\x00' * 786432 + vipc_server.send(VisionStreamType.VISION_STREAM_ROAD, yuv_data) + vipc_server.send(VisionStreamType.VISION_STREAM_WIDE_ROAD, yuv_data) + buf_road = client_road.recv() + buf_wide = client_wide.recv() + assert buf_road is not None + assert buf_wide is not None + + bufs = {'road': buf_road, 'wide': buf_wide} + transforms = {'road': np.eye(3, dtype=np.float32), 'wide': np.eye(3, dtype=np.float32)} + outputs = modeld.run(bufs, transforms, inputs, False) + + assert outputs is not None + assert outputs["position"].shape == (1, 64, 3) + assert outputs["velocity"].shape == (1, 64, 3) + assert outputs["acceleration"].shape == (1, 64, 3) + assert outputs["orientation"].shape == (1, 64, 3) + assert "pred_pulse" in outputs + assert "pred_light" in outputs + assert "pred_lead" in outputs + + assert np.all(np.isfinite(outputs["position"])), "Position contains NaN/Inf" + assert np.all(np.isfinite(outputs["velocity"])), "Velocity contains NaN/Inf" + assert "consistency_error" in outputs + assert outputs["consistency_error"] >= 0.0 + + model = messaging.new_message('modelV2') + CP = car.CarParams.new_message() + CP.longitudinalActuatorDelay = 0.2 + fill_alpamayo_msg(model.modelV2, outputs, 12345, 0.0, 1e9, CP, 0.1, v_ego) + # these just ensure that the model should outputs same action for the same black pixels + assert model.modelV2.action.desiredAcceleration == pytest.approx(-6.75, abs=1e-2) + assert model.modelV2.action.desiredCurvature == pytest.approx(-0.05, abs=1e-2) + assert not model.modelV2.action.shouldStop + assert model.modelV2.frameId == 12345 diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index be0724db0d..0ea8ba7f94 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -66,7 +66,7 @@ class ModelState(ModelStateBase): self.PLANPLUS_CONTROL: float = 1.0 buffer_length = 5 if self.model_runner.is_20hz else 2 - self.frames = {name: DrivingModelFrame(context, buffer_length) for name in self.model_runner.vision_input_names} + self.frames = {name: DrivingModelFrame(context, 512, 256, buffer_length) for name in self.model_runner.vision_input_names} self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32) # img buffers are managed in openCL transform code diff --git a/sunnypilot/modeld_v2/models/commonmodel.cc b/sunnypilot/modeld_v2/models/commonmodel.cc index 5cd3a84fcc..62ff2c849d 100644 --- a/sunnypilot/modeld_v2/models/commonmodel.cc +++ b/sunnypilot/modeld_v2/models/commonmodel.cc @@ -5,7 +5,11 @@ #include "common/clutil.h" -DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, uint8_t buffer_length) : ModelFrame(device_id, context), buffer_length(buffer_length) { +DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, int width, int height, uint8_t buffer_length) : ModelFrame(device_id, context), MODEL_WIDTH(width), MODEL_HEIGHT(height), buffer_length(buffer_length) { + MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; + buf_size = MODEL_FRAME_SIZE * 2; + frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t); + input_frames = std::make_unique(buf_size); input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err)); img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buffer_length*frame_size_bytes, NULL, &err)); diff --git a/sunnypilot/modeld_v2/models/commonmodel.h b/sunnypilot/modeld_v2/models/commonmodel.h index 8203e064e0..0921c965ef 100644 --- a/sunnypilot/modeld_v2/models/commonmodel.h +++ b/sunnypilot/modeld_v2/models/commonmodel.h @@ -64,15 +64,15 @@ protected: class DrivingModelFrame : public ModelFrame { public: - DrivingModelFrame(cl_device_id device_id, cl_context context, uint8_t buffer_length); + DrivingModelFrame(cl_device_id device_id, cl_context context, int width, int height, uint8_t buffer_length); ~DrivingModelFrame(); cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection); - const int MODEL_WIDTH = 512; - const int MODEL_HEIGHT = 256; - const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; - const int buf_size = MODEL_FRAME_SIZE * 2; - const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t); + int MODEL_WIDTH; + int MODEL_HEIGHT; + int MODEL_FRAME_SIZE; + int buf_size; + size_t frame_size_bytes; const uint8_t buffer_length; private: diff --git a/sunnypilot/modeld_v2/models/commonmodel.pxd b/sunnypilot/modeld_v2/models/commonmodel.pxd index 55023ac4b9..06d86c0b46 100644 --- a/sunnypilot/modeld_v2/models/commonmodel.pxd +++ b/sunnypilot/modeld_v2/models/commonmodel.pxd @@ -20,7 +20,7 @@ cdef extern from "sunnypilot/modeld_v2/models/commonmodel.h": cppclass DrivingModelFrame: int buf_size - DrivingModelFrame(cl_device_id, cl_context, unsigned char) + DrivingModelFrame(cl_device_id, cl_context, int, int, unsigned char) cppclass MonitoringModelFrame: int buf_size diff --git a/sunnypilot/modeld_v2/models/commonmodel_pyx.pyx b/sunnypilot/modeld_v2/models/commonmodel_pyx.pyx index 78a891f031..929c1b6886 100644 --- a/sunnypilot/modeld_v2/models/commonmodel_pyx.pyx +++ b/sunnypilot/modeld_v2/models/commonmodel_pyx.pyx @@ -59,8 +59,8 @@ cdef class ModelFrame: cdef class DrivingModelFrame(ModelFrame): cdef cppDrivingModelFrame * _frame - def __cinit__(self, CLContext context, int buffer_length=2): - self._frame = new cppDrivingModelFrame(context.device_id, context.context, buffer_length) + def __cinit__(self, CLContext context, int width, int height, int buffer_length=2): + self._frame = new cppDrivingModelFrame(context.device_id, context.context, width, height, buffer_length) self.frame = (self._frame) self.buf_size = self._frame.buf_size diff --git a/system/manager/manager.py b/system/manager/manager.py index 36e45488f6..5817890d7c 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -215,7 +215,8 @@ def main() -> None: if __name__ == "__main__": - unblock_stdout() + if sys.platform != 'darwin': + unblock_stdout() try: main() diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 793fbc07fa..4f400f3831 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -80,17 +80,21 @@ def use_sunnylink_uploader_shim(started, params, CP: car.CarParams) -> bool: """Shim for use_sunnylink_uploader to match the process manager signature.""" return use_sunnylink_uploader(params) +def is_modeld_sunny(started: bool, params: Params, CP: car.CarParams) -> bool: + """Check if the active model is modeld_sunny.""" + return bool(params.get_bool("ModeldSunny")) + def is_snpe_model(started, params, CP: car.CarParams) -> bool: """Check if the active model runner is SNPE.""" - return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.snpe) + return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.snpe and not is_modeld_sunny(started, params, CP)) def is_tinygrad_model(started, params, CP: car.CarParams) -> bool: """Check if the active model runner is SNPE.""" - return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.tinygrad) + return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.tinygrad and not is_modeld_sunny(started, params, CP)) def is_stock_model(started, params, CP: car.CarParams) -> bool: """Check if the active model runner is stock.""" - return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.stock) + return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.stock and not is_modeld_sunny(started, params, CP)) def mapd_ready(started: bool, params: Params, CP: car.CarParams) -> bool: return bool(os.path.exists(Paths.mapd_root())) @@ -172,6 +176,7 @@ procs += [ PythonProcess("models_manager", "sunnypilot.models.manager", only_offroad), NativeProcess("modeld_snpe", "sunnypilot/modeld", ["./modeld"], and_(only_onroad, is_snpe_model)), NativeProcess("modeld_tinygrad", "sunnypilot/modeld_v2", ["./modeld"], and_(only_onroad, is_tinygrad_model)), + PythonProcess("modeld_sunny", "sunnypilot.modeld_sunny.modeld", and_(only_onroad, is_modeld_sunny)), # Backup PythonProcess("backup_manager", "sunnypilot.sunnylink.backups.manager", and_(only_offroad, sunnylink_ready_shim)),