diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 503a2be86..8ae77875f 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -78,6 +78,11 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { stock @2; } + struct Override { + key @0 :Text; + value @1 :Text; + } + struct ModelBundle { index @0 :UInt32; internalName @1 :Text; @@ -88,8 +93,9 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { environment @6 :Text; runner @7 :Runner; is20hz @8 :Bool; - ref @9 :Text; # New field + ref @9 :Text; minimumSelectorVersion @10 :UInt32; + overrides @11 :List(Override); } } diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 377ce7c2c..4e842f8d1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -178,10 +178,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_a_target = min(output_a_target_mpc, output_a_target_e2e) self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc - if not self.is_stock: - # To support non Tomb Raider models - output_a_target, self.output_should_stop = output_a_target_mpc, output_should_stop_mpc - for idx in range(2): accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) diff --git a/sunnypilot/modeld/constants.py b/sunnypilot/modeld/constants.py index 8f63afa46..18abc6c96 100644 --- a/sunnypilot/modeld/constants.py +++ b/sunnypilot/modeld/constants.py @@ -1,7 +1,8 @@ import numpy as np def index_function(idx, max_val=192, max_idx=32): - return (max_val) * ((idx/max_idx)**2) + return max_val * ((idx/max_idx)**2) + class ModelConstants: # time and distance indices @@ -63,6 +64,7 @@ class ModelConstants: POLY_PATH_DEGREE = 4 + # model outputs slices class Plan: POSITION = slice(0, 3) @@ -71,6 +73,7 @@ class Plan: T_FROM_CURRENT_EULER = slice(9, 12) ORIENTATION_RATE = slice(12, 15) + class Meta: ENGAGED = slice(0, 1) # next 2, 4, 6, 8, 10 seconds diff --git a/sunnypilot/modeld/fill_model_msg.py b/sunnypilot/modeld/fill_model_msg.py index 1c40425bd..608f24424 100644 --- a/sunnypilot/modeld/fill_model_msg.py +++ b/sunnypilot/modeld/fill_model_msg.py @@ -50,7 +50,7 @@ def fill_xyz_poly(builder, degree, x, y, z): builder.zCoefficients = coeffs[:, 2].tolist() def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, - net_output_data: dict[str, np.ndarray], publish_state: PublishState, + net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float, valid: bool, v_ego: float, steer_delay: float, meta_const) -> None: @@ -76,8 +76,11 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.frameDropPerc = frame_drop_perc driving_model_data.modelExecutionTime = model_execution_time - action = driving_model_data.action - action.desiredCurvature = desired_curvature + # Populate drivingModelData.action + driving_model_data_action = driving_model_data.action + driving_model_data_action.desiredAcceleration = action.desiredAcceleration + driving_model_data_action.shouldStop = action.shouldStop + driving_model_data_action.desiredCurvature = desired_curvature modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -111,8 +114,10 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) # lateral planning - action = modelV2.action - action.desiredCurvature = desired_curvature + modelV2_action = modelV2.action + modelV2_action.desiredAcceleration = action.desiredAcceleration + modelV2_action.shouldStop = action.shouldStop + modelV2_action.desiredCurvature = desired_curvature # times at X_IDXS according to model plan PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index 744b90465..2ac3f5700 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -18,12 +18,15 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.system import sentry from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value + from openpilot.sunnypilot.modeld.runners import ModelRunner, Runtime from openpilot.sunnypilot.modeld.parse_model_outputs import Parser from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState -from openpilot.sunnypilot.modeld.constants import ModelConstants +from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan +from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext -from openpilot.sunnypilot.models.helpers import get_model_path, load_metadata, prepare_inputs, load_meta_constants + PROCESS_NAME = "selfdrive.modeld.modeld_snpe" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -34,6 +37,7 @@ MODEL_PATHS = { METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl' + class FrameMeta: frame_id: int = 0 timestamp_sof: int = 0 @@ -55,6 +59,10 @@ class ModelState: self.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) + bundle = get_active_bundle() + overrides = {override.key: override.value for override in bundle.overrides} + self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".2")) + self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0")) model_paths = get_model_path() self.model_metadata = load_metadata() @@ -118,6 +126,15 @@ class ModelState: self.inputs['prev_desired_curv'][-1:] = outputs['desired_curvature'][0, :] return outputs + def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, + long_action_t: 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], ModelConstants.T_IDXS, + action_t=long_action_t) + desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) + + return log.ModelDataV2.Action(desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop)) + def main(demo=False): cloudlog.warning("modeld init") @@ -158,7 +175,7 @@ def main(demo=False): # messaging pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) - sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() params = Params() @@ -184,8 +201,10 @@ def main(demo=False): cloudlog.info("modeld got CarParams: %s", CP.brand) - # TODO this needs more thought, use .2s extra for now to estimate other delays - steer_delay = CP.steerActuatorDelay + .2 + # Enable lagd support for sunnypilot modeld + steer_delay = sm["liveDelay"].lateralDelay + model.LAT_SMOOTH_SECONDS + long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS + prev_action = log.ModelDataV2.Action() DH = DesireHelper() @@ -280,7 +299,8 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, + action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL) + fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, v_ego, steer_delay, model.meta) diff --git a/sunnypilot/modeld_v2/fill_model_msg.py b/sunnypilot/modeld_v2/fill_model_msg.py index c6129936c..411b8177f 100644 --- a/sunnypilot/modeld_v2/fill_model_msg.py +++ b/sunnypilot/modeld_v2/fill_model_msg.py @@ -3,30 +3,21 @@ import capnp import numpy as np from cereal import log from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan -from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED +from openpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') ConfidenceClass = log.ModelDataV2.ConfidenceClass -def curv_from_psis(psi_target, psi_rate, vego, delay): - vego = np.clip(vego, MIN_SPEED, np.inf) - curv_from_psi = psi_target / (vego * delay) # epsilon to prevent divide-by-zero - return 2 * curv_from_psi - psi_rate / vego +def get_curvature_from_output(output, vego, lat_action_t, current_generation=None): + if current_generation != 11: + if desired_curv := output.get('desired_curvature'): # If the model outputs the desired curvature, use that directly + return float(desired_curv[0, 0]) - -def get_curvature_from_plan(plan, vego, delay): - psi_target = np.interp(delay, ModelConstants.T_IDXS, plan[:, Plan.T_FROM_CURRENT_EULER][:, 2]) - psi_rate = plan[:, Plan.ORIENTATION_RATE][0, 2] - return curv_from_psis(psi_target, psi_rate, vego, delay) - - -def get_curvature_from_output(output, vego, delay): - if desired_curv := output.get('desired_curvature'): # If the model outputs the desired curvature, use that directly - return float(desired_curv[0, 0]) - - return float(get_curvature_from_plan(output['plan'][0], vego, delay)) + plan_output = output['plan'][0] + return float(get_curvature_from_plan(plan_output[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan_output[:, Plan.ORIENTATION_RATE][:, 2], + ModelConstants.T_IDXS, vego, lat_action_t)) class PublishState: @@ -76,7 +67,7 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs): builder.rightProb = lane_line_probs[2] def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, - net_output_data: dict[str, np.ndarray], v_ego: float, delay: float, + net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float, valid: bool, model_meta) -> None: @@ -85,15 +76,13 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D extended_msg.valid = valid base_msg.valid = valid - desired_curvature = float(get_curvature_from_output(net_output_data, v_ego, delay)) - driving_model_data = base_msg.drivingModelData driving_model_data.frameId = vipc_frame_id driving_model_data.frameIdExtra = vipc_frame_id_extra driving_model_data.frameDropPerc = frame_drop_perc driving_model_data.modelExecutionTime = model_execution_time - driving_model_data.action.desiredCurvature = desired_curvature + driving_model_data.action = action modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -126,8 +115,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # poly path fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) - # lateral planning - modelV2.action.desiredCurvature = desired_curvature + # action (includes lateral planning now) + modelV2.action = action # times at X_IDXS according to model plan PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N diff --git a/sunnypilot/modeld_v2/get_model_metadata.py b/sunnypilot/modeld_v2/get_model_metadata.py index 144860204..e0b5adc51 100755 --- a/sunnypilot/modeld_v2/get_model_metadata.py +++ b/sunnypilot/modeld_v2/get_model_metadata.py @@ -4,22 +4,34 @@ import pathlib import onnx import codecs import pickle +from typing import Any + def get_name_and_shape(value_info:onnx.ValueInfoProto) -> tuple[str, tuple[int,...]]: shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim]) name = value_info.name return name, shape + +def get_metadata_value_by_name(model:onnx.ModelProto, name:str) -> str | Any: + for prop in model.metadata_props: + if prop.key == name: + return prop.value + return None + + if __name__ == "__main__": model_path = pathlib.Path(sys.argv[1]) model = onnx.load(str(model_path)) - i = [x.key for x in model.metadata_props].index('output_slices') - output_slices = model.metadata_props[i].value + output_slices = get_metadata_value_by_name(model, 'output_slices') + assert output_slices is not None, 'output_slices not found in metadata' - metadata = {} - metadata['output_slices'] = pickle.loads(codecs.decode(output_slices.encode(), "base64")) - metadata['input_shapes'] = dict([get_name_and_shape(x) for x in model.graph.input]) - metadata['output_shapes'] = dict([get_name_and_shape(x) for x in model.graph.output]) + metadata = { + 'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'), + 'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")), + 'input_shapes': dict([get_name_and_shape(x) for x in model.graph.input]), + 'output_shapes': dict([get_name_and_shape(x) for x in model.graph.output]) + } metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl') with open(metadata_path, 'wb') as f: diff --git a/sunnypilot/modeld_v2/model_runner.py b/sunnypilot/modeld_v2/model_runner.py deleted file mode 100644 index 10c80b876..000000000 --- a/sunnypilot/modeld_v2/model_runner.py +++ /dev/null @@ -1,142 +0,0 @@ -import os -import pickle -from abc import ABC, abstractmethod -import numpy as np - -from cereal import custom -from openpilot.sunnypilot.modeld_v2 import MODEL_PATH, MODEL_PKL_PATH, METADATA_PATH -from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLMem -from openpilot.sunnypilot.modeld_v2.runners.ort_helpers import make_onnx_cpu_runner, ORT_TYPES_TO_NP_TYPES -from openpilot.sunnypilot.modeld_v2.runners.tinygrad_helpers import qcom_tensor_from_opencl_address -from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser -from openpilot.system.hardware import TICI -from openpilot.system.hardware.hw import Paths - -from openpilot.sunnypilot.models.helpers import get_active_bundle -from tinygrad.tensor import Tensor - -if TICI: - os.environ['QCOM'] = '1' - -SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') -CUSTOM_MODEL_PATH = Paths.model_root() -ModelManager = custom.ModelManagerSP - - -class ModelRunner(ABC): - """Abstract base class for model runners that defines the interface for running ML models.""" - - def __init__(self): - """Initialize the model runner with paths to model and metadata files.""" - metadata_path = METADATA_PATH - self.is_20hz = None - self._drive_model = None - self._metadata_model = None - - if bundle := get_active_bundle(): - bundle_models = {model.type.raw: model for model in bundle.models} - self._drive_model = bundle_models.get(ModelManager.Model.Type.supercombo) - self._metadata_model = self._drive_model.metadata - self.is_20hz = bundle.is20hz - - # Override the metadata path if a metadata model is found in the active bundle - if self._metadata_model: - metadata_path = f"{CUSTOM_MODEL_PATH}/{self._metadata_model.fileName}" - - with open(metadata_path, 'rb') as f: - self.model_metadata = pickle.load(f) - - self.input_shapes = self.model_metadata['input_shapes'] - self.output_slices = self.model_metadata['output_slices'] - self.inputs: dict = {} - self.parser = Parser() - - @abstractmethod - def prepare_inputs(self, imgs_cl: dict[str, CLMem], numpy_inputs: dict[str, np.ndarray], frames: dict[str, DrivingModelFrame]) -> dict: - """Prepare inputs for model inference.""" - raise NotImplementedError - - @abstractmethod - def _run_model(self): - """Run model inference with prepared inputs.""" - raise NotImplementedError("This method should be implemented in subclasses.") - - def _slice_outputs(self, model_outputs: np.ndarray) -> dict: - """Slice model outputs according to metadata configuration.""" - parsed_outputs = {k: model_outputs[np.newaxis, v] for k, v in self.output_slices.items()} - if SEND_RAW_PRED: - parsed_outputs['raw_pred'] = model_outputs.copy() - return parsed_outputs - - def run_model(self) -> dict[str, np.ndarray]: - """Run model inference with prepared inputs and parse outputs.""" - result: dict[str, np.ndarray] = self.parser.parse_outputs(self._slice_outputs(self._run_model())) - return result - - -class TinygradRunner(ModelRunner): - """Tinygrad implementation of model runner for TICI hardware.""" - - def __init__(self): - super().__init__() - - model_pkl_path = MODEL_PKL_PATH - if self._drive_model: - model_pkl_path = f"{CUSTOM_MODEL_PATH}/{self._drive_model.artifact.fileName}" - assert model_pkl_path.endswith('_tinygrad.pkl'), f"Invalid model file: {model_pkl_path} for TinygradRunner" - - # Load Tinygrad model - with open(model_pkl_path, "rb") as f: - try: - self.model_run = pickle.load(f) - except FileNotFoundError as e: - assert "/dev/kgsl-3d0" not in str(e), "Model was built on C3 or C3X, but is being loaded on PC" - raise - - self.input_to_dtype = {} - self.input_to_device = {} - - for idx, name in enumerate(self.model_run.captured.expected_names): - self.input_to_dtype[name] = self.model_run.captured.expected_st_vars_dtype_device[idx][2] # 2 is the dtype - self.input_to_device[name] = self.model_run.captured.expected_st_vars_dtype_device[idx][3] # 3 is the device - - def prepare_inputs(self, imgs_cl: dict[str, CLMem], numpy_inputs: dict[str, np.ndarray], frames: dict[str, DrivingModelFrame]) -> dict: - # Initialize image tensors if not already done - for key in imgs_cl: - if TICI and key not in self.inputs: - self.inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.input_shapes[key], dtype=self.input_to_dtype[key]) - elif not TICI: - shape = frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key]) - self.inputs[key] = Tensor(shape, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize() - - # Update numpy inputs - for key, value in numpy_inputs.items(): - if key not in imgs_cl: - self.inputs[key] = Tensor(value, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize() - - return self.inputs - - def _run_model(self): - return self.model_run(**self.inputs).numpy().flatten() - - -class ONNXRunner(ModelRunner): - """ONNX implementation of model runner for non-TICI hardware.""" - - def __init__(self): - super().__init__() - self.runner = make_onnx_cpu_runner(MODEL_PATH) - - self.input_to_nptype = { - model_input.name: ORT_TYPES_TO_NP_TYPES[model_input.type] - for model_input in self.runner.get_inputs() - } - - def prepare_inputs(self, imgs_cl: dict[str, CLMem], numpy_inputs: dict[str, np.ndarray], frames: dict[str, DrivingModelFrame]) -> dict: - self.inputs = numpy_inputs - for key in imgs_cl: - self.inputs[key] = frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key]).astype(dtype=self.input_to_nptype[key]) - return self.inputs - - def _run_model(self): - return self.runner.run(None, self.inputs)[0].flatten() diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index a52d844ae..4c8acafca 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -1,7 +1,4 @@ #!/usr/bin/env python3 -from openpilot.system.hardware import TICI - -# import time import numpy as np import cereal.messaging as messaging @@ -13,20 +10,22 @@ from opendbc.car.car_helpers import get_demo_car_params from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.realtime import config_realtime_process +from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.system import sentry from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper -from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState -from openpilot.sunnypilot.modeld_v2.constants import ModelConstants -from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLContext +from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value +from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output +from openpilot.sunnypilot.modeld_v2.constants import Plan +from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLContext from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants -from openpilot.sunnypilot.modeld_v2.model_runner import ONNXRunner, TinygradRunner + +from openpilot.sunnypilot.models.helpers import get_active_bundle +from openpilot.sunnypilot.models.runners.helpers import get_model_runner PROCESS_NAME = "selfdrive.modeld.modeld" -LAT_SMOOTH_SECONDS = 0.0 class FrameMeta: @@ -38,23 +37,32 @@ class FrameMeta: if vipc is not None: self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof + class ModelState: frames: dict[str, DrivingModelFrame] inputs: dict[str, np.ndarray] prev_desire: np.ndarray # for tracking the rising edge of the pulse + temporal_idxs: slice | np.ndarray def __init__(self, context: CLContext): try: - self.model_runner = TinygradRunner() if TICI else ONNXRunner() + self.model_runner = get_model_runner() + self.constants = self.model_runner.constants except Exception as e: cloudlog.exception(f"Failed to initialize model runner: {str(e)}") + raise + + model_bundle = get_active_bundle() + self.generation = model_bundle.generation + overrides = {override.key: override.value for override in model_bundle.overrides} + + self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".2")) + self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0")) + self.MIN_LAT_CONTROL_SPEED = 0.3 buffer_length = 5 if self.model_runner.is_20hz else 2 self.frames = {'input_imgs': DrivingModelFrame(context, buffer_length), 'big_input_imgs': DrivingModelFrame(context, buffer_length)} - self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - if self.model_runner.is_20hz: - self.full_features_buffer = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) - self.full_desire = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32) + self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32) # img buffers are managed in openCL transform code self.numpy_inputs = {} @@ -63,11 +71,19 @@ class ModelState: if key not in self.frames: # Managed by opencl self.numpy_inputs[key] = np.zeros(shape, dtype=np.float32) - if self.model_runner.is_20hz: + if self.model_runner.is_20hz_3d: # split models + self.full_features_buffer = np.zeros((1, self.constants.FULL_HISTORY_BUFFER_LEN, self.constants.FEATURE_LEN), dtype=np.float32) + self.full_desire = np.zeros((1, self.constants.FULL_HISTORY_BUFFER_LEN, self.constants.DESIRE_LEN), dtype=np.float32) + self.full_prev_desired_curv = np.zeros((1, self.constants.FULL_HISTORY_BUFFER_LEN, self.constants.PREV_DESIRED_CURV_LEN), dtype=np.float32) + self.temporal_idxs = slice(-1-(self.constants.TEMPORAL_SKIP*(self.constants.INPUT_HISTORY_BUFFER_LEN-1)), None, self.constants.TEMPORAL_SKIP) + elif self.model_runner.is_20hz and not self.model_runner.is_20hz_3d: + self.full_features_buffer = np.zeros((self.constants.FULL_HISTORY_BUFFER_LEN + 1, self.constants.FEATURE_LEN), dtype=np.float32) + self.full_desire = np.zeros((self.constants.FULL_HISTORY_BUFFER_LEN + 1, self.constants.DESIRE_LEN), dtype=np.float32) num_elements = self.numpy_inputs['features_buffer'].shape[1] step_size = int(-100 / num_elements) - self.full_features_buffer_idxs = np.arange(step_size, step_size * (num_elements + 1), step_size)[::-1] - self.desire_reshape_dims = (self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], -1, self.numpy_inputs['desire'].shape[2]) + self.temporal_idxs = np.arange(step_size, step_size * (num_elements + 1), step_size)[::-1] + self.desire_reshape_dims = (self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], -1, + self.numpy_inputs['desire'].shape[2]) def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray, inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None: @@ -76,11 +92,15 @@ class ModelState: new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) self.prev_desire[:] = inputs['desire'] - if self.model_runner.is_20hz: + if self.model_runner.is_20hz_3d: # split models + self.full_desire[0,:-1] = self.full_desire[0,1:] + self.full_desire[0,-1] = new_desire + self.numpy_inputs['desire'][:] = self.full_desire.reshape((1, self.constants.INPUT_HISTORY_BUFFER_LEN, self.constants.TEMPORAL_SKIP, -1)).max(axis=2) + elif self.model_runner.is_20hz and not self.model_runner.is_20hz_3d: # 20hz supercombo self.full_desire[:-1] = self.full_desire[1:] self.full_desire[-1] = new_desire self.numpy_inputs['desire'][:] = self.full_desire.reshape(self.desire_reshape_dims).max(axis=2) - else: + else: # not 20hz length = inputs['desire'].shape[0] self.numpy_inputs['desire'][0, :-1] = self.numpy_inputs['desire'][0, 1:] self.numpy_inputs['desire'][0, -1, :length] = new_desire[:length] @@ -101,11 +121,15 @@ class ModelState: # Run model inference outputs = self.model_runner.run_model() - if self.model_runner.is_20hz: + if self.model_runner.is_20hz_3d: # split models + self.full_features_buffer[0, :-1] = self.full_features_buffer[0, 1:] + self.full_features_buffer[0, -1] = outputs['hidden_state'][0, :] + self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs] + elif self.model_runner.is_20hz and not self.model_runner.is_20hz_3d: # 20hz supercombo self.full_features_buffer[:-1] = self.full_features_buffer[1:] self.full_features_buffer[-1] = outputs['hidden_state'][0, :] - self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[self.full_features_buffer_idxs] - else: + self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[self.temporal_idxs] + else: # not 20hz feature_len = outputs['hidden_state'].shape[1] self.numpy_inputs['features_buffer'][0, :-1] = self.numpy_inputs['features_buffer'][0, 1:] self.numpy_inputs['features_buffer'][0, -1, :feature_len] = outputs['hidden_state'][0, :feature_len] @@ -119,11 +143,36 @@ class ModelState: input_name_prev = 'prev_desired_curv' if input_name_prev is not None: - length = outputs['desired_curvature'][0].size - self.numpy_inputs[input_name_prev][0, :-length, 0] = self.numpy_inputs[input_name_prev][0, length:, 0] - self.numpy_inputs[input_name_prev][0, -length:, 0] = outputs['desired_curvature'][0] + self.process_desired_curvature(outputs, input_name_prev) return outputs + def process_desired_curvature(self, outputs, input_name_prev): + if self.model_runner.is_20hz_3d: # split models + self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:] + self.full_prev_desired_curv[0,-1,:] = outputs['desired_curvature'][0, :] + self.numpy_inputs[input_name_prev][:] = self.full_prev_desired_curv[0, self.temporal_idxs] + if self.generation == 11: + self.numpy_inputs[input_name_prev][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs] + else: + length = outputs['desired_curvature'][0].size + self.numpy_inputs[input_name_prev][0, :-length, 0] = self.numpy_inputs[input_name_prev][0, length:, 0] + self.numpy_inputs[input_name_prev][0, -length:, 0] = outputs['desired_curvature'][0] + + 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) + + desired_curvature = get_curvature_from_output(model_output, v_ego, lat_action_t, self.generation) + if v_ego > self.MIN_LAT_CONTROL_SPEED: + desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS) + else: + desired_curvature = prev_action.desiredCurvature + + return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop)) + def main(demo=False): cloudlog.warning("modeld init") @@ -170,7 +219,7 @@ def main(demo=False): params = Params() # setup filter to track dropped frames - frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ) + frame_dropped_filter = FirstOrderFilter(0., 10., 1. / model.constants.MODEL_FREQ) frame_id = 0 last_vipc_frame_id = 0 run_count = 0 @@ -189,8 +238,9 @@ def main(demo=False): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("modeld got CarParams: %s", CP.brand) - # Enable lagd support for modeld_v2 - steer_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS + # TODO Move smooth seconds to action function + long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS + prev_action = log.ModelDataV2.Action() DH = DesireHelper() @@ -220,7 +270,7 @@ def main(demo=False): 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})") + extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})") else: # Use single camera @@ -232,18 +282,20 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) + steer_delay = sm["liveDelay"].lateralDelay + model.LAT_SMOOTH_SECONDS 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))] - model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32) + model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, + False).astype(np.float32) model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32) live_calib_seen = True traffic_convention = np.zeros(2) traffic_convention[int(is_rhd)] = 1 - vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - if desire >= 0 and desire < ModelConstants.DESIRE_LEN: + vec_desire = np.zeros(model.constants.DESIRE_LEN, dtype=np.float32) + if desire >= 0 and desire < model.constants.DESIRE_LEN: vec_desire[desire] = 1 # tracked dropped frames @@ -276,7 +328,10 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay, + + action = model.get_action_from_model(model_output, prev_action, steer_delay + DT_MDL, long_delay + DT_MDL, 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, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, load_meta_constants()) diff --git a/sunnypilot/modeld_v2/parse_model_outputs_split.py b/sunnypilot/modeld_v2/parse_model_outputs_split.py new file mode 100644 index 000000000..7fab66e03 --- /dev/null +++ b/sunnypilot/modeld_v2/parse_model_outputs_split.py @@ -0,0 +1,127 @@ +import numpy as np +from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants + + +def safe_exp(x, out=None): + # -11 is around 10**14, more causes float16 overflow + return np.exp(np.clip(x, -np.inf, 11), out=out) + + +def sigmoid(x): + return 1. / (1. + safe_exp(-x)) + + +def softmax(x, axis=-1): + x -= np.max(x, axis=axis, keepdims=True) + if x.dtype == np.float32 or x.dtype == np.float64: + safe_exp(x, out=x) + else: + x = safe_exp(x) + x /= np.sum(x, axis=axis, keepdims=True) + return x + + +class Parser: + def __init__(self, ignore_missing=False): + self.ignore_missing = ignore_missing + + def check_missing(self, outs, name): + if name not in outs and not self.ignore_missing: + raise ValueError(f"Missing output {name}") + return name not in outs + + def parse_categorical_crossentropy(self, name, outs, out_shape=None): + if self.check_missing(outs, name): + return + raw = outs[name] + if out_shape is not None: + raw = raw.reshape((raw.shape[0],) + out_shape) + outs[name] = softmax(raw, axis=-1) + + def parse_binary_crossentropy(self, name, outs): + if self.check_missing(outs, name): + return + raw = outs[name] + outs[name] = sigmoid(raw) + + def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None): + if self.check_missing(outs, name): + return + raw = outs[name] + raw = raw.reshape((raw.shape[0], max(in_N, 1), -1)) + + n_values = (raw.shape[2] - out_N)//2 + pred_mu = raw[:,:,:n_values] + pred_std = safe_exp(raw[:,:,n_values: 2*n_values]) + + if in_N > 1: + weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype) + for i in range(out_N): + weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1) + + if out_N == 1: + for fidx in range(weights.shape[0]): + idxs = np.argsort(weights[fidx][:,0])[::-1] + weights[fidx] = weights[fidx][idxs] + pred_mu[fidx] = pred_mu[fidx][idxs] + pred_std[fidx] = pred_std[fidx][idxs] + full_shape = tuple([raw.shape[0], in_N] + list(out_shape)) + outs[name + '_weights'] = weights + outs[name + '_hypotheses'] = pred_mu.reshape(full_shape) + outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape) + + pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype) + pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype) + for fidx in range(weights.shape[0]): + for hidx in range(out_N): + idxs = np.argsort(weights[fidx,:,hidx])[::-1] + pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]] + pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]] + else: + pred_mu_final = pred_mu + pred_std_final = pred_std + + if out_N > 1: + final_shape = tuple([raw.shape[0], out_N] + list(out_shape)) + else: + final_shape = tuple([raw.shape[0],] + list(out_shape)) + outs[name] = pred_mu_final.reshape(final_shape) + outs[name + '_stds'] = pred_std_final.reshape(final_shape) + + def split_outputs(self, outs: dict[str, np.ndarray]) -> None: + if 'lane_lines' in outs: + self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, + out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH)) + self.parse_mdn('road_edges', outs, in_N=0, out_N=0, + out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH)) + self.parse_mdn('lead', outs, in_N=SplitModelConstants.LEAD_MHP_N, out_N=SplitModelConstants.LEAD_MHP_SELECTION, + out_shape=(SplitModelConstants.LEAD_TRAJ_LEN,SplitModelConstants.LEAD_WIDTH)) + if 'sim_pose' in outs: + self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,)) + for k in ['lead_prob', 'lane_lines_prob']: + self.parse_binary_crossentropy(k, outs) + + 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,)) + self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.WIDE_FROM_DEVICE_WIDTH,)) + self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,)) + self.split_outputs(outs) + self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(SplitModelConstants.DESIRE_PRED_LEN,SplitModelConstants.DESIRE_PRED_WIDTH)) + self.parse_binary_crossentropy('meta', outs) + return outs + + def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: + self.parse_mdn('plan', outs, in_N=SplitModelConstants.PLAN_MHP_N, out_N=SplitModelConstants.PLAN_MHP_SELECTION, + out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.PLAN_WIDTH)) + self.split_outputs(outs) + if 'lat_planner_solution' in outs: + self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.LAT_PLANNER_SOLUTION_WIDTH)) + if 'desired_curvature' in outs: + self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.DESIRED_CURV_WIDTH,)) + self.parse_categorical_crossentropy('desire_state', outs, out_shape=(SplitModelConstants.DESIRE_PRED_WIDTH,)) + return outs + + def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: + outs = self.parse_vision_outputs(outs) + outs = self.parse_policy_outputs(outs) + return outs diff --git a/sunnypilot/models/fetcher.py b/sunnypilot/models/fetcher.py index 7076cf186..27fac0867 100644 --- a/sunnypilot/models/fetcher.py +++ b/sunnypilot/models/fetcher.py @@ -43,6 +43,16 @@ class ModelParser: model.metadata = ModelParser._parse_artifact(metadata) return model + @staticmethod + def _parse_overrides(overrides_data: dict[str, str]) -> list[custom.ModelManagerSP.Override]: + overrides = [] + for key, value in overrides_data.items(): + override = custom.ModelManagerSP.Override() + override.key = key + override.value = value + overrides.append(override) + return overrides + @staticmethod def _parse_bundle(bundle) -> custom.ModelManagerSP.ModelBundle: model_bundle = custom.ModelManagerSP.ModelBundle() @@ -56,6 +66,7 @@ class ModelParser: model_bundle.runner = bundle.get("runner", custom.ModelManagerSP.Runner.snpe) model_bundle.is20hz = bundle.get("is_20hz", False) model_bundle.minimumSelectorVersion = int(bundle["minimum_selector_version"]) + model_bundle.overrides = ModelParser._parse_overrides(bundle.get("overrides", {})) return model_bundle @@ -149,8 +160,9 @@ if __name__ == "__main__": bundles = model_fetcher.get_available_bundles() for bundle in bundles: for model in bundle.models: + model_overrides = {override.key: override.value for override in bundle.overrides} # Print model details - print(f"Bundle: {bundle.internalName}, Type: {model.type}, Status: {bundle.status}") + print(f"Bundle: {bundle.internalName}, Type: {model.type}, Status: {bundle.status}, Overrides: {model_overrides}") # Print artifact details print(f"Artifact: {model.artifact.fileName}, Download URI: {model.artifact.downloadUri.uri}") # Print metadata details diff --git a/sunnypilot/models/helpers.py b/sunnypilot/models/helpers.py index e6001f264..344a5179c 100644 --- a/sunnypilot/models/helpers.py +++ b/sunnypilot/models/helpers.py @@ -19,7 +19,7 @@ from openpilot.system.hardware import PC from openpilot.system.hardware.hw import Paths from pathlib import Path -CURRENT_SELECTOR_VERSION = 3 +CURRENT_SELECTOR_VERSION = 4 REQUIRED_MIN_SELECTOR_VERSION = 2 USE_ONNX = os.getenv('USE_ONNX', PC) diff --git a/sunnypilot/models/runners/constants.py b/sunnypilot/models/runners/constants.py new file mode 100644 index 000000000..cbd1fdb37 --- /dev/null +++ b/sunnypilot/models/runners/constants.py @@ -0,0 +1,18 @@ +import os +import numpy as np +from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLMem +from openpilot.system.hardware.hw import Paths +from cereal import custom + +# Type definitions for clarity +NumpyDict = dict[str, np.ndarray] +ShapeDict = dict[str, tuple[int, ...]] +SliceDict = dict[str, slice] +CLMemDict = dict[str, CLMem] +FrameDict = dict[str, DrivingModelFrame] + +ModelType = custom.ModelManagerSP.Model.Type +Model = custom.ModelManagerSP.Model + +SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') +CUSTOM_MODEL_PATH = Paths.model_root() diff --git a/sunnypilot/models/runners/helpers.py b/sunnypilot/models/runners/helpers.py new file mode 100644 index 000000000..6a128b340 --- /dev/null +++ b/sunnypilot/models/runners/helpers.py @@ -0,0 +1,35 @@ +from openpilot.sunnypilot.models.helpers import get_active_bundle +from openpilot.sunnypilot.models.runners.model_runner import ModelRunner +from openpilot.sunnypilot.models.runners.tinygrad.tinygrad_runner import TinygradRunner, TinygradSplitRunner +from openpilot.sunnypilot.models.runners.constants import ModelType +from openpilot.system.hardware import TICI + +if not TICI: + from openpilot.sunnypilot.models.runners.onnx.onnx_runner import ONNXRunner + +def get_model_runner() -> ModelRunner: + """ + Factory function to create and return the appropriate ModelRunner instance. + + Selects between ONNXRunner (for non-TICI platforms) and TinygradRunner + (for TICI platforms), choosing TinygradSplitRunner if separate vision/policy + models are detected in the active bundle. + + :return: An instance of a ModelRunner subclass (ONNXRunner, TinygradRunner, or TinygradSplitRunner). + """ + if not TICI: + return ONNXRunner() + + # On TICI platforms, use Tinygrad runners + bundle = get_active_bundle() + if bundle and bundle.models: + model_types = {m.type.raw for m in bundle.models} + # Check if the bundle uses separate vision and policy models + if ModelType.vision in model_types or ModelType.policy in model_types: + return TinygradSplitRunner() + # Otherwise, assume a single model (likely supercombo) + if bundle.models: + return TinygradRunner(bundle.models[0].type.raw) + + # Default fallback to TinygradRunner with the supercombo type if bundle info is missing/incomplete + return TinygradRunner(ModelType.supercombo) diff --git a/sunnypilot/models/runners/model_runner.py b/sunnypilot/models/runners/model_runner.py new file mode 100644 index 000000000..c210d5b87 --- /dev/null +++ b/sunnypilot/models/runners/model_runner.py @@ -0,0 +1,176 @@ +import os +from abc import abstractmethod, ABC + +import numpy as np +from openpilot.sunnypilot.models.helpers import get_active_bundle +from openpilot.system.hardware import TICI +from openpilot.sunnypilot.models.runners.constants import NumpyDict, ShapeDict, CLMemDict, FrameDict, Model, SliceDict, SEND_RAW_PRED +from openpilot.system.hardware.hw import Paths +import pickle + +CUSTOM_MODEL_PATH = Paths.model_root() + + +# Set QCOM environment variable for TICI devices, potentially enabling hardware acceleration +if TICI: + os.environ['QCOM'] = '1' + + +class ModelData: + """ + Stores metadata and configuration for a specific machine learning model. + + This class loads model metadata (like input shapes and output slices) + from a pickle file associated with a model instance. + + :param model: The machine learning model object containing metadata. + """ + def __init__(self, model: Model): + self.model = model + self.metadata = model.metadata + self.input_shapes: ShapeDict = {} + self.output_slices: SliceDict = {} + if self.metadata: + self._load_metadata() + + def _load_metadata(self) -> None: + """Loads input shapes and output slices from the model's metadata pickle file.""" + metadata_path = f"{CUSTOM_MODEL_PATH}/{self.metadata.fileName}" + with open(metadata_path, 'rb') as f: + model_metadata = pickle.load(f) + self.input_shapes = model_metadata.get('input_shapes', {}) + self.output_slices = model_metadata.get('output_slices', {}) + + +class ModularRunner(ABC): + """ + Represents a modular runner for handling and slicing model outputs. + + This abstract base class is designed to provide an interface for modular + parsing and processing of model outputs. Classes inheriting from it must + implement the specified abstract methods, defining how model outputs + should be handled and stored. The primary goal is to enable structured + parsing of outputs through a dictionary-based method mapping. + + :ivar parser_method_dict: Mapping dictionary containing parser methods + for handling specific types of outputs. + :type parser_method_dict: dict + """ + + @property + @abstractmethod + def parser_method_dict(self) -> dict: + pass + + @parser_method_dict.setter + @abstractmethod + def parser_method_dict(self, value: dict) -> None: + pass + + @abstractmethod + def _slice_outputs(self, model_outputs: np.ndarray) -> NumpyDict: + pass + + +class ModelRunner(ModularRunner): + """ + Abstract base class for managing and executing machine learning models. + + Provides a common interface for loading models, preparing inputs, running + inference, and slicing/parsing outputs based on model metadata. Derived + classes implement the specifics of input preparation and model execution + for different frameworks (e.g., Tinygrad, ONNX). + """ + + def __init__(self): + """Initializes the model runner, loading the active model bundle.""" + self.is_20hz: bool | None = None + self.is_20hz_3d: bool | None = None + self.models: dict[int, ModelData] = {} + self._model_data: ModelData | None = None # Active model data for current operation + self._parser_method_dict: dict = {} + self.inputs: dict = {} + self._parser = None + self._load_models() + self._constants = None + + @property + def constants(self): + return self._constants + + @property + def parser_method_dict(self) -> dict: + """Returns the dictionary mapping model types to their respective parsing methods.""" + return self._parser_method_dict + + @parser_method_dict.setter + def parser_method_dict(self, value: dict) -> None: + """Sets the dictionary mapping model types to their respective parsing methods.""" + self._parser_method_dict = value + + def _load_models(self) -> None: + """Loads the active model bundle configuration and sets up ModelData.""" + bundle = get_active_bundle() + if not bundle: + raise ValueError("No active model bundle found, why are we being executed?") + + self.models = {model.type.raw: ModelData(model) for model in bundle.models} + self.is_20hz = bundle.is20hz + self.is_20hz_3d = False + + @property + def input_shapes(self) -> ShapeDict: + """Returns the input shapes for the currently active model.""" + if self._model_data: + return self._model_data.input_shapes + raise ValueError("Model data is not available. Ensure the model is loaded correctly.") + + @property + def output_slices(self) -> SliceDict: + """Returns the output slices for the currently active model.""" + if self._model_data: + return self._model_data.output_slices + raise ValueError("Model data is not available. Ensure the model is loaded correctly.") + + @abstractmethod + def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict: + """ + Abstract method to prepare inputs for model inference. + + :param imgs_cl: Dictionary of OpenCL memory objects for image inputs. + :param numpy_inputs: Dictionary of numpy arrays for non-image inputs. + :param frames: Dictionary of DrivingModelFrame objects for context. + :return: Dictionary of prepared inputs ready for the model. + """ + raise NotImplementedError + + @abstractmethod + def _run_model(self) -> NumpyDict: + """ + Abstract method to execute model inference with prepared inputs. + + :return: Dictionary containing the model's raw output arrays. + """ + raise NotImplementedError + + def _slice_outputs(self, model_outputs: np.ndarray) -> NumpyDict: + """ + Slices the raw model output array based on the output_slices metadata. + + :param model_outputs: The raw numpy array output from the model. + :return: A dictionary where keys are output names and values are sliced numpy arrays. + """ + if not self._model_data: + raise ValueError("Model data is not available. Ensure the model is loaded correctly.") + sliced_outputs = {k: model_outputs[np.newaxis, v] for k, v in self._model_data.output_slices.items()} + if SEND_RAW_PRED: + sliced_outputs['raw_pred'] = model_outputs.copy() # Optionally include the full raw output + return sliced_outputs + + def run_model(self) -> NumpyDict: + """ + Executes the model inference pipeline: runs the model and parses outputs. + + :return: Dictionary containing the final parsed model outputs. + """ + return self._run_model() # Parsing is handled within specific runner implementations diff --git a/sunnypilot/models/runners/onnx/onnx_runner.py b/sunnypilot/models/runners/onnx/onnx_runner.py new file mode 100644 index 000000000..1ffead456 --- /dev/null +++ b/sunnypilot/models/runners/onnx/onnx_runner.py @@ -0,0 +1,62 @@ +import numpy as np + +from openpilot.sunnypilot.modeld_v2 import MODEL_PATH +from openpilot.sunnypilot.modeld_v2.runners.ort_helpers import make_onnx_cpu_runner, ORT_TYPES_TO_NP_TYPES +from openpilot.sunnypilot.models.runners.constants import ModelType, ShapeDict, CLMemDict, NumpyDict, FrameDict +from openpilot.sunnypilot.models.runners.model_runner import ModelRunner +from openpilot.sunnypilot.modeld_v2.constants import ModelConstants + + +class ONNXRunner(ModelRunner): + """ + A ModelRunner implementation for executing ONNX models using ONNX Runtime CPU. + + Handles loading the ONNX model, preparing inputs as numpy arrays, running + inference, and parsing outputs. This runner is typically used on non-TICI platforms. + """ + def __init__(self): + super().__init__() + # Initialize ONNX Runtime session for the model at MODEL_PATH + self.runner = make_onnx_cpu_runner(MODEL_PATH) + # Map expected input names to numpy dtypes + self.input_to_nptype = { + model_input.name: ORT_TYPES_TO_NP_TYPES[model_input.type] + for model_input in self.runner.get_inputs() + } + # For ONNX, _model_data isn't strictly necessary as shapes/types come from the runner + # However, we might still need output_slices if custom models define them. + # We assume supercombo type for potentially loading output_slices metadata if available. + self._model_data = self.models.get(ModelType.supercombo) + self._constants = ModelConstants # Constants for ONNX models, if needed + + @property + def input_shapes(self) -> ShapeDict: + """Returns the input shapes defined in the ONNX model.""" + # ONNX shapes are derived directly from the model definition via the runner + return {runner_input.name: runner_input.shape for runner_input in self.runner.get_inputs()} + + def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict: + """Prepares inputs for the ONNX model as numpy arrays.""" + self.inputs = numpy_inputs # Start with non-image numpy inputs + # Convert image inputs from OpenCL buffers to numpy arrays + for key in imgs_cl: + buffer = frames[key].buffer_from_cl(imgs_cl[key]) + reshaped_buffer = buffer.reshape(self.input_shapes[key]) + self.inputs[key] = reshaped_buffer.astype(dtype=self.input_to_nptype[key]) + return self.inputs + + def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict: + """Parses the raw ONNX model outputs using the standard Parser.""" + # Use slicing if metadata is available, otherwise pass raw outputs + if self._model_data is None: + raise ValueError("Model data is not available. Ensure the model is loaded correctly.") + + outputs_to_parse = self._slice_outputs(model_outputs) if self._model_data else {'raw_pred': model_outputs} + result: NumpyDict = self.parser_method_dict[self._model_data.model.type.raw](outputs_to_parse) + return result + + def _run_model(self) -> NumpyDict: + """Runs the ONNX model inference and parses the outputs.""" + # Execute the ONNX Runtime session + outputs = self.runner.run(None, self.inputs)[0].flatten() + return self._parse_outputs(outputs) diff --git a/sunnypilot/models/runners/tinygrad/model_types.py b/sunnypilot/models/runners/tinygrad/model_types.py new file mode 100644 index 000000000..ba388aed9 --- /dev/null +++ b/sunnypilot/models/runners/tinygrad/model_types.py @@ -0,0 +1,59 @@ +import os +from abc import ABC + +import numpy as np +from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser +from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser +from openpilot.sunnypilot.models.runners.constants import ModelType, NumpyDict +from openpilot.sunnypilot.models.runners.model_runner import ModularRunner +from openpilot.system.hardware.hw import Paths + + +SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') +CUSTOM_MODEL_PATH = Paths.model_root() + + +class PolicyTinygrad(ModularRunner, ABC): + """ + A TinygradRunner specialized for policy-only models. + + Uses a SplitParser to handle outputs specific to the policy part of a split model setup. + """ + def __init__(self): + self._policy_parser = SplitParser() + self.parser_method_dict[ModelType.policy] = self._parse_policy_outputs + + def _parse_policy_outputs(self, model_outputs: np.ndarray) -> NumpyDict: + """Parses policy model outputs using SplitParser.""" + result: NumpyDict = self._policy_parser.parse_policy_outputs(self._slice_outputs(model_outputs)) + return result + +class VisionTinygrad(ModularRunner, ABC): + """ + A TinygradRunner specialized for vision-only models. + + Uses a SplitParser to handle outputs specific to the vision part of a split model setup. + """ + def __init__(self): + self._vision_parser = SplitParser() + self.parser_method_dict[ModelType.vision] = self._parse_vision_outputs + + def _parse_vision_outputs(self, model_outputs: np.ndarray) -> NumpyDict: + """Parses vision model outputs using SplitParser.""" + result: NumpyDict = self._vision_parser.parse_vision_outputs(self._slice_outputs(model_outputs)) + return result + +class SupercomboTinygrad(ModularRunner, ABC): + """ + A TinygradRunner specialized for vision-only models. + + Uses a SplitParser to handle outputs specific to the vision part of a split model setup. + """ + def __init__(self): + self._supercombo_parser = CombinedParser() + self.parser_method_dict[ModelType.supercombo] = self._parse_supercombo_outputs + + def _parse_supercombo_outputs(self, model_outputs: np.ndarray) -> NumpyDict: + """Parses vision model outputs using SplitParser.""" + result: NumpyDict = self._supercombo_parser.parse_outputs(self._slice_outputs(model_outputs)) + return result diff --git a/sunnypilot/models/runners/tinygrad/tinygrad_runner.py b/sunnypilot/models/runners/tinygrad/tinygrad_runner.py new file mode 100644 index 000000000..2e4fd4529 --- /dev/null +++ b/sunnypilot/models/runners/tinygrad/tinygrad_runner.py @@ -0,0 +1,129 @@ +import pickle + +import numpy as np +from openpilot.sunnypilot.modeld_v2.runners.tinygrad_helpers import qcom_tensor_from_opencl_address +from openpilot.sunnypilot.models.runners.constants import CLMemDict, FrameDict, NumpyDict, ModelType, ShapeDict, CUSTOM_MODEL_PATH, SliceDict +from openpilot.sunnypilot.models.runners.model_runner import ModelRunner +from openpilot.sunnypilot.models.runners.tinygrad.model_types import PolicyTinygrad, VisionTinygrad, SupercomboTinygrad +from openpilot.system.hardware import TICI +from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants +from openpilot.sunnypilot.modeld_v2.constants import ModelConstants + +from tinygrad.tensor import Tensor + + +class TinygradRunner(ModelRunner, SupercomboTinygrad, PolicyTinygrad, VisionTinygrad): + """ + A ModelRunner implementation for executing Tinygrad models. + + Handles loading Tinygrad model artifacts (.pkl), preparing inputs as Tinygrad + Tensors (potentially using QCOM extensions on TICI), running inference, + and parsing the outputs. + + :param model_type: The type of model (e.g., supercombo) to load and run. + """ + def __init__(self, model_type: int = ModelType.supercombo): + ModelRunner.__init__(self) + SupercomboTinygrad.__init__(self) + PolicyTinygrad.__init__(self) + VisionTinygrad.__init__(self) + self._constants = ModelConstants + self._model_data = self.models.get(model_type) + if not self._model_data or not self._model_data.model: + raise ValueError(f"Model data for type {model_type} not available.") + + artifact_filename = self._model_data.model.artifact.fileName + assert artifact_filename.endswith('_tinygrad.pkl'), \ + f"Invalid model file {artifact_filename} for TinygradRunner" + + model_pkl_path = f"{CUSTOM_MODEL_PATH}/{artifact_filename}" + with open(model_pkl_path, "rb") as f: + try: + # Load the compiled Tinygrad model runner function + self.model_run = pickle.load(f) + except FileNotFoundError as e: + # Provide a helpful error message if the model was built for a different platform + assert "/dev/kgsl-3d0" not in str(e), "Model was built on C3 or C3X, but is being loaded on PC" + raise + + # Map input names to their required dtype and device from the loaded model + self.input_to_dtype = {} + self.input_to_device = {} + for idx, name in enumerate(self.model_run.captured.expected_names): + info = self.model_run.captured.expected_st_vars_dtype_device[idx] + self.input_to_dtype[name] = info[2] # dtype + self.input_to_device[name] = info[3] # device + + def prepare_vision_inputs(self, imgs_cl: CLMemDict, frames: FrameDict): + """Prepares vision (image) inputs as Tinygrad Tensors.""" + for key in imgs_cl: + if TICI and key not in self.inputs: + # On TICI, directly use OpenCL memory address for efficiency via QCOM extensions + self.inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.input_shapes[key], dtype=self.input_to_dtype[key]) + elif not TICI: + # On other platforms, copy data from CL buffer to a numpy array first + shape = frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key]) + self.inputs[key] = Tensor(shape, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize() + + def prepare_policy_inputs(self, numpy_inputs: NumpyDict): + """Prepares non-image (policy) inputs as Tinygrad Tensors.""" + for key, value in numpy_inputs.items(): + self.inputs[key] = Tensor(value, device=self.input_to_device[key], dtype=self.input_to_dtype[key]).realize() + + def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict: + """Prepares all vision and policy inputs for the model.""" + self.prepare_vision_inputs(imgs_cl, frames) + self.prepare_policy_inputs(numpy_inputs) + return self.inputs + + def _run_model(self) -> NumpyDict: + """Runs the Tinygrad model inference and parses the outputs.""" + outputs = self.model_run(**self.inputs).numpy().flatten() + return self._parse_outputs(outputs) + + def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict: + """Parses the raw model outputs using the standard Parser.""" + if self._model_data is None: + raise ValueError("Model data is not available. Ensure the model is loaded correctly.") + + result: NumpyDict = self.parser_method_dict[self._model_data.model.type.raw](model_outputs) + return result + + +class TinygradSplitRunner(ModelRunner): + """ + A ModelRunner that coordinates separate TinygradVisionRunner and TinygradPolicyRunner instances. + + Manages the execution of split vision and policy models, combining their inputs and outputs. + """ + def __init__(self): + super().__init__() + self.is_20hz_3d = True + self.vision_runner = TinygradRunner(ModelType.vision) + self.policy_runner = TinygradRunner(ModelType.policy) + self._constants = SplitModelConstants + + def _run_model(self) -> NumpyDict: + """Runs both vision and policy models and merges their parsed outputs.""" + policy_output = self.policy_runner.run_model() + vision_output = self.vision_runner.run_model() + return {**policy_output, **vision_output} # Combine results + + @property + def input_shapes(self) -> ShapeDict: + """Returns the combined input shapes from both vision and policy models.""" + return {**self.policy_runner.input_shapes, **self.vision_runner.input_shapes} + + @property + def output_slices(self) -> SliceDict: + """Returns the combined output slices from both vision and policy models.""" + return {**self.policy_runner.output_slices, **self.vision_runner.output_slices} + + def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict: + """Prepares inputs for both vision and policy models.""" + # Policy inputs only depend on numpy_inputs + self.policy_runner.prepare_policy_inputs(numpy_inputs) + # Vision inputs depend on imgs_cl and frames + self.vision_runner.prepare_vision_inputs(imgs_cl, frames) + # Return combined inputs (though they are stored within respective runners) + return {**self.policy_runner.inputs, **self.vision_runner.inputs} diff --git a/sunnypilot/models/split_model_constants.py b/sunnypilot/models/split_model_constants.py new file mode 100644 index 000000000..a3e1dce8f --- /dev/null +++ b/sunnypilot/models/split_model_constants.py @@ -0,0 +1,94 @@ +import numpy as np + + +def index_function(idx, max_val=192, max_idx=32): + return max_val * ((idx/max_idx)**2) + + +class SplitModelConstants: + # time and distance indices + IDX_N = 33 + T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)] + X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)] + LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.] + LEAD_T_OFFSETS = [0., 2., 4.] + META_T_IDXS = [2., 4., 6., 8., 10.] + + # model inputs constants + MODEL_FREQ = 20 + HISTORY_FREQ = 5 + HISTORY_LEN_SECONDS = 5 + TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ + FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS + INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS + + FEATURE_LEN = 512 + + DESIRE_LEN = 8 + TRAFFIC_CONVENTION_LEN = 2 + LAT_PLANNER_STATE_LEN = 4 + LATERAL_CONTROL_PARAMS_LEN = 2 + PREV_DESIRED_CURV_LEN = 1 + + # model outputs constants + FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32) + FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32) + FCW_5MS2_PROBS_WIDTH = 5 + FCW_3MS2_PROBS_WIDTH = 2 + + DISENGAGE_WIDTH = 5 + POSE_WIDTH = 6 + WIDE_FROM_DEVICE_WIDTH = 3 + LEAD_WIDTH = 4 + LANE_LINES_WIDTH = 2 + ROAD_EDGES_WIDTH = 2 + PLAN_WIDTH = 15 + DESIRE_PRED_WIDTH = 8 + LAT_PLANNER_SOLUTION_WIDTH = 4 + DESIRED_CURV_WIDTH = 1 + + NUM_LANE_LINES = 4 + NUM_ROAD_EDGES = 2 + + LEAD_TRAJ_LEN = 6 + DESIRE_PRED_LEN = 4 + + PLAN_MHP_N = 5 + LEAD_MHP_N = 2 + PLAN_MHP_SELECTION = 1 + LEAD_MHP_SELECTION = 3 + + FCW_THRESHOLD_5MS2_HIGH = 0.15 + FCW_THRESHOLD_5MS2_LOW = 0.05 + FCW_THRESHOLD_3MS2 = 0.7 + + CONFIDENCE_BUFFER_LEN = 5 + RYG_GREEN = 0.01165 + RYG_YELLOW = 0.06157 + + POLY_PATH_DEGREE = 4 + + +# model outputs slices +class Plan: + POSITION = slice(0, 3) + VELOCITY = slice(3, 6) + ACCELERATION = slice(6, 9) + T_FROM_CURRENT_EULER = slice(9, 12) + ORIENTATION_RATE = slice(12, 15) + + +class Meta: + ENGAGED = slice(0, 1) + # next 2, 4, 6, 8, 10 seconds + GAS_DISENGAGE = slice(1, 31, 6) + BRAKE_DISENGAGE = slice(2, 31, 6) + STEER_OVERRIDE = slice(3, 31, 6) + HARD_BRAKE_3 = slice(4, 31, 6) + HARD_BRAKE_4 = slice(5, 31, 6) + HARD_BRAKE_5 = slice(6, 31, 6) + # next 0, 2, 4, 6, 8, 10 seconds + GAS_PRESS = slice(31, 55, 4) + BRAKE_PRESS = slice(32, 55, 4) + LEFT_BLINKER = slice(33, 55, 4) + RIGHT_BLINKER = slice(34, 55, 4) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 9f0c503fb..56f32373d 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -7,7 +7,6 @@ See the LICENSE.md file in the root directory for more details. from cereal import messaging, custom from opendbc.car import structs -from openpilot.sunnypilot.models.helpers import get_active_model_runner from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState @@ -16,7 +15,6 @@ DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimen class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): self.dec = DynamicExperimentalController(CP, mpc) - self.is_stock = get_active_model_runner() == custom.ModelManagerSP.Runner.stock def get_mpc_mode(self) -> str | None: if not self.dec.active():