diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 7cf70d823..31bfd707b 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -90,7 +90,7 @@ class ModelState: prev_desire: np.ndarray # for tracking the rising edge of the pulse def __init__(self, context: CLContext): - self.LAT_SMOOTH_SECONDS = 0.0 + self.LAT_SMOOTH_SECONDS = LAT_SMOOTH_SECONDS with open(VISION_METADATA_PATH, 'rb') as f: vision_metadata = pickle.load(f) self.vision_input_shapes = vision_metadata['input_shapes'] diff --git a/sunnypilot/modeld/fill_model_msg.py b/sunnypilot/modeld/fill_model_msg.py index 608f24424..dadffc843 100644 --- a/sunnypilot/modeld/fill_model_msg.py +++ b/sunnypilot/modeld/fill_model_msg.py @@ -103,7 +103,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) # temporal pose - temporal_pose = modelV2.temporalPose + temporal_pose = modelV2.temporalPoseDEPRECATED temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist() temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist() temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist() diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index 86968ea26..10725efb7 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -62,7 +62,7 @@ class ModelState: 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.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0")) self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0")) model_paths = get_model_path() diff --git a/sunnypilot/modeld_v2/fill_model_msg.py b/sunnypilot/modeld_v2/fill_model_msg.py index 4d04d6e5e..c7de698f6 100644 --- a/sunnypilot/modeld_v2/fill_model_msg.py +++ b/sunnypilot/modeld_v2/fill_model_msg.py @@ -10,8 +10,8 @@ SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') ConfidenceClass = log.ModelDataV2.ConfidenceClass -def get_curvature_from_output(output, vego, lat_action_t, current_generation=None): - if current_generation != 11: +def get_curvature_from_output(output, vego, lat_action_t, mlsim): + if not mlsim: if desired_curv := output.get('desired_curvature'): # If the model outputs the desired curvature, use that directly return float(desired_curv[0, 0]) @@ -100,7 +100,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) # temporal pose - temporal_pose = modelV2.temporalPose + temporal_pose = modelV2.temporalPoseDEPRECATED if 'sim_pose' in net_output_data: temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist() temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist() diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index 58e79eceb..93a8a3f05 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -54,10 +54,10 @@ class ModelState: raise model_bundle = get_active_bundle() - self.generation = model_bundle.generation + self.generation = model_bundle.generation if model_bundle is not None else None overrides = {override.key: override.value for override in model_bundle.overrides} - self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".2")) + self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0")) self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0")) self.MIN_LAT_CONTROL_SPEED = 0.3 @@ -86,6 +86,10 @@ class ModelState: self.desire_reshape_dims = (self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], -1, self.numpy_inputs['desire'].shape[2]) + @property + def mlsim(self) -> bool: + return bool(self.generation is not None and self.generation >= 11) + 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: # Model decides when action is completed, so desire input is just a pulse triggered on rising edge @@ -151,7 +155,7 @@ class ModelState: 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: + if self.mlsim: self.numpy_inputs[input_name_prev][:] = 0*self.full_prev_desired_curv[0, self.temporal_idxs] else: length = outputs['desired_curvature'][0].size @@ -165,7 +169,7 @@ class ModelState: 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) + desired_curvature = get_curvature_from_output(model_output, v_ego, lat_action_t, self.mlsim) if v_ego > self.MIN_LAT_CONTROL_SPEED: desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS) else: diff --git a/sunnypilot/modeld_v2/parse_model_outputs_split.py b/sunnypilot/modeld_v2/parse_model_outputs_split.py index 7fab66e03..f99d5692c 100644 --- a/sunnypilot/modeld_v2/parse_model_outputs_split.py +++ b/sunnypilot/modeld_v2/parse_model_outputs_split.py @@ -1,5 +1,6 @@ import numpy as np from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants +from openpilot.sunnypilot.models.helpers import get_active_bundle def safe_exp(x, out=None): @@ -24,6 +25,8 @@ def softmax(x, axis=-1): class Parser: def __init__(self, ignore_missing=False): self.ignore_missing = ignore_missing + model_bundle = get_active_bundle() + self.generation = model_bundle.generation if model_bundle is not None else None def check_missing(self, outs, name): if name not in outs and not self.ignore_missing: @@ -88,37 +91,78 @@ class Parser: outs[name] = pred_mu_final.reshape(final_shape) outs[name + '_stds'] = pred_std_final.reshape(final_shape) + def _parse_mhp_output(self, name, output, shape_threshold, in_n_mhp, out_n_mhp, out_shape) -> None: + if name not in output: + return + + shape = output[name].shape[1] + shape_is_expected_size = None + + if name == 'lead': + shape_is_expected_size = shape == 2 * shape_threshold + elif name == 'plan': + shape_is_expected_size = shape <= 2 * shape_threshold + + use_default_format = self.generation >= 12 and shape_is_expected_size + in_n = 0 if use_default_format else in_n_mhp + out_n = 0 if use_default_format else out_n_mhp + + self.parse_mdn(name, output, in_n, out_n, out_shape) + + def parse_dynamic_outputs(self, outs: dict[str, np.ndarray]) -> None: + self._parse_mhp_output( + name='lead', + output=outs, + shape_threshold=SplitModelConstants.LEAD_MHP_SELECTION * SplitModelConstants.LEAD_TRAJ_LEN * SplitModelConstants.LEAD_WIDTH, + in_n_mhp=SplitModelConstants.LEAD_MHP_N, + out_n_mhp=SplitModelConstants.LEAD_MHP_SELECTION, + out_shape=(SplitModelConstants.LEAD_TRAJ_LEN, SplitModelConstants.LEAD_WIDTH), + ) + + self._parse_mhp_output( + name='plan', + output=outs, + shape_threshold=SplitModelConstants.PLAN_WIDTH * SplitModelConstants.IDX_N, + in_n_mhp=SplitModelConstants.PLAN_MHP_N, + out_n_mhp=SplitModelConstants.PLAN_MHP_SELECTION, + out_shape=(SplitModelConstants.IDX_N, SplitModelConstants.PLAN_WIDTH), + ) + def split_outputs(self, outs: dict[str, np.ndarray]) -> None: + if 'desired_curvature' in outs: + self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.DESIRED_CURV_WIDTH,)) + if 'desire_pred' in outs: + self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(SplitModelConstants.DESIRE_PRED_LEN,SplitModelConstants.DESIRE_PRED_WIDTH)) + if 'desire_state' in outs: + self.parse_categorical_crossentropy('desire_state', outs, out_shape=(SplitModelConstants.DESIRE_PRED_WIDTH,)) 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)) + out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH)) + if 'lane_lines_prob' in outs: + self.parse_binary_crossentropy('lane_lines_prob', outs) + if 'lead_prob' in outs: + self.parse_binary_crossentropy('lead_prob', 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 'meta' in outs: + self.parse_binary_crossentropy('meta', outs) + if 'road_edges' in outs: 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) + out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH)) + if 'sim_pose' in outs: + self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,)) 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.parse_dynamic_outputs(outs) 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.parse_dynamic_outputs(outs) 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]: diff --git a/sunnypilot/models/helpers.py b/sunnypilot/models/helpers.py index a614447ce..61a7e1a00 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 = 6 +CURRENT_SELECTOR_VERSION = 7 REQUIRED_MIN_SELECTOR_VERSION = 5 USE_ONNX = os.getenv('USE_ONNX', PC) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 18bfbe925..6952a97f1 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -16,12 +16,12 @@ DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimen class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): self.dec = DynamicExperimentalController(CP, mpc) - model_bundle = get_active_bundle() - self.generation = model_bundle.generation if model_bundle is not None else None + self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None @property def mlsim(self) -> bool: - return self.generation == 11 + # If we don't have a generation set, we assume it's default model. Which as of today are mlsim. + return bool(self.generation is None or self.generation >= 11) def get_mpc_mode(self) -> str | None: if not self.dec.active():