From 9ccc5cc3eaf9503bc05a8453993dac32df83049b Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 04:32:01 +0100 Subject: [PATCH] models: retain pre-20hz drive model support (#531) * modeld: Retain pre-20hz drive model support * Method not available anymore on OP * some fixes * Revert "Long planner get accel: new function args (#34288)" * Revert "Fix low-speed allow_throttle behavior in long planner (#33894)" * Revert "long planner: allow throttle reflects usage (#33792)" * Revert "Gate acceleration on model gas press predictions (#33643)" * Reapply "Gate acceleration on model gas press predictions (#33643)" This reverts commit 76b08e37cb8eb94266ad9f6fed80db227e7c3428. * Reapply "long planner: allow throttle reflects usage (#33792)" This reverts commit c75244ca4e9c48084b0205b7c871e1a4e0f4e693. * Reapply "Fix low-speed allow_throttle behavior in long planner (#33894)" This reverts commit b2b7d21b7b685a2785d1beede3d223f0bb954807. * Reapply "Long planner get accel: new function args (#34288)" This reverts commit 74dca2fccf4da59cc8ac62ba9c0ad10ba3fc264b. * don't need * retain snpe * wrong * they're symlinks * remove * put back into VCS * add back * don't include built * Refactor model runner retrieval with caching support Added caching for active model runner type via `ModelRunnerTypeCache` to enhance performance and avoid redundant checks. Introduced a `force_check` flag to bypass the cache when necessary. Updated related code to handle cache clearing during onroad transitions. * Update model runner determination logic with caching fix Enhances `get_active_model_runner` to utilize caching more effectively by ensuring type consistency and updating cache only when necessary. Also updates `is_snpe_model` to pass the `started` state to the runner determination function, improving behavior for dynamic checks. * parse inputs via metadata * load model and metadata dynamically * cherry pick from devtekve as base * lateral_control_params & prev_desired_curv: MLSIM V0 to Null Pointer * old desired_curv data: MLSIM V1 to Postal Service * Bringing what was on master back then * Cleaning up * Refactor model pipeline for modularity and dynamic input handling Refactored the model pipeline by introducing helper functions to modularize model loading, metadata extraction, and input preparation. Improved flexibility in handling dynamic input keys and parsing outputs based on model configuration. Removed deprecated or unused code segments for cleaner and more maintainable structure. * Push NDv2 because why not and fix modeld * `Refactor model parsing and clean unused code dependencies` Simplified `parse_outputs` by removing unnecessary `input_keys` parameter, ensuring cleaner logic. Updated `PROCESS_NAME` for standardization and eliminated deprecated `Pathlib` dependencies in model paths. Minor adjustments improve input handling for lateral control parameters. * Refactor model and metadata loading functions. Simplified and clarified `load_model` by renaming it to `get_model_path` and removing redundant variable assignments. Streamlined `load_metadata` by directly returning the loaded metadata without intermediate variables. These changes improve code readability and maintainability. * Refactor modeld process selection based on SNPE support. Introduce conditional logic to determine and start the appropriate modeld process (SNPE or default) based on hardware support. This improves flexibility and ensures correct process management. * Walrus baby * Update longitudinal_planner.py * Improve model download progress handling and translations Refactored model download status handling logic for better clarity and added mechanisms to track status changes efficiently. Updated UI text and translations across multiple languages to reflect consistent and accurate model download states. * Revert "Update longitudinal_planner.py" This reverts commit b44a687e4cfb1e2708457478e8626c731f42db0c. * Fix variable naming for curvature size in modeld.py Renamed the variable `len` to `length` to avoid conflict with the built-in `len()` function, improving code clarity and preventing potential errors. Also removed trailing spaces in commented-out sections for better formatting consistency. * sync with upstream * some are different * should work * sim_pose only exists in in ndv3 and prior * dynamic meta constants * fix * uiview: disable power saving * fix this * ain't coasting for y'all * Static analysis * Refactor initialization of model inputs for clarity. Removed redundant pre-initialization of `driving_style`, `nav_features`, and `nav_instructions` variables. Instead, directly initialize these within their respective conditional blocks for better readability and reduced memory usage. * default to none * enable in next PR * more * Revert "more" This reverts commit f5a4220588c5b014b29173f8f89e1fecdec25336. * Revert "enable in next PR" This reverts commit 621cc4f18ead8489cb05a22a73b48e5e4fe33787. * no need to cast bool * nuke * fix desired curvature for pre LAv1 models * mypy * static * fix * new json * Going to a test branch with a different model json list * renamed model json * Update model runner handling in custom.capnp and helpers Refactored model runner logic by introducing a `runner` field in `custom.capnp` and simplifying `get_active_model_runner` logic. Removed deprecated function `get_model_runner_by_filename` and added a temporary filter in `fetcher.py` to enforce `snpe` until full tinygrad support is implemented. * Revert "Update model runner handling in custom.capnp and helpers" This reverts commit f34d872c1369fa5479496ba4e0cce7b3063ab358. * Revert "renamed model json" This reverts commit 15c6ed303b01ab78d1a576dc1d4ad105db7518df. * Revert "Going to a test branch with a different model json list" This reverts commit 4c1408fee524656049ac6502ac4f578e5b632d54. * Reapply "renamed model json" This reverts commit c6fec6912a1517d161dc9b23dce43ddf46866312. Reapply "Going to a test branch with a different model json list" This reverts commit 83e253e9a3b6573191d907584d6368d991395b0a. * Add 'runner' property to 'ModelBundle' and update relevant functions The 'ModelBundle' class in 'custom.capnp' has been extended to include a 'runner' property. This required updating 'fetcher.py' to handle the 'runner' property when parsing model bundles. Additionally, the helper function 'get_model_runner_by_filename' has been removed from 'helpers.py' as it is no longer needed because the 'runner' property provides this information. The 'get_active_model_runner' function has also been updated in light of these changes. * Refine bundle selection logic in SoftwarePanelSP Improve logic for determining which model bundle to display by considering download status and failure state. Remove unnecessary function call to enhance clarity and maintainability. * tmp * Add retrieval of active model bundle in manager loop Introduce a call to `get_active_bundle` to fetch the active model bundle and store it in `self.active_bundle`. This ensures the active bundle is always up-to-date during the model management process. * Add "Use Default" option for model selection Introduced a "Use Default" option in the model selection dropdown, allowing users to reset to the default model. Adjusted logic to handle default selection and ensure proper parameter updates. Fixed bundle status handling during the download process in the model manager. * Refactor type hint in active_bundle assignment. Removed an unnecessary type hint in the active_bundle assignment for cleaner and more maintainable code. This change does not affect functionality but improves code readability. * no nested * update json url * split out * format * condense * more --------- Co-authored-by: Jason Wen Co-authored-by: Kumar <36933347+rav4kumar@users.noreply.github.com> --- cereal/custom.capnp | 1 + selfdrive/debug/uiview.py | 11 ++- .../qt/offroad/settings/software_panel.cc | 68 +++++++++----- .../qt/offroad/settings/software_panel.h | 9 +- selfdrive/ui/translations/main_ar.ts | 36 ++++++-- selfdrive/ui/translations/main_de.ts | 36 ++++++-- selfdrive/ui/translations/main_es.ts | 36 ++++++-- selfdrive/ui/translations/main_fr.ts | 36 ++++++-- selfdrive/ui/translations/main_ja.ts | 36 ++++++-- selfdrive/ui/translations/main_ko.ts | 36 ++++++-- selfdrive/ui/translations/main_pt-BR.ts | 36 ++++++-- selfdrive/ui/translations/main_th.ts | 36 ++++++-- selfdrive/ui/translations/main_tr.ts | 36 ++++++-- selfdrive/ui/translations/main_zh-CHS.ts | 36 ++++++-- selfdrive/ui/translations/main_zh-CHT.ts | 36 ++++++-- sunnypilot/modeld/constants.py | 33 +++++++ sunnypilot/modeld/fill_model_msg.py | 49 ++++++---- sunnypilot/modeld/modeld.py | 64 ++++++++----- sunnypilot/modeld/parse_model_outputs.py | 2 + sunnypilot/modeld/runners/run_helpers.py | 90 +++++++++++++++++++ sunnypilot/models/fetcher.py | 3 + sunnypilot/models/helpers.py | 16 +--- sunnypilot/models/manager.py | 9 +- .../selfdrive/controls/lib/drive_helpers.py | 27 ++++++ system/manager/process_config.py | 5 +- 25 files changed, 607 insertions(+), 176 deletions(-) create mode 100644 sunnypilot/modeld/runners/run_helpers.py create mode 100644 sunnypilot/selfdrive/controls/lib/drive_helpers.py 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."""