mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-03 00:53:44 +08:00
Refactor modeld to support 20Hz models and modularize runners
Replaced legacy runner logic with a unified ONNX and Tinygrad runner to support 20Hz models. Centralized model metadata management and optimized input preparation for adaptability. Updated curvature handling and output parsing for improved modularity and maintainability.
This commit is contained in:
DevTekVE
@@ -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'
|
||||
@@ -3,11 +3,21 @@ import capnp
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
|
||||
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)
|
||||
|
||||
class PublishState:
|
||||
def __init__(self):
|
||||
@@ -59,12 +69,17 @@ 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
|
||||
|
||||
if out_desired_curv := net_output_data.get('desired_curvature'):
|
||||
desired_curv = float(out_desired_curv[0,0])
|
||||
else:
|
||||
desired_curv = float(get_curvature_from_plan(net_output_data['plan'][0], v_ego, delay))
|
||||
|
||||
driving_model_data = base_msg.drivingModelData
|
||||
|
||||
driving_model_data.frameId = vipc_frame_id
|
||||
@@ -73,7 +88,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_curv
|
||||
|
||||
modelV2 = extended_msg.modelV2
|
||||
modelV2.frameId = vipc_frame_id
|
||||
@@ -108,7 +123,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_curv
|
||||
|
||||
# times at X_IDXS according to model plan
|
||||
PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
|
||||
@@ -169,8 +184,10 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
|
||||
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()
|
||||
|
||||
if hasattr(Meta, 'GAS_PRESS') and hasattr(Meta, 'BRAKE_PRESS'):
|
||||
disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist()
|
||||
disengage_predictions.brakePressProbs = net_output_data['meta'][0,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]
|
||||
|
||||
+60
-66
@@ -1,21 +1,13 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
from openpilot.system.hardware import TICI
|
||||
|
||||
from openpilot.sunnypilot.modeld_20hz.model_runner import ONNXRunner, TinygradRunner
|
||||
|
||||
#
|
||||
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 +25,10 @@ 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 sunnypilot.modeld_20hz.meta_helper import load_meta_constants
|
||||
|
||||
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,81 +46,85 @@ 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)}
|
||||
self.model_runner = TinygradRunner() if TICI else ONNXRunner()
|
||||
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)
|
||||
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)
|
||||
# Initialize model runner
|
||||
|
||||
# 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)
|
||||
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:
|
||||
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
|
||||
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
|
||||
inputs['desire'][0] = 0
|
||||
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:
|
||||
len = inputs['desire'].shape[0]
|
||||
self.numpy_inputs['desire'][0, :-1] = self.numpy_inputs['desire'][0, 1:]
|
||||
self.numpy_inputs['desire'][0, -1, :len] = new_desire[:len]
|
||||
|
||||
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:
|
||||
len = outputs['desired_curvature'][0].size
|
||||
self.numpy_inputs[input_name_prev][0, :-len, 0] = self.numpy_inputs[input_name_prev][0, len:, 0]
|
||||
self.numpy_inputs[input_name_prev][0, -len:, 0] = outputs['desired_curvature'][0]
|
||||
return outputs
|
||||
|
||||
|
||||
@@ -242,7 +235,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 +265,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 +281,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]
|
||||
|
||||
Reference in New Issue
Block a user