From 1b10cc85756e2c5f807fd0b467a0c22b2cdb2c79 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Thu, 23 May 2024 17:59:37 +0000 Subject: [PATCH 1/3] Driving Model Selector: Improve progress display for model downloads --- .../offroad/sunnypilot/software_settings_sp.cc | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/selfdrive/ui/qt/offroad/sunnypilot/software_settings_sp.cc b/selfdrive/ui/qt/offroad/sunnypilot/software_settings_sp.cc index eee55f119a..bbe8572797 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/software_settings_sp.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/software_settings_sp.cc @@ -128,8 +128,9 @@ void SoftwarePanelSP::HandleModelDownloadProgressReport() { // Driving model status if (isDownloadingModel()) { - description += QString(tr("Downloading Driving model") + " [%1]... (%2%)") - .arg(drivingModelName, QString::number(modelDownloadProgress.value_or(0.0), 'f', 2)); + auto modelProgress = modelDownloadProgress.value_or(0.0); + auto progressText = modelProgress <= 0.01 ? tr("PENDING")+"..." : QString::number(modelProgress, 'f', 2) + "%"; + description += QString(tr("Downloading Driving model") + " [%1]... (%2)").arg(drivingModelName, progressText); } else { if (modelFromCache) drivingModelName += QString(" " + tr("(CACHED)")); description += QString(tr("Driving model") + " [%1] " + tr("downloaded")).arg(drivingModelName); @@ -137,9 +138,10 @@ void SoftwarePanelSP::HandleModelDownloadProgressReport() { // Navigation model status if (isDownloadingNavModel() && !navModelName.isEmpty()) { + auto navModelProgress = navModelDownloadProgress.value_or(0.0); + auto progressText = navModelProgress <= 0.01 ? tr("PENDING")+"..." : QString::number(navModelProgress, 'f', 2) + "%"; if (!description.isEmpty()) description += "\n"; // Add newline if driving model status is already appended - description += QString(tr("Downloading Navigation model") + " [%1]... (%2%)") - .arg(navModelName, QString::number(navModelDownloadProgress.value_or(0.0), 'f', 2)); + description += QString(tr("Downloading Navigation model") + " [%1]... (%2)").arg(navModelName, progressText); } else if (!navModelName.isEmpty()) { if (navModelFromCache) navModelName += QString(" " + tr("(CACHED)")); if (!description.isEmpty()) description += "\n"; // Ensure newline separation @@ -148,9 +150,10 @@ void SoftwarePanelSP::HandleModelDownloadProgressReport() { // Metadata status if (isDownloadingMetadata() && !metadataName.isEmpty()) { + auto metadataProgress = metadataDownloadProgress.value_or(0.0); + auto progressText = metadataProgress <= 0.01 ? tr("PENDING")+"..." : QString::number(metadataProgress, 'f', 2) + "%"; if (!description.isEmpty()) description += "\n"; - description += QString(tr("Downloading Metadata model") + " [%1]... (%2%)") - .arg(metadataName, QString::number(metadataDownloadProgress.value_or(0.0), 'f', 2)); + description += QString(tr("Downloading Metadata model") + " [%1]... (%2)").arg(metadataName, progressText); } else if (!metadataName.isEmpty()) { if (metadataFromCache) metadataName += QString(" " + tr("(CACHED)")); if (!description.isEmpty()) description += "\n"; @@ -236,6 +239,9 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() { modelDownloadProgress = 0.01; metadataDownloadProgress = 0.01; + // Sunny wants it responsive... Responsive it is... + HandleModelDownloadProgressReport(); + // Start the download, we download the other models on emit of downloadComplete if (params.get("DrivingModelGeneration") != selectedModelToDownload->generation.toStdString()) showResetParamsDialog(); From e78ae2c1405c79e387baf8fc248d3cd15b12a168 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Thu, 23 May 2024 21:39:28 +0000 Subject: [PATCH 2/3] Driving Model Selector: Introduce ModelCapabilities class for model feature handling --- release/files_common | 1 + selfdrive/controls/controlsd.py | 4 ++- selfdrive/controls/lib/desire_helper.py | 4 ++- selfdrive/controls/plannerd.py | 4 ++- selfdrive/modeld/model_capabilities.py | 41 +++++++++++++++++++++ selfdrive/modeld/modeld.py | 48 +++++++++++++------------ 6 files changed, 77 insertions(+), 25 deletions(-) create mode 100644 selfdrive/modeld/model_capabilities.py diff --git a/release/files_common b/release/files_common index 131d61f2c0..0ec43716b0 100644 --- a/release/files_common +++ b/release/files_common @@ -365,6 +365,7 @@ selfdrive/modeld/navmodeld.py selfdrive/modeld/dmonitoringmodeld.py selfdrive/modeld/constants.py selfdrive/modeld/modeld +selfdrive/modeld/model_capabilities.py selfdrive/modeld/models/__init__.py selfdrive/modeld/models/*.pxd diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 48eddc7b84..a0da74f930 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -31,6 +31,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, S from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel +from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities from openpilot.selfdrive.sunnypilot import get_model_generation from openpilot.system.hardware import HARDWARE @@ -179,7 +180,8 @@ class Controls: self.process_not_running = False self.custom_model, self.model_gen = get_model_generation(self.params) - self.model_use_lateral_planner = self.custom_model and self.model_gen == 1 + model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) + self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution self.can_log_mono_time = 0 diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index bd4decbb80..6cac8a70e2 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -3,6 +3,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.drive_helpers import get_road_edge +from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities from openpilot.selfdrive.sunnypilot import get_model_generation LaneChangeState = log.LaneChangeState @@ -63,7 +64,8 @@ class DesireHelper: self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay") self.custom_model, self.model_gen = get_model_generation(self.param_s) - self.model_use_lateral_planner = self.custom_model and self.model_gen == 1 + model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) + self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution def read_param(self): self.edge_toggle = self.param_s.get_bool("RoadEdge") diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index a9e8cabca7..588e044c3f 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -8,6 +8,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner +from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities from openpilot.selfdrive.sunnypilot import get_model_generation import cereal.messaging as messaging @@ -47,7 +48,8 @@ def plannerd_thread(): longitudinal_planner = LongitudinalPlanner(CP) custom_model, model_gen = get_model_generation(params) - model_use_lateral_planner = custom_model and model_gen == 1 + model_capabilities = ModelCapabilities.get_by_gen(model_gen) + model_use_lateral_planner = custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution lateral_planner = LateralPlanner(CP, debug=debug_mode, model_use_lateral_planner=model_use_lateral_planner) lateral_planner_svs = ['lateralPlanDEPRECATED', 'lateralPlanSPDEPRECATED'] diff --git a/selfdrive/modeld/model_capabilities.py b/selfdrive/modeld/model_capabilities.py new file mode 100644 index 0000000000..dea0143a8b --- /dev/null +++ b/selfdrive/modeld/model_capabilities.py @@ -0,0 +1,41 @@ +from enum import IntFlag, auto + + +class ModelCapabilities(IntFlag): + """Model capabilities for different generations of models.""" + + Default = auto() + """Default capability, used for the prebuilt model.""" + + NoO = auto() + """Navigation on Openpilot capability, used for models support navigation.""" + + LateralPlannerSolution = auto() + """LateralPlannerSolution capability, used for models that support the lateral planner solution.""" + + DesiredCurvatureV1 = auto() + """ + DesiredCurvatureV1 capability: This capability is used for models that support the desired curvature. + In this version, 'prev_desired_curvs' is used as the input for the 'desired_curvature' output. + """ + + DesiredCurvatureV2 = auto() + """ + DesiredCurvatureV2 capability: This capability is used for models that support the desired curvature. + In V2, 'prev_desired_curv' (no plural) is used as the input for the same 'desired_curvature' output. + """ + + @staticmethod + def get_by_gen(gen): + """Returns the model capabilities for a given generation.""" + if gen == 1: + return ModelCapabilities.Default | ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + elif gen == 2: + return ModelCapabilities.Default | ModelCapabilities.DesiredCurvatureV1 | ModelCapabilities.NoO + elif gen == 3: + return ModelCapabilities.Default | ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.NoO + elif gen == 4: + return ModelCapabilities.Default | ModelCapabilities.DesiredCurvatureV2 + else: + # Default model is meant to represent the capabilities of the prebuilt model. + return ModelCapabilities.Default diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 2208fa42ea..2b8d05984e 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -20,6 +20,7 @@ from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive import sentry from openpilot.selfdrive.car.car_helpers import get_demo_car_params from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState @@ -61,25 +62,27 @@ class ModelState: def __init__(self, context: CLContext): self.param_s = Params() self.custom_model, self.model_gen = get_model_generation(self.param_s) + self.model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) self.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) + # Default model, and as of time of writing, this model uses DesiredCurvatureV2 _inputs = { 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), 'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), } _inputs_2 = {} - if self.custom_model and self.model_gen != 0: - if self.model_gen == 1: + if self.custom_model and self.model_capabilities != ModelCapabilities.Default: + if self.model_capabilities & ModelCapabilities.LateralPlannerSolution: _inputs = { 'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32), } - if self.model_gen == 2: + if self.model_capabilities & ModelCapabilities.DesiredCurvatureV1: _inputs = { 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), 'prev_desired_curvs': np.zeros(ModelConstants.PREV_DESIRED_CURVS_LEN, dtype=np.float32), } - if self.model_gen not in (0, 4): + if self.model_capabilities & ModelCapabilities.NoO: _inputs_2 = { 'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32), 'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32), @@ -93,7 +96,7 @@ class ModelState: 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), } - if self.custom_model and self.model_gen != 0: + if self.custom_model and self.model_capabilities != ModelCapabilities.Default: _model_name = self.param_s.get("DrivingModelText", encoding="utf8") _model_paths = {ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/supercombo-{_model_name}.thneed"} _metadata_name = self.param_s.get("DrivingModelMetadataText", encoding="utf8") @@ -131,9 +134,9 @@ class ModelState: self.prev_desire[:] = inputs['desire'] self.inputs['traffic_convention'][:] = inputs['traffic_convention'] - if not (self.custom_model and self.model_gen == 1): + if not (self.custom_model and self.model_capabilities & ModelCapabilities.LateralPlannerSolution): self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] - if self.custom_model and self.model_gen not in (0, 4): + if self.custom_model and self.model_capabilities & ModelCapabilities.NoO: self.inputs['nav_features'][:] = inputs['nav_features'] self.inputs['nav_instructions'][:] = inputs['nav_instructions'] @@ -150,14 +153,14 @@ class ModelState: self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:] self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :] - if self.custom_model and self.model_gen != 0: - if self.model_gen == 1: + if self.custom_model and self.model_capabilities != ModelCapabilities.Default: + if self.model_capabilities & ModelCapabilities.LateralPlannerSolution: self.inputs['lat_planner_state'][2] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 2]) self.inputs['lat_planner_state'][3] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 3]) - elif self.model_gen == 2: + elif self.model_capabilities & ModelCapabilities.DesiredCurvatureV1: self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:] self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0] - else: + else: # Default model, and as of time of writing, this model uses DesiredCurvatureV2 self.inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:] self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :] return outputs @@ -202,10 +205,11 @@ def main(demo=False): params = Params() custom_model, model_gen = get_model_generation(params) + model_capabilities = ModelCapabilities.get_by_gen(model_gen) # messaging extended_svs = ["lateralPlanDEPRECATED", "lateralPlanSPDEPRECATED"] - if custom_model and model_gen not in (0, 4): + if custom_model and model_capabilities & ModelCapabilities.NoO: extended_svs += ["navModelDEPRECATED", "navInstruction"] pm = PubMaster(["modelV2", "modelV2SP", "cameraOdometry"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"] + extended_svs) @@ -221,9 +225,9 @@ def main(demo=False): model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) live_calib_seen = False - if custom_model and model_gen == 1: + if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution: driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32) - if custom_model and model_gen not in (0, 4): + if custom_model and model_capabilities & ModelCapabilities.NoO: nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32) nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32) buf_main, buf_extra = None, None @@ -278,10 +282,10 @@ def main(demo=False): meta_extra = meta_main sm.update(0) - desire = sm["lateralPlanDEPRECATED"].desire.raw if custom_model and model_gen == 1 else DH.desire + desire = sm["lateralPlanDEPRECATED"].desire.raw if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution else DH.desire is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId - if not (custom_model and model_gen == 1): + if not (custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution): lateral_control_params = np.array([sm["carState"].vEgo, 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) @@ -299,7 +303,7 @@ def main(demo=False): timestamp_llk = 0 nav_enabled = False - if custom_model and model_gen not in (0, 4): + if custom_model and model_capabilities & ModelCapabilities.NoO: # Enable/disable nav features timestamp_llk = sm["navModelDEPRECATED"].locationMonoTime nav_valid = sm.valid["navModelDEPRECATED"] # and (nanos_since_boot() - timestamp_llk < 1e9) @@ -337,7 +341,7 @@ def main(demo=False): if prepare_only: cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames") - if custom_model and model_gen == 1: + if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution: _inputs = { 'driving_style': driving_style } @@ -345,7 +349,7 @@ def main(demo=False): _inputs = { 'lateral_control_params': lateral_control_params } - if custom_model and model_gen not in (0, 4): + if custom_model and model_capabilities & ModelCapabilities.NoO: _inputs_2 = { 'nav_features': nav_features, 'nav_instructions': nav_instructions @@ -372,9 +376,9 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen, - custom_model and model_gen == 1, custom_model and model_gen not in (0, 4)) + custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution, custom_model and model_capabilities & ModelCapabilities.NoO) - if not (custom_model and model_gen == 1): + if not (custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution): desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] @@ -389,7 +393,7 @@ def main(demo=False): modelv2_sp_send = messaging.new_message('modelV2SP') modelv2_sp_send.valid = True - if not (custom_model and model_gen == 1): + if not (custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution): modelv2_sp_send.modelV2SP.laneChangePrev = DH.prev_lane_change modelv2_sp_send.modelV2SP.laneChangeEdgeBlock = lat_plan_sp.laneChangeEdgeBlockDEPRECATED pm.send('modelV2SP', modelv2_sp_send) From f02c4007d4b2313f0cdda48984f65477cbcd202f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 23 May 2024 17:50:35 -0400 Subject: [PATCH 3/3] Hyundai CAN Longitudinal: Lenient parsing for auto-enabled radar configs --- selfdrive/car/hyundai/radar_interface.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index e470011737..71715ba687 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -48,6 +48,8 @@ class RadarInterface(RadarInterfaceBase): self.radar_off_can = CP.radarUnavailable self.rcp = get_radar_can_parser(CP) + self.sp_radar_tracks = CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS + def update(self, can_strings): if self.radar_off_can or (self.rcp is None): return super().update(None) @@ -70,7 +72,7 @@ class RadarInterface(RadarInterfaceBase): errors = [] - if not self.rcp.can_valid: + if not self.rcp.can_valid and not self.sp_radar_tracks: errors.append("canError") ret.errors = errors