diff --git a/cereal/custom.capnp b/cereal/custom.capnp index b75983e9a..3aa9c7ad1 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -81,6 +81,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { generation @5 :UInt32; environment @6 :Text; runner @7 :Runner; + is20hz @8 :Bool; } } diff --git a/selfdrive/modeld/__init__.py b/selfdrive/modeld/__init__.py index e69de29bb..639622e82 100644 --- a/selfdrive/modeld/__init__.py +++ b/selfdrive/modeld/__init__.py @@ -0,0 +1,5 @@ +from pathlib import Path + +MODEL_PATH = Path(__file__).parent / 'models/supercombo.onnx' +MODEL_PKL_PATH = Path(__file__).parent / 'models/supercombo_tinygrad.pkl' +METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl' diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 3056da937..3f5bf2f27 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -2,13 +2,33 @@ import os import capnp import numpy as np from cereal import log -from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta +from openpilot.selfdrive.modeld.constants import ModelConstants, Plan +from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED 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_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)) + + class PublishState: def __init__(self): self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) @@ -59,12 +79,14 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D net_output_data: dict[str, np.ndarray], v_ego: float, delay: float, 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) -> None: + valid: bool, model_meta) -> None: frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 frame_drop_perc = frame_drop * 100 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 @@ -73,7 +95,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.modelExecutionTime = model_execution_time action = driving_model_data.action - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + action.desiredCurvature = desired_curvature modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -108,7 +130,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # lateral planning action = modelV2.action - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + action.desiredCurvature = desired_curvature # times at X_IDXS according to model plan PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N @@ -159,23 +181,25 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D meta = modelV2.meta meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist() meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist() - meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item() + meta.engagedProb = net_output_data['meta'][0,model_meta.ENGAGED].item() meta.init('disengagePredictions') disengage_predictions = meta.disengagePredictions disengage_predictions.t = ModelConstants.META_T_IDXS - disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist() - disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist() - disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist() - disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist() - disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist() - disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist() - #disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist() - #disengage_predictions.brakePressProbs = net_output_data['meta'][0,Meta.BRAKE_PRESS].tolist() + disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,model_meta.BRAKE_DISENGAGE].tolist() + disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,model_meta.GAS_DISENGAGE].tolist() + disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,model_meta.STEER_OVERRIDE].tolist() + disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,model_meta.HARD_BRAKE_3].tolist() + disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,model_meta.HARD_BRAKE_4].tolist() + disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,model_meta.HARD_BRAKE_5].tolist() + + if hasattr(model_meta, 'GAS_PRESS') and hasattr(model_meta, 'BRAKE_PRESS'): + disengage_predictions.gasPressProbs = net_output_data['meta'][0,model_meta.GAS_PRESS].tolist() + disengage_predictions.brakePressProbs = net_output_data['meta'][0,model_meta.BRAKE_PRESS].tolist() publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:] - publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0] + publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,model_meta.HARD_BRAKE_5][0] publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:] - publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0] + publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,model_meta.HARD_BRAKE_3][0] hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \ (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all() meta.hardBrakePredicted = hard_brake_predicted.item() @@ -183,9 +207,9 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # confidence if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0: # any disengage prob - brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE] - gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE] - steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE] + brake_disengage_probs = net_output_data['meta'][0,model_meta.BRAKE_DISENGAGE] + gas_disengage_probs = net_output_data['meta'][0,model_meta.GAS_DISENGAGE] + steer_override_probs = net_output_data['meta'][0,model_meta.STEER_OVERRIDE] any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs)) # independent disengage prob for each 2s slice ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 5783f6aa8..3d7e385d6 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -1,21 +1,11 @@ #!/usr/bin/env python3 -import os from openpilot.system.hardware import TICI # -if TICI: - from tinygrad.tensor import Tensor - from tinygrad.dtype import dtypes - from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address - os.environ['QCOM'] = '1' -else: - from openpilot.selfdrive.modeld.runners.ort_helpers import make_onnx_cpu_runner import time -import pickle import numpy as np import cereal.messaging as messaging from cereal import car, log -from pathlib import Path from setproctitle import setproctitle from cereal.messaging import PubMaster, SubMaster from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf @@ -33,13 +23,11 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.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 PROCESS_NAME = "selfdrive.modeld.modeld" -SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') -MODEL_PATH = Path(__file__).parent / 'models/supercombo.onnx' -MODEL_PKL_PATH = Path(__file__).parent / 'models/supercombo_tinygrad.pkl' -METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl' class FrameMeta: frame_id: int = 0 @@ -57,39 +45,35 @@ class ModelState: prev_desire: np.ndarray # for tracking the rising edge of the pulse def __init__(self, context: CLContext): - self.frames = {'input_imgs': DrivingModelFrame(context), 'big_input_imgs': DrivingModelFrame(context)} + try: + self.model_runner = TinygradRunner() if TICI else ONNXRunner() + except Exception as e: + cloudlog.exception(f"Failed to initialize model runner: {str(e)}") + + 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_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) + self.desire_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32) # img buffers are managed in openCL transform code - self.numpy_inputs = { - 'desire': np.zeros((1, (ModelConstants.FULL_HISTORY_BUFFER_LEN+1), ModelConstants.DESIRE_LEN), dtype=np.float32), - 'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32), - 'lateral_control_params': np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32), - 'prev_desired_curv': np.zeros((1, (ModelConstants.FULL_HISTORY_BUFFER_LEN+1), ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32), - 'features_buffer': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32), - } + self.numpy_inputs = {} - with open(METADATA_PATH, 'rb') as f: - model_metadata = pickle.load(f) - self.input_shapes = model_metadata['input_shapes'] + for key, shape in self.model_runner.input_shapes.items(): + if key not in self.frames: # Managed by opencl + self.numpy_inputs[key] = np.zeros(shape, dtype=np.float32) - self.output_slices = model_metadata['output_slices'] - net_output_size = model_metadata['output_shapes']['outputs'][1] - self.output = np.zeros(net_output_size, dtype=np.float32) self.parser = Parser() - if TICI: - self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} - with open(MODEL_PKL_PATH, "rb") as f: - self.model_run = pickle.load(f) - else: - self.onnx_cpu_runner = make_onnx_cpu_runner(MODEL_PATH) + if self.model_runner.is_20hz: + net_output_size = self.model_runner.model_metadata['output_shapes']['outputs'][1] + self.output = np.zeros(net_output_size, dtype=np.float32) - def slice_outputs(self, model_outputs: np.ndarray) -> dict[str, np.ndarray]: - parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()} - if SEND_RAW_PRED: - parsed_model_outputs['raw_pred'] = model_outputs.copy() - return parsed_model_outputs + num_elements = self.numpy_inputs['features_buffer'].shape[1] + step_size = int(-100 / num_elements) + self.full_features_20Hz_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: @@ -98,40 +82,53 @@ class ModelState: new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) self.prev_desire[:] = inputs['desire'] - self.numpy_inputs['desire'][0,:-1] = self.numpy_inputs['desire'][0,1:] - self.numpy_inputs['desire'][0,-1] = new_desire + if self.model_runner.is_20hz: + self.desire_20Hz[:-1] = self.desire_20Hz[1:] + self.desire_20Hz[-1] = new_desire + self.numpy_inputs['desire'][:] = self.desire_20Hz.reshape(self.desire_reshape_dims).max(axis=2) + else: + 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] + + for key in self.numpy_inputs: + if key in inputs and key not in ['desire']: + self.numpy_inputs[key][:] = inputs[key] - self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] - self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params'] imgs_cl = {'input_imgs': self.frames['input_imgs'].prepare(buf, transform.flatten()), 'big_input_imgs': self.frames['big_input_imgs'].prepare(wbuf, transform_wide.flatten())} - if TICI: - # The imgs tensors are backed by opencl memory, only need init once - for key in imgs_cl: - if key not in self.tensor_inputs: - self.tensor_inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.input_shapes[key], dtype=dtypes.uint8) - else: - for key in imgs_cl: - self.numpy_inputs[key] = self.frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key]).astype(dtype=np.float32) + # Prepare inputs using the model runner + self.model_runner.prepare_inputs(imgs_cl, self.numpy_inputs, self.frames) if prepare_only: return None - if TICI: - self.output = self.model_run(**self.tensor_inputs).numpy().flatten() + # Run model inference + self.output = self.model_runner.run_model() + outputs = self.parser.parse_outputs(self.model_runner.slice_outputs(self.output)) + + if self.model_runner.is_20hz: + self.full_features_20Hz[:-1] = self.full_features_20Hz[1:] + self.full_features_20Hz[-1] = outputs['hidden_state'][0, :] + self.numpy_inputs['features_buffer'][:] = self.full_features_20Hz[self.full_features_20Hz_idxs] else: - self.output = self.onnx_cpu_runner.run(None, self.numpy_inputs)[0].flatten() + 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] - outputs = self.parser.parse_outputs(self.slice_outputs(self.output)) + if "desired_curvature" in outputs: + input_name_prev = None - self.numpy_inputs['features_buffer'][0,:-1] = self.numpy_inputs['features_buffer'][0,1:] - self.numpy_inputs['features_buffer'][0,-1] = outputs['hidden_state'][0, :] + if "prev_desired_curvs" in self.numpy_inputs.keys(): + input_name_prev = 'prev_desired_curvs' + elif "prev_desired_curv" in self.numpy_inputs.keys(): + input_name_prev = 'prev_desired_curv' - - # TODO model only uses last value now - self.numpy_inputs['prev_desired_curv'][0,:-1] = self.numpy_inputs['prev_desired_curv'][0,1:] - self.numpy_inputs['prev_desired_curv'][0,-1,:] = outputs['desired_curvature'][0, :] + 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] return outputs @@ -242,7 +239,6 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32) 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))] @@ -273,8 +269,10 @@ def main(demo=False): inputs:dict[str, np.ndarray] = { 'desire': vec_desire, 'traffic_convention': traffic_convention, - 'lateral_control_params': lateral_control_params, - } + } + + if "lateral_control_params" in model.numpy_inputs.keys(): + inputs['lateral_control_params'] = np.array([v_ego, steer_delay], dtype=np.float32) mt1 = time.perf_counter() model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only) @@ -287,7 +285,7 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay, 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) + frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, load_meta_constants()) desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 9973d1858..399836b0c 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -5,13 +5,14 @@ #include "common/clutil.h" -DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) { +DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, uint8_t buffer_length) : ModelFrame(device_id, context), buffer_length(buffer_length) { 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, 2*frame_size_bytes, NULL, &err)); - region.origin = 1 * frame_size_bytes; + img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buffer_length*frame_size_bytes, NULL, &err)); + region.origin = (buffer_length - 1) * frame_size_bytes; region.size = frame_size_bytes; last_img_cl = CL_CHECK_ERR(clCreateSubBuffer(img_buffer_20hz_cl, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, ®ion, &err)); + // printf("Buffer length: %d, region origin: %lu, region size: %lu\n", buffer_length, region.origin, region.size); loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT); init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT); @@ -20,7 +21,7 @@ DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection); - for (int i = 0; i < 1; i++) { + for (int i = 0; i < (buffer_length - 1); i++) { CL_CHECK(clEnqueueCopyBuffer(q, img_buffer_20hz_cl, img_buffer_20hz_cl, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes, 0, nullptr, nullptr)); } loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl); diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 14409943e..3b5db05c7 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -64,7 +64,7 @@ protected: class DrivingModelFrame : public ModelFrame { public: - DrivingModelFrame(cl_device_id device_id, cl_context context); + DrivingModelFrame(cl_device_id device_id, cl_context context, 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); @@ -73,6 +73,7 @@ public: 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); + const uint8_t buffer_length; private: LoadYUVState loadyuv; diff --git a/selfdrive/modeld/models/commonmodel.pxd b/selfdrive/modeld/models/commonmodel.pxd index b4f08b12a..f640597b8 100644 --- a/selfdrive/modeld/models/commonmodel.pxd +++ b/selfdrive/modeld/models/commonmodel.pxd @@ -20,7 +20,7 @@ cdef extern from "selfdrive/modeld/models/commonmodel.h": cppclass DrivingModelFrame: int buf_size - DrivingModelFrame(cl_device_id, cl_context) + DrivingModelFrame(cl_device_id, cl_context, unsigned char) cppclass MonitoringModelFrame: int buf_size diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx index 7b3a5bb34..78a891f03 100644 --- a/selfdrive/modeld/models/commonmodel_pyx.pyx +++ b/selfdrive/modeld/models/commonmodel_pyx.pyx @@ -4,7 +4,7 @@ import numpy as np cimport numpy as cnp from libc.string cimport memcpy -from libc.stdint cimport uintptr_t +from libc.stdint cimport uintptr_t, uint8_t from msgq.visionipc.visionipc cimport cl_mem from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext @@ -59,8 +59,8 @@ cdef class ModelFrame: cdef class DrivingModelFrame(ModelFrame): cdef cppDrivingModelFrame * _frame - def __cinit__(self, CLContext context): - self._frame = new cppDrivingModelFrame(context.device_id, context.context) + def __cinit__(self, CLContext context, int buffer_length=2): + self._frame = new cppDrivingModelFrame(context.device_id, context.context, buffer_length) self.frame = (self._frame) self.buf_size = self._frame.buf_size diff --git a/sunnypilot/modeld_v2/__init__.py b/sunnypilot/modeld_v2/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/sunnypilot/modeld_v2/meta_20hz.py b/sunnypilot/modeld_v2/meta_20hz.py new file mode 100644 index 000000000..77a815ee8 --- /dev/null +++ b/sunnypilot/modeld_v2/meta_20hz.py @@ -0,0 +1,17 @@ +from openpilot.selfdrive.modeld.constants import Meta + + +class Meta20hz(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/modeld_v2/meta_helper.py b/sunnypilot/modeld_v2/meta_helper.py new file mode 100644 index 000000000..b9c6b98c9 --- /dev/null +++ b/sunnypilot/modeld_v2/meta_helper.py @@ -0,0 +1,26 @@ +from openpilot.selfdrive.modeld.constants import Meta +from cereal import custom +from openpilot.sunnypilot.modeld_v2.meta_20hz import Meta20hz +from openpilot.sunnypilot.models.helpers import get_active_bundle + +ModelBundle = custom.ModelManagerSP.ModelBundle + + +def load_meta_constants(): + """ + Determines and loads the appropriate meta model class based on the metadata provided. The function checks + specific keys and conditions within the provided metadata dictionary to identify the corresponding meta + model class to return. + + :param model_metadata: Dictionary containing metadata about the model. It includes + details such as input shapes, output slices, and other configurations for identifying + metadata-dependent meta model classes. + :type model_metadata: dict + :return: The appropriate meta model class (Meta, MetaSimPose, or MetaTombRaider) + based on the conditions and metadata provided. + :rtype: type + """ + if (bundle := get_active_bundle()) and bundle.is20hz: + return Meta20hz + + return Meta # Default diff --git a/sunnypilot/modeld_v2/model_runner.py b/sunnypilot/modeld_v2/model_runner.py new file mode 100644 index 000000000..500a34a4e --- /dev/null +++ b/sunnypilot/modeld_v2/model_runner.py @@ -0,0 +1,134 @@ +import os +import pickle +from abc import ABC, abstractmethod +import numpy as np + +from cereal import custom +from openpilot.selfdrive.modeld import MODEL_PATH, MODEL_PKL_PATH, METADATA_PATH +from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLMem +from openpilot.selfdrive.modeld.runners.ort_helpers import make_onnx_cpu_runner, ORT_TYPES_TO_NP_TYPES +from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address +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.Type.drive) + self._metadata_model = bundle_models.get(ModelManager.Type.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 = {} + + @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.""" + + 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 + + +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.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/models/fetcher.py b/sunnypilot/models/fetcher.py index 2a1fa1d8f..f543b00a7 100644 --- a/sunnypilot/models/fetcher.py +++ b/sunnypilot/models/fetcher.py @@ -72,13 +72,12 @@ class ModelParser: model_bundle.generation = int(value["generation"]) model_bundle.environment = value["environment"] model_bundle.runner = value.get("runner", custom.ModelManagerSP.Runner.snpe) + model_bundle.is20hz = value.get("is_20hz", False) return model_bundle @staticmethod def parse_models(json_data: dict) -> list[custom.ModelManagerSP.ModelBundle]: - # TODO-SP: Remove the following filter once we add support for tinygrad model switcher - json_data = {k: v for k, v in json_data.items() if v.get("runner", -1) == custom.ModelManagerSP.Runner.snpe} return [ModelParser._parse_bundle(key, value) for key, value in json_data.items()]