diff --git a/sunnypilot/modeld_v2/compile_modeld.py b/sunnypilot/modeld_v2/compile_modeld.py index b9a392631a..5e980295fe 100755 --- a/sunnypilot/modeld_v2/compile_modeld.py +++ b/sunnypilot/modeld_v2/compile_modeld.py @@ -14,6 +14,22 @@ from collections import defaultdict from functools import partial import numpy as np +def _patch_tinygrad_fetch_fw(): + import hashlib + import pathlib + import zstandard + from tinygrad import helpers + _orig_fetch_fw = helpers.fetch_fw + def fetch_fw(path, name, sha256): + 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 _orig_fetch_fw(path, name, sha256) + helpers.fetch_fw = fetch_fw +_patch_tinygrad_fetch_fw() + from tinygrad import dtypes from tinygrad.device import Device from tinygrad.engine.jit import TinyJit @@ -184,6 +200,19 @@ def _parse_size(size_str: str) -> tuple[int, int]: return int(width), int(height) +def read_file_chunked_to_shm(path): + if not path: + return None + from openpilot.common.file_chunker import read_file_chunked + from openpilot.system.hardware.hw import Paths + import atexit + 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: + f.write(read_file_chunked(path)) + return shm_path + + def _compile_for_resolutions(camera_resolutions: list, model_size: tuple[int, int], frame_skip: int, vision_runner, policy_runners: list, metadata: dict) -> dict: return { @@ -226,6 +255,12 @@ if __name__ == "__main__": args = parser.parse_args() output_data = defaultdict(dict) + args.vision_onnx = read_file_chunked_to_shm(args.vision_onnx) + args.policy_onnx = read_file_chunked_to_shm(args.policy_onnx) + args.off_policy_onnx = read_file_chunked_to_shm(args.off_policy_onnx) + args.on_policy_onnx = read_file_chunked_to_shm(args.on_policy_onnx) + args.supercombo_onnx = read_file_chunked_to_shm(args.supercombo_onnx) + vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None if args.model_type == 'vision_policy': diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index b124e3645a..7557715dd4 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -196,7 +196,7 @@ class ModelState(ModelStateBase): inputs[desire_key][0] = 0 self.numpy_inputs[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0) self.prev_desire[:] = inputs[desire_key] - for key in ('traffic_convention', 'lateral_control_params'): + for key in ('traffic_convention', 'lateral_control_params', 'action_t'): if key in self.numpy_inputs and key in inputs: self.numpy_inputs[key][:] = inputs[key] @@ -240,13 +240,20 @@ class ModelState(ModelStateBase): def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action: - plan = model_output['plan'][0] - desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS, - action_t=long_action_t) - desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) + if 'action' not in model_output: + plan = model_output['plan'][0] + desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS, + action_t=long_action_t) + desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) + + curvature_plan = (plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0] + if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan) + desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim) + else: + desired_accel = model_output['action'][0, 1] + desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2 + should_stop = (v_ego < 0.3 and desired_accel < 0.1) - curvature_plan = plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0] if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan - desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim) if self.generation is not None and self.generation >= 10: # smooth curvature for post FOF models if v_ego > self.MIN_LAT_CONTROL_SPEED: desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS) @@ -399,6 +406,12 @@ def main(demo=False): bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names} transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names} + + 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) + lat_action_t = lat_delay + frame_delay + action_delay + long_action_t = long_delay + frame_delay + action_delay + inputs:dict[str, np.ndarray] = { model.desire_key: vec_desire, 'traffic_convention': traffic_convention, @@ -407,6 +420,9 @@ def main(demo=False): if 'lateral_control_params' in model.numpy_inputs: inputs['lateral_control_params'] = np.array([v_ego, lat_delay], dtype=np.float32) + if 'action_t' in model.numpy_inputs: + inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32) + mt1 = time.perf_counter() model_output = model.run(bufs, transforms, inputs, prepare_only) mt2 = time.perf_counter() @@ -418,7 +434,7 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') mdv2sp_send = messaging.new_message('modelDataV2SP') - action = model.get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) + action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, diff --git a/sunnypilot/modeld_v2/parse_model_outputs_split.py b/sunnypilot/modeld_v2/parse_model_outputs_split.py index 831649e3c1..7848e3e185 100644 --- a/sunnypilot/modeld_v2/parse_model_outputs_split.py +++ b/sunnypilot/modeld_v2/parse_model_outputs_split.py @@ -134,6 +134,8 @@ class Parser: out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH)) if 'sim_pose' in outs: self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,)) + if 'action' in outs: + self.parse_mdn('action', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.ACTION_WIDTH,)) def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,)) diff --git a/sunnypilot/models/split_model_constants.py b/sunnypilot/models/split_model_constants.py index a3e1dce8f6..a5f57e5453 100644 --- a/sunnypilot/models/split_model_constants.py +++ b/sunnypilot/models/split_model_constants.py @@ -43,6 +43,7 @@ class SplitModelConstants: LANE_LINES_WIDTH = 2 ROAD_EDGES_WIDTH = 2 PLAN_WIDTH = 15 + ACTION_WIDTH = 2 DESIRE_PRED_WIDTH = 8 LAT_PLANNER_SOLUTION_WIDTH = 4 DESIRED_CURV_WIDTH = 1