diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5007d06752..3ddeed75f6 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -78,6 +78,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { status @4 :DownloadStatus; generation @5 :UInt32; environment @6 :Text; + runner @7 :Runner; } } diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py index 8e75769a85..ad7b997fa5 100755 --- a/selfdrive/debug/uiview.py +++ b/selfdrive/debug/uiview.py @@ -3,14 +3,19 @@ import time from cereal import car, log, messaging from openpilot.common.params import Params -from openpilot.system.manager.process_config import managed_processes +from openpilot.system.manager.process_config import managed_processes, is_snpe_model from openpilot.system.hardware import HARDWARE if __name__ == "__main__": CP = car.CarParams(notCar=True, wheelbase=1, steerRatio=10) - Params().put("CarParams", CP.to_bytes()) + params = Params() + params.put("CarParams", CP.to_bytes()) + if use_snpe_modeld := is_snpe_model(False, params, CP): + print("Using SNPE modeld") + HARDWARE.set_power_save(False) - procs = ['camerad', 'ui', 'modeld', 'calibrationd', 'plannerd', 'dmonitoringmodeld', 'dmonitoringd'] + procs = ['camerad', 'ui', 'calibrationd', 'plannerd', 'dmonitoringmodeld', 'dmonitoringd'] + procs += ["modeld_snpe" if use_snpe_modeld else "modeld"] for p in procs: managed_processes[p].start() diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc index e3a9214483..1c05bb32e2 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc @@ -29,16 +29,17 @@ SoftwarePanelSP::SoftwarePanelSP(QWidget *parent) : SoftwarePanel(parent) { * Reads status from modelManagerSP cereal message and displays status for all models */ void SoftwarePanelSP::handleBundleDownloadProgress() { - const SubMaster &sm = *(uiStateSP()->sm); - const auto model_manager = sm["modelManagerSP"].getModelManagerSP(); - - if (!model_manager.hasSelectedBundle()) { - currentModelLblBtn->setDescription(""); + using DS = cereal::ModelManagerSP::DownloadStatus; + if (!model_manager.hasSelectedBundle() && !model_manager.hasActiveBundle()) { + currentModelLblBtn->setDescription(tr("No custom model selected!")); return; } - const auto &bundle = model_manager.getSelectedBundle(); + const bool showSelectedBundle = model_manager.hasSelectedBundle() && (isDownloading() || model_manager.getSelectedBundle().getStatus() == DS::FAILED); + const auto &bundle = showSelectedBundle ? model_manager.getSelectedBundle() : model_manager.getActiveBundle(); const auto &models = bundle.getModels(); + download_status = bundle.getStatus(); + const auto download_status_changed = prev_download_status != download_status; QStringList status; // Get status for each model type in order @@ -67,9 +68,9 @@ void SoftwarePanelSP::handleBundleDownloadProgress() { if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADING) { line = tr("Downloading %1 model [%2]... (%3%)").arg(typeName, modelName).arg(progress.getProgress(), 0, 'f', 2); } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADED) { - line = tr("%1 model [%2] downloaded").arg(typeName, modelName); + line = tr("%1 model [%2] %3").arg(typeName, modelName, download_status_changed ? tr("downloaded") : tr("ready")); } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::CACHED) { - line = tr("%1 model [%2] from cache").arg(typeName, modelName); + line = tr("%1 model [%2] %3").arg(typeName, modelName, download_status_changed ? tr("from cache") : tr("ready")); } else if (progress.getStatus() == cereal::ModelManagerSP::DownloadStatus::FAILED) { line = tr("%1 model [%2] download failed").arg(typeName, modelName); } else { @@ -80,9 +81,19 @@ void SoftwarePanelSP::handleBundleDownloadProgress() { currentModelLblBtn->setDescription(status.join("\n")); - if (bundle.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADING) { - currentModelLblBtn->showDescription(); + if (prev_download_status != download_status) { + switch (bundle.getStatus()) { + case cereal::ModelManagerSP::DownloadStatus::DOWNLOADING: + case cereal::ModelManagerSP::DownloadStatus::CACHED: + case cereal::ModelManagerSP::DownloadStatus::DOWNLOADED: + currentModelLblBtn->showDescription(); + break; + case cereal::ModelManagerSP::DownloadStatus::FAILED: + default: + break; + } } + prev_download_status = download_status; } /** @@ -90,9 +101,6 @@ void SoftwarePanelSP::handleBundleDownloadProgress() { * @return Display name of the selected bundle or default model name */ QString SoftwarePanelSP::GetActiveModelName() { - const SubMaster &sm = *(uiStateSP()->sm); - const auto model_manager = sm["modelManagerSP"].getModelManagerSP(); - if (model_manager.hasActiveBundle()) { return QString::fromStdString(model_manager.getActiveBundle().getDisplayName()); } @@ -100,6 +108,11 @@ QString SoftwarePanelSP::GetActiveModelName() { return ""; } +void SoftwarePanelSP::updateModelManagerState() { + const SubMaster &sm = *(uiStateSP()->sm); + model_manager = sm["modelManagerSP"].getModelManagerSP(); +} + /** * @brief Handles the model bundle selection button click * Displays available bundles, allows selection, and initiates download @@ -108,9 +121,6 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() { currentModelLblBtn->setEnabled(false); currentModelLblBtn->setValue(tr("Fetching models...")); - const SubMaster &sm = *(uiStateSP()->sm); - const auto model_manager = sm["modelManagerSP"].getModelManagerSP(); - // Create mapping of bundle indices to display names QMap index_to_bundle; const auto bundles = model_manager.getAvailableBundles(); @@ -120,6 +130,9 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() { // Sort bundles by index in descending order QStringList bundleNames; + // Add "Default" as the first option + bundleNames.append(tr("Use Default")); + auto indices = index_to_bundle.keys(); std::sort(indices.begin(), indices.end(), std::greater()); for (const auto &index: indices) { @@ -135,14 +148,21 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() { return; } - // Find selected bundle and initiate download - for (const auto &bundle: bundles) { - if (QString::fromStdString(bundle.getDisplayName()) == selectedBundleName) { - params.put("ModelManager_DownloadIndex", std::to_string(bundle.getIndex())); - if (bundle.getGeneration() != model_manager.getActiveBundle().getGeneration()) { - showResetParamsDialog(); + // Handle "Stock" selection differently + if (selectedBundleName == tr("Use Default")) { + params.remove("ModelManager_ActiveBundle"); + currentModelLblBtn->setValue(tr("Default")); + showResetParamsDialog(); + } else { + // Find selected bundle and initiate download + for (const auto &bundle: bundles) { + if (QString::fromStdString(bundle.getDisplayName()) == selectedBundleName) { + params.put("ModelManager_DownloadIndex", std::to_string(bundle.getIndex())); + if (bundle.getGeneration() != model_manager.getActiveBundle().getGeneration()) { + showResetParamsDialog(); + } + break; } - break; } } @@ -157,9 +177,11 @@ void SoftwarePanelSP::updateLabels() { return; } + updateModelManagerState(); handleBundleDownloadProgress(); currentModelLblBtn->setEnabled(!is_onroad && !isDownloading()); currentModelLblBtn->setValue(GetActiveModelName()); + SoftwarePanel::updateLabels(); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h index 7dd9bbc5ec..6e93c19e3e 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h @@ -19,12 +19,10 @@ public: private: QString GetActiveModelName(); + void updateModelManagerState(); bool isDownloading() const { - const SubMaster &sm = *(uiStateSP()->sm); - const auto model_manager = sm["modelManagerSP"].getModelManagerSP(); - - if (!model_manager.hasSelectedBundle() || !sm.updated("modelManagerSP")) { + if (!model_manager.hasSelectedBundle()) { return false; } @@ -37,6 +35,9 @@ private: void handleCurrentModelLblBtnClicked(); void handleBundleDownloadProgress(); void showResetParamsDialog(); + cereal::ModelManagerSP::Reader model_manager; + cereal::ModelManagerSP::DownloadStatus download_status{}; + cereal::ModelManagerSP::DownloadStatus prev_download_status{}; bool canContinueOnMeteredDialog() { if (!is_metered) return true; diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index f3fd4ea956..5dbd71ff19 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -1098,18 +1098,10 @@ This may take up to a minute. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1122,6 +1114,34 @@ This may take up to a minute. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 520699c993..9d6d626552 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -1082,18 +1082,10 @@ This may take up to a minute. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1106,6 +1098,34 @@ This may take up to a minute. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index d3a0844e1f..773704061a 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -1090,22 +1090,42 @@ Esto puede tardar un minuto. Downloading %1 model [%2]... (%3%) Descargando modelo de %1 [%2]... (%3%) - - %1 model [%2] downloaded - Modelo de %1 [%2] descargado - %1 model [%2] download failed Falló descarga modelo de %1 [%2] - - %1 model [%2] from cache - Modelo de %1 [%2] desde caché - Select a Model Selecciona un Modelo + + %1 model [%2] %3 + Modelo de %1 [%2] %3 + + + downloaded + descargado + + + ready + listo + + + from cache + desde caché + + + Use Default + Por Defecto + + + No custom model selected! + No hay ningún modelo personalizado elegido!. + + + Default + Por Defecto + SshControl diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index f3630f1cef..4173a6218a 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -1082,18 +1082,10 @@ Cela peut prendre jusqu'à une minute. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1106,6 +1098,34 @@ Cela peut prendre jusqu'à une minute. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index ff83139cfc..5e0ac26d29 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -1076,18 +1076,10 @@ This may take up to a minute. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1100,6 +1092,34 @@ This may take up to a minute. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index fad75d6ac6..899f92c033 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -1078,18 +1078,10 @@ This may take up to a minute. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1102,6 +1094,34 @@ This may take up to a minute. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 32a289557f..997e202cc2 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -1082,18 +1082,10 @@ Isso pode levar até um minuto. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1106,6 +1098,34 @@ Isso pode levar até um minuto. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index 39baaf5e7d..54221383b4 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -1078,18 +1078,10 @@ This may take up to a minute. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1102,6 +1094,34 @@ This may take up to a minute. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index 349afb7a0b..9deae8ba4f 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -1076,18 +1076,10 @@ This may take up to a minute. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1100,6 +1092,34 @@ This may take up to a minute. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 1ab9de1d7f..30550519a5 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -1078,18 +1078,10 @@ This may take up to a minute. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1102,6 +1094,34 @@ This may take up to a minute. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index e4f4480b7e..f5e89aa4db 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -1078,18 +1078,10 @@ This may take up to a minute. Downloading %1 model [%2]... (%3%) - - %1 model [%2] downloaded - - %1 model [%2] download failed - - %1 model [%2] from cache - - Select a Model @@ -1102,6 +1094,34 @@ This may take up to a minute. Model download has started in the background. + + %1 model [%2] %3 + + + + downloaded + + + + ready + + + + from cache + + + + Use Default + + + + No custom model selected! + + + + Default + + SshControl diff --git a/sunnypilot/modeld/constants.py b/sunnypilot/modeld/constants.py index 82161a5b67..8f63afa461 100644 --- a/sunnypilot/modeld/constants.py +++ b/sunnypilot/modeld/constants.py @@ -18,6 +18,8 @@ class ModelConstants: HISTORY_BUFFER_LEN = 99 DESIRE_LEN = 8 TRAFFIC_CONVENTION_LEN = 2 + NAV_FEATURE_LEN = 256 + NAV_INSTRUCTION_LEN = 150 LAT_PLANNER_STATE_LEN = 4 LATERAL_CONTROL_PARAMS_LEN = 2 PREV_DESIRED_CURV_LEN = 1 @@ -83,3 +85,34 @@ class Meta: BRAKE_PRESS = slice(32, 55, 4) LEFT_BLINKER = slice(33, 55, 4) RIGHT_BLINKER = slice(34, 55, 4) + + +class MetaTombRaider: + ENGAGED = slice(0, 1) + # next 2, 4, 6, 8, 10 seconds + GAS_DISENGAGE = slice(1, 41, 8) + BRAKE_DISENGAGE = slice(2, 41, 8) + STEER_OVERRIDE = slice(3, 41, 8) + HARD_BRAKE_3 = slice(4, 41, 8) + HARD_BRAKE_4 = slice(5, 41, 8) + HARD_BRAKE_5 = slice(6, 41, 8) + GAS_PRESS = slice(7, 41, 8) + BRAKE_PRESS = slice(8, 41, 8) + # next 0, 2, 4, 6, 8, 10 seconds + LEFT_BLINKER = slice(41, 53, 2) + RIGHT_BLINKER = slice(42, 53, 2) + + +class MetaSimPose: + ENGAGED = slice(0, 1) + # next 2, 4, 6, 8, 10 seconds + GAS_DISENGAGE = slice(1, 36, 7) + BRAKE_DISENGAGE = slice(2, 36, 7) + STEER_OVERRIDE = slice(3, 36, 7) + HARD_BRAKE_3 = slice(4, 36, 7) + HARD_BRAKE_4 = slice(5, 36, 7) + HARD_BRAKE_5 = slice(6, 36, 7) + GAS_PRESS = slice(7, 36, 7) + # next 0, 2, 4, 6, 8, 10 seconds + LEFT_BLINKER = slice(36, 48, 2) + RIGHT_BLINKER = slice(37, 48, 2) diff --git a/sunnypilot/modeld/fill_model_msg.py b/sunnypilot/modeld/fill_model_msg.py index 672e01dbe9..1c40425bdd 100644 --- a/sunnypilot/modeld/fill_model_msg.py +++ b/sunnypilot/modeld/fill_model_msg.py @@ -2,7 +2,8 @@ import os import capnp import numpy as np from cereal import log -from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan, Meta +from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan +from openpilot.sunnypilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -51,12 +52,23 @@ def fill_xyz_poly(builder, degree, x, y, z): def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], 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: + timestamp_eof: int, model_execution_time: float, valid: bool, + v_ego: float, steer_delay: float, meta_const) -> 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 'lat_planner_solution' in net_output_data: + x, y, yaw, yawRate = [net_output_data['lat_planner_solution'][0, :, i].tolist() for i in range(4)] + x_sol = np.column_stack([x, y, yaw, yawRate]) + v_ego = max(MIN_SPEED, v_ego) + psis = x_sol[0:CONTROL_N, 2].tolist() + curvatures = (x_sol[0:CONTROL_N, 3] / v_ego).tolist() + desired_curvature = get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures) + else: + desired_curvature = float(net_output_data['desired_curvature'][0, 0]) + driving_model_data = base_msg.drivingModelData driving_model_data.frameId = vipc_frame_id @@ -65,7 +77,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 @@ -100,7 +112,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 @@ -154,23 +166,24 @@ 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,meta_const.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,meta_const.BRAKE_DISENGAGE].tolist() + disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,meta_const.GAS_DISENGAGE].tolist() + disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,meta_const.STEER_OVERRIDE].tolist() + disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,meta_const.HARD_BRAKE_3].tolist() + disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,meta_const.HARD_BRAKE_4].tolist() + disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,meta_const.HARD_BRAKE_5].tolist() + if 'sim_pose' not in net_output_data: + disengage_predictions.gasPressProbs = net_output_data['meta'][0,meta_const.GAS_PRESS].tolist() + disengage_predictions.brakePressProbs = net_output_data['meta'][0,meta_const.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,meta_const.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,meta_const.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() @@ -178,9 +191,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,meta_const.BRAKE_DISENGAGE] + gas_disengage_probs = net_output_data['meta'][0,meta_const.GAS_DISENGAGE] + steer_override_probs = net_output_data['meta'][0,meta_const.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/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index ea531dfe18..ff5b83def4 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import os import time -import pickle import numpy as np import cereal.messaging as messaging from cereal import car, log @@ -13,7 +12,8 @@ from opendbc.car.car_helpers import get_demo_car_params from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.realtime import config_realtime_process +from openpilot.common.realtime import DT_MDL, config_realtime_process +from openpilot.common.numpy_fast import interp from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.system import sentry @@ -23,8 +23,9 @@ from openpilot.sunnypilot.modeld.parse_model_outputs import Parser from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.sunnypilot.modeld.constants import ModelConstants from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext +from openpilot.sunnypilot.modeld.runners.run_helpers import get_model_path, load_metadata, prepare_inputs, load_meta_constants -PROCESS_NAME = "sunnypilot.modeld.modeld" +PROCESS_NAME = "selfdrive.modeld.modeld_snpe" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') MODEL_PATHS = { @@ -54,23 +55,18 @@ class ModelState: self.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - self.inputs = { - 'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), - 'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32), - '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), - 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), - } - with open(METADATA_PATH, 'rb') as f: - model_metadata = pickle.load(f) + model_paths = get_model_path() + self.model_metadata = load_metadata() + self.inputs = prepare_inputs(self.model_metadata) + self.meta = load_meta_constants(self.model_metadata) - self.output_slices = model_metadata['output_slices'] - net_output_size = model_metadata['output_shapes']['outputs'][1] + self.output_slices = self.model_metadata['output_slices'] + net_output_size = self.model_metadata['output_shapes']['outputs'][1] self.output = np.zeros(net_output_size, dtype=np.float32) self.parser = Parser() - self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.GPU, False, context) + self.model = ModelRunner(model_paths, self.output, Runtime.GPU, False, context) self.model.addInput("input_imgs", None) self.model.addInput("big_input_imgs", None) for k,v in self.inputs.items(): @@ -90,8 +86,9 @@ class ModelState: self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) self.prev_desire[:] = inputs['desire'] - self.inputs['traffic_convention'][:] = inputs['traffic_convention'] - self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] + for k in self.inputs: + if k in inputs and k != 'desire': + self.inputs[k][:] = inputs[k] # if getCLBuffer is not None, frame will be None self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs"))) @@ -106,8 +103,19 @@ 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, :] - 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, :] + + if "lat_planner_solution" in outputs and "lat_planner_state" in self.inputs.keys(): + 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]) + + if "desired_curvature" in outputs: + if "prev_desired_curvs" in self.inputs.keys(): + self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:] + self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0] + + if "prev_desired_curv" in self.inputs.keys(): + self.inputs['prev_desired_curv'][:-1] = self.inputs['prev_desired_curv'][1:] + self.inputs['prev_desired_curv'][-1:] = outputs['desired_curvature'][0, :] return outputs @@ -216,9 +224,9 @@ def main(demo=False): sm.update(0) desire = DH.desire + v_ego = sm["carState"].vEgo is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId - 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) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -249,9 +257,20 @@ 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.inputs.keys(): + inputs['lateral_control_params'] = np.array([max(v_ego, 0.), steer_delay], dtype=np.float32) + + if "driving_style" in model.inputs.keys(): + inputs['driving_style'] = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32) + + if "nav_features" in model.inputs.keys(): + inputs['nav_features'] = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32) + + if "nav_instructions" in model.inputs.keys(): + inputs['nav_instructions'] = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32) + mt1 = time.perf_counter() model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only) mt2 = time.perf_counter() @@ -262,7 +281,8 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') fill_model_msg(drivingdata_send, modelv2_send, model_output, 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, + v_ego, steer_delay, model.meta) desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] diff --git a/sunnypilot/modeld/parse_model_outputs.py b/sunnypilot/modeld/parse_model_outputs.py index 8cd4aacd90..9cd5f1465c 100644 --- a/sunnypilot/modeld/parse_model_outputs.py +++ b/sunnypilot/modeld/parse_model_outputs.py @@ -91,6 +91,8 @@ class Parser: self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) + if 'sim_pose' in outs: + self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,)) self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH)) diff --git a/sunnypilot/modeld/runners/run_helpers.py b/sunnypilot/modeld/runners/run_helpers.py new file mode 100644 index 0000000000..cc4c644583 --- /dev/null +++ b/sunnypilot/modeld/runners/run_helpers.py @@ -0,0 +1,90 @@ +# Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. +# +# This file is part of sunnypilot and is licensed under the MIT License. +# See the LICENSE.md file in the root directory for more details. + +import os +import pickle +import numpy as np + +from cereal import custom +from openpilot.sunnypilot.modeld.constants import Meta, MetaTombRaider, MetaSimPose +from openpilot.sunnypilot.modeld.runners import ModelRunner +from openpilot.sunnypilot.models.helpers import get_active_bundle +from openpilot.system.hardware import PC +from openpilot.system.hardware.hw import Paths +from pathlib import Path + +USE_ONNX = os.getenv('USE_ONNX', PC) + +CUSTOM_MODEL_PATH = Paths.model_root() +METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl' + +ModelManager = custom.ModelManagerSP + + +def get_model_path(): + if USE_ONNX: + return {ModelRunner.ONNX: Path(__file__).parent / '../models/supercombo.onnx'} + + if bundle := get_active_bundle(): + drive_model = next(model for model in bundle.models if model.type == ModelManager.Type.drive) + return {ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/{drive_model.fileName}"} + + return {ModelRunner.THNEED: Path(__file__).parent / '../models/supercombo.thneed'} + + +def load_metadata(): + metadata_path = METADATA_PATH + + if bundle := get_active_bundle(): + metadata_model = next(model for model in bundle.models if model.type == ModelManager.Type.metadata) + metadata_path = f"{CUSTOM_MODEL_PATH}/{metadata_model.fileName}" + + with open(metadata_path, 'rb') as f: + return pickle.load(f) + + +def prepare_inputs(model_metadata) -> dict[str, np.ndarray]: + # img buffers are managed in openCL transform code so we don't pass them as inputs + inputs = { + k: np.zeros(v, dtype=np.float32).flatten() + for k, v in model_metadata['input_shapes'].items() + if 'img' not in k + } + + return inputs + + +def load_meta_constants(model_metadata): + """ + 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 + """ + meta = Meta # Default Meta + + if 'sim_pose' in model_metadata['input_shapes'].keys(): + # Meta for models with sim_pose input + meta = MetaSimPose + else: + # Meta for Tomb Raider, it does not include sim_pose input but has the same meta slice as previous models + meta_slice = model_metadata['output_slices']['meta'] + meta_tf_slice = slice(5868, 5921, None) + + if ( + meta_slice.start == meta_tf_slice.start and + meta_slice.stop == meta_tf_slice.stop and + meta_slice.step == meta_tf_slice.step + ): + meta = MetaTombRaider + + return meta diff --git a/sunnypilot/models/fetcher.py b/sunnypilot/models/fetcher.py index 451092ab4c..2a1fa1d8f2 100644 --- a/sunnypilot/models/fetcher.py +++ b/sunnypilot/models/fetcher.py @@ -71,11 +71,14 @@ class ModelParser: model_bundle.status = 0 model_bundle.generation = int(value["generation"]) model_bundle.environment = value["environment"] + model_bundle.runner = value.get("runner", custom.ModelManagerSP.Runner.snpe) 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()] diff --git a/sunnypilot/models/helpers.py b/sunnypilot/models/helpers.py index ec6dee465d..868d1fa5e5 100644 --- a/sunnypilot/models/helpers.py +++ b/sunnypilot/models/helpers.py @@ -33,14 +33,6 @@ def get_active_bundle(params: Params = None) -> custom.ModelManagerSP.ModelBundl return None -def get_model_runner_by_filename(filename: str) -> custom.ModelManagerSP.Runner: - if filename.endswith(".thneed"): - return custom.ModelManagerSP.Runner.snpe - - if filename.endswith("_tinygrad.pkl"): - return custom.ModelManagerSP.Runner.tinygrad - - def get_active_model_runner(params: Params = None, force_check=False) -> custom.ModelManagerSP.Runner: """ Determines and returns the active model runner type, based on provided parameters. @@ -65,15 +57,15 @@ def get_active_model_runner(params: Params = None, force_check=False) -> custom. params = Params() if (cached_runner_type := params.get("ModelRunnerTypeCache")) and not force_check: - return int(cached_runner_type) + if isinstance(cached_runner_type, str) and cached_runner_type.isdigit(): + return int(cached_runner_type) runner_type = custom.ModelManagerSP.Runner.tinygrad if active_bundle := get_active_bundle(params): - drive_model = next(model for model in active_bundle.models if model.type == custom.ModelManagerSP.Type.drive) - runner_type = get_model_runner_by_filename(drive_model.fileName) + runner_type = active_bundle.runner.raw if cached_runner_type != runner_type: - params.put("ModelRunnerTypeCache", str(runner_type)) + params.put("ModelRunnerTypeCache", str(int(runner_type))) return runner_type diff --git a/sunnypilot/models/manager.py b/sunnypilot/models/manager.py index 4226c6c679..06705bd5ce 100644 --- a/sunnypilot/models/manager.py +++ b/sunnypilot/models/manager.py @@ -128,12 +128,12 @@ class ModelManagerSP: os.makedirs(destination_path, exist_ok=True) try: - tasks = [self._process_model(model, destination_path) - for model in self.selected_bundle.models] + tasks = [self._process_model(model, destination_path) for model in self.selected_bundle.models] await asyncio.gather(*tasks) - self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.downloaded self.active_bundle = self.selected_bundle - self.params.put("ModelManager_ActiveBundle", self.selected_bundle.to_bytes()) + self.active_bundle.status = custom.ModelManagerSP.DownloadStatus.downloaded + self.params.put("ModelManager_ActiveBundle", self.active_bundle.to_bytes()) + self.selected_bundle = None except Exception: self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.failed @@ -153,6 +153,7 @@ class ModelManagerSP: while True: try: self.available_models = self.model_fetcher.get_available_models() + self.active_bundle = get_active_bundle(self.params) if index_to_download := self.params.get("ModelManager_DownloadIndex", block=False, encoding="utf-8"): if model_to_download := next((model for model in self.available_models if model.index == int(index_to_download)), None): diff --git a/sunnypilot/selfdrive/controls/lib/drive_helpers.py b/sunnypilot/selfdrive/controls/lib/drive_helpers.py new file mode 100644 index 0000000000..393166ae75 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/drive_helpers.py @@ -0,0 +1,27 @@ +from openpilot.common.numpy_fast import clip, interp +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, MAX_LATERAL_JERK +from openpilot.sunnypilot.modeld.constants import ModelConstants + + +def get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures): + if len(psis) != CONTROL_N: + psis = [0.0]*CONTROL_N + curvatures = [0.0]*CONTROL_N + v_ego = max(MIN_SPEED, v_ego) + + # MPC can plan to turn the wheel and turn back before t_delay. This means + # in high delay cases some corrections never even get commanded. So just use + # psi to calculate a simple linearization of desired curvature + current_curvature_desired = curvatures[0] + psi = interp(steer_delay, ModelConstants.T_IDXS[:CONTROL_N], psis) + average_curvature_desired = psi / (v_ego * steer_delay) + desired_curvature = 2 * average_curvature_desired - current_curvature_desired + + # This is the "desired rate of the setpoint" not an actual desired rate + max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 + safe_desired_curvature = clip(desired_curvature, + current_curvature_desired - max_curvature_rate * DT_MDL, + current_curvature_desired + max_curvature_rate * DT_MDL) + + return safe_desired_curvature diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 2f677f2073..c23c6ca248 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -1,11 +1,12 @@ import os import operator -from cereal import car +from cereal import car, custom from openpilot.common.params import Params from openpilot.system.hardware import PC, TICI from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess +from sunnypilot.models.helpers import get_active_model_runner from sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready, use_sunnylink_uploader WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -73,7 +74,7 @@ def use_sunnylink_uploader_shim(started, params, CP: car.CarParams) -> bool: def is_snpe_model(started, params, CP: car.CarParams) -> bool: """Check if the active model runner is SNPE.""" - return False # FIXME-SP: Enable in future PR + return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.snpe) def is_stock_model(started, params, CP: car.CarParams) -> bool: """Check if the active model runner is stock."""