mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-06-08 11:25:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

modeld formatting

Browse Source
This commit is contained in:
Shane Smiskol
2026-05-31 03:14:57 -07:00
parent bc2b0d8272
commit ce08f5290b
1 changed files with 25 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
selfdrive/modeld/modeld.py
View File

@@ -26,7 +26,6 @@ from openpilot.common.file_chunker import read_file_chunked, get_manifest_path
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path, get_tg_input_devices
PROCESS_NAME = "selfdrive.modeld.modeld"
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -35,29 +34,29 @@ LONG_SMOOTH_SECONDS = 0.3
MIN_LAT_CONTROL_SPEED = 0.3
def get_action_from_model(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],
ModelConstants.T_IDXS,
action_t=long_action_t)
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
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, LONG_SMOOTH_SECONDS)
desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2],
plan[:,Plan.ORIENTATION_RATE][:,2],
ModelConstants.T_IDXS,
v_ego,
lat_action_t)
if v_ego > MIN_LAT_CONTROL_SPEED:
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
else:
desired_curvature = prev_action.desiredCurvature
desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2],
plan[:,Plan.ORIENTATION_RATE][:,2],
ModelConstants.T_IDXS,
v_ego,
lat_action_t)
if v_ego > MIN_LAT_CONTROL_SPEED:
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, 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))
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
desiredAcceleration=float(desired_accel),
shouldStop=bool(should_stop))
class FrameMeta:
frame_id: int = 0
@@ -77,20 +76,20 @@ class ModelState:
self.WARP_DEV, self.QUEUE_DEV = input_devices['WARP_DEV'], input_devices['QUEUE_DEV']
jits = pickle.loads(read_file_chunked(modeld_pkl_path(usbgpu)))
vision_metadata = jits['metadata']['vision']
self.vision_input_shapes = vision_metadata['input_shapes']
self.vision_input_shapes = vision_metadata['input_shapes']
self.vision_input_names = list(self.vision_input_shapes.keys())
self.vision_output_slices = vision_metadata['output_slices']
policy_metadata = jits['metadata']['policy']
self.policy_input_shapes = policy_metadata['input_shapes']
self.policy_input_shapes = policy_metadata['input_shapes']
self.policy_output_slices = policy_metadata['output_slices']
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
self.input_queues, self.npy = make_input_queues(self.vision_input_shapes, self.policy_input_shapes, self.frame_skip, device=self.QUEUE_DEV)
self.full_frames : dict[str, Tensor] = {}
self._blob_cache : dict[int, Tensor] = {}
self.full_frames: dict[str, Tensor] = {}
self._blob_cache: dict[int, Tensor] = {}
self.parser = Parser()
self.frame_buf_params = {k: get_nv12_info(cam_w, cam_h) for k in ('img', 'big_img')}
self.run_policy = jits['run_policy']
@@ -101,7 +100,7 @@ class ModelState:
return parsed_model_outputs
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, 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:
for key in bufs.keys():
ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data
yuv_size = self.frame_buf_params[key][3]
@@ -202,7 +201,6 @@ def main(demo=False):
meta_main = FrameMeta()
meta_extra = FrameMeta()
if demo:
CP = get_demo_car_params()
else:
@@ -284,7 +282,7 @@ def main(demo=False):
bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
inputs:dict[str, np.ndarray] = {
inputs: dict[str, np.ndarray] = {
'desire_pulse': vec_desire,
'traffic_convention': traffic_convention,
}