mirror of
https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git
synced 2026-09-01 09:03:43 +08:00
IQ.Pilot Release Commit @ b6534c0
This commit is contained in:
IQ.Lvbs CI [bot]
@@ -0,0 +1,53 @@
|
||||
* text=auto eol=lf
|
||||
|
||||
# to move existing files into LFS:
|
||||
# git add --renormalize .
|
||||
iqpilot/selfdrive/assets/icons/iq/rotate-ccw.svg -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons/iq/rotate-ccw.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons/iq/rotate-cw.svg -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons/iq/rotate-cw.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons/iq/square-parking.svg -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons/iq/square-parking.png -filter -diff -merge -text
|
||||
|
||||
# Keep this icon in regular git (not LFS) for lightweight branding iteration.
|
||||
iqpilot/selfdrive/assets/icons_mici/experimental_mode_mici.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/experimental_mode_tizi.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/iqdynamic_mode_mici.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/iqdynamic_mode_tizi.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/iqstandard_mode_mici.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/iqstandard_mode_tizi.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_center.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_cone.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/settings/konn3kt_icon.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/buttons/toggle_dot_enabled.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/buttons/toggle_pill_enabled.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/offroad_alerts/green_wheel.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/setup/green_button.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/setup/green_button_pressed.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/setup/green_dm.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/setup/green_info.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/setup/small_slider/slider_green_rounded_rectangle.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/images/spinner_comma.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/images/k3_spinner.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons/camera.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/fonts/Syncopate-Regular.ttf -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/fonts/Tektur-Variable.ttf -filter -diff -merge -text
|
||||
iqpilot/tools/jotpluggler/assets/bootstrap-icons.ttf -filter -diff -merge -text
|
||||
|
||||
# mici UI icons stored as plain PNG — gitlab LFS server lacks these objects
|
||||
iqpilot/selfdrive/assets/icons_mici/buttons/button_circle_pressed.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/buttons/button_circle_red_pressed.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/models.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/settings/brightness.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/settings/camera.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/settings/keyboard/enter.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/settings/keyboard/enter_disabled.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/settings/sd_card.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/settings/software.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/settings/trips.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/settings/vehicle.png -filter -diff -merge -text
|
||||
iqpilot/selfdrive/assets/icons_mici/speedometer.png -filter -diff -merge -text
|
||||
|
||||
iqpilot/selfdrive/assets/icons/iq/*.png -filter -diff -merge -text
|
||||
artifacts/runtime_wheels/*.whl -filter -diff -merge -text
|
||||
+194
@@ -0,0 +1,194 @@
|
||||
venv/
|
||||
.venv/
|
||||
!artifacts/runtime/ble/rootfs/usr/local/venv/
|
||||
!artifacts/runtime/ble/rootfs/usr/local/venv/**
|
||||
# Runtime install dir for proprietary bundles — rewritten by ensure_private_installed
|
||||
# on every service start. Tracking anything under it keeps the working tree
|
||||
# perpetually dirty and sends the updater into an apply/restart loop that kills
|
||||
# ble-transportd (and comma.service) every few minutes.
|
||||
/.iqpilot/
|
||||
/.iqpilot-package-lock-sha256
|
||||
/gi
|
||||
/k3_log.txt
|
||||
/iqpilot/system/loggerd/encoder/v4l_decode
|
||||
.ci_cache
|
||||
.env
|
||||
.clang-format
|
||||
http-client.private.env.json
|
||||
.DS_Store
|
||||
.tags
|
||||
.ipynb_checkpoints
|
||||
.idea
|
||||
.overlay_init
|
||||
.overlay_consistent
|
||||
.sconsign.dblite
|
||||
a.out
|
||||
.hypothesis
|
||||
.cache/
|
||||
|
||||
/iqdbc
|
||||
/msgq
|
||||
/openpilot
|
||||
/tinygrad
|
||||
|
||||
/docs_site/
|
||||
|
||||
*.mp4
|
||||
*.dylib
|
||||
*.DSYM
|
||||
*.d
|
||||
!artifacts/runtime/ble/rootfs/etc/udev/rules.d/
|
||||
!artifacts/runtime/ble/rootfs/etc/udev/rules.d/**
|
||||
*.pyc
|
||||
!artifacts/iqpilot_*_private/**/*.pyc
|
||||
*.pyo
|
||||
.*.swp
|
||||
.*.swo
|
||||
.*.un~
|
||||
*.tmp
|
||||
*.o
|
||||
*.o-*
|
||||
*.os
|
||||
*.os-*
|
||||
*.so
|
||||
!artifacts/iqpilot_*_private/**/*.so
|
||||
!artifacts/runtime/ble/rootfs/usr/local/venv/**/*.so
|
||||
artifacts/iqpilot_private/
|
||||
artifacts/iqpilot_*_private/python/iqpilot_private/**/*.cpython-*-darwin.so
|
||||
artifacts/test-results/
|
||||
*.a
|
||||
*.clb
|
||||
*.class
|
||||
*.pyxbldc
|
||||
*.vcd
|
||||
*.mo
|
||||
*_pyx.cpp
|
||||
*.stats
|
||||
config.json
|
||||
clcache
|
||||
compile_commands.json
|
||||
compare_runtime*.html
|
||||
|
||||
iqpilot/selfdrive/pandad/pandad
|
||||
iqpilot/cereal/services.h
|
||||
iqpilot/cereal/gen
|
||||
iqpilot/cereal/messaging/bridge
|
||||
iqpilot/selfdrive/ui/translations/tmp
|
||||
iqpilot/selfdrive/car/tests/cars_dump
|
||||
iqpilot/system/camerad/camerad
|
||||
iqpilot/system/camerad/test/ae_gray_test
|
||||
|
||||
.coverage*
|
||||
coverage.xml
|
||||
htmlcov
|
||||
pandaextra
|
||||
|
||||
.mypy_cache/
|
||||
flycheck_*
|
||||
|
||||
cppcheck_report.txt
|
||||
comma*.sh
|
||||
|
||||
iqpilot/selfdrive/modeld/models/*.pkl
|
||||
!iqpilot/selfdrive/modeld/models/driving_vision_tinygrad.pkl
|
||||
!iqpilot/selfdrive/modeld/models/driving_policy_tinygrad.pkl
|
||||
!iqpilot/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl
|
||||
!iqpilot/selfdrive/modeld/models/driving_vision_metadata.pkl
|
||||
!iqpilot/selfdrive/modeld/models/driving_policy_metadata.pkl
|
||||
!iqpilot/selfdrive/modeld/models/dmonitoring_model_metadata.pkl
|
||||
iqpilot/modeld*/thneed/compile
|
||||
iqpilot/modeld*/models/*.thneed
|
||||
iqpilot/modeld*/models/*.pkl
|
||||
|
||||
# openpilot log files
|
||||
*.bz2
|
||||
*.zst
|
||||
|
||||
build/
|
||||
|
||||
!**/.gitkeep
|
||||
|
||||
poetry.toml
|
||||
Pipfile
|
||||
|
||||
### VisualStudioCode ###
|
||||
.vscode/*
|
||||
!.vscode/settings.json
|
||||
!.vscode/tasks.json
|
||||
!.vscode/launch.json
|
||||
!.vscode/extensions.json
|
||||
!.vscode/*.code-snippets
|
||||
|
||||
# Local History for Visual Studio Code
|
||||
.history/
|
||||
|
||||
# Built Visual Studio Code Extensions
|
||||
*.vsix
|
||||
|
||||
### VisualStudioCode Patch ###
|
||||
# Ignore all local history of files
|
||||
.history
|
||||
.ionide
|
||||
|
||||
.claude/
|
||||
PLAN.md
|
||||
TASK.md
|
||||
|
||||
/.antigravitycli/
|
||||
/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/_iqcrypto.cpython-312-aarch64-linux-gnu.so
|
||||
/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/alc_handshake.cpython-312-aarch64-linux-gnu.so
|
||||
/artifacts/iqpilot_alc_bundle.json
|
||||
/iqpilot/selfdrive/iqlocd/iqlocd
|
||||
/iqpilot/selfdrive/iqlocd/models/generated/car.cpp
|
||||
/iqpilot/selfdrive/iqlocd/models/generated/car.h
|
||||
/iqpilot/tools/scripts/debug/
|
||||
/iqpilot/tools/scripts/plans/
|
||||
!scripts/iqpilot/iqos_usrlocal_larch64.tar.zst
|
||||
|
||||
# pretty-build capture logs (model compile / mpc + dbc codegen stdout+stderr)
|
||||
*_tinygrad.pkl.log
|
||||
*_metadata.pkl.log
|
||||
iqpilot/selfdrive/controls/lib/*_mpc_lib/gen.log
|
||||
iqpilot/tools/cabana/dbc/*.json.log
|
||||
iqpilot/selfdrive/iqlocd/models/generated/
|
||||
osmaps_build/
|
||||
|
||||
# --- local scratch/debug + large model artifacts (never commit) ---
|
||||
/infinite_driver/
|
||||
/konn3kt_private/
|
||||
/.mull/
|
||||
/_pq*.py
|
||||
/bus_topo.py
|
||||
/cluster_bus.py
|
||||
/real_cluster_bus.py
|
||||
/dbg_big.py
|
||||
/decode_*.py
|
||||
/dtc_decoder.py
|
||||
/eps_decode.py
|
||||
/extract_nm_*.py
|
||||
/ffff_map.py
|
||||
/kmatrix_*.py
|
||||
/mcd_*.py
|
||||
/shot_*.py
|
||||
/verify_crc.py
|
||||
/CODEX_HANDOFF_*.md
|
||||
/SUNNYPILOT_REMEDIATION_PLAN.md
|
||||
/prompt.md
|
||||
/iqos44_bluetooth_gamepad_spec.md
|
||||
iqpilot/selfdrive/ui/tests/test_ui/nav_demo_report/
|
||||
**/__pycache__/
|
||||
*.pyc
|
||||
*.mlpackage/
|
||||
|
||||
|
||||
# fetcher: closed-source precompiled component — ship only the obfuscated .so
|
||||
/iqpilot/models_private_src/fetcher.py
|
||||
|
||||
# build output: a signed .app is a 233-file sealed bundle, never track a subset
|
||||
iqpilot/tools/iqmacd/macos/dist/IQ eMac.app/
|
||||
iqpilot/tools/iqmacd/macos/dist/.stage/
|
||||
iqpilot/tools/iqmacd/macos/dist/.DS_Store
|
||||
|
||||
iqpilot/tools/iqmacd/macos/dist/
|
||||
|
||||
iqpilot/tools/scripts/iqemac/clips/README.md
|
||||
Vendored
+271
@@ -0,0 +1,271 @@
|
||||
def retryWithDelay(int maxRetries, int delay, Closure body) {
|
||||
for (int i = 0; i < maxRetries; i++) {
|
||||
try {
|
||||
return body()
|
||||
} catch (Exception e) {
|
||||
sleep(delay)
|
||||
}
|
||||
}
|
||||
throw Exception("Failed after ${maxRetries} retries")
|
||||
}
|
||||
|
||||
def device(String ip, String step_label, String cmd) {
|
||||
withCredentials([file(credentialsId: 'id_rsa', variable: 'key_file')]) {
|
||||
def ssh_cmd = """
|
||||
ssh -o ConnectTimeout=5 -o ServerAliveInterval=5 -o ServerAliveCountMax=2 -o BatchMode=yes -o StrictHostKeyChecking=no -i ${key_file} 'comma@${ip}' exec /usr/bin/bash <<'END'
|
||||
|
||||
set -e
|
||||
|
||||
export TERM=xterm-256color
|
||||
|
||||
shopt -s huponexit # kill all child processes when the shell exits
|
||||
|
||||
export CI=1
|
||||
export PYTHONWARNINGS=error
|
||||
#export LOGPRINT=debug # this has gotten too spammy...
|
||||
export TEST_DIR=${env.TEST_DIR}
|
||||
export SOURCE_DIR=${env.SOURCE_DIR}
|
||||
export GIT_BRANCH=${env.GIT_BRANCH}
|
||||
export GIT_COMMIT=${env.GIT_COMMIT}
|
||||
export CI_ARTIFACTS_TOKEN=${env.CI_ARTIFACTS_TOKEN}
|
||||
export GITHUB_COMMENTS_TOKEN=${env.GITHUB_COMMENTS_TOKEN}
|
||||
export AZURE_TOKEN='${env.AZURE_TOKEN}'
|
||||
# only use 1 thread for tici tests since most require HIL
|
||||
export PYTEST_ADDOPTS="-n0 -s"
|
||||
|
||||
|
||||
export GIT_SSH_COMMAND="ssh -i /data/gitkey"
|
||||
|
||||
source ~/.bash_profile
|
||||
if [ -f /TICI ]; then
|
||||
source /etc/profile
|
||||
|
||||
rm -rf /tmp/tmp*
|
||||
rm -rf ~/.commacache
|
||||
rm -rf /dev/shm/*
|
||||
rm -rf /dev/tmp/tmp*
|
||||
|
||||
if ! systemctl is-active --quiet systemd-resolved; then
|
||||
echo "restarting resolved"
|
||||
sudo systemctl start systemd-resolved
|
||||
sleep 3
|
||||
fi
|
||||
|
||||
# restart aux USB
|
||||
if [ -e /sys/bus/usb/drivers/hub/3-0:1.0 ]; then
|
||||
echo "restarting aux usb"
|
||||
echo "3-0:1.0" | sudo tee /sys/bus/usb/drivers/hub/unbind
|
||||
sleep 0.5
|
||||
echo "3-0:1.0" | sudo tee /sys/bus/usb/drivers/hub/bind
|
||||
fi
|
||||
fi
|
||||
if [ -f /data/openpilot/launch_env.sh ]; then
|
||||
source /data/openpilot/launch_env.sh
|
||||
fi
|
||||
|
||||
ln -snf ${env.TEST_DIR} /data/pythonpath
|
||||
|
||||
cd ${env.TEST_DIR} || true
|
||||
time ${cmd}
|
||||
END"""
|
||||
|
||||
sh script: ssh_cmd, label: step_label
|
||||
}
|
||||
}
|
||||
|
||||
def deviceStage(String stageName, String deviceType, List extra_env, def steps) {
|
||||
stage(stageName) {
|
||||
if (currentBuild.result != null) {
|
||||
return
|
||||
}
|
||||
|
||||
if (isReplay()) {
|
||||
error("REPLAYING TESTS IS NOT ALLOWED. FIX THEM INSTEAD.")
|
||||
}
|
||||
|
||||
def extra = extra_env.collect { "export ${it}" }.join('\n');
|
||||
def branch = env.BRANCH_NAME ?: 'master';
|
||||
def gitDiff = sh returnStdout: true, script: 'curl -s -H "Authorization: Bearer ${GITHUB_COMMENTS_TOKEN}" https://api.github.com/repos/commaai/openpilot/compare/master...${GIT_BRANCH} | jq .files[].filename || echo "/"', label: 'Getting changes'
|
||||
|
||||
lock(resource: "", label: deviceType, inversePrecedence: true, variable: 'device_ip', quantity: 1, resourceSelectStrategy: 'random') {
|
||||
docker.image('ghcr.io/commaai/alpine-ssh').inside('--user=root') {
|
||||
timeout(time: 35, unit: 'MINUTES') {
|
||||
retry (3) {
|
||||
def date = sh(script: 'date', returnStdout: true).trim();
|
||||
device(device_ip, "set time", "date -s '" + date + "'")
|
||||
device(device_ip, "git checkout", extra + "\n" + readFile("iqpilot/selfdrive/test/setup_device_ci.sh"))
|
||||
}
|
||||
steps.each { item ->
|
||||
def name = item[0]
|
||||
def cmd = item[1]
|
||||
|
||||
def args = item[2]
|
||||
def diffPaths = args.diffPaths ?: []
|
||||
def cmdTimeout = args.timeout ?: 9999
|
||||
|
||||
if (branch != "master" && !branch.contains("__jenkins_loop_") && diffPaths && !hasPathChanged(gitDiff, diffPaths)) {
|
||||
println "Skipping ${name}: no changes in ${diffPaths}."
|
||||
return
|
||||
} else {
|
||||
timeout(time: cmdTimeout, unit: 'SECONDS') {
|
||||
device(device_ip, name, cmd)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
def hasPathChanged(String gitDiff, List<String> paths) {
|
||||
for (path in paths) {
|
||||
if (gitDiff.contains(path)) {
|
||||
return true
|
||||
}
|
||||
}
|
||||
return false
|
||||
}
|
||||
|
||||
def isReplay() {
|
||||
def replayClass = "org.jenkinsci.plugins.workflow.cps.replay.ReplayCause"
|
||||
return currentBuild.rawBuild.getCauses().any{ cause -> cause.toString().contains(replayClass) }
|
||||
}
|
||||
|
||||
def setupCredentials() {
|
||||
withCredentials([
|
||||
string(credentialsId: 'azure_token', variable: 'AZURE_TOKEN'),
|
||||
]) {
|
||||
env.AZURE_TOKEN = "${AZURE_TOKEN}"
|
||||
}
|
||||
|
||||
withCredentials([
|
||||
string(credentialsId: 'ci_artifacts_pat', variable: 'CI_ARTIFACTS_TOKEN'),
|
||||
]) {
|
||||
env.CI_ARTIFACTS_TOKEN = "${CI_ARTIFACTS_TOKEN}"
|
||||
}
|
||||
|
||||
withCredentials([
|
||||
string(credentialsId: 'post_comments_github_pat', variable: 'GITHUB_COMMENTS_TOKEN'),
|
||||
]) {
|
||||
env.GITHUB_COMMENTS_TOKEN = "${GITHUB_COMMENTS_TOKEN}"
|
||||
}
|
||||
}
|
||||
|
||||
def step(String name, String cmd, Map args = [:]) {
|
||||
return [name, cmd, args]
|
||||
}
|
||||
|
||||
node {
|
||||
env.CI = "1"
|
||||
env.PYTHONWARNINGS = "error"
|
||||
env.TEST_DIR = "/data/openpilot"
|
||||
env.SOURCE_DIR = "/data/openpilot_source/"
|
||||
setupCredentials()
|
||||
|
||||
env.GIT_BRANCH = checkout(scm).GIT_BRANCH
|
||||
env.GIT_COMMIT = checkout(scm).GIT_COMMIT
|
||||
|
||||
def excludeBranches = ['__nightly', 'devel', 'devel-staging', 'release3', 'release3-staging',
|
||||
'release-tici', 'release-tizi', 'release-tizi-staging', 'testing-closet*', 'hotfix-*']
|
||||
def excludeRegex = excludeBranches.join('|').replaceAll('\\*', '.*')
|
||||
|
||||
if (env.BRANCH_NAME != 'master' && !env.BRANCH_NAME.contains('__jenkins_loop_')) {
|
||||
properties([
|
||||
disableConcurrentBuilds(abortPrevious: true)
|
||||
])
|
||||
}
|
||||
|
||||
try {
|
||||
if (env.BRANCH_NAME == 'devel-staging') {
|
||||
deviceStage("build release-tizi-staging", "tizi-needs-can", [], [
|
||||
step("build release-tizi-staging", "RELEASE_BRANCH=release-tizi-staging $SOURCE_DIR/release/build_release.sh"),
|
||||
])
|
||||
}
|
||||
|
||||
if (env.BRANCH_NAME == '__nightly') {
|
||||
parallel (
|
||||
'nightly': {
|
||||
deviceStage("build nightly", "tizi-needs-can", [], [
|
||||
step("build nightly", "RELEASE_BRANCH=nightly $SOURCE_DIR/release/build_release.sh"),
|
||||
])
|
||||
},
|
||||
'nightly-dev': {
|
||||
deviceStage("build nightly-dev", "tizi-needs-can", [], [
|
||||
step("build nightly-dev", "PANDA_DEBUG_BUILD=1 RELEASE_BRANCH=nightly-dev $SOURCE_DIR/release/build_release.sh"),
|
||||
])
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
if (!env.BRANCH_NAME.matches(excludeRegex)) {
|
||||
parallel (
|
||||
'onroad tests': {
|
||||
deviceStage("onroad", "tizi-needs-can", ["UNSAFE=1"], [
|
||||
step("build openpilot", "cd iqpilot/system/manager && ./build.py"),
|
||||
step("check dirty", "release/check-dirty.sh"),
|
||||
step("onroad tests", "pytest iqpilot/selfdrive/test/test_onroad.py -s", [timeout: 60]),
|
||||
])
|
||||
},
|
||||
'HW + Unit Tests': {
|
||||
deviceStage("tizi-hardware", "tizi-common", ["UNSAFE=1"], [
|
||||
step("audit proprietary entrypoints", "python3 scripts/iqpilot/audit_proprietary_entrypoints.py --check"),
|
||||
step("audit proprietary runtime", "python3 scripts/iqpilot/audit_proprietary_runtime.py"),
|
||||
step("build", "cd iqpilot/system/manager && ./build.py"),
|
||||
step("test pandad", "pytest iqpilot/selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "iqpilot/selfdrive/pandad/"]]),
|
||||
step("test power draw", "pytest -s iqpilot/system/hardware/tici/tests/test_power_draw.py"),
|
||||
step("test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest iqpilot/system/loggerd/tests/test_encoder.py", [diffPaths: ["iqpilot/system/loggerd/"]]),
|
||||
step("test manager", "pytest iqpilot/system/manager/test/test_manager.py"),
|
||||
])
|
||||
},
|
||||
'loopback': {
|
||||
deviceStage("loopback", "tizi-loopback", ["UNSAFE=1"], [
|
||||
step("build openpilot", "cd iqpilot/system/manager && ./build.py"),
|
||||
step("test pandad loopback", "pytest iqpilot/selfdrive/pandad/tests/test_pandad_loopback.py"),
|
||||
])
|
||||
},
|
||||
'camerad OX03C10': {
|
||||
deviceStage("OX03C10", "tizi-ox03c10", ["UNSAFE=1"], [
|
||||
step("build", "cd iqpilot/system/manager && ./build.py"),
|
||||
step("test camerad", "pytest iqpilot/system/camerad/test/test_camerad.py", [timeout: 60]),
|
||||
step("test exposure", "pytest iqpilot/system/camerad/test/test_exposure.py"),
|
||||
])
|
||||
},
|
||||
'camerad OS04C10': {
|
||||
deviceStage("OS04C10", "tici-os04c10", ["UNSAFE=1"], [
|
||||
step("build", "cd iqpilot/system/manager && ./build.py"),
|
||||
step("test camerad", "pytest iqpilot/system/camerad/test/test_camerad.py", [timeout: 60]),
|
||||
step("test exposure", "pytest iqpilot/system/camerad/test/test_exposure.py"),
|
||||
])
|
||||
},
|
||||
'sensord': {
|
||||
deviceStage("LSM + MMC", "tizi-lsmc", ["UNSAFE=1"], [
|
||||
step("build", "cd iqpilot/system/manager && ./build.py"),
|
||||
step("test sensord", "pytest iqpilot/system/sensord/tests/test_sensord.py"),
|
||||
])
|
||||
},
|
||||
'replay': {
|
||||
deviceStage("model-replay", "tizi-replay", ["UNSAFE=1"], [
|
||||
step("build", "cd iqpilot/system/manager && ./build.py", [diffPaths: ["iqpilot/selfdrive/dmonitoringmodeld/", "iqpilot/selfdrive/iqmodeld/", "pyproject.toml", "iqpilot/selfdrive/test/process_replay/model_replay.py"]]),
|
||||
step("model replay", "iqpilot/selfdrive/test/process_replay/model_replay.py", [diffPaths: ["iqpilot/selfdrive/dmonitoringmodeld/", "iqpilot/selfdrive/iqmodeld/", "pyproject.toml", "iqpilot/selfdrive/test/process_replay/model_replay.py"]]),
|
||||
])
|
||||
},
|
||||
'tizi': {
|
||||
deviceStage("tizi", "tizi", ["UNSAFE=1"], [
|
||||
step("build openpilot", "cd iqpilot/system/manager && ./build.py"),
|
||||
step("test pandad loopback", "SINGLE_PANDA=1 pytest iqpilot/selfdrive/pandad/tests/test_pandad_loopback.py"),
|
||||
step("test pandad spi", "pytest iqpilot/selfdrive/pandad/tests/test_pandad_spi.py"),
|
||||
step("test amp", "pytest iqpilot/system/hardware/tici/tests/test_amplifier.py"),
|
||||
// TODO: enable once new AGNOS is available
|
||||
// step("test esim", "pytest iqpilot/system/hardware/tici/tests/test_esim.py"),
|
||||
step("test qcomgpsd", "pytest iqpilot/system/qcomgpsd/tests/test_qcomgpsd.py", [diffPaths: ["iqpilot/system/qcomgpsd/"]]),
|
||||
])
|
||||
},
|
||||
|
||||
)
|
||||
}
|
||||
} catch (Exception e) {
|
||||
currentBuild.result = 'FAILED'
|
||||
throw e
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
IQ.Lvbs License v1.8.2
|
||||
|
||||
Copyright (c) 2026 IQ.Lvbs LLC, a part of Project Teal Lvbs Inc. All Rights Reserved.
|
||||
|
||||
DEFINITIONS
|
||||
|
||||
"Software" refers to IQ.Pilot, konn3kt, and all associated source code,
|
||||
documentation, and assets owned by the Copyright Holder.
|
||||
|
||||
"Open Components" refers to portions of the Software explicitly marked as
|
||||
open source.
|
||||
|
||||
"Proprietary Components" refers to all portions of the Software not made
|
||||
available to the public in source form.
|
||||
|
||||
"Copyright Holder" refers to IQ.Lvbs LLC, a part of Project Teal Lvbs Inc.
|
||||
|
||||
GRANT OF LICENSE
|
||||
|
||||
Subject to the terms of this license, you are granted a limited,
|
||||
non-exclusive, revocable license to:
|
||||
|
||||
1. View, study, and learn from the Open Components
|
||||
2. Modify the Open Components for personal, internal, or open-source public use
|
||||
3. Run the Software for personal, non-commercial purposes
|
||||
|
||||
RESTRICTIONS
|
||||
|
||||
You may NOT:
|
||||
|
||||
1. Claim ownership of any part of the Software, excluding your own
|
||||
modifications that do not incorporate Proprietary Components.
|
||||
|
||||
2. Reverse engineer, decompile, disassemble, or in any way attempt to
|
||||
circumvent the obfuscation of the Proprietary Components.
|
||||
|
||||
3. Use the Software or any derivative for commercial purposes without
|
||||
explicit written permission from the Copyright Holder.
|
||||
|
||||
4. Remove or alter any copyright notices or this license.
|
||||
|
||||
5. Sublicense, sell, or transfer rights to the Software.
|
||||
|
||||
6. Use the Software and/or its source code to compete with or create a
|
||||
substantially similar product.
|
||||
|
||||
7. Use the Software in closed source software not licensed by IQ.Lvbs LLC.
|
||||
|
||||
CONSEQUENCES OF VIOLATION
|
||||
|
||||
In the event any Restriction is violated, any product created using inspiration from, or source code from, IQ.Pilot or Konn3kt shall be subject to a licensing fee determined solely by the Copyright Holder. Additionally, the violating party hereby grants IQ.Lvbs LLC an exclusive, irrevocable, worldwide, royalty-free license to use any and all assets from the infringing product on IQ.Lvbs webpages, advertising materials, and in any other manner IQ.Lvbs sees fit.
|
||||
|
||||
OWNERSHIP
|
||||
|
||||
All rights, title, and interest in the Software remain exclusively with the
|
||||
Copyright Holder. Any modifications, improvements, or derivative works you
|
||||
create based on the Software are owned by the Copyright Holder. By
|
||||
contributing modifications, you irrevocably assign all rights to the
|
||||
Copyright Holder.
|
||||
|
||||
PROPRIETARY COMPONENTS
|
||||
|
||||
The Proprietary Components are provided in binary or obfuscated form only.
|
||||
Reverse engineering, decompilation, or disassembly of Proprietary Components
|
||||
is strictly prohibited. Violation of this provision entitles IQ.Lvbs LLC to
|
||||
pursue all available legal remedies to protect its intellectual property and
|
||||
trade secrets.
|
||||
|
||||
NO WARRANTY
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT
|
||||
HOLDER DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT
|
||||
LIMITED TO MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT.
|
||||
|
||||
LIMITATION OF LIABILITY
|
||||
|
||||
IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES, OR
|
||||
OTHER LIABILITY ARISING FROM THE USE OF THE SOFTWARE. THE USER ACCEPTS FULL
|
||||
RESPONSIBILITY FOR ANY AND ALL LIABILITIES WHEN USING IQ.LVBS SOFTWARE.
|
||||
|
||||
TERMINATION
|
||||
|
||||
This license terminates automatically if you violate any of its terms. Upon
|
||||
termination, you must destroy all copies of the Software in your possession.
|
||||
|
||||
The Copyright Holder reserves the right to revoke this license at any time
|
||||
for any reason.
|
||||
|
||||
GOVERNING LAW
|
||||
|
||||
This license shall be governed by the laws of the State of Illinois, United
|
||||
States of America. Any disputes arising under this license shall be subject
|
||||
to the exclusive jurisdiction of the courts located in Henry County, Illinois.
|
||||
|
||||
---
|
||||
|
||||
For commercial licensing inquiries, contact: support@iqlvbs.com
|
||||
|
||||
IQ.Pilot is a fork of Stock OpenPilot maintained by Comma.ai, Inc. licensed under the following notice:
|
||||
Copyright (c) 2018, Comma.ai, Inc.
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
@@ -0,0 +1,63 @@
|
||||
# IQ.Pilot:
|
||||
|
||||
|
||||
|
||||
## Join Our Public Beta at: https://discord.iqlvbs.com
|
||||
|
||||
## Running IQ.Pilot
|
||||
* A modern comma, or clone device to run this software (Comma 3, 3x, 4, Konik A1/M, Mr.One C3, C3 Lite)
|
||||
* One of [the supported cars](https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot/src/branch/release/artifacts/package_sources/iqdbc/docs/CARS.md).
|
||||
* A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car
|
||||
#### Wondering if IQ.Pilot supports your car? IQ.Pilot supports every car [stock openpilot](https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot/src/branch/release/artifacts/package_sources/iqdbc/docs/CARS.md) supports!
|
||||
## Installation
|
||||
#### Installing Via Installer URL:
|
||||
#### Enter the following into your device custom URL box to install IQ.Pilot:
|
||||
`installer.iqlvbs.com/release`
|
||||
#### Having Trouble? If your device is currently running AGNOS 13.1 or older, you should install latest stock openpilot, then install IQ.Pilot, or try one of the alternative methods listed below!
|
||||
|
||||
|
||||
## Alternative Methods of Installation:
|
||||
#### Enabling SSH:
|
||||
* In your device's settings, go into "Developer Settings"
|
||||
* Enable the "Enable SSH" toggle if it is not already on.
|
||||
* Next to "SSH Keys", click on "Add" and then enter your GitHub username.
|
||||
* Run the command below (replace your_email@example.com with your GitHub account email, then paste the output [here](https://github.com/settings/keys) after clicking on "New SSH Key", then reboot your comma device.
|
||||
#### SSH Key Command: `ssh-keygen -t ed25519 -C "your_email@example.com" -f ~/.ssh/id_ed25519 -N "" && cat ~/.ssh/id_ed25519.pub`
|
||||
#### Note: If you have already setup SSH and can SSH into your device, skip this section.
|
||||
|
||||
#### Installing Via SSH:
|
||||
#### Once you are connected to your device via SSH, you can paste the following command below to install IQ.Pilot:
|
||||
`cd .. && rm -rf openpilot && git clone https://git.konn3kt.com/IQ.Lvbs/IQ.Pilot openpilot -b release && cd openpilot && sudo reboot`
|
||||
#### If you'd like to backup your previous installation as well, paste the following command below to install IQ.Pilot:
|
||||
`cd .. && mv openpilot openpilot_backup_X && git clone https://git.konn3kt.com/IQ.Lvbs/IQ.Pilot -b release && cd openpilot && sudo reboot`
|
||||
#### Alternatively, you can use your existing fork's built in tools to switch your branch as well:
|
||||
`git remote add iqpilot https://git.konn3kt.com/IQ.Lvbs/IQ.Pilot && iq switch iqpilot release`
|
||||
|
||||
---
|
||||
|
||||
## Releases
|
||||
|
||||
| Branch | Status | Description |
|
||||
|---|---|---------------------------------------------------------------------------------------------------------------------------------------------------|
|
||||
| `beta` | Pre-release | Preview of the next release in progress. Expect bugs. Use it if you want early access, just know things can break. |
|
||||
| `release` | **Stable 1.0c** | Current stable production release of IQ.Pilot. |
|
||||
| `release-meb` | **Stable 1.0c** | Up to date with `release`, built specifically for VW MEB and MQBevo platform vehicles (ID.4, ID.3, ID.5, Golf MK8, Tiguan/Atlas 2024+, and more). |
|
||||
|
||||
---
|
||||
#### Side Note: Beta's are releases of IQ.Pilot that are released publically for beta testing by the IQ.Pilot team, expect bugs, and unstable behavior, for stability please use release.
|
||||
|
||||
## 📊 User Data
|
||||
### IQ.Pilot uploads your data to Konn3kt, by IQ.Lvbs.
|
||||
#### Konn3kt is the most secure, encrypted, feature rich management platform for your IQ.Pilot device. Konn3kt has dual end-to-end encryption, your data is encrypted in transit, and at rest, you, and only you have access to your device, and your data.
|
||||
|
||||
#### IQ.Pilot allows users to disable uploading entirely if they wish.
|
||||
#### Konn3kt encrypts your logs, and video data with dual end-to-end encryption, ensuring that your data is accessible to you, and only you, not even accessible to IQ.Lvbs.
|
||||
|
||||
## Terms of Service / Privacy Policy / Licensing
|
||||
#### IQ.Pilot is subject to the License found in this repository, [Terms of Service](https://konn3kt.com/tos), and, [Privacy Policy](https://konn3kt.com/privacy).
|
||||
|
||||
|
||||
## Support IQ.Pilot?
|
||||
Sorry, I have better things to do than ask my users for donations. - Teal
|
||||
|
||||
<span>-</span> IQ.Lvbs, by Project Teal Lvbs
|
||||
+272
@@ -0,0 +1,272 @@
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
import sysconfig
|
||||
import platform
|
||||
import shlex
|
||||
import numpy as np
|
||||
|
||||
import iqdbc
|
||||
import msgq as msgq_package
|
||||
import panda
|
||||
import tinygrad
|
||||
|
||||
import SCons.Errors
|
||||
|
||||
SCons.Warnings.warningAsException(True)
|
||||
|
||||
# scons only auto-loads a site dir named site_scons at the repo root; ours lives under tools/,
|
||||
# so replicate what _load_site_scons_dir does (sys.path for site_tools imports + run site_init)
|
||||
SITE_DIR = Dir('#iqpilot/tools/scons').abspath
|
||||
if SITE_DIR not in sys.path:
|
||||
sys.path.insert(0, SITE_DIR)
|
||||
import site_init # noqa: F401
|
||||
|
||||
# capnp's kj library warns when $PWD is stale (doesn't match the real cwd); keep them in sync
|
||||
os.environ.pop('PWD', None)
|
||||
|
||||
Decider('MD5-timestamp')
|
||||
|
||||
SetOption('num_jobs', max(1, int(os.cpu_count()/2)))
|
||||
|
||||
AddOption('--asan', action='store_true', help='turn on ASAN')
|
||||
AddOption('--ubsan', action='store_true', help='turn on UBSan')
|
||||
AddOption('--mutation', action='store_true', help='generate mutation-ready code')
|
||||
AddOption('--ccflags', action='store', type='string', default='', help='pass arbitrary flags over the command line')
|
||||
AddOption('--minimal',
|
||||
action='store_false',
|
||||
dest='extras',
|
||||
default=os.path.exists(File('#.gitattributes').abspath), # minimal by default on release branch (where there's no LFS)
|
||||
help='the minimum IQ.Pilot build. no tests, tools, etc.')
|
||||
AddOption('--verbose', action='store_true', help='show full compiler/linker command lines instead of short build lines')
|
||||
|
||||
python_paths = [
|
||||
Dir("#").abspath,
|
||||
]
|
||||
for p in reversed(python_paths):
|
||||
if p not in sys.path:
|
||||
sys.path.insert(0, p)
|
||||
|
||||
if external_pythonpath := os.environ.get("PYTHONPATH"):
|
||||
python_paths += [p for p in external_pythonpath.split(os.pathsep) if p and p not in python_paths]
|
||||
|
||||
# Detect platform
|
||||
arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
|
||||
if platform.system() == "Darwin":
|
||||
arch = "Darwin"
|
||||
brew_prefix = subprocess.check_output(['brew', '--prefix'], encoding='utf8').strip()
|
||||
elif arch == "aarch64" and os.path.isfile('/TICI'):
|
||||
arch = "larch64"
|
||||
try:
|
||||
from iqpilot.system.hardware import HARDWARE
|
||||
HARDWARE.set_power_save(False)
|
||||
os.sched_setaffinity(0, range(8))
|
||||
except Exception:
|
||||
# host tuning needs real device sysfs; the prebuilt arm64 container fakes /TICI and has none
|
||||
pass
|
||||
assert arch in [
|
||||
"larch64", # linux tici arm64
|
||||
"aarch64", # linux pc arm64
|
||||
"x86_64", # linux pc x64
|
||||
"Darwin", # macOS arm64 (x86 not supported)
|
||||
]
|
||||
|
||||
# ffmpeg comes from the system (brew on macOS, distro packages elsewhere) rather than
|
||||
# a vendored wheel, so it always needs the static-link deps. Exported so tools/ can
|
||||
# take upstream's `ffmpeg_libs` form instead of hand-listing codecs per SConscript.
|
||||
ffmpeg_libs = ['avformat', 'avcodec', 'avutil', 'x264', 'z']
|
||||
if arch != "Darwin":
|
||||
ffmpeg_libs += ['va', 'va-drm', 'drm']
|
||||
|
||||
env = Environment(
|
||||
ENV={
|
||||
"PATH": os.environ['PATH'],
|
||||
"PYTHONPATH": os.pathsep.join(python_paths + [Dir(f"#iqpilot/third_party/acados").abspath]),
|
||||
"ACADOS_SOURCE_DIR": Dir("#iqpilot/third_party/acados").abspath,
|
||||
"ACADOS_PYTHON_INTERFACE_PATH": Dir("#iqpilot/third_party/acados/acados_template").abspath,
|
||||
"TERA_PATH": Dir("#").abspath + f"/iqpilot/third_party/acados/{arch}/t_renderer"
|
||||
},
|
||||
CC='clang',
|
||||
CXX='clang++',
|
||||
CCFLAGS=[
|
||||
"-g",
|
||||
"-fPIC",
|
||||
"-O2",
|
||||
"-Wunused",
|
||||
"-Werror",
|
||||
"-Wshadow",
|
||||
"-Wno-unknown-warning-option",
|
||||
"-Wno-inconsistent-missing-override",
|
||||
"-Wno-c99-designator",
|
||||
"-Wno-reorder-init-list",
|
||||
"-Wno-vla-cxx-extension",
|
||||
],
|
||||
CFLAGS=["-std=gnu11"],
|
||||
CXXFLAGS=["-std=c++1z"],
|
||||
CPPPATH=[
|
||||
"#",
|
||||
"#iqpilot",
|
||||
iqdbc.INCLUDE_PATH,
|
||||
msgq_package.INCLUDE_PATH,
|
||||
panda.INCLUDE_PATH,
|
||||
"#iqpilot/cereal/gen/cpp",
|
||||
"#iqpilot/third_party",
|
||||
"#iqpilot/third_party/json11",
|
||||
"#iqpilot/third_party/linux/include",
|
||||
"#iqpilot/third_party/acados/include",
|
||||
"#iqpilot/third_party/acados/include/blasfeo/include",
|
||||
"#iqpilot/third_party/acados/include/hpipm/include",
|
||||
"#iqpilot/third_party/catch2/include",
|
||||
"#iqpilot/third_party/libyuv/include",
|
||||
],
|
||||
LIBPATH=[
|
||||
"#iqpilot/common",
|
||||
"#iqpilot/third_party",
|
||||
"#iqpilot/selfdrive/pandad",
|
||||
f"#iqpilot/third_party/libyuv/{arch}/lib",
|
||||
f"#iqpilot/third_party/acados/{arch}/lib",
|
||||
],
|
||||
RPATH=[],
|
||||
CYTHONCFILESUFFIX=".cpp",
|
||||
COMPILATIONDB_USE_ABSPATH=True,
|
||||
tools=["default", "cython", "compilation_db"],
|
||||
toolpath=["#iqpilot/tools/scons/site_tools"],
|
||||
)
|
||||
|
||||
# Arch-specific flags and paths
|
||||
if arch == "larch64":
|
||||
env.Append(CPPPATH=[
|
||||
"#iqpilot/third_party/opencl/include",
|
||||
"/usr/include/aarch64-linux-gnu",
|
||||
])
|
||||
env.Append(LIBPATH=[
|
||||
"/usr/local/lib",
|
||||
"/usr/lib/aarch64-linux-gnu",
|
||||
"/system/vendor/lib64",
|
||||
])
|
||||
arch_flags = ["-D__TICI__", "-mcpu=cortex-a57", "-DQCOM2"]
|
||||
env.Append(CCFLAGS=arch_flags)
|
||||
env.Append(CXXFLAGS=arch_flags)
|
||||
elif arch == "Darwin":
|
||||
env.Append(LIBPATH=[
|
||||
f"{brew_prefix}/lib",
|
||||
f"{brew_prefix}/opt/openssl@3.0/lib",
|
||||
f"{brew_prefix}/opt/llvm/lib/c++",
|
||||
"/System/Library/Frameworks/OpenGL.framework/Libraries",
|
||||
])
|
||||
env.Append(CCFLAGS=["-DGL_SILENCE_DEPRECATION"])
|
||||
env.Append(CXXFLAGS=["-DGL_SILENCE_DEPRECATION"])
|
||||
env.Append(CPPPATH=[
|
||||
f"{brew_prefix}/include",
|
||||
f"{brew_prefix}/opt/openssl@3.0/include",
|
||||
])
|
||||
# the same static libs are pulled in by multiple deps; harmless, quiet the noise
|
||||
env.Append(LINKFLAGS=["-Wl,-no_warn_duplicate_libraries"])
|
||||
# spi.cc is device-only, so it archives with no symbols on host. 'ar rcS' skips the
|
||||
# symbol table (no warning) and ranlib rebuilds it quietly with -no_warning_for_no_symbols.
|
||||
env.Replace(ARFLAGS="rcS")
|
||||
env.Append(RANLIBFLAGS=["-no_warning_for_no_symbols"])
|
||||
else:
|
||||
env.Append(LIBPATH=[
|
||||
"/usr/lib",
|
||||
"/usr/local/lib",
|
||||
])
|
||||
|
||||
# Sanitizers and extra CCFLAGS from CLI
|
||||
if GetOption('asan'):
|
||||
env.Append(CCFLAGS=["-fsanitize=address", "-fno-omit-frame-pointer"])
|
||||
env.Append(LINKFLAGS=["-fsanitize=address"])
|
||||
elif GetOption('ubsan'):
|
||||
env.Append(CCFLAGS=["-fsanitize=undefined"])
|
||||
env.Append(LINKFLAGS=["-fsanitize=undefined"])
|
||||
|
||||
_extra_cc = shlex.split(GetOption('ccflags') or '')
|
||||
if _extra_cc:
|
||||
env.Append(CCFLAGS=_extra_cc)
|
||||
|
||||
# no --as-needed on mac linker
|
||||
if arch != "Darwin":
|
||||
env.Append(LINKFLAGS=["-Wl,--as-needed", "-Wl,--no-undefined"])
|
||||
|
||||
# pretty build output (short colored lines; pass --verbose for full commands)
|
||||
env.Tool('pretty')
|
||||
|
||||
# progress output
|
||||
node_interval = 5
|
||||
node_count = 0
|
||||
def progress_function(node):
|
||||
global node_count
|
||||
node_count += node_interval
|
||||
sys.stderr.write("progress: %d\n" % node_count)
|
||||
if os.environ.get('SCONS_PROGRESS'):
|
||||
Progress(progress_function, interval=node_interval)
|
||||
|
||||
# ********** Cython build environment **********
|
||||
py_include = sysconfig.get_paths()['include']
|
||||
envCython = env.Clone()
|
||||
envCython["CPPPATH"] += [py_include, np.get_include()]
|
||||
envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"]
|
||||
envCython["CCFLAGS"].remove("-Werror")
|
||||
|
||||
envCython["LIBS"] = []
|
||||
if arch == "Darwin":
|
||||
envCython["LINKFLAGS"] = env["LINKFLAGS"] + ["-bundle", "-undefined", "dynamic_lookup"]
|
||||
else:
|
||||
envCython["LINKFLAGS"] = ["-pthread", "-shared"]
|
||||
|
||||
np_version = SCons.Script.Value(np.__version__)
|
||||
Export('envCython', 'np_version')
|
||||
|
||||
tinygrad_dir = os.path.dirname(tinygrad.__file__)
|
||||
Export('env', 'arch', 'ffmpeg_libs', 'tinygrad_dir')
|
||||
|
||||
# Setup cache dir
|
||||
default_cache_dir = os.environ.get('SCONS_CACHE_DIR') or ('/data/scons_cache' if arch == "larch64" else '/tmp/scons_cache')
|
||||
cache_dir = ARGUMENTS.get('cache_dir', default_cache_dir)
|
||||
CacheDir(cache_dir)
|
||||
Clean(["."], cache_dir)
|
||||
|
||||
# ********** start building stuff **********
|
||||
|
||||
# Build common module
|
||||
SConscript(['iqpilot/common/SConscript'])
|
||||
Import('_common')
|
||||
common = [_common, 'json11', 'zmq']
|
||||
Export('common')
|
||||
|
||||
msgq = File(msgq_package.LIB_PATH)
|
||||
visionipc = File(msgq_package.VISIONIPC_LIB_PATH)
|
||||
msgq_python = File(msgq_package.PYTHON_LIB_PATH)
|
||||
Export('msgq', 'visionipc', 'msgq_python')
|
||||
|
||||
SConscript(['iqpilot/cereal/SConscript'])
|
||||
|
||||
Import('socketmaster')
|
||||
messaging = [socketmaster, msgq, 'capnp', 'kj',]
|
||||
Export('messaging')
|
||||
|
||||
# Build system services
|
||||
SConscript([
|
||||
'iqpilot/system/loggerd/SConscript',
|
||||
'iqpilot/system/proprietary_runtime/SConscript',
|
||||
])
|
||||
|
||||
if arch == "larch64":
|
||||
SConscript(['iqpilot/system/camerad/SConscript'])
|
||||
|
||||
# Build openpilot
|
||||
SConscript(['iqpilot/third_party/SConscript'])
|
||||
|
||||
SConscript(['iqpilot/selfdrive/SConscript'])
|
||||
|
||||
SConscript(['iqpilot/SConscript'])
|
||||
|
||||
if Dir('#iqpilot/tools/cabana/').exists() and GetOption('extras'):
|
||||
SConscript(['iqpilot/tools/replay/SConscript'])
|
||||
if arch != "larch64":
|
||||
SConscript(['iqpilot/tools/cabana/SConscript'])
|
||||
if Dir('#iqpilot/tools/jotpluggler/').exists():
|
||||
SConscript(['iqpilot/tools/jotpluggler/SConscript'])
|
||||
|
||||
|
||||
env.CompilationDatabase('compile_commands.json')
|
||||
+52
@@ -0,0 +1,52 @@
|
||||
# Security Policy
|
||||
|
||||
## Supported Versions
|
||||
|
||||
| Component | Version | Supported |
|
||||
| --------- | ------- | --------- |
|
||||
| IQ.Pilot | latest | TRUE |
|
||||
| konn3kt | latest | TRUE |
|
||||
|
||||
## Scope
|
||||
|
||||
This security policy covers the following components, but applies to all IQ.Lvbs software:
|
||||
|
||||
- **IQ.Pilot**
|
||||
- **konn3kt**
|
||||
|
||||
## Reporting a Vulnerability
|
||||
|
||||
If you discover a security vulnerability in IQ.Pilot or konn3kt, please report it responsibly, abuse is against the terms of service for IQ.Lvbs software.
|
||||
|
||||
**Email:** security@iqlvbs.com
|
||||
|
||||
Please include:
|
||||
|
||||
- A description of the vulnerability and its impact, and potential impact
|
||||
- Steps to reproduce the issue
|
||||
- Affected component(s)
|
||||
- Any relevant logs, screenshots, or proof of concept code that can assist with reproduction.
|
||||
|
||||
## Response Timeline
|
||||
|
||||
- **Acknowledgment:** Within 168 hours of receipt
|
||||
- **Initial Assessment:** Within 1-14 days
|
||||
- **Resolution Target:** Depends on severity; critical issues are prioritized immediately
|
||||
|
||||
## Responsible Disclosure
|
||||
|
||||
We ask that you:
|
||||
|
||||
- Allow reasonable time for us to investigate and address the issue.
|
||||
- Avoid accessing or modifying other users' data, this is against IQ.Lvbs policy, and illegal in many jurisdictions.
|
||||
- Do not exploit vulnerabilities beyond what is necessary to demonstrate the issue.
|
||||
|
||||
## Out of Scope
|
||||
|
||||
- Issues in third-party dependencies unless the vulnerability is triggered by our specific usage
|
||||
|
||||
## Recognition
|
||||
|
||||
We appreciate and recognize security researchers who help keep IQ.Lvbs software safe. With your permission, we will credit you in our release notes when a reported vulnerability is fixed.
|
||||
|
||||
- IQ.Lvbs, a part of Project Teal Lvbs Inc ©️
|
||||
@@ -0,0 +1 @@
|
||||
076c1d057f97b067222d74180fbb63ba5467254a
|
||||
@@ -0,0 +1,48 @@
|
||||
{
|
||||
"python/_iqclosure/_vendored.json": {
|
||||
"mode": 420,
|
||||
"sha256": "37517e5f3dc66819f61f5a7bb8ace1921282415f10551d2defa5c3eb0985b570",
|
||||
"size": 3
|
||||
},
|
||||
"python/iqpilot_private/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/iqlvbs/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "268267fdfb6ae438f1b3c6f5f2c378a5b878e68357d5560f604f77d5c36a2cea",
|
||||
"size": 402144
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "b3d33e86493408e5a02f2df753e1b257f003e5028480b5b040aa5a07eeed98be",
|
||||
"size": 69712
|
||||
},
|
||||
"runtime": {
|
||||
"entries": {
|
||||
"iqpilot_private.konn3kt.iqlvbs.alc": {
|
||||
"path": "python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so",
|
||||
"type": "python-module"
|
||||
},
|
||||
"iqpilot_private.konn3kt.iqlvbs.vehicle_state": {
|
||||
"path": "python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so",
|
||||
"type": "python-module"
|
||||
}
|
||||
}
|
||||
},
|
||||
"signatures": {
|
||||
"python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": "v0mM7HrM8Ssbm3E/TxIUefAh5aCkzbZIKDQk5yFN6kr4EgqmVyPMNkqu7kCsn4ah0fTzr7xCrhs/gXjGf+6yBQ==",
|
||||
"python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": "5KQhJ61bt8tNmKuegPVMnMiqEHKcnjhqMtCDv2CphlSS54L/GmsOZjieHQEcgpOIDw7n/7qz2k6iFnwJ6clLCg=="
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
Xr+Nn9b/ZoT2/S94B/I16aHVQI5dZZE3hGvpS6elgasC7e4JZouZvln/ue3Lh5Hg/NTJCCEStRmpEtNfgcjbAA==
|
||||
@@ -0,0 +1 @@
|
||||
[]
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
@@ -0,0 +1,38 @@
|
||||
{
|
||||
"python/_iqclosure/_vendored.json": {
|
||||
"mode": 420,
|
||||
"sha256": "37517e5f3dc66819f61f5a7bb8ace1921282415f10551d2defa5c3eb0985b570",
|
||||
"size": 3
|
||||
},
|
||||
"python/iqpilot_private/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/iqlvbs/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/iqlvbs/iqlvbs_commander.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "9b48803b46906943d7cb10b7e4b4f38c880f06a5c281f254a89e2254965b025f",
|
||||
"size": 68080
|
||||
},
|
||||
"runtime": {
|
||||
"entries": {
|
||||
"iqpilot_private.konn3kt.iqlvbs.iqlvbs_commander": {
|
||||
"path": "python/iqpilot_private/konn3kt/iqlvbs/iqlvbs_commander.cpython-312-aarch64-linux-gnu.so",
|
||||
"type": "python-module"
|
||||
}
|
||||
}
|
||||
},
|
||||
"signatures": {
|
||||
"python/iqpilot_private/konn3kt/iqlvbs/iqlvbs_commander.cpython-312-aarch64-linux-gnu.so": "RwstZkEGexV6fia/R9BRk+6CQ9Q6SGJopSJf04UpMpaw5fRtoPjLOrDFOt9kU8GPmCmW40gaXhJK+rimnBYnBw=="
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
TV78NXdEYQiabAzLIqS2lsZbp5bO0y4EPGwLRUMyKeEH2BStZiFfpNPvlqSn5HGI+RjmTq+p5rRUDMNofbZGBw==
|
||||
@@ -0,0 +1 @@
|
||||
[]
|
||||
BIN
Binary file not shown.
@@ -0,0 +1,561 @@
|
||||
{
|
||||
"python/_iqclosure/_vendored.json": {
|
||||
"mode": 420,
|
||||
"sha256": "c5dde802e17116d9759e661e7d80a1a86d396fb3040734c1eadb2a50c5a3cf4c",
|
||||
"size": 3439
|
||||
},
|
||||
"python/_iqclosure/iqpilot/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/api/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "97d6e0d8cbd3e47797d734322b42d04efea4d95a2dae5a56f1baef3fd847aa0d",
|
||||
"size": 939
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/api/base.py": {
|
||||
"mode": 420,
|
||||
"sha256": "be55c83869bad911ee0204e8779b4673653f7d29ae4f524c978a280bd84c7777",
|
||||
"size": 3187
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/api/comma_connect.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e7c2dd0b56fb8533c2cbc614e728a9ffc5ce7360524cfa62a02f5235bc23b9a8",
|
||||
"size": 267
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/atlas_alerts.py": {
|
||||
"mode": 420,
|
||||
"sha256": "ef0d867f6213bb67ed6b7fd6ef4e694eb1705d9dc694163a80c8dd9572932954",
|
||||
"size": 9448
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/basedir.py": {
|
||||
"mode": 420,
|
||||
"sha256": "58cfa5d6405903ee1acecc4544463bb8f47a66921b120c382143aa80d8539b28",
|
||||
"size": 106
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/constants.py": {
|
||||
"mode": 420,
|
||||
"sha256": "fc439d59c24b95ed349b84f1a5862df3ea50e0891dbec4ea3cf5e1e0ccff6513",
|
||||
"size": 434
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/file_chunker.py": {
|
||||
"mode": 420,
|
||||
"sha256": "0c6dfabdbf1aa737f4dc306b10d12e6dcc3a87720dbbfe23df2f40490453b06e",
|
||||
"size": 1898
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/filter_simple.py": {
|
||||
"mode": 420,
|
||||
"sha256": "4b34753a3d3eea6d1578d88d21350045173a3809d38aed232875d1c5f34b4141",
|
||||
"size": 1929
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/gpio.py": {
|
||||
"mode": 420,
|
||||
"sha256": "01ba8cb32a644a2df545e764ba19bf1c7a320a217f983f8bac73b45d3a6891cf",
|
||||
"size": 2408
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/i2c.py": {
|
||||
"mode": 420,
|
||||
"sha256": "4af8f9da7504a548d15f18b91d71c1379cb7c05c84be031448764cc49920ce68",
|
||||
"size": 2449
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/iq_perf.py": {
|
||||
"mode": 420,
|
||||
"sha256": "16b9a744fc1346d9660eab59c87c05a56015fea6bfc30317047e5f95aba2f7d2",
|
||||
"size": 7218
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/issue_debug.py": {
|
||||
"mode": 420,
|
||||
"sha256": "73a10d846c735d4bcfda65f7828aa82192349b53920cb9d1b01cef8c211982fe",
|
||||
"size": 1128
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/logging_extra.py": {
|
||||
"mode": 420,
|
||||
"sha256": "ce9b3b3d8498d76e1c4c0352a3e98e72759bb7f367ff8629c1329c46040595c3",
|
||||
"size": 6628
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/realtime.py": {
|
||||
"mode": 420,
|
||||
"sha256": "f137ef62c1602a1f7c329731883dc518c62288bd25596891858d460ae3209ae2",
|
||||
"size": 3966
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/spinner.py": {
|
||||
"mode": 420,
|
||||
"sha256": "33a2eb2aa3148492414035c5dc7150b3a69467905d7a3e58c8f65cfe70d3552c",
|
||||
"size": 1371
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/steer_delay.py": {
|
||||
"mode": 420,
|
||||
"sha256": "933220f41a027fe73bfcf7af1d40a7b58d1a751dc9a4fe16ddccc66f2c58c549",
|
||||
"size": 2541
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/swaglog.py": {
|
||||
"mode": 420,
|
||||
"sha256": "0a133b4ef49f80b8890f115e81273f3f3e21351ae79f92dc02abc033e6d24027",
|
||||
"size": 5274
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/time_helpers.py": {
|
||||
"mode": 420,
|
||||
"sha256": "c14cda1f84149d5ffa63a2adec5ca24126f77c1412a78412e5e3e68a6fd12bcf",
|
||||
"size": 474
|
||||
},
|
||||
"python/_iqclosure/iqpilot/common/utils.py": {
|
||||
"mode": 420,
|
||||
"sha256": "6475ad94bc89252398c41e9eabf7929ba5eb8fa23a08ad7545e7a67b400af9f3",
|
||||
"size": 8090
|
||||
},
|
||||
"python/_iqclosure/iqpilot/konn3kt/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/konn3kt/cloud_client.py": {
|
||||
"mode": 420,
|
||||
"sha256": "4d58e62f8d628a8570881f2b87c82c801b46928fbb84f8dab8a81a9b55ba4e15",
|
||||
"size": 493
|
||||
},
|
||||
"python/_iqclosure/iqpilot/konn3kt/registration.py": {
|
||||
"mode": 420,
|
||||
"sha256": "d8da8fa6373d54c3090b4466a975969317c9f5cd7c0ba20fd905e11ce9b95efd",
|
||||
"size": 7948
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/controls/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/controls/lib/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/controls/lib/desire_helper.py": {
|
||||
"mode": 420,
|
||||
"sha256": "407b7fcdd2699f7f64404df420b9a2ccfef2cfa4a66d5634f6dd7cb2f23ff3f4",
|
||||
"size": 11737
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/controls/lib/drive_helpers.py": {
|
||||
"mode": 420,
|
||||
"sha256": "4246e7ac3a1c554e068a967472b2fe2793fb291d6468308badb25f5769fd1fb9",
|
||||
"size": 3599
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_change.py": {
|
||||
"mode": 420,
|
||||
"sha256": "3e7557a578a971f45e704b4386f6ac386745c8d044f45dab4b9b6cdd492a7f42",
|
||||
"size": 7875
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_turn.py": {
|
||||
"mode": 420,
|
||||
"sha256": "faa6f87b227c805e8890a91139e8d16d7874c4b99b330dda08080392ce3479ec",
|
||||
"size": 4618
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/big_catalog.py": {
|
||||
"mode": 420,
|
||||
"sha256": "7ec5df43ce10b47ce925467e61100123e828b8e72bdab20bef4c8ac0ffd3cc7a",
|
||||
"size": 369
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/camera.py": {
|
||||
"mode": 420,
|
||||
"sha256": "6d6b309a557a6f7a97e97dd0227d2ec66c215523f19c8e3fcf7256940115da5c",
|
||||
"size": 2646
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/config.py": {
|
||||
"mode": 420,
|
||||
"sha256": "056c744bbd9f1fe1053555d9b1fc1257d53093d69ddc94be3f05d840eb70781c",
|
||||
"size": 3590
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/daemon.py": {
|
||||
"mode": 420,
|
||||
"sha256": "bbfe0c839d22286c2293c3fac11c5cae2206c2e69f0ae2aa4bdf64a3c772fc4f",
|
||||
"size": 30210
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/driving_action.py": {
|
||||
"mode": 420,
|
||||
"sha256": "863a9b18e82c7b2ab821541f498228ca7f5904ee38d5ac282c66dfaab7fdc492",
|
||||
"size": 2381
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_helpers.py": {
|
||||
"mode": 420,
|
||||
"sha256": "5d00df0cf831ea75a91d1d817818dc3847d971e581170671f12e289461eccabc",
|
||||
"size": 4846
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_model.py": {
|
||||
"mode": 420,
|
||||
"sha256": "3b213b60799cb514affa1423ff04a516000c3d8dcbab936316c7f74e87a101ef",
|
||||
"size": 3417
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/emac_model_meta.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e47406eabf9c96bdf393ddaba8936fbd01755b2728ebe9413b2ab24b94de5d48",
|
||||
"size": 377
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/messaging.py": {
|
||||
"mode": 420,
|
||||
"sha256": "17d48331eeeaf746ee6982a9c65f28d740a15a4c34c37617115c104894d49c06",
|
||||
"size": 11920
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/metadata.py": {
|
||||
"mode": 420,
|
||||
"sha256": "dd1559099407687ee2dc81e180e05b9954e712c39e1a82e39c1b00b70c43374a",
|
||||
"size": 3016
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_channel.py": {
|
||||
"mode": 420,
|
||||
"sha256": "2d706993df418501d93dc766ffe452131e565bfbc2e8412b527dc14915e92832",
|
||||
"size": 1872
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_warp.py": {
|
||||
"mode": 420,
|
||||
"sha256": "075c89f8740945b84d0c554cccf4b0198d0738c85bde52e64bab69b01e482987",
|
||||
"size": 3325
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "5865243a95ca4557d2a960302124a9898545cc3a8387e88ef569308e3d0f7077",
|
||||
"size": 119
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/combined_artifact.py": {
|
||||
"mode": 420,
|
||||
"sha256": "321e7223249dc7dba93effe8e834f225fb6067abf49da17fe9a6faeb71ba8e49",
|
||||
"size": 2786
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py": {
|
||||
"mode": 420,
|
||||
"sha256": "23622cdec9eed5649a05422ba19eae956ba16a63850927af4a222c3d9030a1bb",
|
||||
"size": 399
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e4f524c7595754fd9db86724a7bd9b67524c630275ac78599ab6838d1aa7d809",
|
||||
"size": 11037
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/inference_state.py": {
|
||||
"mode": 420,
|
||||
"sha256": "bdfd1c1b538e2342b17d08aa6ebe0c041c2f721025bcd7334434d1139fe6063d",
|
||||
"size": 268
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e0eb9d7bff1da89d52b09596f7e7109fc25c3b9b86a89674382a7dde9e0a4199",
|
||||
"size": 8555
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/combined_split_runner.py": {
|
||||
"mode": 420,
|
||||
"sha256": "b7c15991f4fe9c6b4d7fb6a2a37256c142ab40cdd53f52bd3c59accc3a3618b1",
|
||||
"size": 10929
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/fused_runner.py": {
|
||||
"mode": 420,
|
||||
"sha256": "25304fe7ae034d1bb8c9c86a2757704944539be6a0c69c322e75adc84f66e221",
|
||||
"size": 7885
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/model_types.py": {
|
||||
"mode": 420,
|
||||
"sha256": "b68fe2c7f0013fe6780e765882b4a1ac7074834ea6d370cbca90a51d2f614091",
|
||||
"size": 1981
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/supercombo_runner.py": {
|
||||
"mode": 420,
|
||||
"sha256": "10ae83948aba757a48a781fed90be1cce65e8853c8b67dcb7b4ec6928e312005",
|
||||
"size": 14219
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/tinygrad_runner.py": {
|
||||
"mode": 420,
|
||||
"sha256": "58ffe0afb2cd9527d2a1d5bbebf0471af8c8210534e266ab80488a7f89bb06ba",
|
||||
"size": 7684
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/split_model_constants.py": {
|
||||
"mode": 420,
|
||||
"sha256": "1fe374d5eebef362923a5d90f1218bcd6b916eb60584d4a55f840de2ef6c4cf3",
|
||||
"size": 2145
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/parser.py": {
|
||||
"mode": 420,
|
||||
"sha256": "1dc47b2c167f1271c5c45d6ba0582cb906f4b59b0621b6a6d971da3f329d66e8",
|
||||
"size": 9212
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/tinygrad.py": {
|
||||
"mode": 420,
|
||||
"sha256": "316988acf63f98aca83c5662ffb2c6028ef1f8b94fd8d6fcc15e96d9ac0d8181",
|
||||
"size": 779
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/temporal_state.py": {
|
||||
"mode": 420,
|
||||
"sha256": "06ab9349b786559037ce68d5882b54ffbe39f76737c4d19a213b251240023d4a",
|
||||
"size": 5501
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_supercombo.py": {
|
||||
"mode": 420,
|
||||
"sha256": "d8fbc18f67ea1e330130ce0b228af298919c5304d299da07eb94d557c0827d25",
|
||||
"size": 17739
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py": {
|
||||
"mode": 420,
|
||||
"sha256": "2cc299b2cfda88ee2dbc52658f145fb7840e2dfc49a7371e22fe671beed0e57f",
|
||||
"size": 5436
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/locationd/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/locationd/calibration_helpers.py": {
|
||||
"mode": 420,
|
||||
"sha256": "1501058881da2720b35915f0949c2bc311020386c2549d48111ab32a34386a6c",
|
||||
"size": 982
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/locationd/calibrationd.py": {
|
||||
"mode": 420,
|
||||
"sha256": "6bceebb5a15895d2b6d20a28cbd14b9112f9422e0a0cf466314a7e8779da39dc",
|
||||
"size": 15698
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/locationd/helpers.py": {
|
||||
"mode": 420,
|
||||
"sha256": "be47384b3437670192f8bfd3dc679d57f0678f9f4fe34759419b0f2eb01ab117",
|
||||
"size": 6828
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/locationd/locationd.py": {
|
||||
"mode": 420,
|
||||
"sha256": "8a7d7c89ea11998e418a52355a3d719ef3bf4ddfd0af13da2234d08348c25fe7",
|
||||
"size": 15169
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/locationd/models/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/locationd/models/constants.py": {
|
||||
"mode": 420,
|
||||
"sha256": "276b20ca6187466efe6273e66100d92f4f54bd2b393c77329fb196a512b707bf",
|
||||
"size": 1952
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/locationd/models/pose_kf.py": {
|
||||
"mode": 420,
|
||||
"sha256": "98aabe0a0b1e94f0c34af459ead9488901f08722ada3a87d6a33c00241dee211",
|
||||
"size": 2930
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/selfdrived/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/selfdrived/alertmanager.py": {
|
||||
"mode": 420,
|
||||
"sha256": "c601bac30951a750d3ebf3d30f06c597bacd148210b1c405d5d3eca79bb7c4e3",
|
||||
"size": 2045
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e298175d13a99b55e183954d82346fa7adab6b6c161fc72b789f12601d1a9b43",
|
||||
"size": 37038
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/state_estimation/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "85a0c22d638eb8f87b00ca2ea21aa4c0c76708fe3e2f3ccc1fa8f87f0c784450",
|
||||
"size": 319
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/state_estimation/estimator.py": {
|
||||
"mode": 420,
|
||||
"sha256": "59ae4dd78c8f850a5acd28d2743abe1b9d19ebcaaf9c472f95b2766a73e056c6",
|
||||
"size": 12577
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/ui/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/ui/feedback/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/selfdrive/ui/feedback/feedbackd.py": {
|
||||
"mode": 420,
|
||||
"sha256": "7c1e6cb2c3c2c6aae91210f6cbc5c372f33c28cc3edb292ef71439b83844615d",
|
||||
"size": 2615
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/camerad/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/camerad/cameras/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/camerad/cameras/nv12_info.py": {
|
||||
"mode": 420,
|
||||
"sha256": "90b0b00df8a7baa638940c2e73cbe2e79059af0acd19812ca6d765793ce4fda0",
|
||||
"size": 829
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "889391bffca493e74bfc108cae2976a53b644caca13ab600061fd29e9622c627",
|
||||
"size": 695
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/base.py": {
|
||||
"mode": 420,
|
||||
"sha256": "a56293f7c23e503ff62d0d3ed39f1831b5544a65b37378165934361a5ddcd304",
|
||||
"size": 4966
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/hw.py": {
|
||||
"mode": 420,
|
||||
"sha256": "c69185d8f6482c8b021f687b793246d2e2ede853e89a88748c64b85f60711b59",
|
||||
"size": 3638
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/pc/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/pc/hardware.py": {
|
||||
"mode": 420,
|
||||
"sha256": "2520895597c375ae570bd04d5f189e874c89d642f3f07bdbeb20388cb6691433",
|
||||
"size": 258
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/tici/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/tici/amplifier.py": {
|
||||
"mode": 420,
|
||||
"sha256": "98f46f8705259b00efae7274186b4e68201aad467a19de4f16d5f850d10ab0b5",
|
||||
"size": 7585
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/tici/hardware.py": {
|
||||
"mode": 420,
|
||||
"sha256": "f1ddffa9bdb5071a49fd65eb4b49958b60b1873e1ec70e6f3f2e2b2501abce2f",
|
||||
"size": 25584
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/tici/iwlist.py": {
|
||||
"mode": 420,
|
||||
"sha256": "d22858982705531732d5a20a45a73795496e5bbd9e23028c24609a7e58ac7aed",
|
||||
"size": 890
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/tici/lpa.py": {
|
||||
"mode": 420,
|
||||
"sha256": "2bffb84aef35892c4788e80b6d937bf2eebd15e226b72a82c4f39217667c1ca0",
|
||||
"size": 53692
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/tici/pins.py": {
|
||||
"mode": 420,
|
||||
"sha256": "b57ba4262e6a6a79f2db15c27356acf3e54f579a7edd13b483b2e16d140aca6d",
|
||||
"size": 678
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/tici/usb_storage.py": {
|
||||
"mode": 420,
|
||||
"sha256": "3352f2cf70d38e7158ef8ac20bb736dba55c73239cbdd3ab7f431dbacfb5027e",
|
||||
"size": 2189
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/hardware/usb.py": {
|
||||
"mode": 420,
|
||||
"sha256": "f0812c71df268e640385b97deb5f600b49022abde91b7016ae491606a71abece",
|
||||
"size": 7021
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/micd.py": {
|
||||
"mode": 420,
|
||||
"sha256": "1a59cbe6b72fcca5723ab56dc09f52039817299071a8d8da37f04acea58776bc",
|
||||
"size": 5977
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/sentry.py": {
|
||||
"mode": 420,
|
||||
"sha256": "c3da338feb533937b6a5258ee450b6a3ff7a77b69cc9e48e31d16b875855caf3",
|
||||
"size": 5229
|
||||
},
|
||||
"python/_iqclosure/iqpilot/system/version.py": {
|
||||
"mode": 420,
|
||||
"sha256": "3a6bc7520dae7944a1da6f911cd090704a463e011f39949ea6c9df51b0e8c616",
|
||||
"size": 6033
|
||||
},
|
||||
"python/iqpilot_private/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
|
||||
"size": 0
|
||||
},
|
||||
"python/iqpilot_private/emac/__init__.py": {
|
||||
"mode": 420,
|
||||
"sha256": "5865243a95ca4557d2a960302124a9898545cc3a8387e88ef569308e3d0f7077",
|
||||
"size": 119
|
||||
},
|
||||
"python/iqpilot_private/emac/bulk_transport.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "04b145b2e2781a52337214f8eb6dd3b0f036010abe62d55ff5e3cbab2332d5c3",
|
||||
"size": 202152
|
||||
},
|
||||
"python/iqpilot_private/emac/mac_client.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "7e872e990d02b96280984b1f4a8f6f9e9cafeb4c2dd5c4b4d976da9b5e3fa8eb",
|
||||
"size": 267376
|
||||
},
|
||||
"python/iqpilot_private/emac/mac_protocol.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "c9b37b91141d5198ffe3d6320af00f20121730f293d7e369ba7a250204d47085",
|
||||
"size": 203544
|
||||
},
|
||||
"python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "d46ec4c30996853e169f74cc9be22770c34b762af3a5cfcc01cc5a24ba833925",
|
||||
"size": 333336
|
||||
},
|
||||
"runtime": {
|
||||
"entries": {
|
||||
"iqpilot_private.emac.maciqmodeld": {
|
||||
"path": "python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so",
|
||||
"type": "python-module"
|
||||
}
|
||||
}
|
||||
},
|
||||
"signatures": {
|
||||
"python/iqpilot_private/emac/bulk_transport.cpython-312-aarch64-linux-gnu.so": "X0Gc97xJ0vZ9WyfwKDmxjw8WodcrunJD1HIFPxictDvfEnwo/dWxI/BrLsngU2FuHlYeYTMQWRPiSOP/tnT0Cw==",
|
||||
"python/iqpilot_private/emac/mac_client.cpython-312-aarch64-linux-gnu.so": "YiCCfRBkLpDNrd863ImXmN+T+NnYhI2W7lF4sGp89wQsfVu+8OLelwQsUSuTlYQChEpjbj+z73T3HiOMaYZGBA==",
|
||||
"python/iqpilot_private/emac/mac_protocol.cpython-312-aarch64-linux-gnu.so": "RvUnCvUzLE9acpWof8m0RfLPQkc6Cm7xVbPdoO5qKghCdXLbjvPXHHWtpUzBuyS4Dn2KOYOs9brow1hltC8QAg==",
|
||||
"python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so": "f1w6QbT+42vHoM1WUHYwtgrv7leuT1XsFElTxVn4QqBQaBgy8TKDOpuDE5Img0fSwcloCiw4OIvCZo7YcvGmDQ=="
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
OAos+ziIlj+Mf6Z1EBbdFHSf/mcVdhtcEn5f+kJI4G+IgNAj6tsgwmFpsQRSGoY09eFOgnK0Q0sDvI+G4kjbCw==
|
||||
@@ -0,0 +1,84 @@
|
||||
[
|
||||
"iqpilot.common.api",
|
||||
"iqpilot.common.api.base",
|
||||
"iqpilot.common.api.comma_connect",
|
||||
"iqpilot.common.atlas_alerts",
|
||||
"iqpilot.common.basedir",
|
||||
"iqpilot.common.constants",
|
||||
"iqpilot.common.file_chunker",
|
||||
"iqpilot.common.filter_simple",
|
||||
"iqpilot.common.gpio",
|
||||
"iqpilot.common.i2c",
|
||||
"iqpilot.common.iq_perf",
|
||||
"iqpilot.common.issue_debug",
|
||||
"iqpilot.common.logging_extra",
|
||||
"iqpilot.common.realtime",
|
||||
"iqpilot.common.spinner",
|
||||
"iqpilot.common.steer_delay",
|
||||
"iqpilot.common.swaglog",
|
||||
"iqpilot.common.time_helpers",
|
||||
"iqpilot.common.utils",
|
||||
"iqpilot.konn3kt.cloud_client",
|
||||
"iqpilot.konn3kt.registration",
|
||||
"iqpilot.selfdrive.controls.lib.desire_helper",
|
||||
"iqpilot.selfdrive.controls.lib.drive_helpers",
|
||||
"iqpilot.selfdrive.controls.lib.helpers.lane_change",
|
||||
"iqpilot.selfdrive.controls.lib.helpers.lane_turn",
|
||||
"iqpilot.selfdrive.iqmodeld.big_catalog",
|
||||
"iqpilot.selfdrive.iqmodeld.camera",
|
||||
"iqpilot.selfdrive.iqmodeld.config",
|
||||
"iqpilot.selfdrive.iqmodeld.daemon",
|
||||
"iqpilot.selfdrive.iqmodeld.driving_action",
|
||||
"iqpilot.selfdrive.iqmodeld.egpu_helpers",
|
||||
"iqpilot.selfdrive.iqmodeld.egpu_model",
|
||||
"iqpilot.selfdrive.iqmodeld.emac_model_meta",
|
||||
"iqpilot.selfdrive.iqmodeld.messaging",
|
||||
"iqpilot.selfdrive.iqmodeld.metadata",
|
||||
"iqpilot.selfdrive.iqmodeld.model_channel",
|
||||
"iqpilot.selfdrive.iqmodeld.model_warp",
|
||||
"iqpilot.selfdrive.iqmodeld.models",
|
||||
"iqpilot.selfdrive.iqmodeld.models.combined_artifact",
|
||||
"iqpilot.selfdrive.iqmodeld.models.fetcher",
|
||||
"iqpilot.selfdrive.iqmodeld.models.helpers",
|
||||
"iqpilot.selfdrive.iqmodeld.models.inference_state",
|
||||
"iqpilot.selfdrive.iqmodeld.models.runners.model_runner",
|
||||
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.combined_split_runner",
|
||||
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.fused_runner",
|
||||
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.model_types",
|
||||
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.supercombo_runner",
|
||||
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.tinygrad_runner",
|
||||
"iqpilot.selfdrive.iqmodeld.models.split_model_constants",
|
||||
"iqpilot.selfdrive.iqmodeld.parser",
|
||||
"iqpilot.selfdrive.iqmodeld.runtime.tinygrad",
|
||||
"iqpilot.selfdrive.iqmodeld.temporal_state",
|
||||
"iqpilot.selfdrive.iqmodeld.tools.compile_supercombo",
|
||||
"iqpilot.selfdrive.iqmodeld.tools.compile_warp",
|
||||
"iqpilot.selfdrive.locationd.calibration_helpers",
|
||||
"iqpilot.selfdrive.locationd.calibrationd",
|
||||
"iqpilot.selfdrive.locationd.helpers",
|
||||
"iqpilot.selfdrive.locationd.locationd",
|
||||
"iqpilot.selfdrive.locationd.models.constants",
|
||||
"iqpilot.selfdrive.locationd.models.pose_kf",
|
||||
"iqpilot.selfdrive.selfdrived.alertmanager",
|
||||
"iqpilot.selfdrive.selfdrived.events",
|
||||
"iqpilot.selfdrive.state_estimation",
|
||||
"iqpilot.selfdrive.state_estimation.estimator",
|
||||
"iqpilot.selfdrive.ui.feedback.feedbackd",
|
||||
"iqpilot.system",
|
||||
"iqpilot.system.camerad.cameras.nv12_info",
|
||||
"iqpilot.system.hardware",
|
||||
"iqpilot.system.hardware.base",
|
||||
"iqpilot.system.hardware.hw",
|
||||
"iqpilot.system.hardware.pc.hardware",
|
||||
"iqpilot.system.hardware.tici",
|
||||
"iqpilot.system.hardware.tici.amplifier",
|
||||
"iqpilot.system.hardware.tici.hardware",
|
||||
"iqpilot.system.hardware.tici.iwlist",
|
||||
"iqpilot.system.hardware.tici.lpa",
|
||||
"iqpilot.system.hardware.tici.pins",
|
||||
"iqpilot.system.hardware.tici.usb_storage",
|
||||
"iqpilot.system.hardware.usb",
|
||||
"iqpilot.system.micd",
|
||||
"iqpilot.system.sentry",
|
||||
"iqpilot.system.version"
|
||||
]
|
||||
@@ -0,0 +1,26 @@
|
||||
import iqpilot.common.api.comma_connect
|
||||
|
||||
|
||||
class Api:
|
||||
def __init__(self, dongle_id):
|
||||
self.service = iqpilot.common.api.comma_connect.CommaConnectApi(dongle_id)
|
||||
|
||||
def request(self, method, endpoint, **params):
|
||||
return self.service.request(method, endpoint, **params)
|
||||
|
||||
def get(self, *args, **kwargs):
|
||||
return self.service.get(*args, **kwargs)
|
||||
|
||||
def post(self, *args, **kwargs):
|
||||
return self.service.post(*args, **kwargs)
|
||||
|
||||
def get_token(self, payload_extra=None, expiry_hours=1):
|
||||
return self.service.get_token(payload_extra, expiry_hours)
|
||||
|
||||
|
||||
def api_get(endpoint, method='GET', timeout=None, access_token=None, session=None, **params):
|
||||
return iqpilot.common.api.comma_connect.CommaConnectApi(None).api_get(endpoint, method, timeout, access_token, session, **params)
|
||||
|
||||
|
||||
def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]:
|
||||
return iqpilot.common.api.comma_connect.CommaConnectApi(None).get_key_pair()
|
||||
@@ -0,0 +1,84 @@
|
||||
import jwt
|
||||
import os
|
||||
import requests
|
||||
import unicodedata
|
||||
from datetime import datetime, timedelta, UTC
|
||||
from functools import lru_cache
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
from iqpilot.system.version import get_version
|
||||
|
||||
# name: jwt signature algorithm
|
||||
KEYS = {"id_rsa": "RS256",
|
||||
"id_ecdsa": "ES256"}
|
||||
|
||||
|
||||
@lru_cache(maxsize=4)
|
||||
def load_signing_key(private_key: str):
|
||||
# PyJWT re-parses a PEM string on every encode; an RSA parse is ~40ms, so cache the key object
|
||||
try:
|
||||
from cryptography.hazmat.primitives.serialization import load_pem_private_key
|
||||
return load_pem_private_key(private_key.encode(), password=None)
|
||||
except Exception:
|
||||
return private_key
|
||||
|
||||
|
||||
class BaseApi:
|
||||
def __init__(self, dongle_id, api_host, user_agent="openpilot-"):
|
||||
self.dongle_id = dongle_id
|
||||
self.api_host = api_host
|
||||
self.user_agent = user_agent
|
||||
self.jwt_algorithm, self.private_key, _ = self.get_key_pair()
|
||||
|
||||
def get(self, *args, **kwargs):
|
||||
return self.request('GET', *args, **kwargs)
|
||||
|
||||
def post(self, *args, **kwargs):
|
||||
return self.request('POST', *args, **kwargs)
|
||||
|
||||
def request(self, method, endpoint, timeout=None, access_token=None, **params):
|
||||
return self.api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params)
|
||||
|
||||
def _get_token(self, payload_extra=None, expiry_hours=1, **extra_payload):
|
||||
now = datetime.now(UTC).replace(tzinfo=None)
|
||||
payload = {
|
||||
'identity': self.dongle_id,
|
||||
'nbf': now,
|
||||
'iat': now,
|
||||
'exp': now + timedelta(hours=expiry_hours),
|
||||
**extra_payload
|
||||
}
|
||||
if payload_extra is not None:
|
||||
payload.update(payload_extra)
|
||||
key = load_signing_key(self.private_key) if self.private_key else self.private_key
|
||||
token = jwt.encode(payload, key, algorithm=self.jwt_algorithm)
|
||||
if isinstance(token, bytes):
|
||||
token = token.decode('utf8')
|
||||
return token
|
||||
|
||||
def get_token(self, payload_extra=None, expiry_hours=1):
|
||||
return self._get_token(payload_extra, expiry_hours)
|
||||
|
||||
def remove_non_ascii_chars(self, text):
|
||||
normalized_text = unicodedata.normalize('NFD', text)
|
||||
ascii_encoded_text = normalized_text.encode('ascii', 'ignore')
|
||||
return ascii_encoded_text.decode()
|
||||
|
||||
def api_get(self, endpoint, method='GET', timeout=None, access_token=None, session=None, json=None, **params):
|
||||
headers = {}
|
||||
if access_token is not None:
|
||||
headers['Authorization'] = "JWT " + access_token
|
||||
|
||||
version = self.remove_non_ascii_chars(get_version())
|
||||
headers['User-Agent'] = self.user_agent + version
|
||||
|
||||
# TODO: add session to Api
|
||||
req = requests if session is None else session
|
||||
return req.request(method, f"{self.api_host}/{endpoint}", timeout=timeout, headers=headers, json=json, params=params)
|
||||
|
||||
@staticmethod
|
||||
def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]:
|
||||
for key in KEYS:
|
||||
if os.path.isfile(Paths.persist_root() + f'/comma/{key}') and os.path.isfile(Paths.persist_root() + f'/comma/{key}.pub'):
|
||||
with open(Paths.persist_root() + f'/comma/{key}') as private, open(Paths.persist_root() + f'/comma/{key}.pub') as public:
|
||||
return KEYS[key], private.read(), public.read()
|
||||
return None, None, None
|
||||
@@ -0,0 +1,11 @@
|
||||
import os
|
||||
|
||||
from iqpilot.common.api.base import BaseApi
|
||||
|
||||
API_HOST = os.getenv('API_HOST', 'https://api-iqlabs.konn3kt.com')
|
||||
|
||||
|
||||
class CommaConnectApi(BaseApi):
|
||||
def __init__(self, dongle_id):
|
||||
super().__init__(dongle_id, API_HOST)
|
||||
self.user_agent = "openpilot-"
|
||||
@@ -0,0 +1,281 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from abc import ABC, abstractmethod
|
||||
from bisect import insort
|
||||
from collections.abc import Callable, Iterable
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntEnum
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal import car, log
|
||||
from iqpilot.common.realtime import DT_CTRL
|
||||
from iqpilot.system.hardware import HARDWARE
|
||||
|
||||
AlertSize = log.SelfdriveState.AlertSize
|
||||
AlertStatus = log.SelfdriveState.AlertStatus
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
|
||||
|
||||
def _frames_for(seconds: float) -> int:
|
||||
return int(seconds / DT_CTRL)
|
||||
|
||||
|
||||
class Tier(IntEnum):
|
||||
LOWEST = 0
|
||||
LOWER = 1
|
||||
LOW = 2
|
||||
MID = 3
|
||||
HIGH = 4
|
||||
HIGHEST = 5
|
||||
|
||||
|
||||
class Tags:
|
||||
ENABLE = "enable"
|
||||
PRE_ENABLE = "preEnable"
|
||||
OVERRIDE_LATERAL = "overrideLateral"
|
||||
OVERRIDE_LONGITUDINAL = "overrideLongitudinal"
|
||||
NO_ENTRY = "noEntry"
|
||||
WARNING = "warning"
|
||||
USER_DISABLE = "userDisable"
|
||||
SOFT_DISABLE = "softDisable"
|
||||
IMMEDIATE_DISABLE = "immediateDisable"
|
||||
PERMANENT = "permanent"
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class AlertCard:
|
||||
alert_text_1: str
|
||||
alert_text_2: str
|
||||
alert_status: log.SelfdriveState.AlertStatus
|
||||
alert_size: log.SelfdriveState.AlertSize
|
||||
priority: Tier
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert
|
||||
audible_alert: car.CarControl.HUDControl.AudibleAlert
|
||||
duration: int
|
||||
creation_delay: float = 0.0
|
||||
alert_type: str = field(default="", init=False)
|
||||
event_type: str | None = field(default=None, init=False)
|
||||
|
||||
def __init__(self,
|
||||
alert_text_1: str,
|
||||
alert_text_2: str,
|
||||
alert_status: log.SelfdriveState.AlertStatus,
|
||||
alert_size: log.SelfdriveState.AlertSize,
|
||||
priority: Tier,
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert,
|
||||
audible_alert: car.CarControl.HUDControl.AudibleAlert,
|
||||
duration: float,
|
||||
creation_delay: float = 0.0):
|
||||
self.alert_text_1 = alert_text_1
|
||||
self.alert_text_2 = alert_text_2
|
||||
self.alert_status = alert_status
|
||||
self.alert_size = alert_size
|
||||
self.priority = priority
|
||||
self.visual_alert = visual_alert
|
||||
self.audible_alert = audible_alert
|
||||
self.duration = _frames_for(duration)
|
||||
self.creation_delay = creation_delay
|
||||
self.alert_type = ""
|
||||
self.event_type = None
|
||||
|
||||
def __str__(self) -> str:
|
||||
return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
|
||||
|
||||
|
||||
AlertFactory = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], AlertCard]
|
||||
|
||||
|
||||
def car_mode_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> AlertCard:
|
||||
del CS, sm, metric, soft_disable_time, personality
|
||||
headline = "Enable Adaptive Cruise to Engage"
|
||||
if CP.brand == "honda":
|
||||
headline = "Enable Main Switch to Engage"
|
||||
return NoEntryCard(headline)
|
||||
|
||||
|
||||
class EventBook(ABC):
|
||||
def __init__(self):
|
||||
self._live_names: list[int] = []
|
||||
self._latched_names: list[int] = []
|
||||
self.event_counters: dict[int, int] = {}
|
||||
|
||||
@property
|
||||
def events(self) -> list[int]:
|
||||
return self._live_names
|
||||
|
||||
@events.setter
|
||||
def events(self, values: list[int]) -> None:
|
||||
self._live_names = values
|
||||
|
||||
@property
|
||||
def static_events(self) -> list[int]:
|
||||
return self._latched_names
|
||||
|
||||
@static_events.setter
|
||||
def static_events(self, values: list[int]) -> None:
|
||||
self._latched_names = values
|
||||
|
||||
@property
|
||||
def names(self) -> list[int]:
|
||||
return list(self._live_names)
|
||||
|
||||
def __len__(self) -> int:
|
||||
return len(self._live_names)
|
||||
|
||||
def add(self, event_name: int, static: bool = False) -> None:
|
||||
if static:
|
||||
insort(self._latched_names, event_name)
|
||||
insort(self._live_names, event_name)
|
||||
|
||||
def clear(self) -> None:
|
||||
refreshed: dict[int, int] = {}
|
||||
for event_name, frames_seen in self.event_counters.items():
|
||||
refreshed[event_name] = frames_seen + 1 if event_name in self._live_names else 0
|
||||
self.event_counters = refreshed
|
||||
self._live_names = list(self._latched_names)
|
||||
|
||||
def contains(self, event_type: str) -> bool:
|
||||
board = self.get_events_mapping()
|
||||
return any(event_type in board.get(event_name, {}) for event_name in self._live_names)
|
||||
|
||||
def has(self, event_name: int) -> bool:
|
||||
return event_name in self._live_names
|
||||
|
||||
def contains_in_list(self, events_list: list[int]) -> bool:
|
||||
return any(event_name in self._live_names for event_name in events_list)
|
||||
|
||||
def remove(self, event_name: int, static: bool = False) -> None:
|
||||
if static and event_name in self._latched_names:
|
||||
self._latched_names.remove(event_name)
|
||||
|
||||
if event_name in self._live_names:
|
||||
self.event_counters[event_name] = self.event_counters.get(event_name, 0) + 1
|
||||
self._live_names.remove(event_name)
|
||||
|
||||
def add_from_msg(self, events: Iterable) -> None:
|
||||
for event in events:
|
||||
insort(self._live_names, event.name.raw)
|
||||
|
||||
def to_msg(self):
|
||||
board = self.get_events_mapping()
|
||||
outbound = []
|
||||
for event_name in self._live_names:
|
||||
msg = self.get_event_msg_type().new_message()
|
||||
msg.name = event_name
|
||||
for event_kind in board.get(event_name, {}):
|
||||
setattr(msg, event_kind, True)
|
||||
outbound.append(msg)
|
||||
return outbound
|
||||
|
||||
def create_alerts(self, event_types: list[str], callback_args=None):
|
||||
callback_args = [] if callback_args is None else callback_args
|
||||
board = self.get_events_mapping()
|
||||
spawned: list[AlertCard] = []
|
||||
for event_name in self._live_names:
|
||||
variants = board.get(event_name, {})
|
||||
for event_type in event_types:
|
||||
chosen = variants.get(event_type)
|
||||
if chosen is None:
|
||||
continue
|
||||
alert = self._realize(chosen, callback_args)
|
||||
age_frames = self.event_counters.get(event_name, 0) + 1
|
||||
if age_frames * DT_CTRL < alert.creation_delay:
|
||||
continue
|
||||
alert.alert_type = f"{self.get_event_name(event_name)}/{event_type}"
|
||||
alert.event_type = event_type
|
||||
spawned.append(alert)
|
||||
return spawned
|
||||
|
||||
@staticmethod
|
||||
def _realize(candidate: AlertCard | AlertFactory, callback_args: list) -> AlertCard:
|
||||
return candidate if isinstance(candidate, AlertCard) else candidate(*callback_args)
|
||||
|
||||
@abstractmethod
|
||||
def get_events_mapping(self) -> dict[int, dict[str, AlertCard | AlertFactory]]:
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def get_event_name(self, event: int) -> str:
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def get_event_msg_type(self):
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
def _mici_reframe(primary: str, secondary: str) -> tuple[str, str, log.SelfdriveState.AlertSize]:
|
||||
if HARDWARE.get_device_type() == "mici":
|
||||
return secondary, primary, AlertSize.small
|
||||
return primary, secondary, AlertSize.mid
|
||||
|
||||
|
||||
class NoEntryCard(AlertCard):
|
||||
def __init__(self,
|
||||
alert_text_2: str,
|
||||
alert_text_1: str = "IQ.Pilot Unavailable",
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none,
|
||||
priority: Tier = Tier.LOW):
|
||||
primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2)
|
||||
super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0)
|
||||
|
||||
|
||||
class GentleDisableCard(AlertCard):
|
||||
def __init__(self, alert_text_2: str):
|
||||
super().__init__(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
alert_text_2,
|
||||
AlertStatus.userPrompt,
|
||||
AlertSize.full,
|
||||
Tier.MID,
|
||||
VisualAlert.steerRequired,
|
||||
AudibleAlert.warningSoft,
|
||||
2.0,
|
||||
)
|
||||
|
||||
|
||||
class PendingDisableCard(GentleDisableCard):
|
||||
def __init__(self, alert_text_2: str):
|
||||
super().__init__(alert_text_2)
|
||||
self.alert_text_1 = "IQ.Pilot will disengage"
|
||||
|
||||
|
||||
class HardDisableCard(AlertCard):
|
||||
def __init__(self, alert_text_2: str):
|
||||
super().__init__(
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
alert_text_2,
|
||||
AlertStatus.critical,
|
||||
AlertSize.full,
|
||||
Tier.HIGHEST,
|
||||
VisualAlert.steerRequired,
|
||||
AudibleAlert.warningImmediate,
|
||||
4.0,
|
||||
)
|
||||
|
||||
|
||||
class ChimeCard(AlertCard):
|
||||
def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert):
|
||||
super().__init__("", "", AlertStatus.normal, AlertSize.none, Tier.MID, VisualAlert.none, audible_alert, 0.2)
|
||||
|
||||
|
||||
class BannerCard(AlertCard):
|
||||
def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Tier = Tier.LOWER, creation_delay: float = 0.0):
|
||||
size = AlertSize.mid if alert_text_2 else AlertSize.small
|
||||
super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, size, priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay)
|
||||
|
||||
|
||||
class BootCard(AlertCard):
|
||||
def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal):
|
||||
if HARDWARE.get_device_type() == "mici":
|
||||
compact_secondary = "" if alert_text_2 == "Always keep hands on wheel and eyes on road" else alert_text_2
|
||||
super().__init__(alert_text_1, compact_secondary, alert_status, AlertSize.small, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
|
||||
else:
|
||||
super().__init__(alert_text_1, alert_text_2, alert_status, AlertSize.mid, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
|
||||
|
||||
|
||||
class AlertBase(AlertCard):
|
||||
pass
|
||||
|
||||
|
||||
NULL_ALERT = AlertCard("", "", AlertStatus.normal, AlertSize.none, Tier.LOWEST, VisualAlert.none, AudibleAlert.none, 0.0)
|
||||
@@ -0,0 +1,4 @@
|
||||
import os
|
||||
|
||||
|
||||
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../.."))
|
||||
@@ -0,0 +1,23 @@
|
||||
import numpy as np
|
||||
|
||||
# conversions
|
||||
class CV:
|
||||
# Speed
|
||||
MPH_TO_KPH = 1.609344
|
||||
KPH_TO_MPH = 1. / MPH_TO_KPH
|
||||
MS_TO_KPH = 3.6
|
||||
KPH_TO_MS = 1. / MS_TO_KPH
|
||||
MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
|
||||
MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
|
||||
MS_TO_KNOTS = 1.9438
|
||||
KNOTS_TO_MS = 1. / MS_TO_KNOTS
|
||||
|
||||
# Angle
|
||||
DEG_TO_RAD = np.pi / 180.
|
||||
RAD_TO_DEG = 1. / DEG_TO_RAD
|
||||
|
||||
# Mass
|
||||
LB_TO_KG = 0.453592
|
||||
|
||||
|
||||
ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2
|
||||
@@ -0,0 +1,55 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
CHUNK_SIZE = 45 * 1024 * 1024 # 45MB, under GitHub's 50MB limit
|
||||
|
||||
def get_chunk_name(name, idx, num_chunks):
|
||||
return f"{name}.chunk{idx+1:02d}of{num_chunks:02d}"
|
||||
|
||||
def get_manifest_path(name):
|
||||
return f"{name}.chunkmanifest"
|
||||
|
||||
def _chunk_paths(path, num_chunks):
|
||||
return [get_manifest_path(path)] + [get_chunk_name(path, i, num_chunks) for i in range(num_chunks)]
|
||||
|
||||
def get_chunk_targets(path, file_size):
|
||||
num_chunks = math.ceil(file_size / CHUNK_SIZE)
|
||||
return _chunk_paths(path, num_chunks)
|
||||
|
||||
def chunk_file(path, targets):
|
||||
manifest_path, *chunk_paths = targets
|
||||
with open(path, 'rb') as f:
|
||||
data = f.read()
|
||||
actual_num_chunks = max(1, math.ceil(len(data) / CHUNK_SIZE))
|
||||
assert len(chunk_paths) >= actual_num_chunks, f"Allowed {len(chunk_paths)} chunks but needs at least {actual_num_chunks}, for path {path}"
|
||||
for i, chunk_path in enumerate(chunk_paths):
|
||||
with open(chunk_path, 'wb') as f:
|
||||
f.write(data[i * CHUNK_SIZE:(i + 1) * CHUNK_SIZE])
|
||||
Path(manifest_path).write_text(str(len(chunk_paths)))
|
||||
os.remove(path)
|
||||
|
||||
def get_existing_chunks(path):
|
||||
if os.path.isfile(path):
|
||||
return [path]
|
||||
if os.path.isfile(manifest := get_manifest_path(path)):
|
||||
num_chunks = int(Path(manifest).read_text().strip())
|
||||
return _chunk_paths(path, num_chunks)
|
||||
raise FileNotFoundError(path)
|
||||
|
||||
def read_file_chunked(path):
|
||||
manifest_path = get_manifest_path(path)
|
||||
if os.path.isfile(manifest_path):
|
||||
num_chunks = int(Path(manifest_path).read_text().strip())
|
||||
return b''.join(Path(get_chunk_name(path, i, num_chunks)).read_bytes() for i in range(num_chunks))
|
||||
if os.path.isfile(path):
|
||||
return Path(path).read_bytes()
|
||||
raise FileNotFoundError(path)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
path = sys.argv[1]
|
||||
chunk_paths = get_chunk_targets(path, os.path.getsize(path))
|
||||
chunk_file(path, chunk_paths)
|
||||
@@ -0,0 +1,71 @@
|
||||
from collections import deque
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class FirstOrderFilter:
|
||||
def __init__(self, x0, rc, dt, initialized=True):
|
||||
self.x = x0
|
||||
self.dt = dt
|
||||
self.update_alpha(rc)
|
||||
self.initialized = initialized
|
||||
|
||||
def update_alpha(self, rc):
|
||||
self.alpha = self.dt / (rc + self.dt)
|
||||
|
||||
def update(self, x):
|
||||
if self.initialized:
|
||||
self.x = (1. - self.alpha) * self.x + self.alpha * x
|
||||
else:
|
||||
self.initialized = True
|
||||
self.x = x
|
||||
return self.x
|
||||
|
||||
|
||||
class BounceFilter(FirstOrderFilter):
|
||||
def __init__(self, x0, rc, dt, initialized=True, bounce=2):
|
||||
self.velocity = FirstOrderFilter(0.0, 0.15, dt)
|
||||
self.bounce = bounce
|
||||
super().__init__(x0, rc, dt, initialized)
|
||||
|
||||
def update(self, x):
|
||||
super().update(x)
|
||||
scale = self.dt / (1.0 / 60.0) # tuned at 60 fps
|
||||
self.velocity.x += (x - self.x) * self.bounce * scale * self.dt
|
||||
self.velocity.update(0.0)
|
||||
if abs(self.velocity.x) < 1e-5:
|
||||
self.velocity.x = 0.0
|
||||
self.x += self.velocity.x
|
||||
return self.x
|
||||
|
||||
|
||||
class MyMovingAverage:
|
||||
def __init__(self, window_size, value=None):
|
||||
self.window_size = window_size
|
||||
if value is not None:
|
||||
self.values = deque([value] * window_size, maxlen=window_size)
|
||||
self.sum = value * window_size
|
||||
self.result = value
|
||||
else:
|
||||
self.values = deque(maxlen=window_size)
|
||||
self.sum = 0
|
||||
self.result = 0
|
||||
|
||||
def set(self, value):
|
||||
self.values.clear()
|
||||
self.values.append(value)
|
||||
self.sum = value
|
||||
self.result = value
|
||||
return value
|
||||
|
||||
def set_all(self, value):
|
||||
self.values = deque([value] * self.window_size, maxlen=self.window_size)
|
||||
self.sum = value * self.window_size
|
||||
self.result = value
|
||||
return value
|
||||
|
||||
def process(self, value, median=False):
|
||||
self.values.append(value)
|
||||
self.sum = sum(self.values)
|
||||
self.result = float(np.median(self.values)) if median else float(self.sum) / len(self.values)
|
||||
return self.result
|
||||
@@ -0,0 +1,89 @@
|
||||
import os
|
||||
import fcntl
|
||||
import ctypes
|
||||
from functools import cache
|
||||
|
||||
def gpio_init(pin: int, output: bool) -> None:
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f:
|
||||
f.write(b"out" if output else b"in")
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} direction: {e}")
|
||||
|
||||
def gpio_set(pin: int, high: bool) -> None:
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/value", 'wb') as f:
|
||||
f.write(b"1" if high else b"0")
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} value: {e}")
|
||||
|
||||
def gpio_read(pin: int) -> bool | None:
|
||||
val = None
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f:
|
||||
val = bool(int(f.read().strip()))
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} value: {e}")
|
||||
|
||||
return val
|
||||
|
||||
def gpio_export(pin: int) -> None:
|
||||
if os.path.isdir(f"/sys/class/gpio/gpio{pin}"):
|
||||
return
|
||||
|
||||
try:
|
||||
with open("/sys/class/gpio/export", 'w') as f:
|
||||
f.write(str(pin))
|
||||
except Exception:
|
||||
print(f"Failed to export gpio {pin}")
|
||||
|
||||
@cache
|
||||
def get_irq_action(irq: int) -> list[str]:
|
||||
try:
|
||||
with open(f"/sys/kernel/irq/{irq}/actions") as f:
|
||||
actions = f.read().strip().split(',')
|
||||
return actions
|
||||
except FileNotFoundError:
|
||||
return []
|
||||
|
||||
def get_irqs_for_action(action: str) -> list[str]:
|
||||
ret = []
|
||||
with open("/proc/interrupts") as f:
|
||||
for l in f.readlines():
|
||||
irq = l.split(':')[0].strip()
|
||||
if irq.isdigit() and action in get_irq_action(irq):
|
||||
ret.append(irq)
|
||||
return ret
|
||||
|
||||
# *** gpiochip ***
|
||||
|
||||
class gpioevent_data(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("timestamp", ctypes.c_uint64),
|
||||
("id", ctypes.c_uint32),
|
||||
]
|
||||
|
||||
class gpioevent_request(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("lineoffset", ctypes.c_uint32),
|
||||
("handleflags", ctypes.c_uint32),
|
||||
("eventflags", ctypes.c_uint32),
|
||||
("label", ctypes.c_char * 32),
|
||||
("fd", ctypes.c_int)
|
||||
]
|
||||
|
||||
def gpiochip_get_ro_value_fd(label: str, gpiochip_id: int, pin: int) -> int:
|
||||
GPIOEVENT_REQUEST_BOTH_EDGES = 0x3
|
||||
GPIOHANDLE_REQUEST_INPUT = 0x1
|
||||
GPIO_GET_LINEEVENT_IOCTL = 0xc030b404
|
||||
|
||||
rq = gpioevent_request()
|
||||
rq.lineoffset = pin
|
||||
rq.handleflags = GPIOHANDLE_REQUEST_INPUT
|
||||
rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES
|
||||
rq.label = label.encode('utf-8')[:31] + b'\0'
|
||||
|
||||
fd = os.open(f"/dev/gpiochip{gpiochip_id}", os.O_RDONLY)
|
||||
fcntl.ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, rq)
|
||||
os.close(fd)
|
||||
return int(rq.fd)
|
||||
@@ -0,0 +1,81 @@
|
||||
import os
|
||||
import fcntl
|
||||
import ctypes
|
||||
|
||||
# I2C constants from /usr/include/linux/i2c-dev.h
|
||||
I2C_SLAVE = 0x0703
|
||||
I2C_SLAVE_FORCE = 0x0706
|
||||
I2C_SMBUS = 0x0720
|
||||
|
||||
# SMBus transfer types
|
||||
I2C_SMBUS_READ = 1
|
||||
I2C_SMBUS_WRITE = 0
|
||||
I2C_SMBUS_BYTE_DATA = 2
|
||||
I2C_SMBUS_I2C_BLOCK_DATA = 8
|
||||
|
||||
I2C_SMBUS_BLOCK_MAX = 32
|
||||
|
||||
|
||||
class _I2cSmbusData(ctypes.Union):
|
||||
_fields_ = [
|
||||
("byte", ctypes.c_uint8),
|
||||
("word", ctypes.c_uint16),
|
||||
("block", ctypes.c_uint8 * (I2C_SMBUS_BLOCK_MAX + 2)),
|
||||
]
|
||||
|
||||
|
||||
class _I2cSmbusIoctlData(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("read_write", ctypes.c_uint8),
|
||||
("command", ctypes.c_uint8),
|
||||
("size", ctypes.c_uint32),
|
||||
("data", ctypes.POINTER(_I2cSmbusData)),
|
||||
]
|
||||
|
||||
|
||||
class SMBus:
|
||||
def __init__(self, bus: int):
|
||||
self._fd = os.open(f'/dev/i2c-{bus}', os.O_RDWR)
|
||||
|
||||
def __enter__(self) -> 'SMBus':
|
||||
return self
|
||||
|
||||
def __exit__(self, *args) -> None:
|
||||
self.close()
|
||||
|
||||
def close(self) -> None:
|
||||
if hasattr(self, '_fd') and self._fd >= 0:
|
||||
os.close(self._fd)
|
||||
self._fd = -1
|
||||
|
||||
def _set_address(self, addr: int, force: bool = False) -> None:
|
||||
ioctl_arg = I2C_SLAVE_FORCE if force else I2C_SLAVE
|
||||
fcntl.ioctl(self._fd, ioctl_arg, addr)
|
||||
|
||||
def _smbus_access(self, read_write: int, command: int, size: int, data: _I2cSmbusData) -> None:
|
||||
ioctl_data = _I2cSmbusIoctlData(read_write, command, size, ctypes.pointer(data))
|
||||
fcntl.ioctl(self._fd, I2C_SMBUS, ioctl_data)
|
||||
|
||||
def read_byte_data(self, addr: int, register: int, force: bool = False) -> int:
|
||||
self._set_address(addr, force)
|
||||
data = _I2cSmbusData()
|
||||
self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_BYTE_DATA, data)
|
||||
return int(data.byte)
|
||||
|
||||
def write_byte_data(self, addr: int, register: int, value: int, force: bool = False) -> None:
|
||||
self._set_address(addr, force)
|
||||
data = _I2cSmbusData()
|
||||
data.byte = value & 0xFF
|
||||
self._smbus_access(I2C_SMBUS_WRITE, register, I2C_SMBUS_BYTE_DATA, data)
|
||||
|
||||
def read_i2c_block_data(self, addr: int, register: int, length: int, force: bool = False) -> list[int]:
|
||||
self._set_address(addr, force)
|
||||
if not (0 <= length <= I2C_SMBUS_BLOCK_MAX):
|
||||
raise ValueError(f"length must be 0..{I2C_SMBUS_BLOCK_MAX}")
|
||||
|
||||
data = _I2cSmbusData()
|
||||
data.block[0] = length
|
||||
self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_I2C_BLOCK_DATA, data)
|
||||
read_len = int(data.block[0]) or length
|
||||
read_len = min(read_len, length)
|
||||
return [int(b) for b in data.block[1 : read_len + 1]]
|
||||
@@ -0,0 +1,187 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal import custom
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
TRACE_SERVICE = "iqPerfTrace"
|
||||
MAX_TRACE_SAMPLES = 16
|
||||
_SHARED_PM: messaging.PubMaster | None = None
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class PerfSample:
|
||||
frame_id: int = 0
|
||||
loop_dt_us: int = 0
|
||||
update_us: int = 0
|
||||
state_control_us: int = 0
|
||||
publish_us: int = 0
|
||||
tail_work_us: int = 0
|
||||
rk_remaining_us: int = 0
|
||||
stale_carcontrol_us: int = 0
|
||||
stale_carcontrol_frames: int = 0
|
||||
sendcan_gap_us: int = 0
|
||||
model_eval_us: int = 0
|
||||
model_dropped_frames: int = 0
|
||||
model_backlog: int = 0
|
||||
texture_decode_us: int = 0
|
||||
texture_upload_us: int = 0
|
||||
texture_unload_us: int = 0
|
||||
texture_prune_us: int = 0
|
||||
texture_consume_us: int = 0
|
||||
texture_batch_size: int = 0
|
||||
texture_bytes: int = 0
|
||||
texture_cache_before: int = 0
|
||||
texture_cache_after: int = 0
|
||||
texture_unloaded: int = 0
|
||||
memory_usage_percent: int = 0
|
||||
gpu_usage_percent: int = 0
|
||||
cpu_usage_percent: int = 0
|
||||
flags: int = 0
|
||||
|
||||
|
||||
class PerfTraceRing:
|
||||
def __init__(self, size: int = MAX_TRACE_SAMPLES):
|
||||
self._samples: deque[PerfSample] = deque(maxlen=size)
|
||||
|
||||
def push(self, sample: PerfSample) -> None:
|
||||
self._samples.append(sample)
|
||||
|
||||
def snapshot(self) -> list[PerfSample]:
|
||||
return list(self._samples)
|
||||
|
||||
|
||||
class PerfTraceEmitter:
|
||||
_SEVERITY_MAP = {
|
||||
"info": custom.IQPerfTrace.Severity.info,
|
||||
"warning": custom.IQPerfTrace.Severity.warning,
|
||||
"error": custom.IQPerfTrace.Severity.error,
|
||||
"critical": custom.IQPerfTrace.Severity.critical,
|
||||
}
|
||||
|
||||
def __init__(self, process_name: str, pubmaster: messaging.PubMaster | None = None):
|
||||
self.process_name = process_name
|
||||
self._pm: messaging.PubMaster | None = pubmaster
|
||||
self._last_emit_mono: dict[str, float] = {}
|
||||
self._disabled = False
|
||||
|
||||
def _pubmaster(self) -> messaging.PubMaster:
|
||||
global _SHARED_PM
|
||||
if self._pm is not None:
|
||||
return self._pm
|
||||
if _SHARED_PM is None:
|
||||
_SHARED_PM = messaging.PubMaster([TRACE_SERVICE])
|
||||
self._pm = _SHARED_PM
|
||||
return self._pm
|
||||
|
||||
@staticmethod
|
||||
def _clamp_uint(value: int, bits: int) -> int:
|
||||
return max(0, min(value, (1 << bits) - 1))
|
||||
|
||||
@staticmethod
|
||||
def _clamp_int(value: int, bits: int) -> int:
|
||||
lo = -(1 << (bits - 1))
|
||||
hi = (1 << (bits - 1)) - 1
|
||||
return max(lo, min(value, hi))
|
||||
|
||||
def emit(self, event_class: str, *,
|
||||
severity: str = "warning",
|
||||
frame_id: int = 0,
|
||||
total_time_us: int = 0,
|
||||
rk_remaining_us: int = 0,
|
||||
batch_size: int = 0,
|
||||
dropped_frames: int = 0,
|
||||
backlog: int = 0,
|
||||
flags: int = 0,
|
||||
samples: list[PerfSample] | None = None,
|
||||
missing_services: list[str] | None = None,
|
||||
top_processes: list[str] | None = None,
|
||||
detail: str = "",
|
||||
min_interval_s: float = 0.0,
|
||||
mirror_cloudlog: bool = True) -> bool:
|
||||
if self._disabled:
|
||||
return False
|
||||
now = time.monotonic()
|
||||
last_emit = self._last_emit_mono.get(event_class, 0.0)
|
||||
if min_interval_s > 0.0 and (now - last_emit) < min_interval_s:
|
||||
return False
|
||||
self._last_emit_mono[event_class] = now
|
||||
|
||||
msg = messaging.new_message(TRACE_SERVICE)
|
||||
trace = msg.iqPerfTrace
|
||||
trace.process = self.process_name
|
||||
trace.eventClass = event_class
|
||||
trace.severity = self._SEVERITY_MAP.get(severity, custom.IQPerfTrace.Severity.warning)
|
||||
trace.frameId = self._clamp_uint(int(frame_id), 32)
|
||||
trace.totalTimeUs = self._clamp_uint(int(total_time_us), 32)
|
||||
trace.rkRemainingUs = self._clamp_int(int(rk_remaining_us), 32)
|
||||
trace.batchSize = self._clamp_uint(int(batch_size), 16)
|
||||
trace.droppedFrames = self._clamp_uint(int(dropped_frames), 16)
|
||||
trace.backlog = self._clamp_uint(int(backlog), 16)
|
||||
trace.flags = self._clamp_uint(int(flags), 32)
|
||||
trace.missingServices = list(missing_services or [])
|
||||
trace.topProcesses = list(top_processes or [])
|
||||
trace.detail = detail
|
||||
|
||||
trace_samples = samples or []
|
||||
samples_builder = trace.init("samples", len(trace_samples))
|
||||
for i, sample in enumerate(trace_samples):
|
||||
builder = samples_builder[i]
|
||||
builder.frameId = self._clamp_uint(int(sample.frame_id), 32)
|
||||
builder.loopDtUs = self._clamp_uint(int(sample.loop_dt_us), 32)
|
||||
builder.updateUs = self._clamp_uint(int(sample.update_us), 32)
|
||||
builder.stateControlUs = self._clamp_uint(int(sample.state_control_us), 32)
|
||||
builder.publishUs = self._clamp_uint(int(sample.publish_us), 32)
|
||||
builder.tailWorkUs = self._clamp_uint(int(sample.tail_work_us), 32)
|
||||
builder.rkRemainingUs = self._clamp_int(int(sample.rk_remaining_us), 32)
|
||||
builder.staleCarControlUs = self._clamp_uint(int(sample.stale_carcontrol_us), 32)
|
||||
builder.staleCarControlFrames = self._clamp_uint(int(sample.stale_carcontrol_frames), 16)
|
||||
builder.sendcanGapUs = self._clamp_uint(int(sample.sendcan_gap_us), 32)
|
||||
builder.modelEvalUs = self._clamp_uint(int(sample.model_eval_us), 32)
|
||||
builder.modelDroppedFrames = self._clamp_uint(int(sample.model_dropped_frames), 16)
|
||||
builder.modelBacklog = self._clamp_uint(int(sample.model_backlog), 16)
|
||||
builder.textureDecodeUs = self._clamp_uint(int(sample.texture_decode_us), 32)
|
||||
builder.textureUploadUs = self._clamp_uint(int(sample.texture_upload_us), 32)
|
||||
builder.textureUnloadUs = self._clamp_uint(int(sample.texture_unload_us), 32)
|
||||
builder.texturePruneUs = self._clamp_uint(int(sample.texture_prune_us), 32)
|
||||
builder.textureConsumeUs = self._clamp_uint(int(sample.texture_consume_us), 32)
|
||||
builder.textureBatchSize = self._clamp_uint(int(sample.texture_batch_size), 16)
|
||||
builder.textureBytes = self._clamp_uint(int(sample.texture_bytes), 32)
|
||||
builder.textureCacheBefore = self._clamp_uint(int(sample.texture_cache_before), 16)
|
||||
builder.textureCacheAfter = self._clamp_uint(int(sample.texture_cache_after), 16)
|
||||
builder.textureUnloaded = self._clamp_uint(int(sample.texture_unloaded), 16)
|
||||
builder.memoryUsagePercent = self._clamp_uint(int(sample.memory_usage_percent), 16)
|
||||
builder.gpuUsagePercent = self._clamp_uint(int(sample.gpu_usage_percent), 16)
|
||||
builder.cpuUsagePercent = self._clamp_uint(int(sample.cpu_usage_percent), 16)
|
||||
builder.flags = self._clamp_uint(int(sample.flags), 32)
|
||||
|
||||
try:
|
||||
self._pubmaster().send(TRACE_SERVICE, msg)
|
||||
except messaging.MultiplePublishersError:
|
||||
self._disabled = True
|
||||
cloudlog.error(f"iq_perf_trace disabled for {self.process_name}: duplicate publisher for {TRACE_SERVICE}")
|
||||
return False
|
||||
except Exception:
|
||||
cloudlog.exception(f"iq_perf_trace publish failed for {self.process_name}")
|
||||
return False
|
||||
|
||||
if mirror_cloudlog:
|
||||
cloudlog.event(
|
||||
"iq_perf_trace",
|
||||
process=self.process_name,
|
||||
event_class=event_class,
|
||||
severity=severity,
|
||||
frame_id=int(frame_id),
|
||||
total_time_us=int(total_time_us),
|
||||
dropped_frames=int(dropped_frames),
|
||||
flags=int(flags),
|
||||
detail=detail,
|
||||
)
|
||||
return True
|
||||
@@ -0,0 +1,44 @@
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
from iqpilot.system.hardware import PC
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
DEBUG_FILENAME = "iqpilot_issue_debug.txt"
|
||||
DEBUG_PATH = Path(Paths.comma_home()) / "community" / DEBUG_FILENAME if PC else Path("/data/community") / DEBUG_FILENAME
|
||||
|
||||
_lock = threading.Lock()
|
||||
_last_log_times: dict[str, float] = {}
|
||||
|
||||
|
||||
def log_issue(tag: str, message: str) -> None:
|
||||
try:
|
||||
DEBUG_PATH.parent.mkdir(parents=True, exist_ok=True)
|
||||
with _lock:
|
||||
with open(DEBUG_PATH, "a", encoding="utf-8") as f:
|
||||
timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
|
||||
f.write(f"[{timestamp}] [{tag}] {message}\n")
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def log_issue_limited(key: str, tag: str, message: str, interval_sec: float = 1.0) -> None:
|
||||
now = time.monotonic()
|
||||
with _lock:
|
||||
last = _last_log_times.get(key, 0.0)
|
||||
if now - last < interval_sec:
|
||||
return
|
||||
_last_log_times[key] = now
|
||||
|
||||
log_issue(tag, message)
|
||||
|
||||
|
||||
def clear_issue_debug_log() -> None:
|
||||
try:
|
||||
os.remove(DEBUG_PATH)
|
||||
except OSError:
|
||||
pass
|
||||
@@ -0,0 +1,249 @@
|
||||
import io
|
||||
import os
|
||||
import sys
|
||||
import copy
|
||||
import json
|
||||
import time
|
||||
import uuid
|
||||
import socket
|
||||
import logging
|
||||
import traceback
|
||||
import numpy as np
|
||||
from threading import local
|
||||
from collections import OrderedDict
|
||||
from contextlib import contextmanager
|
||||
|
||||
LOG_TIMESTAMPS = "LOG_TIMESTAMPS" in os.environ
|
||||
|
||||
def json_handler(obj):
|
||||
if isinstance(obj, np.bool_):
|
||||
return bool(obj)
|
||||
# if isinstance(obj, (datetime.date, datetime.time)):
|
||||
# return obj.isoformat()
|
||||
return repr(obj)
|
||||
|
||||
def json_robust_dumps(obj):
|
||||
return json.dumps(obj, default=json_handler)
|
||||
|
||||
class NiceOrderedDict(OrderedDict):
|
||||
def __str__(self):
|
||||
return json_robust_dumps(self)
|
||||
|
||||
class SwagFormatter(logging.Formatter):
|
||||
def __init__(self, swaglogger):
|
||||
logging.Formatter.__init__(self, None, '%a %b %d %H:%M:%S %Z %Y')
|
||||
|
||||
self.swaglogger = swaglogger
|
||||
self.host = socket.gethostname()
|
||||
|
||||
def format_dict(self, record):
|
||||
record_dict = NiceOrderedDict()
|
||||
|
||||
if isinstance(record.msg, dict):
|
||||
record_dict['msg'] = record.msg
|
||||
else:
|
||||
try:
|
||||
record_dict['msg'] = record.getMessage()
|
||||
except (ValueError, TypeError):
|
||||
record_dict['msg'] = [record.msg]+record.args
|
||||
|
||||
record_dict['ctx'] = self.swaglogger.get_ctx()
|
||||
|
||||
if record.exc_info:
|
||||
record_dict['exc_info'] = self.formatException(record.exc_info)
|
||||
|
||||
record_dict['level'] = record.levelname
|
||||
record_dict['levelnum'] = record.levelno
|
||||
record_dict['name'] = record.name
|
||||
record_dict['filename'] = record.filename
|
||||
record_dict['lineno'] = record.lineno
|
||||
record_dict['pathname'] = record.pathname
|
||||
record_dict['module'] = record.module
|
||||
record_dict['funcName'] = record.funcName
|
||||
record_dict['host'] = self.host
|
||||
record_dict['process'] = record.process
|
||||
record_dict['thread'] = record.thread
|
||||
record_dict['threadName'] = record.threadName
|
||||
record_dict['created'] = record.created
|
||||
|
||||
return record_dict
|
||||
|
||||
def format(self, record):
|
||||
if self.swaglogger is None:
|
||||
raise Exception("must set swaglogger before calling format()")
|
||||
return json_robust_dumps(self.format_dict(record))
|
||||
|
||||
class SwagLogFileFormatter(SwagFormatter):
|
||||
def fix_kv(self, k, v):
|
||||
# append type to names to preserve legacy naming in logs
|
||||
# avoids overlapping key namespaces with different types
|
||||
# e.g. log.info() creates 'msg' -> 'msg$s'
|
||||
# log.event() creates 'msg.health.logMonoTime' -> 'msg.health.logMonoTime$i'
|
||||
# because overlapping namespace 'msg' caused problems
|
||||
if isinstance(v, (str, bytes)):
|
||||
k += "$s"
|
||||
elif isinstance(v, float):
|
||||
k += "$f"
|
||||
elif isinstance(v, bool):
|
||||
k += "$b"
|
||||
elif isinstance(v, int):
|
||||
k += "$i"
|
||||
elif isinstance(v, dict):
|
||||
nv = {}
|
||||
for ik, iv in v.items():
|
||||
ik, iv = self.fix_kv(ik, iv)
|
||||
nv[ik] = iv
|
||||
v = nv
|
||||
elif isinstance(v, list):
|
||||
k += "$a"
|
||||
return k, v
|
||||
|
||||
def format(self, record):
|
||||
if isinstance(record, str):
|
||||
v = json.loads(record)
|
||||
else:
|
||||
v = self.format_dict(record)
|
||||
|
||||
mk, mv = self.fix_kv('msg', v['msg'])
|
||||
del v['msg']
|
||||
v[mk] = mv
|
||||
v['id'] = uuid.uuid4().hex
|
||||
|
||||
return json_robust_dumps(v)
|
||||
|
||||
class SwagErrorFilter(logging.Filter):
|
||||
def filter(self, record):
|
||||
return record.levelno < logging.ERROR
|
||||
|
||||
def _tmpfunc():
|
||||
return 0
|
||||
|
||||
def _srcfile():
|
||||
return os.path.normcase(_tmpfunc.__code__.co_filename)
|
||||
|
||||
class SwagLogger(logging.Logger):
|
||||
def __init__(self):
|
||||
logging.Logger.__init__(self, "swaglog")
|
||||
|
||||
self.global_ctx = {}
|
||||
|
||||
self.log_local = local()
|
||||
self.log_local.ctx = {}
|
||||
|
||||
def local_ctx(self):
|
||||
try:
|
||||
return self.log_local.ctx
|
||||
except AttributeError:
|
||||
self.log_local.ctx = {}
|
||||
return self.log_local.ctx
|
||||
|
||||
def get_ctx(self):
|
||||
return dict(self.local_ctx(), **self.global_ctx)
|
||||
|
||||
@contextmanager
|
||||
def ctx(self, **kwargs):
|
||||
old_ctx = self.local_ctx()
|
||||
self.log_local.ctx = copy.copy(old_ctx) or {}
|
||||
self.log_local.ctx.update(kwargs)
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
self.log_local.ctx = old_ctx
|
||||
|
||||
def bind(self, **kwargs):
|
||||
self.local_ctx().update(kwargs)
|
||||
|
||||
def bind_global(self, **kwargs):
|
||||
self.global_ctx.update(kwargs)
|
||||
|
||||
def event(self, event, *args, **kwargs):
|
||||
evt = NiceOrderedDict()
|
||||
evt['event'] = event
|
||||
if args:
|
||||
evt['args'] = args
|
||||
evt.update(kwargs)
|
||||
if 'error' in kwargs:
|
||||
self.error(evt)
|
||||
elif 'debug' in kwargs:
|
||||
self.debug(evt)
|
||||
else:
|
||||
self.info(evt)
|
||||
|
||||
def timestamp(self, event_name):
|
||||
if LOG_TIMESTAMPS:
|
||||
t = time.monotonic()
|
||||
tstp = NiceOrderedDict()
|
||||
tstp['timestamp'] = NiceOrderedDict()
|
||||
tstp['timestamp']["event"] = event_name
|
||||
tstp['timestamp']["time"] = t*1e9
|
||||
self.debug(tstp)
|
||||
|
||||
def findCaller(self, stack_info=False, stacklevel=1):
|
||||
"""
|
||||
Find the stack frame of the caller so that we can note the source
|
||||
file name, line number and function name.
|
||||
"""
|
||||
f = sys._getframe(3)
|
||||
#On some versions of IronPython, currentframe() returns None if
|
||||
#IronPython isn't run with -X:Frames.
|
||||
if f is not None:
|
||||
f = f.f_back
|
||||
orig_f = f
|
||||
while f and stacklevel > 1:
|
||||
f = f.f_back
|
||||
stacklevel -= 1
|
||||
if not f:
|
||||
f = orig_f
|
||||
rv = "(unknown file)", 0, "(unknown function)", None
|
||||
while hasattr(f, "f_code"):
|
||||
co = f.f_code
|
||||
filename = os.path.normcase(co.co_filename)
|
||||
|
||||
if filename == _srcfile:
|
||||
f = f.f_back
|
||||
continue
|
||||
sinfo = None
|
||||
if stack_info:
|
||||
sio = io.StringIO()
|
||||
sio.write('Stack (most recent call last):\n')
|
||||
traceback.print_stack(f, file=sio)
|
||||
sinfo = sio.getvalue()
|
||||
if sinfo[-1] == '\n':
|
||||
sinfo = sinfo[:-1]
|
||||
sio.close()
|
||||
rv = (co.co_filename, f.f_lineno, co.co_name, sinfo)
|
||||
break
|
||||
return rv
|
||||
|
||||
if __name__ == "__main__":
|
||||
log = SwagLogger()
|
||||
|
||||
stdout_handler = logging.StreamHandler(sys.stdout)
|
||||
stdout_handler.setLevel(logging.INFO)
|
||||
stdout_handler.addFilter(SwagErrorFilter())
|
||||
log.addHandler(stdout_handler)
|
||||
|
||||
stderr_handler = logging.StreamHandler(sys.stderr)
|
||||
stderr_handler.setLevel(logging.ERROR)
|
||||
log.addHandler(stderr_handler)
|
||||
|
||||
log.info("asdasd %s", "a")
|
||||
log.info({'wut': 1})
|
||||
log.warning("warning")
|
||||
log.error("error")
|
||||
log.critical("critical")
|
||||
log.event("test", x="y")
|
||||
|
||||
with log.ctx():
|
||||
stdout_handler.setFormatter(SwagFormatter(log))
|
||||
stderr_handler.setFormatter(SwagFormatter(log))
|
||||
log.bind(user="some user")
|
||||
log.info("in req")
|
||||
print("")
|
||||
log.warning("warning")
|
||||
print("")
|
||||
log.error("error")
|
||||
print("")
|
||||
log.critical("critical")
|
||||
print("")
|
||||
log.event("do_req", a=1, b="c")
|
||||
@@ -0,0 +1,134 @@
|
||||
"""Utilities for reading real time clocks and keeping soft real time constraints."""
|
||||
import gc
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
|
||||
from setproctitle import getproctitle
|
||||
|
||||
from iqpilot.common.utils import MovingAverage
|
||||
from iqpilot.system.hardware import PC
|
||||
|
||||
|
||||
# time step for each process
|
||||
DT_CTRL = 0.01 # controlsd
|
||||
DT_MDL = 0.05 # model
|
||||
DT_HW = 0.5 # hardwared and manager
|
||||
DT_DMON = 0.05 # driver monitoring
|
||||
|
||||
|
||||
class Priority:
|
||||
# CORE 2
|
||||
# - modeld = 55
|
||||
# - camerad = 54
|
||||
CTRL_LOW = 51 # plannerd & radard
|
||||
|
||||
# CORE 3
|
||||
# - pandad = 55
|
||||
CTRL_HIGH = 53
|
||||
|
||||
|
||||
def set_core_affinity(cores: list[int]) -> None:
|
||||
if sys.platform == 'linux' and not PC:
|
||||
os.sched_setaffinity(0, cores)
|
||||
|
||||
|
||||
def config_realtime_process(cores: int | list[int], priority: int) -> None:
|
||||
gc.disable()
|
||||
if sys.platform == 'linux' and not PC:
|
||||
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority))
|
||||
c = cores if isinstance(cores, list) else [cores, ]
|
||||
set_core_affinity(c)
|
||||
|
||||
|
||||
def config_background_thread() -> None:
|
||||
if sys.platform == 'linux' and not PC:
|
||||
os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
|
||||
set_core_affinity(list(range(os.cpu_count() or 1)))
|
||||
|
||||
|
||||
def lock_memory() -> None:
|
||||
"""mlockall this process so memory reclaim/compaction can't stall it. RT control
|
||||
procs only (locking ui/modeld would worsen pressure). Best-effort."""
|
||||
if sys.platform != 'linux' or PC:
|
||||
return
|
||||
try:
|
||||
import ctypes
|
||||
import resource
|
||||
resource.setrlimit(resource.RLIMIT_MEMLOCK, (resource.RLIM_INFINITY, resource.RLIM_INFINITY))
|
||||
MCL_CURRENT, MCL_FUTURE = 0x1, 0x2
|
||||
libc = ctypes.CDLL("libc.so.6", use_errno=True)
|
||||
if libc.mlockall(MCL_CURRENT | MCL_FUTURE) != 0:
|
||||
raise OSError(ctypes.get_errno(), os.strerror(ctypes.get_errno()))
|
||||
except Exception as e:
|
||||
try:
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
cloudlog.warning(f"lock_memory (mlockall) failed: {e}")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class Ratekeeper:
|
||||
def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None:
|
||||
"""Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
|
||||
self._interval = 1. / rate
|
||||
self._print_delay_threshold = print_delay_threshold
|
||||
self._frame = 0
|
||||
self._remaining = 0.0
|
||||
self._process_name = getproctitle()
|
||||
self._last_monitor_time = -1.
|
||||
self._next_frame_time = -1.
|
||||
|
||||
self.avg_dt = MovingAverage(100)
|
||||
self.avg_dt.add_value(self._interval)
|
||||
|
||||
def reset(self) -> None:
|
||||
self._remaining = 0.0
|
||||
self._last_monitor_time = -1.
|
||||
self._next_frame_time = -1.
|
||||
self.avg_dt = MovingAverage(100)
|
||||
self.avg_dt.add_value(self._interval)
|
||||
|
||||
@property
|
||||
def frame(self) -> int:
|
||||
return self._frame
|
||||
|
||||
@property
|
||||
def remaining(self) -> float:
|
||||
return self._remaining
|
||||
|
||||
@property
|
||||
def lag(self) -> float:
|
||||
return max(0., -self._remaining)
|
||||
|
||||
@property
|
||||
def lagging(self) -> bool:
|
||||
expected_dt = self._interval * (1 / 0.9)
|
||||
return self.avg_dt.get_average() > expected_dt
|
||||
|
||||
# Maintain loop rate by calling this at the end of each loop
|
||||
def keep_time(self) -> bool:
|
||||
lagged = self.monitor_time()
|
||||
if self._remaining > 0:
|
||||
time.sleep(self._remaining)
|
||||
return lagged
|
||||
|
||||
# Monitors the cumulative lag, but does not enforce a rate
|
||||
def monitor_time(self) -> bool:
|
||||
if self._last_monitor_time < 0:
|
||||
self._next_frame_time = time.monotonic() + self._interval
|
||||
self._last_monitor_time = time.monotonic()
|
||||
|
||||
prev = self._last_monitor_time
|
||||
self._last_monitor_time = time.monotonic()
|
||||
self.avg_dt.add_value(self._last_monitor_time - prev)
|
||||
|
||||
lagged = False
|
||||
remaining = self._next_frame_time - time.monotonic()
|
||||
self._next_frame_time += self._interval
|
||||
if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
|
||||
print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
|
||||
lagged = True
|
||||
self._frame += 1
|
||||
self._remaining = remaining
|
||||
return lagged
|
||||
@@ -0,0 +1,52 @@
|
||||
import os
|
||||
import subprocess
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
|
||||
|
||||
class Spinner:
|
||||
def __init__(self):
|
||||
try:
|
||||
self.spinner_proc = subprocess.Popen(["./spinner.py"],
|
||||
stdin=subprocess.PIPE,
|
||||
cwd=os.path.join(BASEDIR, "iqpilot", "system", "ui"),
|
||||
close_fds=True)
|
||||
except OSError:
|
||||
self.spinner_proc = None
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def update(self, spinner_text: str):
|
||||
if self.spinner_proc is not None:
|
||||
self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n")
|
||||
try:
|
||||
self.spinner_proc.stdin.flush()
|
||||
except BrokenPipeError:
|
||||
pass
|
||||
|
||||
def update_progress(self, cur: float, total: float):
|
||||
self.update(str(round(100 * cur / total)))
|
||||
|
||||
def close(self):
|
||||
if self.spinner_proc is not None:
|
||||
self.spinner_proc.kill()
|
||||
try:
|
||||
self.spinner_proc.communicate(timeout=2.)
|
||||
except subprocess.TimeoutExpired:
|
||||
print("WARNING: failed to kill spinner")
|
||||
self.spinner_proc = None
|
||||
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def __exit__(self, exc_type, exc_value, traceback):
|
||||
self.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import time
|
||||
with Spinner() as s:
|
||||
s.update("Spinner text")
|
||||
time.sleep(5.0)
|
||||
print("gone")
|
||||
time.sleep(5.0)
|
||||
@@ -0,0 +1,60 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Chooses which steer-actuator delay the lateral controllers run with: the value the
|
||||
live estimator learned, or the driver's fixed software delay — gated by the
|
||||
"IQLiveSteerDelay" param. The pick is mirrored into "IQSteerDelayCache" so consumers that do
|
||||
not subscribe to lateralDelay can still read the current value.
|
||||
"""
|
||||
from iqpilot.cereal import car
|
||||
from iqpilot.common.params import Params
|
||||
|
||||
_ENABLE_KEY = "IQLiveSteerDelay"
|
||||
_FIXED_KEY = "IQSoftwareSteerDelay"
|
||||
_CACHE_KEY = "IQSteerDelayCache"
|
||||
|
||||
|
||||
def fixed_steer_delay(params, stock_delay):
|
||||
"""The rack's own delay plus the driver's IQSoftwareSteerDelay offset, as the UI reports it."""
|
||||
return stock_delay + float(params.get(_FIXED_KEY, return_default=True))
|
||||
|
||||
|
||||
def resolve_steer_delay(params, stock_delay):
|
||||
"""Learned lateral delay while live-learning is enabled, otherwise the driver's fixed delay."""
|
||||
if not params.get_bool(_ENABLE_KEY):
|
||||
return fixed_steer_delay(params, stock_delay)
|
||||
return float(params.get(_CACHE_KEY, return_default=True))
|
||||
|
||||
|
||||
def lateral_action_delay(params, car_params, live_delay):
|
||||
"""Delay the lateral path should be planned against.
|
||||
|
||||
Angle cars honour the IQLiveSteerDelay toggle so that with live learning off the
|
||||
estimate never reaches the path: lagd cross-correlates against localizer lateral
|
||||
accel, so it reports whole-vehicle response (~0.36 s measured on VW MQB, 0.44 s on
|
||||
Tesla) where the lookahead wants actuator delay (~0.10 s). Torque cars keep the
|
||||
live estimate.
|
||||
"""
|
||||
if car_params.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
return resolve_steer_delay(params, car_params.steerActuatorDelay)
|
||||
return live_delay
|
||||
|
||||
|
||||
def cached_steer_delay():
|
||||
"""Last value SteerDelayPublisher mirrored into the param — usable without a
|
||||
lateralDelay subscription (e.g. at process startup)."""
|
||||
return Params().get(_CACHE_KEY, return_default=True)
|
||||
|
||||
|
||||
class SteerDelayPublisher:
|
||||
"""Refreshes IQSteerDelayCache every lag message: the learned live delay when the
|
||||
toggle is on, else the actuator delay plus the driver's fixed software offset."""
|
||||
|
||||
def __init__(self, car_params):
|
||||
self._params = Params()
|
||||
self._actuator_delay = car_params.steerActuatorDelay
|
||||
|
||||
def update(self, lag_msg):
|
||||
live = self._params.get_bool(_ENABLE_KEY)
|
||||
value = lag_msg.lateralDelay.lateralDelay if live else fixed_steer_delay(self._params, self._actuator_delay)
|
||||
self._params.put_nonblocking(_CACHE_KEY, value)
|
||||
@@ -0,0 +1,165 @@
|
||||
import logging
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import warnings
|
||||
from pathlib import Path
|
||||
from logging.handlers import BaseRotatingHandler
|
||||
|
||||
import zmq
|
||||
|
||||
from iqpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
def get_file_handler():
|
||||
Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True)
|
||||
base_filename = os.path.join(Paths.swaglog_root(), "swaglog")
|
||||
handler = SwaglogRotatingFileHandler(base_filename)
|
||||
return handler
|
||||
|
||||
class SwaglogRotatingFileHandler(BaseRotatingHandler):
|
||||
def __init__(self, base_filename, interval=60, max_bytes=1024*256, backup_count=2500, encoding=None):
|
||||
super().__init__(base_filename, mode="a", encoding=encoding, delay=True)
|
||||
self.base_filename = base_filename
|
||||
self.interval = interval # seconds
|
||||
self.max_bytes = max_bytes
|
||||
self.backup_count = backup_count
|
||||
self.log_files = self.get_existing_logfiles()
|
||||
log_indexes = [f.split(".")[-1] for f in self.log_files]
|
||||
self.last_file_idx = max([int(i) for i in log_indexes if i.isdigit()] or [-1])
|
||||
self.last_rollover = None
|
||||
self.doRollover()
|
||||
|
||||
def _open(self):
|
||||
self.last_rollover = time.monotonic()
|
||||
self.last_file_idx += 1
|
||||
next_filename = f"{self.base_filename}.{self.last_file_idx:010}"
|
||||
stream = open(next_filename, self.mode, encoding=self.encoding)
|
||||
self.log_files.insert(0, next_filename)
|
||||
return stream
|
||||
|
||||
def get_existing_logfiles(self):
|
||||
log_files = list()
|
||||
base_dir = os.path.dirname(self.base_filename)
|
||||
for fn in os.listdir(base_dir):
|
||||
fp = os.path.join(base_dir, fn)
|
||||
if fp.startswith(self.base_filename) and os.path.isfile(fp):
|
||||
log_files.append(fp)
|
||||
return sorted(log_files)
|
||||
|
||||
def shouldRollover(self, record):
|
||||
size_exceeded = self.max_bytes > 0 and self.stream.tell() >= self.max_bytes
|
||||
time_exceeded = self.interval > 0 and self.last_rollover + self.interval <= time.monotonic()
|
||||
return size_exceeded or time_exceeded
|
||||
|
||||
def doRollover(self):
|
||||
if self.stream:
|
||||
self.stream.close()
|
||||
self.stream = self._open()
|
||||
|
||||
if self.backup_count > 0:
|
||||
while len(self.log_files) > self.backup_count:
|
||||
to_delete = self.log_files.pop()
|
||||
if os.path.exists(to_delete): # just being safe, should always exist
|
||||
os.remove(to_delete)
|
||||
|
||||
class UnixDomainSocketHandler(logging.Handler):
|
||||
def __init__(self, formatter):
|
||||
logging.Handler.__init__(self)
|
||||
self.setFormatter(formatter)
|
||||
self.pid = None
|
||||
|
||||
self.zctx = None
|
||||
self.sock = None
|
||||
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def close(self):
|
||||
if self.sock is not None:
|
||||
self.sock.close()
|
||||
if self.zctx is not None:
|
||||
self.zctx.term()
|
||||
|
||||
def connect(self):
|
||||
self.zctx = zmq.Context()
|
||||
self.sock = self.zctx.socket(zmq.PUSH)
|
||||
self.sock.setsockopt(zmq.LINGER, 10)
|
||||
self.sock.connect(Paths.swaglog_ipc())
|
||||
self.pid = os.getpid()
|
||||
|
||||
def emit(self, record):
|
||||
if os.getpid() != self.pid:
|
||||
# TODO suppresses warning about forking proc with zmq socket, fix root cause
|
||||
warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*<zmq.*>")
|
||||
self.connect()
|
||||
|
||||
msg = self.format(record).rstrip('\n')
|
||||
# print("SEND".format(repr(msg)))
|
||||
try:
|
||||
s = chr(record.levelno)+msg
|
||||
self.sock.send(s.encode('utf8'), zmq.NOBLOCK)
|
||||
except zmq.error.Again:
|
||||
# drop :/
|
||||
pass
|
||||
|
||||
|
||||
class ForwardingHandler(logging.Handler):
|
||||
def __init__(self, target_logger):
|
||||
super().__init__()
|
||||
self.target_logger = target_logger
|
||||
|
||||
def emit(self, record):
|
||||
self.target_logger.handle(record)
|
||||
|
||||
|
||||
def add_file_handler(log):
|
||||
"""
|
||||
Function to add the file log handler to swaglog.
|
||||
This can be used to store logs when logmessaged is not running.
|
||||
"""
|
||||
handler = get_file_handler()
|
||||
handler.setFormatter(SwagLogFileFormatter(log))
|
||||
log.addHandler(handler)
|
||||
|
||||
|
||||
cloudlog = log = SwagLogger()
|
||||
log.setLevel(logging.DEBUG)
|
||||
|
||||
|
||||
class PrettyConsoleFormatter(logging.Formatter):
|
||||
# StreamHandler writes to stderr, so tty-gate on that
|
||||
_COLOR = sys.stderr.isatty() and os.environ.get('NO_COLOR') is None
|
||||
|
||||
def format(self, record):
|
||||
msg = record.getMessage()
|
||||
if not self._COLOR:
|
||||
return f"{record.filename}: {msg}"
|
||||
lvl = record.levelno
|
||||
if lvl >= 50: lc, ln = "\033[1;38;5;196m", "CRIT"
|
||||
elif lvl >= 40: lc, ln = "\033[1;38;5;203m", " ERR"
|
||||
elif lvl >= 30: lc, ln = "\033[38;5;214m", "WARN"
|
||||
elif lvl >= 20: lc, ln = "\033[38;5;110m", "info"
|
||||
else: lc, ln = "\033[38;5;244m", " dbg"
|
||||
body = f"\033[1;38;5;210m{msg}\033[0m" if lvl >= 40 else msg
|
||||
src = "" if record.filename == "(unknown file)" else f"\033[2m{record.filename}\033[0m "
|
||||
return f"{lc}{ln:>4}\033[0m {src}{body}"
|
||||
|
||||
|
||||
outhandler = logging.StreamHandler()
|
||||
outhandler.setFormatter(PrettyConsoleFormatter())
|
||||
|
||||
print_level = os.environ.get('LOGPRINT', 'warning')
|
||||
if print_level == 'debug':
|
||||
outhandler.setLevel(logging.DEBUG)
|
||||
elif print_level == 'info':
|
||||
outhandler.setLevel(logging.INFO)
|
||||
elif print_level == 'warning':
|
||||
outhandler.setLevel(logging.WARNING)
|
||||
|
||||
ipchandler = UnixDomainSocketHandler(SwagFormatter(log))
|
||||
|
||||
log.addHandler(outhandler)
|
||||
# logs are sent through IPC before writing to disk to prevent disk I/O blocking
|
||||
log.addHandler(ipchandler)
|
||||
@@ -0,0 +1,15 @@
|
||||
import datetime
|
||||
from pathlib import Path
|
||||
|
||||
MIN_DATE = datetime.datetime(year=2025, month=2, day=21)
|
||||
|
||||
def min_date():
|
||||
# on systemd systems, the default time is the systemd build time
|
||||
systemd_path = Path("/lib/systemd/systemd")
|
||||
if systemd_path.exists():
|
||||
d = datetime.datetime.fromtimestamp(systemd_path.stat().st_mtime)
|
||||
return max(MIN_DATE, d + datetime.timedelta(days=1))
|
||||
return MIN_DATE
|
||||
|
||||
def system_time_valid():
|
||||
return datetime.datetime.now() > min_date()
|
||||
@@ -0,0 +1,271 @@
|
||||
import io
|
||||
import os
|
||||
import tempfile
|
||||
import contextlib
|
||||
import subprocess
|
||||
import time
|
||||
import functools
|
||||
from subprocess import Popen, PIPE, TimeoutExpired
|
||||
import zstandard as zstd
|
||||
|
||||
LOG_COMPRESSION_LEVEL = 10 # little benefit up to level 15. level ~17 is a small step change
|
||||
|
||||
class Timer:
|
||||
"""Simple lap timer for profiling sequential operations."""
|
||||
|
||||
def __init__(self):
|
||||
self._start = self._lap = time.monotonic()
|
||||
self._sections = {}
|
||||
|
||||
def lap(self, name):
|
||||
now = time.monotonic()
|
||||
self._sections[name] = now - self._lap
|
||||
self._lap = now
|
||||
|
||||
@property
|
||||
def total(self):
|
||||
return time.monotonic() - self._start
|
||||
|
||||
def fmt(self, duration):
|
||||
parts = ", ".join(f"{k}={v:.2f}s" + (f" ({duration/v:.0f}x)" if k == 'render' and v > 0 else "") for k, v in self._sections.items())
|
||||
total = self.total
|
||||
realtime = f"{duration/total:.1f}x realtime" if total > 0 else "N/A"
|
||||
return f"{duration}s in {total:.1f}s ({realtime}) | {parts}"
|
||||
|
||||
def sudo_write(val: str, path: str) -> None:
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
os.system(f"sudo chmod a+w {path}")
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
# fallback for debugfs files
|
||||
os.system(f"sudo su -c 'echo {val} > {path}'")
|
||||
|
||||
|
||||
def sudo_read(path: str) -> str:
|
||||
try:
|
||||
return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip()
|
||||
except Exception:
|
||||
return ""
|
||||
|
||||
|
||||
class MovingAverage:
|
||||
def __init__(self, window_size: int):
|
||||
self.window_size: int = window_size
|
||||
self.buffer: list[float] = [0.0] * window_size
|
||||
self.index: int = 0
|
||||
self.count: int = 0
|
||||
self.sum: float = 0.0
|
||||
|
||||
def add_value(self, new_value: float):
|
||||
# Update the sum: subtract the value being replaced and add the new value
|
||||
self.sum -= self.buffer[self.index]
|
||||
self.buffer[self.index] = new_value
|
||||
self.sum += new_value
|
||||
|
||||
# Update the index in a circular manner
|
||||
self.index = (self.index + 1) % self.window_size
|
||||
|
||||
# Track the number of added values (for partial windows)
|
||||
self.count = min(self.count + 1, self.window_size)
|
||||
|
||||
def get_average(self) -> float:
|
||||
if self.count == 0:
|
||||
return float('nan')
|
||||
return self.sum / self.count
|
||||
|
||||
|
||||
class CallbackReader:
|
||||
"""Wraps a file, but overrides the read method to also
|
||||
call a callback function with the number of bytes read so far."""
|
||||
|
||||
def __init__(self, f, callback, *args):
|
||||
self.f = f
|
||||
self.callback = callback
|
||||
self.cb_args = args
|
||||
self.total_read = 0
|
||||
|
||||
def __getattr__(self, attr):
|
||||
return getattr(self.f, attr)
|
||||
|
||||
def read(self, *args, **kwargs):
|
||||
chunk = self.f.read(*args, **kwargs)
|
||||
self.total_read += len(chunk)
|
||||
self.callback(*self.cb_args, self.total_read)
|
||||
return chunk
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
def atomic_write(path: str, mode: str = 'w', buffering: int = -1, encoding: str | None = None, newline: str | None = None,
|
||||
overwrite: bool = False):
|
||||
"""Write to a file atomically using a temporary file in the same directory as the destination file."""
|
||||
dir_name = os.path.dirname(path)
|
||||
|
||||
if not overwrite and os.path.exists(path):
|
||||
raise FileExistsError(f"File '{path}' already exists. To overwrite it, set 'overwrite' to True.")
|
||||
|
||||
with tempfile.NamedTemporaryFile(mode=mode, buffering=buffering, encoding=encoding, newline=newline, dir=dir_name, delete=False) as tmp_file:
|
||||
yield tmp_file
|
||||
tmp_file_name = tmp_file.name
|
||||
os.replace(tmp_file_name, path)
|
||||
|
||||
|
||||
def get_upload_stream(filepath: str, should_compress: bool) -> tuple[io.BufferedIOBase, int]:
|
||||
if not should_compress:
|
||||
file_size = os.path.getsize(filepath)
|
||||
file_stream = open(filepath, "rb")
|
||||
return file_stream, file_size
|
||||
|
||||
# Compress the file on the fly
|
||||
compressed_stream = io.BytesIO()
|
||||
compressor = zstd.ZstdCompressor(level=LOG_COMPRESSION_LEVEL)
|
||||
|
||||
with open(filepath, "rb") as f:
|
||||
compressor.copy_stream(f, compressed_stream)
|
||||
compressed_size = compressed_stream.tell()
|
||||
compressed_stream.seek(0)
|
||||
return compressed_stream, compressed_size
|
||||
|
||||
|
||||
# remove all keys that end in DEPRECATED
|
||||
def strip_deprecated_keys(d):
|
||||
for k in list(d.keys()):
|
||||
if isinstance(k, str):
|
||||
if k.endswith('DEPRECATED'):
|
||||
d.pop(k)
|
||||
elif isinstance(d[k], dict):
|
||||
strip_deprecated_keys(d[k])
|
||||
return d
|
||||
|
||||
|
||||
def run_cmd(cmd: list[str], cwd=None, env=None) -> str:
|
||||
return subprocess.check_output(cmd, encoding='utf8', cwd=cwd, env=env).strip()
|
||||
|
||||
|
||||
def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> str:
|
||||
try:
|
||||
return run_cmd(cmd, cwd=cwd, env=env)
|
||||
except subprocess.CalledProcessError:
|
||||
return default
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
def managed_proc(cmd: list[str], env: dict[str, str]):
|
||||
proc = Popen(cmd, env=env, stdout=PIPE, stderr=PIPE)
|
||||
try:
|
||||
yield proc
|
||||
finally:
|
||||
if proc.poll() is None:
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except TimeoutExpired:
|
||||
proc.kill()
|
||||
|
||||
|
||||
def tabulate(tabular_data, headers=(), tablefmt="simple", floatfmt="g", stralign="left", numalign=None):
|
||||
rows = [list(row) for row in tabular_data]
|
||||
|
||||
def fmt(val):
|
||||
if isinstance(val, str):
|
||||
return val
|
||||
if isinstance(val, (bool, int)):
|
||||
return str(val)
|
||||
try:
|
||||
return format(val, floatfmt)
|
||||
except (TypeError, ValueError):
|
||||
return str(val)
|
||||
|
||||
formatted = [[fmt(c) for c in row] for row in rows]
|
||||
hdrs = [str(h) for h in headers] if headers else None
|
||||
|
||||
ncols = max((len(r) for r in formatted), default=0)
|
||||
if hdrs:
|
||||
ncols = max(ncols, len(hdrs))
|
||||
if ncols == 0:
|
||||
return ""
|
||||
|
||||
for r in formatted:
|
||||
r.extend([""] * (ncols - len(r)))
|
||||
if hdrs:
|
||||
hdrs.extend([""] * (ncols - len(hdrs)))
|
||||
|
||||
widths = [0] * ncols
|
||||
if hdrs:
|
||||
for i in range(ncols):
|
||||
widths[i] = len(hdrs[i])
|
||||
for row in formatted:
|
||||
for i in range(ncols):
|
||||
widths[i] = max(widths[i], max(len(ln) for ln in row[i].split('\n')))
|
||||
|
||||
def _align(s, w):
|
||||
if stralign == "center":
|
||||
return s.center(w)
|
||||
return s.ljust(w)
|
||||
|
||||
if tablefmt == "html":
|
||||
parts = ["<table>"]
|
||||
if hdrs:
|
||||
parts.append("<thead>")
|
||||
parts.append("<tr>" + "".join(f"<th>{h}</th>" for h in hdrs) + "</tr>")
|
||||
parts.append("</thead>")
|
||||
parts.append("<tbody>")
|
||||
for row in formatted:
|
||||
parts.append("<tr>" + "".join(f"<td>{c}</td>" for c in row) + "</tr>")
|
||||
parts.append("</tbody>")
|
||||
parts.append("</table>")
|
||||
return "\n".join(parts)
|
||||
|
||||
if tablefmt == "simple_grid":
|
||||
def _sep(left, mid, right):
|
||||
return left + mid.join("─" * (w + 2) for w in widths) + right
|
||||
|
||||
top, mid_sep, bot = _sep("┌", "┬", "┐"), _sep("├", "┼", "┤"), _sep("└", "┴", "┘")
|
||||
|
||||
def _fmt_row(cells):
|
||||
split = [c.split('\n') for c in cells]
|
||||
nlines = max(len(s) for s in split)
|
||||
for s in split:
|
||||
s.extend([""] * (nlines - len(s)))
|
||||
return ["│" + "│".join(f" {_align(split[i][li], widths[i])} " for i in range(ncols)) + "│" for li in range(nlines)]
|
||||
|
||||
lines = [top]
|
||||
if hdrs:
|
||||
lines.extend(_fmt_row(hdrs))
|
||||
lines.append(mid_sep)
|
||||
for ri, row in enumerate(formatted):
|
||||
lines.extend(_fmt_row(row))
|
||||
lines.append(mid_sep if ri < len(formatted) - 1 else bot)
|
||||
return "\n".join(lines)
|
||||
|
||||
gap = " "
|
||||
lines = []
|
||||
if hdrs:
|
||||
lines.append(gap.join(h.ljust(w) for h, w in zip(hdrs, widths, strict=True)))
|
||||
lines.append(gap.join("-" * w for w in widths))
|
||||
for row in formatted:
|
||||
lines.append(gap.join(_align(row[i], widths[i]) for i in range(ncols)))
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def retry(attempts=3, delay=1.0, ignore_failure=False):
|
||||
def decorator(func):
|
||||
@functools.wraps(func)
|
||||
def wrapper(*args, **kwargs):
|
||||
for _ in range(attempts):
|
||||
try:
|
||||
return func(*args, **kwargs)
|
||||
except Exception:
|
||||
print(f"{func.__name__} failed, trying again")
|
||||
time.sleep(delay)
|
||||
|
||||
if ignore_failure:
|
||||
print(f"{func.__name__} failed after retry")
|
||||
else:
|
||||
raise Exception(f"{func.__name__} failed after retry")
|
||||
return wrapper
|
||||
return decorator
|
||||
@@ -0,0 +1,17 @@
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
|
||||
import os
|
||||
from iqpilot.common.api.base import BaseApi
|
||||
API_HOST = os.getenv('KONN3KT_API_HOST', 'https://api-iqlabs.konn3kt.com')
|
||||
|
||||
class Konn3ktApi(BaseApi):
|
||||
|
||||
def __init__(self, dongle_id):
|
||||
super().__init__(dongle_id, API_HOST)
|
||||
self.user_agent = "konn3kt-device-"
|
||||
|
||||
def get_token(self, expiry_hours=1):
|
||||
return super()._get_token(expiry_hours=expiry_hours)
|
||||
@@ -0,0 +1,226 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import json
|
||||
import jwt
|
||||
import re
|
||||
import secrets
|
||||
from typing import cast
|
||||
from pathlib import Path
|
||||
|
||||
from datetime import datetime, timedelta, UTC
|
||||
from iqpilot.common.api import api_get, get_key_pair
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.spinner import Spinner
|
||||
from iqpilot.system.hardware import HARDWARE, PC
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
UNREGISTERED_DONGLE_ID = "UnregisteredDevice"
|
||||
|
||||
_DONGLE_ID_RE = re.compile(r"^[a-fA-F0-9]{16}$")
|
||||
IMEI_WAIT_TIMEOUT = 15.0
|
||||
|
||||
|
||||
def _read_persist_dongle_id() -> str | None:
|
||||
p = Path(Paths.persist_root()) / "comma" / "dongle_id"
|
||||
try:
|
||||
if not p.is_file():
|
||||
return None
|
||||
s = p.read_text().strip()
|
||||
return s or None
|
||||
except Exception:
|
||||
cloudlog.exception("failed to read persist dongle_id")
|
||||
return None
|
||||
|
||||
|
||||
def get_cached_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str | None:
|
||||
ro = _read_persist_dongle_id()
|
||||
if is_valid_dongle_id(ro):
|
||||
ro = ro.lower()
|
||||
if prefer_readonly and ro:
|
||||
return ro
|
||||
p = Params() if params is None else params
|
||||
v = p.get("DongleId")
|
||||
if v and v != UNREGISTERED_DONGLE_ID:
|
||||
return v.lower() if is_valid_dongle_id(v) else v
|
||||
return ro or None
|
||||
def is_valid_dongle_id(dongle_id: str | None) -> bool:
|
||||
return bool(dongle_id and _DONGLE_ID_RE.fullmatch(dongle_id))
|
||||
def get_or_create_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str:
|
||||
p = Params() if params is None else params
|
||||
dongle_id = get_cached_dongle_id(p, prefer_readonly=prefer_readonly)
|
||||
if dongle_id and dongle_id != UNREGISTERED_DONGLE_ID:
|
||||
return dongle_id
|
||||
dongle_id = secrets.token_hex(8)
|
||||
p.put("DongleId", dongle_id)
|
||||
cloudlog.warning(f"generated new DongleId={dongle_id} (no readonly dongle_id found)")
|
||||
return dongle_id
|
||||
def ensure_dev_pairing_identity(params: Params | None = None, force_reset: bool = False) -> dict[str, str]:
|
||||
p = Params() if params is None else params
|
||||
|
||||
persist_dir = Path(Paths.persist_root()) / "comma"
|
||||
persist_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
dongle_path = persist_dir / "dongle_id"
|
||||
priv_path = persist_dir / "id_rsa"
|
||||
pub_path = persist_dir / "id_rsa.pub"
|
||||
|
||||
if force_reset:
|
||||
for fp in (dongle_path, priv_path, pub_path):
|
||||
try:
|
||||
fp.unlink(missing_ok=True)
|
||||
except Exception:
|
||||
cloudlog.exception(f"failed to remove {fp}")
|
||||
try:
|
||||
(persist_dir / "konn3kt_prime_type").unlink(missing_ok=True)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
p.remove("PrimeType")
|
||||
except Exception:
|
||||
pass
|
||||
forced_dongle = os.getenv("KONN3KT_DEV_DONGLE_ID")
|
||||
dongle_id = forced_dongle.strip().lower() if forced_dongle else None
|
||||
if dongle_id and not is_valid_dongle_id(dongle_id):
|
||||
cloudlog.error("KONN3KT_DEV_DONGLE_ID must be 16 hex chars")
|
||||
dongle_id = None
|
||||
if dongle_id is None:
|
||||
existing = None
|
||||
try:
|
||||
existing = dongle_path.read_text().strip().lower() if dongle_path.is_file() else None
|
||||
except Exception:
|
||||
cloudlog.exception("failed reading existing dev dongle_id")
|
||||
dongle_id = existing if is_valid_dongle_id(existing) else secrets.token_hex(8)
|
||||
try:
|
||||
dongle_path.write_text(dongle_id)
|
||||
except Exception:
|
||||
cloudlog.exception("failed writing dev dongle_id")
|
||||
p.put("DongleId", dongle_id)
|
||||
p.put("HardwareSerial", p.get("HardwareSerial") or f"DEV-{dongle_id}")
|
||||
if force_reset or (not priv_path.is_file()) or (not pub_path.is_file()):
|
||||
try:
|
||||
from cryptography.hazmat.primitives import serialization
|
||||
from cryptography.hazmat.primitives.asymmetric import rsa
|
||||
key = rsa.generate_private_key(public_exponent=65537, key_size=2048)
|
||||
priv_bytes = key.private_bytes(
|
||||
encoding=serialization.Encoding.PEM,
|
||||
format=serialization.PrivateFormat.TraditionalOpenSSL,
|
||||
encryption_algorithm=serialization.NoEncryption(),
|
||||
)
|
||||
pub_bytes = key.public_key().public_bytes(
|
||||
encoding=serialization.Encoding.PEM,
|
||||
format=serialization.PublicFormat.SubjectPublicKeyInfo,
|
||||
)
|
||||
priv_path.write_bytes(priv_bytes)
|
||||
pub_path.write_bytes(pub_bytes)
|
||||
except Exception:
|
||||
cloudlog.exception("failed generating dev RSA keys")
|
||||
raise
|
||||
return {
|
||||
"dongle_id": dongle_id,
|
||||
"serial": p.get("HardwareSerial") or f"DEV-{dongle_id}",
|
||||
"persist_dir": str(persist_dir),
|
||||
}
|
||||
def is_registered_device() -> bool:
|
||||
dongle = Params().get("DongleId")
|
||||
return dongle not in (None, UNREGISTERED_DONGLE_ID)
|
||||
|
||||
|
||||
def _normalize_imei(value: str | None) -> str:
|
||||
return value or ""
|
||||
|
||||
|
||||
def get_registration_identifiers(wait_timeout: float = IMEI_WAIT_TIMEOUT, show_spinner: bool = False) -> tuple[str, str, str]:
|
||||
serial = HARDWARE.get_serial()
|
||||
spinner = Spinner() if show_spinner else None
|
||||
start_time = time.monotonic()
|
||||
imei1: str | None = None
|
||||
imei2: str | None = None
|
||||
|
||||
while time.monotonic() - start_time < wait_timeout:
|
||||
try:
|
||||
imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
|
||||
if imei1 or imei2:
|
||||
break
|
||||
except RuntimeError as e:
|
||||
if "no modems" in str(e).lower():
|
||||
cloudlog.warning("No cellular modem available, proceeding without IMEI")
|
||||
break
|
||||
cloudlog.exception("Error getting imei, trying again...")
|
||||
except Exception:
|
||||
cloudlog.exception("Error getting imei, trying again...")
|
||||
time.sleep(1)
|
||||
|
||||
imei1 = _normalize_imei(imei1)
|
||||
imei2 = _normalize_imei(imei2)
|
||||
|
||||
if not imei1 and not imei2:
|
||||
cloudlog.warning(f"proceeding with serial-only registration for serial={serial}")
|
||||
if spinner is not None:
|
||||
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})")
|
||||
spinner.close()
|
||||
|
||||
return serial, imei1, imei2
|
||||
|
||||
|
||||
def register(show_spinner=False) -> str | None:
|
||||
params = Params()
|
||||
|
||||
dongle_id: str | None = get_cached_dongle_id(params, prefer_readonly=True)
|
||||
if dongle_id in ("", UNREGISTERED_DONGLE_ID):
|
||||
dongle_id = None
|
||||
|
||||
jwt_algo, private_key, public_key = get_key_pair()
|
||||
|
||||
if not public_key:
|
||||
dongle_id = UNREGISTERED_DONGLE_ID
|
||||
cloudlog.warning("missing public key")
|
||||
elif dongle_id is None:
|
||||
if show_spinner:
|
||||
spinner = Spinner()
|
||||
spinner.update("registering device")
|
||||
|
||||
serial, imei1, imei2 = get_registration_identifiers(wait_timeout=IMEI_WAIT_TIMEOUT, show_spinner=False)
|
||||
|
||||
backoff = 0
|
||||
start_time = time.monotonic()
|
||||
while True:
|
||||
try:
|
||||
register_token = jwt.encode({'register': True, 'exp': datetime.now(UTC).replace(tzinfo=None) + timedelta(hours=1)},
|
||||
cast(str, private_key), algorithm=jwt_algo)
|
||||
cloudlog.info("getting pilotauth")
|
||||
cloudlog.info("getting pilotauth")
|
||||
resp = api_get("v2/pilotauth/", method='POST', timeout=15,
|
||||
imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token)
|
||||
|
||||
if resp.status_code in (400, 402, 403):
|
||||
cloudlog.info(f"Unable to register device, got {resp.status_code}")
|
||||
dongle_id = UNREGISTERED_DONGLE_ID
|
||||
else:
|
||||
dongleauth = json.loads(resp.text)
|
||||
dongle_id = dongleauth["dongle_id"]
|
||||
break
|
||||
except Exception:
|
||||
cloudlog.exception("failed to authenticate")
|
||||
backoff = min(backoff + 1, 15)
|
||||
time.sleep(backoff)
|
||||
|
||||
if time.monotonic() - start_time > 60 and show_spinner:
|
||||
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})")
|
||||
return UNREGISTERED_DONGLE_ID
|
||||
|
||||
if show_spinner:
|
||||
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})")
|
||||
spinner.close()
|
||||
|
||||
if dongle_id:
|
||||
params.put("DongleId", dongle_id)
|
||||
from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
set_offroad_alert("Offroad_UnregisteredHardware", False)
|
||||
return dongle_id
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(register())
|
||||
+286
@@ -0,0 +1,286 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from iqpilot.cereal import car, custom, log
|
||||
|
||||
from iqpilot.common.constants import CV
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import DT_MDL
|
||||
from iqpilot.selfdrive.controls.lib.helpers.lane_change import (
|
||||
IQLaneSwapController,
|
||||
AutoLaneChangeMode,
|
||||
NavExitLaneChangeController,
|
||||
)
|
||||
from iqpilot.selfdrive.controls.lib.helpers.lane_turn import IQNavTurnController
|
||||
|
||||
LaneChangeState = log.LaneChangeState
|
||||
LaneChangeDirection = log.LaneChangeDirection
|
||||
TurnDirection = custom.IQTurnSignalDirection
|
||||
NavManeuverPhase = custom.IQNavState.ManeuverPhase
|
||||
|
||||
LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
|
||||
LANE_CHANGE_TIME_MAX = 10.0
|
||||
TURN_DESIRE_STOP_HOLD_TIME = 3.4
|
||||
TURN_DESIRE_STOP_GAP_TIME = 0.2
|
||||
TURN_DESIRE_STOP_CYCLE_TIME = TURN_DESIRE_STOP_HOLD_TIME + TURN_DESIRE_STOP_GAP_TIME
|
||||
TURN_DESIRE_CYCLE_SPEED_MAX = 5 * CV.MPH_TO_MS
|
||||
TURN_DESIRE_COMMIT_YAW_RATE = 0.08
|
||||
|
||||
_LANE_CHANGE_DESIRES = {
|
||||
(LaneChangeDirection.none, LaneChangeState.off): log.Desire.none,
|
||||
(LaneChangeDirection.none, LaneChangeState.preLaneChange): log.Desire.none,
|
||||
(LaneChangeDirection.none, LaneChangeState.laneChangeStarting): log.Desire.none,
|
||||
(LaneChangeDirection.none, LaneChangeState.laneChangeFinishing): log.Desire.none,
|
||||
(LaneChangeDirection.left, LaneChangeState.off): log.Desire.none,
|
||||
(LaneChangeDirection.left, LaneChangeState.preLaneChange): log.Desire.none,
|
||||
(LaneChangeDirection.left, LaneChangeState.laneChangeStarting): log.Desire.laneChangeLeft,
|
||||
(LaneChangeDirection.left, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeLeft,
|
||||
(LaneChangeDirection.right, LaneChangeState.off): log.Desire.none,
|
||||
(LaneChangeDirection.right, LaneChangeState.preLaneChange): log.Desire.none,
|
||||
(LaneChangeDirection.right, LaneChangeState.laneChangeStarting): log.Desire.laneChangeRight,
|
||||
(LaneChangeDirection.right, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeRight,
|
||||
}
|
||||
|
||||
_TURN_DESIRES = {
|
||||
TurnDirection.none: log.Desire.none,
|
||||
TurnDirection.turnLeft: log.Desire.turnLeft,
|
||||
TurnDirection.turnRight: log.Desire.turnRight,
|
||||
}
|
||||
|
||||
_STOP_CYCLING_TURN_DESIRES = {
|
||||
log.Desire.turnLeft,
|
||||
log.Desire.turnRight,
|
||||
}
|
||||
|
||||
|
||||
def turn_desire(turn_direction) -> log.Desire:
|
||||
return _TURN_DESIRES[getattr(turn_direction, "raw", turn_direction)]
|
||||
|
||||
|
||||
def _direction_from_blinkers(carstate) -> int:
|
||||
if carstate.leftBlinker:
|
||||
return LaneChangeDirection.left
|
||||
if carstate.rightBlinker:
|
||||
return LaneChangeDirection.right
|
||||
return LaneChangeDirection.none
|
||||
|
||||
|
||||
def _steering_nudge_matches(carstate, direction: int) -> bool:
|
||||
if not carstate.steeringPressed:
|
||||
return False
|
||||
return (
|
||||
(direction == LaneChangeDirection.left and carstate.steeringTorque > 0) or
|
||||
(direction == LaneChangeDirection.right and carstate.steeringTorque < 0)
|
||||
)
|
||||
|
||||
|
||||
def _blindspot_matches(carstate, direction: int) -> bool:
|
||||
return (
|
||||
(direction == LaneChangeDirection.left and carstate.leftBlindspot) or
|
||||
(direction == LaneChangeDirection.right and carstate.rightBlindspot)
|
||||
)
|
||||
|
||||
|
||||
def _read_enable_bsm() -> bool:
|
||||
try:
|
||||
with car.CarParams.from_bytes(Params().get("CarParams")) as cp:
|
||||
return bool(cp.enableBsm)
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
|
||||
class DesireHelper:
|
||||
def __init__(self):
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
self.lane_change_ll_prob = 1.0
|
||||
self.prev_one_blinker = False
|
||||
self.prev_nav_exit_active = False
|
||||
self.desire = log.Desire.none
|
||||
|
||||
self.alc = IQLaneSwapController(self)
|
||||
self.lane_turn_controller = IQNavTurnController(self)
|
||||
self.nav_exit = NavExitLaneChangeController(_read_enable_bsm())
|
||||
self.lane_turn_direction = TurnDirection.none
|
||||
self.nav_turn_direction = TurnDirection.none
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
self.turn_desire_cycle_input = log.Desire.none
|
||||
self.turn_desire_committed = False
|
||||
|
||||
@staticmethod
|
||||
def get_lane_change_direction(carstate):
|
||||
return _direction_from_blinkers(carstate)
|
||||
|
||||
@staticmethod
|
||||
def _nav_turn_desire(nav_state):
|
||||
if nav_state is None or not getattr(nav_state, "active", False):
|
||||
return TurnDirection.none
|
||||
if getattr(nav_state, "maneuverPhase", NavManeuverPhase.none) != NavManeuverPhase.turnActive:
|
||||
return TurnDirection.none
|
||||
if not getattr(nav_state, "shouldSendTurnDesire", False):
|
||||
return TurnDirection.none
|
||||
return getattr(nav_state, "turnDesireDirection", TurnDirection.none)
|
||||
|
||||
def _clear_lane_change(self) -> None:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
|
||||
def _refresh_turn_overrides(self, carstate, nav_state) -> bool:
|
||||
speed_mps = carstate.vEgo
|
||||
self.lane_turn_controller.update_params()
|
||||
self.lane_turn_controller.update_lane_turn(
|
||||
blindspot_left=carstate.leftBlindspot,
|
||||
blindspot_right=carstate.rightBlindspot,
|
||||
left_blinker=carstate.leftBlinker,
|
||||
right_blinker=carstate.rightBlinker,
|
||||
v_ego=speed_mps,
|
||||
)
|
||||
self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
|
||||
self.nav_turn_direction = self._nav_turn_desire(nav_state)
|
||||
|
||||
self.nav_exit.update_params()
|
||||
self.nav_exit.update(nav_state, carstate)
|
||||
return bool(self.nav_exit.active)
|
||||
|
||||
def _reset_required(self, lateral_active: bool, nav_exit_active: bool) -> bool:
|
||||
timed_out = self.lane_change_timer > LANE_CHANGE_TIME_MAX
|
||||
feature_disabled = self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF and not nav_exit_active
|
||||
return (not lateral_active) or timed_out or feature_disabled
|
||||
|
||||
def _begin_from_idle(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
|
||||
if below_speed:
|
||||
return
|
||||
if one_blinker and not self.prev_one_blinker:
|
||||
self.lane_change_state = LaneChangeState.preLaneChange
|
||||
self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
|
||||
self.lane_change_ll_prob = 1.0
|
||||
return
|
||||
if nav_exit_active and not self.prev_nav_exit_active:
|
||||
self.lane_change_state = LaneChangeState.preLaneChange
|
||||
self.lane_change_direction = self.nav_exit.direction
|
||||
self.lane_change_ll_prob = 1.0
|
||||
|
||||
def _refresh_requested_direction(self, one_blinker: bool, nav_exit_active: bool) -> None:
|
||||
if one_blinker:
|
||||
self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
|
||||
elif nav_exit_active:
|
||||
self.lane_change_direction = self.nav_exit.direction
|
||||
|
||||
def _step_pre_lane_change(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
|
||||
self._refresh_requested_direction(one_blinker, nav_exit_active)
|
||||
blindspot_detected = _blindspot_matches(self._last_carstate, self.lane_change_direction)
|
||||
steering_ready = _steering_nudge_matches(self._last_carstate, self.lane_change_direction)
|
||||
nav_auto_start = nav_exit_active and self.nav_exit.auto_allowed
|
||||
|
||||
self.alc.update_lane_change(blindspot_detected=blindspot_detected, brake_pressed=self._last_carstate.brakePressed)
|
||||
allowed_to_launch = steering_ready or self.alc.auto_lane_change_allowed or nav_auto_start
|
||||
|
||||
if (not (one_blinker or nav_exit_active)) or below_speed:
|
||||
self._clear_lane_change()
|
||||
elif allowed_to_launch and not blindspot_detected:
|
||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||
|
||||
def _step_lane_change_starting(self, lane_change_prob: float) -> None:
|
||||
self.lane_change_ll_prob = max(self.lane_change_ll_prob - (2.0 * DT_MDL), 0.0)
|
||||
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
|
||||
self.lane_change_state = LaneChangeState.laneChangeFinishing
|
||||
|
||||
def _step_lane_change_finishing(self, one_blinker: bool) -> None:
|
||||
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
|
||||
if self.lane_change_ll_prob <= 0.99:
|
||||
return
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_state = LaneChangeState.preLaneChange if one_blinker else LaneChangeState.off
|
||||
|
||||
def _advance_lane_change_machine(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool, lane_change_prob: float) -> None:
|
||||
if self.lane_change_state == LaneChangeState.off:
|
||||
self._begin_from_idle(one_blinker, nav_exit_active, below_speed)
|
||||
return
|
||||
if self.lane_change_state == LaneChangeState.preLaneChange:
|
||||
self._step_pre_lane_change(one_blinker, nav_exit_active, below_speed)
|
||||
return
|
||||
if self.lane_change_state == LaneChangeState.laneChangeStarting:
|
||||
self._step_lane_change_starting(lane_change_prob)
|
||||
return
|
||||
if self.lane_change_state == LaneChangeState.laneChangeFinishing:
|
||||
self._step_lane_change_finishing(one_blinker)
|
||||
|
||||
def _update_timer(self) -> None:
|
||||
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
|
||||
self.lane_change_timer = 0.0
|
||||
else:
|
||||
self.lane_change_timer += DT_MDL
|
||||
|
||||
def _clear_turn_desire_stop_cycle(self) -> None:
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
self.turn_desire_cycle_input = log.Desire.none
|
||||
self.turn_desire_committed = False
|
||||
|
||||
def _cycle_turn_desire_when_stopped(self, desired_output: log.Desire) -> log.Desire:
|
||||
if desired_output not in _STOP_CYCLING_TURN_DESIRES:
|
||||
self._clear_turn_desire_stop_cycle()
|
||||
return desired_output
|
||||
|
||||
if desired_output != self.turn_desire_cycle_input:
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
self.turn_desire_cycle_input = desired_output
|
||||
self.turn_desire_committed = False
|
||||
|
||||
if abs(getattr(self._last_carstate, "yawRate", 0.0)) >= TURN_DESIRE_COMMIT_YAW_RATE:
|
||||
self.turn_desire_committed = True
|
||||
|
||||
if self.turn_desire_committed:
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
return desired_output
|
||||
|
||||
if self._last_carstate.vEgo > TURN_DESIRE_CYCLE_SPEED_MAX:
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
self.turn_desire_stop_active = False
|
||||
return desired_output
|
||||
|
||||
if not self.turn_desire_stop_active:
|
||||
self.turn_desire_stop_active = True
|
||||
self.turn_desire_stop_timer = 0.0
|
||||
|
||||
cycle_phase = self.turn_desire_stop_timer % TURN_DESIRE_STOP_CYCLE_TIME
|
||||
self.turn_desire_stop_timer += DT_MDL
|
||||
if cycle_phase >= TURN_DESIRE_STOP_HOLD_TIME:
|
||||
return log.Desire.none
|
||||
return desired_output
|
||||
|
||||
def _pick_desire_output(self) -> None:
|
||||
desired_output = log.Desire.none
|
||||
if self.nav_turn_direction != TurnDirection.none:
|
||||
desired_output = turn_desire(self.nav_turn_direction)
|
||||
elif self.lane_turn_direction != TurnDirection.none:
|
||||
desired_output = turn_desire(self.lane_turn_direction)
|
||||
else:
|
||||
desired_output = _LANE_CHANGE_DESIRES[(self.lane_change_direction, self.lane_change_state)]
|
||||
|
||||
self.desire = self._cycle_turn_desire_when_stopped(desired_output)
|
||||
|
||||
def update(self, carstate, lateral_active, lane_change_prob, nav_state=None, modeldata=None, radar_state=None):
|
||||
self._last_carstate = carstate
|
||||
one_blinker = carstate.leftBlinker != carstate.rightBlinker
|
||||
below_speed = carstate.vEgo < LANE_CHANGE_SPEED_MIN
|
||||
nav_exit_active = self._refresh_turn_overrides(carstate, nav_state)
|
||||
|
||||
self.alc.update_params()
|
||||
if self._reset_required(lateral_active, nav_exit_active):
|
||||
self._clear_lane_change()
|
||||
else:
|
||||
self._advance_lane_change_machine(one_blinker, nav_exit_active, below_speed, lane_change_prob)
|
||||
|
||||
self._update_timer()
|
||||
self.prev_one_blinker = one_blinker and lateral_active
|
||||
self.prev_nav_exit_active = nav_exit_active
|
||||
self.alc.update_state()
|
||||
self._pick_desire_output()
|
||||
+80
@@ -0,0 +1,80 @@
|
||||
import numpy as np
|
||||
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
||||
from iqpilot.common.realtime import DT_CTRL, DT_MDL
|
||||
|
||||
MIN_SPEED = 1.0
|
||||
CONTROL_N = 17
|
||||
CAR_ROTATION_RADIUS = 0.0
|
||||
# This is a turn radius smaller than most cars can achieve
|
||||
MAX_CURVATURE = 0.4
|
||||
MAX_VEL_ERR = 5.0 # m/s
|
||||
|
||||
MAX_LATERAL_JERK = 5.0 # m/s^3
|
||||
MAX_LATERAL_ACCEL_NO_ROLL = 5.0 # m/s^2
|
||||
MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE = 5.0 # m/s^2
|
||||
DEFAULT_STOPPING_SPEED = 0.25 # m/s
|
||||
|
||||
|
||||
def should_stop(v_ego: float, a_target: float, stopping_speed: float = DEFAULT_STOPPING_SPEED) -> bool:
|
||||
return bool(v_ego < stopping_speed and a_target < 0.1)
|
||||
|
||||
|
||||
def clamp(val, min_val, max_val):
|
||||
clamped_val = float(np.clip(val, min_val, max_val))
|
||||
return clamped_val, clamped_val != val
|
||||
|
||||
def smooth_value(val, prev_val, tau, dt=DT_MDL):
|
||||
alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
|
||||
return alpha * val + (1 - alpha) * prev_val
|
||||
|
||||
# "Model smoothing": when the policy's own predicted uncertainty (plan_stds) for the
|
||||
# 1s-ahead lateral position spikes, temporarily lengthen the desiredCurvature smoothing
|
||||
# time constant so a noisy/uncertain model output doesn't jerk the wheel.
|
||||
MODEL_SMOOTHING_STD_LOW = 0.15 # m, plan y_std at 1s below which no extra smoothing is added
|
||||
MODEL_SMOOTHING_STD_HIGH = 0.25 # m, plan y_std at 1s at/above which the full max_extra_seconds is added
|
||||
MODEL_SMOOTHING_MAX_TOTAL_SEC = 0.60 # hard ceiling on base + dynamic lat smoothing seconds
|
||||
|
||||
def dynamic_lat_smooth_extra_seconds(y_std_1s: float, max_extra_seconds: float) -> float:
|
||||
if max_extra_seconds <= 0.0:
|
||||
return 0.0
|
||||
return float(np.interp(y_std_1s, [MODEL_SMOOTHING_STD_LOW, MODEL_SMOOTHING_STD_HIGH], [0.0, max_extra_seconds]))
|
||||
|
||||
def clip_curvature(v_ego, prev_curvature, new_curvature, roll, override=False) -> tuple[float, bool]:
|
||||
# This function respects ISO lateral jerk and acceleration limits + a max curvature
|
||||
v_ego = max(v_ego, MIN_SPEED)
|
||||
max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
|
||||
new_curvature = np.clip(new_curvature,
|
||||
prev_curvature - max_curvature_rate * DT_CTRL,
|
||||
prev_curvature + max_curvature_rate * DT_CTRL)
|
||||
|
||||
max_lat_accel_no_roll = MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE if override else MAX_LATERAL_ACCEL_NO_ROLL
|
||||
roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
|
||||
max_lat_accel = max_lat_accel_no_roll + roll_compensation
|
||||
min_lat_accel = -max_lat_accel_no_roll + roll_compensation
|
||||
new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
|
||||
|
||||
new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
|
||||
return float(new_curvature), limited_accel or limited_max_curv
|
||||
|
||||
|
||||
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, stopping_speed=DEFAULT_STOPPING_SPEED):
|
||||
if len(speeds) == len(t_idxs):
|
||||
v_now = speeds[0]
|
||||
a_now = accels[0]
|
||||
v_target = np.interp(action_t, t_idxs, speeds)
|
||||
a_target = 2 * (v_target - v_now) / (action_t) - a_now
|
||||
else:
|
||||
v_now = 0.0
|
||||
v_target = 0.0
|
||||
a_target = 0.0
|
||||
return a_target, should_stop(v_now, a_target, stopping_speed)
|
||||
|
||||
def curv_from_psis(psi_target, psi_rate, vego, action_t):
|
||||
vego = np.clip(vego, MIN_SPEED, np.inf)
|
||||
curv_from_psi = psi_target / (vego * action_t)
|
||||
return 2*curv_from_psi - psi_rate / vego
|
||||
|
||||
def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
|
||||
psi_target = np.interp(action_t, t_idxs, yaws)
|
||||
psi_rate = yaw_rates[0]
|
||||
return curv_from_psis(psi_target, psi_rate, vego, action_t)
|
||||
+293
@@ -0,0 +1,293 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from iqpilot.cereal import custom, log
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import DT_MDL
|
||||
|
||||
NAV_EXIT_COMMIT_DISTANCE = 500.0 # m before a route exit to begin moving into the exit lane
|
||||
_ManeuverType = custom.IQNavState.ManeuverType
|
||||
_NavDirection = custom.NavDirection
|
||||
|
||||
|
||||
class LaneSwapPreset:
|
||||
DISABLED = -1
|
||||
STEERING_NUDGE = 0
|
||||
DIRECT = 1
|
||||
DELAY_HALF = 2
|
||||
DELAY_ONE = 3
|
||||
DELAY_TWO = 4
|
||||
DELAY_THREE = 5
|
||||
OFF = DISABLED
|
||||
NUDGE = STEERING_NUDGE
|
||||
NUDGELESS = DIRECT
|
||||
HALF_SECOND = DELAY_HALF
|
||||
ONE_SECOND = DELAY_ONE
|
||||
TWO_SECONDS = DELAY_TWO
|
||||
THREE_SECONDS = DELAY_THREE
|
||||
|
||||
|
||||
PRESET_SECONDS = {
|
||||
LaneSwapPreset.DISABLED: 0.0,
|
||||
LaneSwapPreset.STEERING_NUDGE: 0.0,
|
||||
LaneSwapPreset.DIRECT: 0.05,
|
||||
LaneSwapPreset.DELAY_HALF: 0.5,
|
||||
LaneSwapPreset.DELAY_ONE: 1.0,
|
||||
LaneSwapPreset.DELAY_TWO: 2.0,
|
||||
LaneSwapPreset.DELAY_THREE: 3.0,
|
||||
}
|
||||
|
||||
LANE_SWAP_SECONDS = dict(PRESET_SECONDS)
|
||||
BLINDSPOT_WAIT_OFFSET = -1
|
||||
|
||||
|
||||
class LaneSwapEngine:
|
||||
def __init__(self, desire_hub):
|
||||
self._hub = desire_hub
|
||||
self._kv = Params()
|
||||
self._mem = {
|
||||
"sec": 0.0,
|
||||
"tick": 0,
|
||||
"gate": 0.0,
|
||||
"preset": self._kv.get("IQLaneChangeTimer", return_default=True),
|
||||
"bsm_hold": False,
|
||||
"braked": False,
|
||||
"ready": False,
|
||||
"used": False,
|
||||
}
|
||||
self.reload_setup()
|
||||
|
||||
def _pull_setup(self) -> None:
|
||||
self._mem["bsm_hold"] = self._kv.get_bool("IQLaneChangeBsmDelay")
|
||||
self._mem["preset"] = self._kv.get("IQLaneChangeTimer", return_default=True)
|
||||
|
||||
def _idle_phase(self) -> bool:
|
||||
return (
|
||||
self._hub.lane_change_state == log.LaneChangeState.off and
|
||||
self._hub.lane_change_direction == log.LaneChangeDirection.none
|
||||
)
|
||||
|
||||
def _seconds_for_preset(self) -> float:
|
||||
picked = self._mem["preset"]
|
||||
return PRESET_SECONDS.get(picked, PRESET_SECONDS[LaneSwapPreset.STEERING_NUDGE])
|
||||
|
||||
def _auto_preset_active(self) -> bool:
|
||||
picked = self._mem["preset"]
|
||||
return picked not in (LaneSwapPreset.DISABLED, LaneSwapPreset.STEERING_NUDGE)
|
||||
|
||||
def _advance_clock(self, blindspot_now: bool) -> None:
|
||||
wait_s = self._seconds_for_preset()
|
||||
self._mem["gate"] = wait_s
|
||||
self._mem["sec"] += DT_MDL
|
||||
if self._mem["bsm_hold"] and blindspot_now and wait_s > 0.0:
|
||||
if wait_s == PRESET_SECONDS[LaneSwapPreset.DIRECT]:
|
||||
self._mem["sec"] = BLINDSPOT_WAIT_OFFSET
|
||||
else:
|
||||
self._mem["sec"] = wait_s + BLINDSPOT_WAIT_OFFSET
|
||||
|
||||
def _ready_to_fire(self) -> bool:
|
||||
return (
|
||||
self._auto_preset_active() and
|
||||
(not self._mem["braked"]) and
|
||||
(not self._mem["used"]) and
|
||||
(self._mem["sec"] > self._mem["gate"])
|
||||
)
|
||||
|
||||
def reload_setup(self) -> None:
|
||||
self._pull_setup()
|
||||
|
||||
def heartbeat(self) -> None:
|
||||
if (self._mem["tick"] % 50) == 0:
|
||||
self._pull_setup()
|
||||
self._mem["tick"] += 1
|
||||
|
||||
def sample(self, blindspot_now: bool = False, brake_now: bool = False, **legacy) -> None:
|
||||
blindspot_now = bool(legacy.get("blindspot_detected", blindspot_now))
|
||||
brake_now = bool(legacy.get("brake_pressed", brake_now))
|
||||
self._mem["braked"] = self._mem["braked"] or brake_now
|
||||
self._advance_clock(blindspot_now)
|
||||
self._mem["ready"] = self._ready_to_fire()
|
||||
|
||||
def finalize(self) -> None:
|
||||
started = self._hub.lane_change_state == log.LaneChangeState.laneChangeStarting
|
||||
self._mem["used"] = self._mem["used"] or started
|
||||
if self._idle_phase():
|
||||
self._mem["sec"] = 0.0
|
||||
self._mem["braked"] = False
|
||||
self._mem["used"] = False
|
||||
|
||||
@property
|
||||
def ready(self):
|
||||
return self._mem["ready"]
|
||||
|
||||
@property
|
||||
def delay(self):
|
||||
return self._mem["gate"]
|
||||
|
||||
@property
|
||||
def elapsed(self):
|
||||
return self._mem["sec"]
|
||||
|
||||
@property
|
||||
def preset(self):
|
||||
return self._mem["preset"]
|
||||
|
||||
@preset.setter
|
||||
def preset(self, value):
|
||||
self._mem["preset"] = value
|
||||
|
||||
@property
|
||||
def bsm_hold(self):
|
||||
return self._mem["bsm_hold"]
|
||||
|
||||
@bsm_hold.setter
|
||||
def bsm_hold(self, value):
|
||||
self._mem["bsm_hold"] = bool(value)
|
||||
|
||||
@property
|
||||
def braked(self):
|
||||
return self._mem["braked"]
|
||||
|
||||
@braked.setter
|
||||
def braked(self, value):
|
||||
self._mem["braked"] = bool(value)
|
||||
|
||||
@property
|
||||
def used(self):
|
||||
return self._mem["used"]
|
||||
|
||||
@used.setter
|
||||
def used(self, value):
|
||||
self._mem["used"] = bool(value)
|
||||
|
||||
|
||||
class NavExitLaneChangeController:
|
||||
def __init__(self, enable_bsm: bool):
|
||||
self._params = Params()
|
||||
self._enable_bsm = bool(enable_bsm)
|
||||
self.enabled = self._read_enabled()
|
||||
self._tick = 0
|
||||
self.active = False
|
||||
self.direction = log.LaneChangeDirection.none
|
||||
self.auto_allowed = False
|
||||
|
||||
def _read_enabled(self) -> bool:
|
||||
try:
|
||||
return self._params.get_bool("NavExitLaneChange")
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
def update_params(self) -> None:
|
||||
if self._tick % 50 == 0:
|
||||
self.enabled = self._read_enabled()
|
||||
self._tick += 1
|
||||
|
||||
@staticmethod
|
||||
def _raw(value):
|
||||
return getattr(value, "raw", value)
|
||||
|
||||
def update(self, nav_state, carstate) -> None:
|
||||
self.active = False
|
||||
self.direction = log.LaneChangeDirection.none
|
||||
self.auto_allowed = False
|
||||
|
||||
if not self.enabled or nav_state is None or not getattr(nav_state, "active", False):
|
||||
return
|
||||
if not getattr(nav_state, "nextManeuverValid", False):
|
||||
return
|
||||
if self._raw(getattr(nav_state, "nextManeuverType", _ManeuverType.none)) != int(_ManeuverType.exit):
|
||||
return
|
||||
distance = float(getattr(nav_state, "nextManeuverDistance", 0.0))
|
||||
if not 0.0 < distance <= NAV_EXIT_COMMIT_DISTANCE:
|
||||
return
|
||||
|
||||
direction = self._raw(getattr(nav_state, "nextManeuverDirection", _NavDirection.none))
|
||||
if direction == int(_NavDirection.left):
|
||||
self.direction = log.LaneChangeDirection.left
|
||||
elif direction == int(_NavDirection.right):
|
||||
self.direction = log.LaneChangeDirection.right
|
||||
else:
|
||||
return
|
||||
|
||||
self.active = True
|
||||
blindspot = carstate.leftBlindspot if self.direction == log.LaneChangeDirection.left else carstate.rightBlindspot
|
||||
self.auto_allowed = (not blindspot) if self._enable_bsm else False
|
||||
|
||||
|
||||
AutoLaneChangeMode = LaneSwapPreset
|
||||
AUTO_LANE_CHANGE_TIMER = LANE_SWAP_SECONDS
|
||||
ONE_SECOND_DELAY = BLINDSPOT_WAIT_OFFSET
|
||||
|
||||
|
||||
class IQLaneSwapController(LaneSwapEngine):
|
||||
def __init__(self, desire_helper):
|
||||
super().__init__(desire_helper)
|
||||
|
||||
def reset(self) -> None:
|
||||
self.finalize()
|
||||
|
||||
def update_params(self) -> None:
|
||||
self.heartbeat()
|
||||
|
||||
def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
|
||||
self.sample(blindspot_now=blindspot_detected, brake_now=brake_pressed)
|
||||
|
||||
def update_state(self) -> None:
|
||||
self.finalize()
|
||||
|
||||
@property
|
||||
def lane_change_wait_timer(self):
|
||||
return self.elapsed
|
||||
|
||||
@lane_change_wait_timer.setter
|
||||
def lane_change_wait_timer(self, value):
|
||||
self._mem["sec"] = float(value)
|
||||
|
||||
@property
|
||||
def lane_change_delay(self):
|
||||
return self.delay
|
||||
|
||||
@lane_change_delay.setter
|
||||
def lane_change_delay(self, value):
|
||||
self._mem["gate"] = float(value)
|
||||
|
||||
@property
|
||||
def lane_change_set_timer(self):
|
||||
return self.preset
|
||||
|
||||
@lane_change_set_timer.setter
|
||||
def lane_change_set_timer(self, value):
|
||||
self.preset = value
|
||||
|
||||
@property
|
||||
def lane_change_bsm_delay(self):
|
||||
return self.bsm_hold
|
||||
|
||||
@lane_change_bsm_delay.setter
|
||||
def lane_change_bsm_delay(self, value):
|
||||
self.bsm_hold = value
|
||||
|
||||
@property
|
||||
def prev_brake_pressed(self):
|
||||
return self.braked
|
||||
|
||||
@prev_brake_pressed.setter
|
||||
def prev_brake_pressed(self, value):
|
||||
self.braked = value
|
||||
|
||||
@property
|
||||
def auto_lane_change_allowed(self):
|
||||
return self.ready
|
||||
|
||||
@auto_lane_change_allowed.setter
|
||||
def auto_lane_change_allowed(self, value):
|
||||
self._mem["ready"] = bool(value)
|
||||
|
||||
@property
|
||||
def prev_lane_change(self):
|
||||
return self.used
|
||||
|
||||
@prev_lane_change.setter
|
||||
def prev_lane_change(self, value):
|
||||
self.used = value
|
||||
+157
@@ -0,0 +1,157 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
from iqpilot.cereal import custom
|
||||
|
||||
from iqpilot.common.constants import CV
|
||||
from iqpilot.common.params import Params
|
||||
|
||||
TurnDirection = custom.IQTurnSignalDirection
|
||||
|
||||
TURN_TRIGGER_MPS = 20 * CV.MPH_TO_MS
|
||||
TURN_SPEED_GATE_MPS = TURN_TRIGGER_MPS
|
||||
LANE_CHANGE_SPEED_MIN = TURN_SPEED_GATE_MPS
|
||||
|
||||
|
||||
@dataclass
|
||||
class _TurnGateState:
|
||||
active: bool = False
|
||||
speed_limit_mps: float = TURN_TRIGGER_MPS
|
||||
outcome: int = TurnDirection.none
|
||||
refresh_tick: int = 0
|
||||
|
||||
|
||||
def _mph_param_to_mps(raw_value) -> float:
|
||||
try:
|
||||
return float(raw_value) * CV.MPH_TO_MS
|
||||
except (TypeError, ValueError):
|
||||
return TURN_TRIGGER_MPS
|
||||
|
||||
|
||||
def _resolve_signal_choice(speed_mps: float,
|
||||
speed_limit_mps: float,
|
||||
left_signal: bool,
|
||||
right_signal: bool,
|
||||
left_blocked: bool,
|
||||
right_blocked: bool) -> int:
|
||||
if speed_mps >= speed_limit_mps:
|
||||
return TurnDirection.none
|
||||
if left_signal and not right_signal and not left_blocked:
|
||||
return TurnDirection.turnLeft
|
||||
if right_signal and not left_signal and not right_blocked:
|
||||
return TurnDirection.turnRight
|
||||
return TurnDirection.none
|
||||
|
||||
|
||||
class TurnSignalPlanner:
|
||||
_REFRESH_STRIDE = 50
|
||||
|
||||
def __init__(self, desire_hub):
|
||||
self._desire_hub = desire_hub
|
||||
self._params = Params()
|
||||
self._state = _TurnGateState()
|
||||
self.reload_setup()
|
||||
|
||||
def _refresh_from_params(self) -> None:
|
||||
requested_gate = _mph_param_to_mps(self._params.get("IQLaneTurnValue", return_default=True))
|
||||
self._state.active = self._params.get_bool("IQLaneTurnDesire")
|
||||
self._state.speed_limit_mps = min(TURN_TRIGGER_MPS, requested_gate)
|
||||
|
||||
def _consume_legacy_kwargs(self, **legacy) -> tuple[bool, bool, bool, bool, float]:
|
||||
return (
|
||||
bool(legacy.get("blindspot_left", False)),
|
||||
bool(legacy.get("blindspot_right", False)),
|
||||
bool(legacy.get("left_blinker", False)),
|
||||
bool(legacy.get("right_blinker", False)),
|
||||
float(legacy.get("v_ego", 0.0)),
|
||||
)
|
||||
|
||||
def reload_setup(self):
|
||||
self._refresh_from_params()
|
||||
|
||||
def heartbeat(self) -> None:
|
||||
if self._state.refresh_tick % self._REFRESH_STRIDE == 0:
|
||||
self._refresh_from_params()
|
||||
self._state.refresh_tick += 1
|
||||
|
||||
def sample(self,
|
||||
blocked_l: bool = False,
|
||||
blocked_r: bool = False,
|
||||
blink_l: bool = False,
|
||||
blink_r: bool = False,
|
||||
speed_mps: float = 0.0,
|
||||
**legacy) -> None:
|
||||
if legacy:
|
||||
blocked_l, blocked_r, blink_l, blink_r, speed_mps = self._consume_legacy_kwargs(**legacy)
|
||||
self._state.outcome = _resolve_signal_choice(speed_mps,
|
||||
self._state.speed_limit_mps,
|
||||
blink_l,
|
||||
blink_r,
|
||||
blocked_l,
|
||||
blocked_r)
|
||||
|
||||
def output(self):
|
||||
return self._state.outcome if self._state.active else TurnDirection.none
|
||||
|
||||
@property
|
||||
def enabled(self):
|
||||
return self._state.active
|
||||
|
||||
@enabled.setter
|
||||
def enabled(self, value):
|
||||
self._state.active = bool(value)
|
||||
|
||||
@property
|
||||
def speed_gate(self):
|
||||
return self._state.speed_limit_mps
|
||||
|
||||
@speed_gate.setter
|
||||
def speed_gate(self, value):
|
||||
self._state.speed_limit_mps = float(value)
|
||||
|
||||
@property
|
||||
def turn_direction(self):
|
||||
return self._state.outcome
|
||||
|
||||
@turn_direction.setter
|
||||
def turn_direction(self, value):
|
||||
self._state.outcome = value
|
||||
|
||||
|
||||
class IQNavTurnController(TurnSignalPlanner):
|
||||
def __init__(self, desire_helper):
|
||||
super().__init__(desire_helper)
|
||||
|
||||
def read_params(self):
|
||||
self.reload_setup()
|
||||
|
||||
def update_params(self) -> None:
|
||||
self.heartbeat()
|
||||
|
||||
def update_lane_turn(self,
|
||||
blindspot_left: bool,
|
||||
blindspot_right: bool,
|
||||
left_blinker: bool,
|
||||
right_blinker: bool,
|
||||
v_ego: float) -> None:
|
||||
self.sample(blocked_l=blindspot_left,
|
||||
blocked_r=blindspot_right,
|
||||
blink_l=left_blinker,
|
||||
blink_r=right_blinker,
|
||||
speed_mps=v_ego)
|
||||
|
||||
def get_turn_direction(self):
|
||||
return self.output()
|
||||
|
||||
@property
|
||||
def lane_turn_value(self):
|
||||
return self.speed_gate
|
||||
|
||||
@lane_turn_value.setter
|
||||
def lane_turn_value(self, value):
|
||||
self.speed_gate = value
|
||||
+9
@@ -0,0 +1,9 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
|
||||
|
||||
try:
|
||||
load_private_module(__name__, "iqpilot_private.models.big_catalog")
|
||||
except ProprietaryModuleMissing:
|
||||
from iqpilot.models_private_src.big_catalog import *
|
||||
@@ -0,0 +1,70 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
|
||||
MAX_CAMERA_OFFSET_METERS = 0.35
|
||||
|
||||
|
||||
class _OffsetSmoother:
|
||||
def __init__(self, blend: float = 0.1):
|
||||
self._blend = blend
|
||||
self._value = 0.0
|
||||
|
||||
def step(self, target: float) -> float:
|
||||
self._value = ((1.0 - self._blend) * self._value) + (self._blend * float(target))
|
||||
return self._value
|
||||
|
||||
|
||||
def _clamped_offset(raw_offset) -> float:
|
||||
try:
|
||||
parsed = float(raw_offset)
|
||||
except (TypeError, ValueError):
|
||||
parsed = 0.0
|
||||
return float(np.clip(parsed, -MAX_CAMERA_OFFSET_METERS, MAX_CAMERA_OFFSET_METERS))
|
||||
|
||||
|
||||
def _camera_profile(sm):
|
||||
return DEVICE_CAMERAS[(str(sm["deviceState"].deviceType), str(sm["roadCameraState"].sensor))]
|
||||
|
||||
|
||||
def _calibration_height(sm) -> float:
|
||||
from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_SANE_MIN, HEIGHT_SANE_MAX
|
||||
h = sm["extrinsicsCalibration"].height[0] if sm["extrinsicsCalibration"].height else 1.22
|
||||
return h if HEIGHT_SANE_MIN <= h <= HEIGHT_SANE_MAX else 1.22
|
||||
|
||||
|
||||
def _sheared_transform(model_transform, intrinsics, height: float, lateral_offset: float):
|
||||
optical_center_y = intrinsics[1, 2]
|
||||
projection_bias = np.eye(3, dtype=np.float32)
|
||||
projection_bias[0, 1] = lateral_offset / height
|
||||
projection_bias[0, 2] = -(lateral_offset / height) * optical_center_y
|
||||
return (projection_bias @ model_transform).astype(np.float32)
|
||||
|
||||
|
||||
class CameraOffsetHelper:
|
||||
def __init__(self):
|
||||
self.camera_offset = 0.0
|
||||
self.actual_camera_offset = 0.0
|
||||
self._smoother = _OffsetSmoother()
|
||||
|
||||
@staticmethod
|
||||
def apply_camera_offset(model_transform, intrinsics, height, offset_param):
|
||||
return _sheared_transform(model_transform, intrinsics, height, offset_param)
|
||||
|
||||
def set_offset(self, offset):
|
||||
self.camera_offset = _clamped_offset(offset)
|
||||
|
||||
def update(self, model_transform_main, model_transform_extra, sm, main_wide_camera, extra_uses_wide_camera=True):
|
||||
self.actual_camera_offset = self._smoother.step(self.camera_offset)
|
||||
camera_bundle = _camera_profile(sm)
|
||||
camera_height = _calibration_height(sm)
|
||||
main_intrinsics = camera_bundle.ecam.intrinsics if main_wide_camera else camera_bundle.fcam.intrinsics
|
||||
extra_intrinsics = camera_bundle.ecam.intrinsics if extra_uses_wide_camera else camera_bundle.fcam.intrinsics
|
||||
|
||||
return (
|
||||
self.apply_camera_offset(model_transform_main, main_intrinsics, camera_height, self.actual_camera_offset),
|
||||
self.apply_camera_offset(model_transform_extra, extra_intrinsics, camera_height, self.actual_camera_offset),
|
||||
)
|
||||
+133
@@ -0,0 +1,133 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def index_function(index: int, max_val: float = 192, max_idx: int = 32) -> float:
|
||||
return max_val * ((index / max_idx) ** 2)
|
||||
|
||||
|
||||
def _quadratic_series(limit: float, steps: int) -> list[float]:
|
||||
return [index_function(index, max_val=limit, max_idx=steps - 1) for index in range(steps)]
|
||||
|
||||
|
||||
def _probability_window(*values: float) -> np.ndarray:
|
||||
return np.asarray(values, dtype=np.float32)
|
||||
|
||||
|
||||
def _field_group(start: int, stop: int, stride: int) -> slice:
|
||||
return slice(start, stop, stride)
|
||||
|
||||
|
||||
_IDX_COUNT = 33
|
||||
_T_AXIS = _quadratic_series(10.0, _IDX_COUNT)
|
||||
_X_AXIS = _quadratic_series(192.0, _IDX_COUNT)
|
||||
|
||||
|
||||
class ModelConstants:
|
||||
IDX_N = _IDX_COUNT
|
||||
T_IDXS = _T_AXIS
|
||||
X_IDXS = _X_AXIS
|
||||
LEAD_T_IDXS = [0.0, 2.0, 4.0, 6.0, 8.0, 10.0]
|
||||
LEAD_T_OFFSETS = [0.0, 2.0, 4.0]
|
||||
META_T_IDXS = [2.0, 4.0, 6.0, 8.0, 10.0]
|
||||
|
||||
MODEL_FREQ = 20
|
||||
FEATURE_LEN = 512
|
||||
FULL_HISTORY_BUFFER_LEN = 99
|
||||
HISTORY_BUFFER_LEN = FULL_HISTORY_BUFFER_LEN
|
||||
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
|
||||
|
||||
FCW_THRESHOLDS_5MS2 = _probability_window(0.05, 0.05, 0.15, 0.15, 0.15)
|
||||
FCW_THRESHOLDS_3MS2 = _probability_window(0.7, 0.7)
|
||||
FCW_5MS2_PROBS_WIDTH = 5
|
||||
FCW_3MS2_PROBS_WIDTH = 2
|
||||
|
||||
DISENGAGE_WIDTH = 5
|
||||
POSE_WIDTH = 6
|
||||
WIDE_FROM_DEVICE_WIDTH = 3
|
||||
SIM_POSE_WIDTH = 6
|
||||
LEAD_WIDTH = 4
|
||||
LANE_LINES_WIDTH = 2
|
||||
ROAD_EDGES_WIDTH = 2
|
||||
PLAN_WIDTH = 15
|
||||
DESIRE_PRED_WIDTH = 8
|
||||
LAT_PLANNER_SOLUTION_WIDTH = 4
|
||||
DESIRED_CURV_WIDTH = 1
|
||||
|
||||
NUM_LANE_LINES = 4
|
||||
NUM_ROAD_EDGES = 2
|
||||
LEAD_TRAJ_LEN = 6
|
||||
DESIRE_PRED_LEN = 4
|
||||
|
||||
PLAN_MHP_N = 5
|
||||
LEAD_MHP_N = 2
|
||||
PLAN_MHP_SELECTION = 1
|
||||
LEAD_MHP_SELECTION = 3
|
||||
|
||||
FCW_THRESHOLD_5MS2_HIGH = 0.15
|
||||
FCW_THRESHOLD_5MS2_LOW = 0.05
|
||||
FCW_THRESHOLD_3MS2 = 0.7
|
||||
|
||||
CONFIDENCE_BUFFER_LEN = 5
|
||||
RYG_GREEN = 0.01165
|
||||
RYG_YELLOW = 0.06157
|
||||
POLY_PATH_DEGREE = 4
|
||||
|
||||
|
||||
class Plan:
|
||||
POSITION = slice(0, 3)
|
||||
VELOCITY = slice(3, 6)
|
||||
ACCELERATION = slice(6, 9)
|
||||
T_FROM_CURRENT_EULER = slice(9, 12)
|
||||
ORIENTATION_RATE = slice(12, 15)
|
||||
|
||||
|
||||
class Meta:
|
||||
ENGAGED = _field_group(0, 1, 1)
|
||||
GAS_DISENGAGE = _field_group(1, 31, 6)
|
||||
BRAKE_DISENGAGE = _field_group(2, 31, 6)
|
||||
STEER_OVERRIDE = _field_group(3, 31, 6)
|
||||
HARD_BRAKE_3 = _field_group(4, 31, 6)
|
||||
HARD_BRAKE_4 = _field_group(5, 31, 6)
|
||||
HARD_BRAKE_5 = _field_group(6, 31, 6)
|
||||
GAS_PRESS = _field_group(31, 55, 4)
|
||||
BRAKE_PRESS = _field_group(32, 55, 4)
|
||||
LEFT_BLINKER = _field_group(33, 55, 4)
|
||||
RIGHT_BLINKER = _field_group(34, 55, 4)
|
||||
|
||||
|
||||
class MetaTombRaider:
|
||||
ENGAGED = _field_group(0, 1, 1)
|
||||
GAS_DISENGAGE = _field_group(1, 41, 8)
|
||||
BRAKE_DISENGAGE = _field_group(2, 41, 8)
|
||||
STEER_OVERRIDE = _field_group(3, 41, 8)
|
||||
HARD_BRAKE_3 = _field_group(4, 41, 8)
|
||||
HARD_BRAKE_4 = _field_group(5, 41, 8)
|
||||
HARD_BRAKE_5 = _field_group(6, 41, 8)
|
||||
GAS_PRESS = _field_group(7, 41, 8)
|
||||
BRAKE_PRESS = _field_group(8, 41, 8)
|
||||
LEFT_BLINKER = _field_group(41, 53, 2)
|
||||
RIGHT_BLINKER = _field_group(42, 53, 2)
|
||||
|
||||
|
||||
class MetaSimPose:
|
||||
ENGAGED = _field_group(0, 1, 1)
|
||||
GAS_DISENGAGE = _field_group(1, 36, 7)
|
||||
BRAKE_DISENGAGE = _field_group(2, 36, 7)
|
||||
STEER_OVERRIDE = _field_group(3, 36, 7)
|
||||
HARD_BRAKE_3 = _field_group(4, 36, 7)
|
||||
HARD_BRAKE_4 = _field_group(5, 36, 7)
|
||||
HARD_BRAKE_5 = _field_group(6, 36, 7)
|
||||
GAS_PRESS = _field_group(7, 36, 7)
|
||||
LEFT_BLINKER = _field_group(36, 48, 2)
|
||||
RIGHT_BLINKER = _field_group(37, 48, 2)
|
||||
+766
@@ -0,0 +1,766 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.cereal import car, custom, log
|
||||
from iqpilot.cereal.messaging import PubMaster, SubMaster
|
||||
from iqpilot.cereal.visionipc import VisionStreamType
|
||||
from msgq.visionipc import VisionBuf, VisionIpcClient
|
||||
from iqdbc.car.car_helpers import get_demo_car_params
|
||||
from setproctitle import setproctitle
|
||||
|
||||
from iqpilot.common.filter_simple import FirstOrderFilter
|
||||
from iqpilot.common.iq_perf import PerfSample, PerfTraceEmitter, PerfTraceRing
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import DT_MDL, config_realtime_process
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from iqpilot.common.transformations.model import get_warp_matrix
|
||||
from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from iqpilot.selfdrive.controls.lib.drive_helpers import (
|
||||
MODEL_SMOOTHING_MAX_TOTAL_SEC,
|
||||
dynamic_lat_smooth_extra_seconds,
|
||||
get_accel_from_plan,
|
||||
smooth_value,
|
||||
)
|
||||
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
|
||||
from iqpilot.system import sentry
|
||||
|
||||
from iqpilot.common.steer_delay import lateral_action_delay
|
||||
from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
|
||||
from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import get_model_runner
|
||||
from iqpilot.selfdrive.iqmodeld.camera import CameraOffsetHelper
|
||||
from iqpilot.selfdrive.iqmodeld.config import Plan
|
||||
from iqpilot.selfdrive.iqmodeld.messaging import (
|
||||
DrivePacketMemory,
|
||||
pick_curvature,
|
||||
populate_drive_messages,
|
||||
populate_odometry_message,
|
||||
)
|
||||
from iqpilot.selfdrive.iqmodeld.metadata import select_meta_layout
|
||||
|
||||
try:
|
||||
from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector, WarpContext
|
||||
except ModuleNotFoundError:
|
||||
class WarpContext:
|
||||
def __init__(self, *args, **kwargs):
|
||||
raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
|
||||
|
||||
class RoadProjector:
|
||||
def __init__(self, *args, **kwargs):
|
||||
raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
|
||||
|
||||
|
||||
PROCESS_NAME = "iqpilot.selfdrive.iqmodeld.daemon"
|
||||
IQP_NAV_MODEL_INFLUENCE_ENABLED = False
|
||||
TurnDirection = custom.IQTurnSignalDirection
|
||||
IQMODEL_EVAL_WARN_US = int(DT_MDL * 1_000_000)
|
||||
IQMODEL_EVAL_ERROR_US = IQMODEL_EVAL_WARN_US * 2
|
||||
_FRAME_STARVED_BACKOFF_POLLS = 5
|
||||
_FRAME_STARVED_BACKOFF_SECONDS = 0.005
|
||||
_FRAME_STARVED_LOG_EVERY = 200
|
||||
|
||||
|
||||
def _plan_y_std_1s(outputs: dict[str, np.ndarray]) -> float:
|
||||
# plan_stds is (batch, IDX_N, PLAN_WIDTH); index 10 ~= 1s ahead (see ModelConstants.T_IDXS),
|
||||
# POSITION is an (x, y, z) slice within PLAN_WIDTH so [1] picks the lateral (y) std.
|
||||
try:
|
||||
return float(outputs["plan_stds"][0, 10, Plan.POSITION][1])
|
||||
except (KeyError, IndexError):
|
||||
return 0.0
|
||||
|
||||
|
||||
def _model_lat_smooth_max_sec(params: Params) -> float:
|
||||
if not params.get_bool("ModelSmoothingEnabled"):
|
||||
return 0.0
|
||||
try:
|
||||
raw = params.get("ModelLatSmoothSec", return_default=True)
|
||||
raw = 0 if raw is None else int(raw)
|
||||
except (ValueError, TypeError):
|
||||
raw = 0
|
||||
return min(max(raw, 0), 30) * 0.01
|
||||
|
||||
|
||||
@dataclass
|
||||
class CaptureStamp:
|
||||
frame_id: int = 0
|
||||
timestamp_sof: int = 0
|
||||
timestamp_eof: int = 0
|
||||
|
||||
@classmethod
|
||||
def from_vipc(cls, client: VisionIpcClient) -> "CaptureStamp":
|
||||
return cls(client.frame_id, client.timestamp_sof, client.timestamp_eof)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class StreamLayout:
|
||||
dual_camera: bool
|
||||
main_is_wide: bool
|
||||
primary_stream: VisionStreamType
|
||||
|
||||
|
||||
class ReplayLedger:
|
||||
def __init__(self, tensor_shapes: dict[str, tuple[int, ...]], frame_inputs: list[str]):
|
||||
self.inputs: dict[str, np.ndarray] = {}
|
||||
self.archive: dict[str, np.ndarray] = {}
|
||||
self.selectors: dict[str, np.ndarray] = {}
|
||||
self._frame_inputs = set(frame_inputs)
|
||||
self._pulse_name: str | None = None
|
||||
self._pulse_memory: np.ndarray | None = None
|
||||
|
||||
feature_shape = tensor_shapes.get("features_buffer")
|
||||
for tensor_name, tensor_shape in tensor_shapes.items():
|
||||
if tensor_name in self._frame_inputs:
|
||||
continue
|
||||
|
||||
self.inputs[tensor_name] = np.zeros(tensor_shape, dtype=np.float32)
|
||||
if len(tensor_shape) != 3 or tensor_shape[1] <= 1:
|
||||
continue
|
||||
|
||||
history_len = self._history_length(tensor_shape, feature_shape)
|
||||
self.archive[tensor_name] = np.zeros((1, history_len, tensor_shape[2]), dtype=np.float32)
|
||||
export_index = self._export_index(tensor_shape, history_len, feature_shape)
|
||||
if export_index is not None:
|
||||
self.selectors[tensor_name] = export_index
|
||||
|
||||
if tensor_name.startswith("desire"):
|
||||
self._pulse_name = tensor_name
|
||||
self._pulse_memory = np.zeros(tensor_shape[2], dtype=np.float32)
|
||||
|
||||
@staticmethod
|
||||
def _history_length(tensor_shape: tuple[int, ...], feature_shape: tuple[int, ...] | None) -> int:
|
||||
if tensor_shape[1] >= 99:
|
||||
return tensor_shape[1]
|
||||
if tensor_shape[1] in (24, 25) and feature_shape is not None and feature_shape[1] == 24:
|
||||
return (feature_shape[1] + 1) * 4
|
||||
return tensor_shape[1] * 4
|
||||
|
||||
@staticmethod
|
||||
def _export_index(tensor_shape: tuple[int, ...], history_len: int,
|
||||
feature_shape: tuple[int, ...] | None) -> np.ndarray | None:
|
||||
if tensor_shape[1] in (24, 25) and feature_shape is not None and feature_shape[1] == 24:
|
||||
stride = int(-history_len / tensor_shape[1])
|
||||
return np.arange(stride, stride * (tensor_shape[1] + 1), stride)[::-1]
|
||||
if tensor_shape[1] == 25:
|
||||
skip = history_len // tensor_shape[1]
|
||||
return np.arange(history_len)[-1 - (skip * (tensor_shape[1] - 1))::skip]
|
||||
if tensor_shape[1] >= 99:
|
||||
return np.arange(tensor_shape[1])
|
||||
return None
|
||||
|
||||
@property
|
||||
def pulse_name(self) -> str:
|
||||
if self._pulse_name is None:
|
||||
raise KeyError("No desire-like pulse input present in model inputs")
|
||||
return self._pulse_name
|
||||
|
||||
def _shift_archive(self, tensor_name: str) -> np.ndarray:
|
||||
history = self.archive[tensor_name]
|
||||
history[0, :-1] = history[0, 1:]
|
||||
return history
|
||||
|
||||
def inject_pulse(self, pulse_values: np.ndarray) -> None:
|
||||
pulse = pulse_values.copy()
|
||||
pulse[0] = 0
|
||||
assert self._pulse_memory is not None
|
||||
rising = np.where(pulse - self._pulse_memory > 0.99, pulse, 0)
|
||||
self._pulse_memory[:] = pulse
|
||||
|
||||
history = self._shift_archive(self.pulse_name)
|
||||
history[0, -1] = rising
|
||||
exported_shape = self.inputs[self.pulse_name].shape
|
||||
if history.shape[1] > exported_shape[1]:
|
||||
stride = history.shape[1] // exported_shape[1]
|
||||
self.inputs[self.pulse_name][:] = history[0].reshape(
|
||||
exported_shape[0], exported_shape[1], stride, -1
|
||||
).max(axis=2)
|
||||
return
|
||||
self.inputs[self.pulse_name][:] = history[0, self.selectors[self.pulse_name]]
|
||||
|
||||
def merge_inputs(self, fresh_inputs: dict[str, np.ndarray]) -> None:
|
||||
pulse_name = self.pulse_name
|
||||
for tensor_name, tensor_value in fresh_inputs.items():
|
||||
if tensor_name in self.inputs and tensor_name != pulse_name:
|
||||
self.inputs[tensor_name][:] = tensor_value
|
||||
|
||||
def note_hidden_state(self, hidden_state: np.ndarray) -> None:
|
||||
if "features_buffer" not in self.archive:
|
||||
return
|
||||
history = self._shift_archive("features_buffer")
|
||||
history[0, -1] = hidden_state[0]
|
||||
self.inputs["features_buffer"][:] = history[0, self.selectors["features_buffer"]]
|
||||
|
||||
def note_feedback(self, tensor_name: str, values: np.ndarray, zero_export: bool = False) -> None:
|
||||
if tensor_name not in self.archive:
|
||||
return
|
||||
history = self._shift_archive(tensor_name)
|
||||
history[0, -1, :] = values[0]
|
||||
exported = history[0, self.selectors[tensor_name]]
|
||||
self.inputs[tensor_name][:] = 0 * exported if zero_export else exported
|
||||
|
||||
|
||||
def _planplus_gain(vehicle_speed: float) -> float:
|
||||
return 0.75 if vehicle_speed >= 25.0 else 1.0
|
||||
|
||||
|
||||
def _merged_plan(runtime_state: "NeuralEngineState", outputs: dict[str, np.ndarray], vehicle_speed: float) -> np.ndarray:
|
||||
base_plan = outputs["plan"][0]
|
||||
if "planplus" not in outputs:
|
||||
return base_plan
|
||||
return base_plan + (runtime_state.PLANPLUS_CONTROL * _planplus_gain(vehicle_speed)) * outputs["planplus"][0]
|
||||
|
||||
|
||||
class NeuralEngineState(InferenceStateBase):
|
||||
frames: dict[str, RoadProjector]
|
||||
|
||||
def __init__(self, gpu_context: WarpContext):
|
||||
super().__init__()
|
||||
runner = get_model_runner()
|
||||
bundle = get_active_bundle()
|
||||
|
||||
self.model_runner = runner
|
||||
self.constants = runner.constants
|
||||
self.generation = bundle.generation if bundle is not None else None
|
||||
|
||||
knob_values = {entry.key: entry.value for entry in bundle.overrides} if bundle is not None else {}
|
||||
self.LAT_SMOOTH_SECONDS = float(knob_values.get("lat", ".0"))
|
||||
self.LONG_SMOOTH_SECONDS = float(knob_values.get("long", ".0"))
|
||||
self.MIN_LAT_CONTROL_SPEED = 0.3
|
||||
self.PLANPLUS_CONTROL = 1.0
|
||||
self.model_smoothing_max_extra_sec = 0.0
|
||||
|
||||
context_depth = 5 if runner.is_20hz else 2
|
||||
self.frames = {
|
||||
stream_name: RoadProjector(gpu_context, context_depth)
|
||||
for stream_name in runner.vision_input_names
|
||||
}
|
||||
|
||||
self._ledger = ReplayLedger(runner.input_shapes, runner.vision_input_names)
|
||||
self.numpy_inputs = self._ledger.inputs
|
||||
self.temporal_buffers = self._ledger.archive
|
||||
self.temporal_idxs_map = self._ledger.selectors
|
||||
|
||||
@property
|
||||
def mlsim(self) -> bool:
|
||||
return bool(self.generation is not None and self.generation >= 11)
|
||||
|
||||
@property
|
||||
def desire_key(self) -> str:
|
||||
return self._ledger.pulse_name
|
||||
|
||||
def _warp_frames(self, vision_bufs: dict[str, VisionBuf],
|
||||
transform_map: dict[str, np.ndarray]) -> dict[str, Any]:
|
||||
return {
|
||||
stream_name: self.frames[stream_name].stage(vision_bufs[stream_name], transform_map[stream_name].flatten())
|
||||
for stream_name in self.model_runner.vision_input_names
|
||||
}
|
||||
|
||||
def _run_split_model(self) -> dict[str, np.ndarray]:
|
||||
if hasattr(self.model_runner, "run_vision"):
|
||||
vision_packet = self.model_runner.run_vision()
|
||||
self._ledger.note_hidden_state(vision_packet["hidden_state"])
|
||||
self.model_runner.refresh_policy_features(self.numpy_inputs["features_buffer"])
|
||||
return {**vision_packet, **self.model_runner.run_policy()}
|
||||
|
||||
result = self.model_runner.run_model()
|
||||
if "hidden_state" in result:
|
||||
self._ledger.note_hidden_state(result["hidden_state"])
|
||||
return result
|
||||
|
||||
def _write_curvature_memory(self, outputs: dict[str, np.ndarray]) -> None:
|
||||
if "desired_curvature" not in outputs:
|
||||
return
|
||||
|
||||
feedback_slot = None
|
||||
if "prev_desired_curvs" in self.numpy_inputs:
|
||||
feedback_slot = "prev_desired_curvs"
|
||||
elif "prev_desired_curv" in self.numpy_inputs:
|
||||
feedback_slot = "prev_desired_curv"
|
||||
|
||||
if feedback_slot is not None:
|
||||
self._ledger.note_feedback(feedback_slot, outputs["desired_curvature"], zero_export=self.mlsim)
|
||||
|
||||
def run(self, vision_bufs: dict[str, VisionBuf], transform_map: dict[str, np.ndarray],
|
||||
fresh_inputs: dict[str, np.ndarray]) -> dict[str, np.ndarray] | None:
|
||||
if not getattr(self.model_runner, "uses_opencl_warp", True):
|
||||
return self.model_runner.run_fused(vision_bufs, transform_map, fresh_inputs)
|
||||
|
||||
self._ledger.inject_pulse(fresh_inputs[self.desire_key])
|
||||
self._ledger.merge_inputs(fresh_inputs)
|
||||
warped_frames = self._warp_frames(vision_bufs, transform_map)
|
||||
self.model_runner.prepare_inputs(warped_frames, self.numpy_inputs, self.frames)
|
||||
|
||||
outputs = self._run_split_model()
|
||||
self._write_curvature_memory(outputs)
|
||||
return outputs
|
||||
|
||||
def get_action_from_model(self, outputs: dict[str, np.ndarray], previous_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float, vehicle_speed: float,
|
||||
lat_smooth_seconds: float | None = None) -> log.ModelDataV2.Action:
|
||||
if lat_smooth_seconds is None:
|
||||
lat_smooth_seconds = self.LAT_SMOOTH_SECONDS
|
||||
|
||||
if "action" in outputs:
|
||||
curvature_cmd = outputs["action"][0, 0] / (max(1.0, vehicle_speed)) ** 2
|
||||
accel_cmd = outputs["action"][0, 1]
|
||||
should_stop = bool(vehicle_speed < 0.3 and accel_cmd < 0.1)
|
||||
|
||||
accel_cmd = smooth_value(accel_cmd, previous_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
if vehicle_speed > self.MIN_LAT_CONTROL_SPEED:
|
||||
curvature_cmd = smooth_value(curvature_cmd, previous_action.desiredCurvature, lat_smooth_seconds)
|
||||
else:
|
||||
curvature_cmd = previous_action.desiredCurvature
|
||||
|
||||
return log.ModelDataV2.Action(
|
||||
desiredCurvature=float(curvature_cmd),
|
||||
desiredAcceleration=float(accel_cmd),
|
||||
shouldStop=should_stop,
|
||||
)
|
||||
|
||||
plan_rows = _merged_plan(self, outputs, vehicle_speed)
|
||||
accel_cmd, should_stop = get_accel_from_plan(
|
||||
plan_rows[:, Plan.VELOCITY][:, 0],
|
||||
plan_rows[:, Plan.ACCELERATION][:, 0],
|
||||
self.constants.T_IDXS,
|
||||
action_t=long_action_t,
|
||||
)
|
||||
accel_cmd = smooth_value(accel_cmd, previous_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
|
||||
curvature_cmd = pick_curvature(outputs, plan_rows, vehicle_speed, lat_action_t, self.mlsim)
|
||||
if self.generation is not None and self.generation >= 10:
|
||||
if vehicle_speed > self.MIN_LAT_CONTROL_SPEED:
|
||||
curvature_cmd = smooth_value(curvature_cmd, previous_action.desiredCurvature, lat_smooth_seconds)
|
||||
else:
|
||||
curvature_cmd = previous_action.desiredCurvature
|
||||
|
||||
return log.ModelDataV2.Action(
|
||||
desiredCurvature=float(curvature_cmd),
|
||||
desiredAcceleration=float(accel_cmd),
|
||||
shouldStop=bool(should_stop),
|
||||
)
|
||||
|
||||
|
||||
class CameraIngress:
|
||||
def __init__(self, gpu_context: WarpContext):
|
||||
self.layout = self._discover_layout()
|
||||
self._primary = VisionIpcClient("camerad", self.layout.primary_stream, True, gpu_context)
|
||||
self._secondary = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, gpu_context)
|
||||
|
||||
while not self._primary.connect(False):
|
||||
time.sleep(0.1)
|
||||
while self.layout.dual_camera and not self._secondary.connect(False):
|
||||
time.sleep(0.1)
|
||||
|
||||
cloudlog.warning(
|
||||
f"connected main cam with buffer size: {self._primary.buffer_len} ({self._primary.width} x {self._primary.height})"
|
||||
)
|
||||
if self.layout.dual_camera:
|
||||
cloudlog.warning(
|
||||
f"connected extra cam with buffer size: {self._secondary.buffer_len} ({self._secondary.width} x {self._secondary.height})"
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _discover_layout() -> StreamLayout:
|
||||
while True:
|
||||
available = VisionIpcClient.available_streams("camerad", block=False)
|
||||
if available:
|
||||
dual_camera = (
|
||||
VisionStreamType.VISION_STREAM_WIDE_ROAD in available
|
||||
and VisionStreamType.VISION_STREAM_ROAD in available
|
||||
)
|
||||
main_is_wide = VisionStreamType.VISION_STREAM_ROAD not in available
|
||||
primary_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_is_wide else VisionStreamType.VISION_STREAM_ROAD
|
||||
cloudlog.warning(
|
||||
f"vision stream set up, main_wide_camera: {main_is_wide}, use_extra_client: {dual_camera}"
|
||||
)
|
||||
return StreamLayout(dual_camera=dual_camera, main_is_wide=main_is_wide, primary_stream=primary_stream)
|
||||
time.sleep(0.1)
|
||||
|
||||
def pull(self) -> tuple[VisionBuf, VisionBuf, CaptureStamp, CaptureStamp] | None:
|
||||
main_buf = None
|
||||
wide_buf = None
|
||||
main_stamp = CaptureStamp()
|
||||
wide_stamp = CaptureStamp()
|
||||
|
||||
while main_stamp.timestamp_sof < wide_stamp.timestamp_sof + 25000000:
|
||||
main_buf = self._primary.recv()
|
||||
main_stamp = CaptureStamp.from_vipc(self._primary)
|
||||
if main_buf is None:
|
||||
return None
|
||||
|
||||
if not self.layout.dual_camera:
|
||||
return main_buf, main_buf, main_stamp, main_stamp
|
||||
|
||||
while True:
|
||||
wide_buf = self._secondary.recv()
|
||||
wide_stamp = CaptureStamp.from_vipc(self._secondary)
|
||||
if wide_buf is None or main_stamp.timestamp_sof < wide_stamp.timestamp_sof + 25000000:
|
||||
break
|
||||
|
||||
if wide_buf is None:
|
||||
return None
|
||||
|
||||
if abs(main_stamp.timestamp_sof - wide_stamp.timestamp_sof) > 10000000:
|
||||
cloudlog.error(
|
||||
f"frames out of sync! main: {main_stamp.frame_id} ({main_stamp.timestamp_sof / 1e9:.5f}),"
|
||||
f" extra: {wide_stamp.frame_id} ({wide_stamp.timestamp_sof / 1e9:.5f})"
|
||||
)
|
||||
return main_buf, wide_buf, main_stamp, wide_stamp
|
||||
|
||||
|
||||
class CalibrationAtlas:
|
||||
def __init__(self):
|
||||
self.main_warp = np.zeros((3, 3), dtype=np.float32)
|
||||
self.extra_warp = np.zeros((3, 3), dtype=np.float32)
|
||||
self.ready = False
|
||||
self._offset_tuner = CameraOffsetHelper()
|
||||
|
||||
def set_offset(self, offset_value: Any) -> None:
|
||||
self._offset_tuner.set_offset(offset_value)
|
||||
|
||||
def refresh(self, sm: SubMaster, main_is_wide: bool, dual_camera: bool) -> tuple[np.ndarray, np.ndarray, bool]:
|
||||
if not (sm.seen["extrinsicsCalibration"] and sm.seen["roadCameraState"] and sm.seen["deviceState"]):
|
||||
return self.main_warp, self.extra_warp, self.ready
|
||||
|
||||
rpy = get_calibrated_rpy(sm["extrinsicsCalibration"])
|
||||
if rpy is None:
|
||||
live_calib = sm["extrinsicsCalibration"]
|
||||
if len(live_calib.rpyCalib) == 3:
|
||||
rpy = np.array(live_calib.rpyCalib, dtype=np.float32)
|
||||
else:
|
||||
rpy = np.zeros(3, dtype=np.float32)
|
||||
|
||||
device_key = (str(sm["deviceState"].deviceType), str(sm["roadCameraState"].sensor))
|
||||
device_camera = DEVICE_CAMERAS[device_key]
|
||||
main_intrinsics = device_camera.ecam.intrinsics if main_is_wide else device_camera.fcam.intrinsics
|
||||
extra_uses_wide_camera = dual_camera or main_is_wide
|
||||
extra_intrinsics = device_camera.ecam.intrinsics if extra_uses_wide_camera else device_camera.fcam.intrinsics
|
||||
self.main_warp = get_warp_matrix(rpy, main_intrinsics, False).astype(np.float32)
|
||||
self.extra_warp = get_warp_matrix(rpy, extra_intrinsics, True).astype(np.float32)
|
||||
self.main_warp, self.extra_warp = self._offset_tuner.update(
|
||||
self.main_warp, self.extra_warp, sm, main_is_wide, extra_uses_wide_camera
|
||||
)
|
||||
self.ready = True
|
||||
return self.main_warp, self.extra_warp, self.ready
|
||||
|
||||
|
||||
class FrameDropMeter:
|
||||
def __init__(self, model_freq: float):
|
||||
self._smoother = FirstOrderFilter(0.0, 10.0, 1.0 / model_freq)
|
||||
self._warm_frames = 0
|
||||
self._last_frame_id = 0
|
||||
|
||||
def sample(self, frame_id: int) -> tuple[int, float, bool]:
|
||||
dropped = max(0, frame_id - self._last_frame_id - 1)
|
||||
smooth = self._smoother.update(min(dropped, 10))
|
||||
if self._warm_frames < 10:
|
||||
self._smoother.x = 0.0
|
||||
smooth = 0.0
|
||||
self._warm_frames += 1
|
||||
return dropped, smooth / (1 + smooth), dropped > 0
|
||||
|
||||
def commit(self, frame_id: int) -> None:
|
||||
self._last_frame_id = frame_id
|
||||
|
||||
|
||||
class InferenceDaemon:
|
||||
def __init__(self, demo: bool = False, channel_path: str | None = None):
|
||||
cloudlog.warning("iqmodeld init")
|
||||
sentry.set_tag("daemon", PROCESS_NAME)
|
||||
cloudlog.bind(daemon=PROCESS_NAME)
|
||||
setproctitle(PROCESS_NAME)
|
||||
config_realtime_process(7, 54)
|
||||
|
||||
cloudlog.warning("setting up CL context")
|
||||
self._gpu = WarpContext()
|
||||
cloudlog.warning("CL context ready; loading model")
|
||||
self._runtime = NeuralEngineState(self._gpu)
|
||||
self._meta_layout = select_meta_layout()
|
||||
cloudlog.warning("models loaded, iqmodeld starting")
|
||||
|
||||
self._channel = None
|
||||
if channel_path is not None:
|
||||
from iqpilot.selfdrive.iqmodeld.model_channel import ModelChannel
|
||||
self._channel = ModelChannel(channel_path, create=True)
|
||||
|
||||
self._cameras = CameraIngress(self._gpu)
|
||||
pub_services = ["iqPerfTrace"] if self._channel is not None else [
|
||||
"modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData", "iqPerfTrace"]
|
||||
self._pub = PubMaster(pub_services)
|
||||
self._sub = SubMaster([
|
||||
"deviceState", "carState", "roadCameraState", "extrinsicsCalibration",
|
||||
"driverMonitoringState", "carControl", "lateralDelay", "iqNavState", "radarState",
|
||||
])
|
||||
self._message_memory = DrivePacketMemory()
|
||||
self._params = Params()
|
||||
self._frame_meter = FrameDropMeter(self._runtime.constants.MODEL_FREQ)
|
||||
self._warps = CalibrationAtlas()
|
||||
self._perf = PerfTraceEmitter("iqmodeld", pubmaster=self._pub)
|
||||
self._perf_ring = PerfTraceRing()
|
||||
|
||||
self._car_params = self._load_car_params(demo)
|
||||
self._long_action_delay = self._car_params.longitudinalActuatorDelay + self._runtime.LONG_SMOOTH_SECONDS
|
||||
self._previous_action = log.ModelDataV2.Action()
|
||||
self._desire_logic = DesireHelper()
|
||||
self._lat_smooth_extra_sec = 0.0
|
||||
|
||||
def _load_car_params(self, demo: bool):
|
||||
car_params = get_demo_car_params() if demo else messaging.log_from_bytes(
|
||||
self._params.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("iqmodeld got CarParams: %s", car_params.brand)
|
||||
return car_params
|
||||
|
||||
def _refresh_tunables(self, tick: int) -> None:
|
||||
if tick % 60 != 0:
|
||||
return
|
||||
from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected
|
||||
big_enabled = self._params.get_bool("IQEmacEnabled") or egpu_selected(self._params)
|
||||
if big_enabled != (self._channel is not None):
|
||||
# publish mode is fixed at startup: staying up would fight the selector for modelV2
|
||||
cloudlog.warning("iqmodeld: big backend toggled, restarting to switch publish mode")
|
||||
sys.exit(0)
|
||||
self._runtime.lat_delay = lateral_action_delay(self._params, self._car_params, self._sub["lateralDelay"].lateralDelay)
|
||||
self._runtime.PLANPLUS_CONTROL = self._params.get("PlanplusControl", return_default=True)
|
||||
self._runtime.model_smoothing_max_extra_sec = _model_lat_smooth_max_sec(self._params)
|
||||
self._warps.set_offset(self._params.get("CameraOffset", return_default=True))
|
||||
|
||||
def _traffic_side(self) -> np.ndarray:
|
||||
traffic = np.zeros(2, dtype=np.float32)
|
||||
traffic[int(self._sub["driverMonitoringState"].isRHD)] = 1
|
||||
return traffic
|
||||
|
||||
def _desire_pulse(self) -> np.ndarray:
|
||||
pulse = np.zeros(self._runtime.constants.DESIRE_LEN, dtype=np.float32)
|
||||
desire_idx = self._desire_logic.desire
|
||||
if 0 <= desire_idx < self._runtime.constants.DESIRE_LEN:
|
||||
pulse[desire_idx] = 1
|
||||
return pulse
|
||||
|
||||
def _compose_inputs(self, vehicle_speed: float, lat_horizon: float, long_horizon: float) -> dict[str, np.ndarray]:
|
||||
inputs: dict[str, np.ndarray] = {
|
||||
self._runtime.desire_key: self._desire_pulse(),
|
||||
"traffic_convention": self._traffic_side(),
|
||||
}
|
||||
if "lateral_control_params" in self._runtime.numpy_inputs:
|
||||
inputs["lateral_control_params"] = np.array([vehicle_speed, lat_horizon], dtype=np.float32)
|
||||
if "action_t" in self._runtime.numpy_inputs:
|
||||
inputs["action_t"] = np.array([lat_horizon, long_horizon], dtype=np.float32)
|
||||
return inputs
|
||||
|
||||
def _publish(self, outputs: dict[str, np.ndarray], main_stamp: CaptureStamp, extra_stamp: CaptureStamp,
|
||||
road_frame_id: int, frame_drop_ratio: float, dropped_frames: int,
|
||||
execution_time: float, live_calib_seen: bool,
|
||||
lat_horizon: float, long_horizon: float, vehicle_speed: float) -> None:
|
||||
model_msg = messaging.new_message("modelV2")
|
||||
driving_msg = messaging.new_message("drivingModelData")
|
||||
pose_msg = messaging.new_message("cameraOdometry")
|
||||
iq_msg = messaging.new_message("iqDriveModelData")
|
||||
|
||||
self._lat_smooth_extra_sec = dynamic_lat_smooth_extra_seconds(
|
||||
_plan_y_std_1s(outputs), self._runtime.model_smoothing_max_extra_sec
|
||||
)
|
||||
lat_smooth_total_sec = min(self._runtime.LAT_SMOOTH_SECONDS + self._lat_smooth_extra_sec, MODEL_SMOOTHING_MAX_TOTAL_SEC)
|
||||
action = self._runtime.get_action_from_model(
|
||||
outputs, self._previous_action, lat_horizon, long_horizon, vehicle_speed, lat_smooth_total_sec
|
||||
)
|
||||
self._previous_action = action
|
||||
|
||||
populate_drive_messages(
|
||||
driving_msg,
|
||||
model_msg,
|
||||
outputs,
|
||||
action,
|
||||
self._message_memory,
|
||||
main_stamp.frame_id,
|
||||
extra_stamp.frame_id,
|
||||
road_frame_id,
|
||||
frame_drop_ratio,
|
||||
main_stamp.timestamp_eof,
|
||||
execution_time,
|
||||
live_calib_seen,
|
||||
self._meta_layout,
|
||||
)
|
||||
|
||||
desire_state = model_msg.modelV2.meta.desireState
|
||||
lane_change_prob = desire_state[log.Desire.laneChangeLeft] + desire_state[log.Desire.laneChangeRight]
|
||||
self._desire_logic.update(
|
||||
self._sub["carState"],
|
||||
self._sub["carControl"].latActive,
|
||||
lane_change_prob,
|
||||
self._sub["iqNavState"],
|
||||
model_msg.modelV2,
|
||||
self._sub["radarState"],
|
||||
)
|
||||
model_msg.modelV2.meta.laneChangeState = self._desire_logic.lane_change_state
|
||||
model_msg.modelV2.meta.laneChangeDirection = self._desire_logic.lane_change_direction
|
||||
driving_msg.drivingModelData.meta.laneChangeState = self._desire_logic.lane_change_state
|
||||
driving_msg.drivingModelData.meta.laneChangeDirection = self._desire_logic.lane_change_direction
|
||||
iq_msg.iqDriveModelData.turnSignalDirection = self._desire_logic.lane_turn_direction
|
||||
|
||||
populate_odometry_message(
|
||||
pose_msg,
|
||||
outputs,
|
||||
main_stamp.frame_id,
|
||||
dropped_frames,
|
||||
main_stamp.timestamp_eof,
|
||||
live_calib_seen,
|
||||
)
|
||||
|
||||
if self._channel is not None:
|
||||
self._channel.write(main_stamp.frame_id, {
|
||||
"source": "small",
|
||||
"frame_id": main_stamp.frame_id,
|
||||
"timestamp_sof": int(main_stamp.timestamp_sof),
|
||||
"live_calib_seen": bool(live_calib_seen),
|
||||
"model_execution_time": float(execution_time),
|
||||
"msgs": {
|
||||
"modelV2": model_msg.to_bytes(),
|
||||
"drivingModelData": driving_msg.to_bytes(),
|
||||
"cameraOdometry": pose_msg.to_bytes(),
|
||||
"iqDriveModelData": iq_msg.to_bytes(),
|
||||
},
|
||||
})
|
||||
return
|
||||
|
||||
self._pub.send("modelV2", model_msg)
|
||||
self._pub.send("drivingModelData", driving_msg)
|
||||
self._pub.send("cameraOdometry", pose_msg)
|
||||
self._pub.send("iqDriveModelData", iq_msg)
|
||||
|
||||
def serve(self) -> None:
|
||||
tick = 0
|
||||
starved_polls = 0
|
||||
while True:
|
||||
frame_pair = self._cameras.pull()
|
||||
if frame_pair is None:
|
||||
starved_polls += 1
|
||||
if starved_polls >= _FRAME_STARVED_BACKOFF_POLLS:
|
||||
time.sleep(_FRAME_STARVED_BACKOFF_SECONDS)
|
||||
if starved_polls % _FRAME_STARVED_LOG_EVERY == 0:
|
||||
cloudlog.error(f"visionipc delivered no frames for {starved_polls} polls; model is not running")
|
||||
continue
|
||||
|
||||
if starved_polls:
|
||||
cloudlog.warning(f"visionipc recovered after {starved_polls} frameless polls")
|
||||
starved_polls = 0
|
||||
|
||||
main_buf, extra_buf, main_stamp, extra_stamp = frame_pair
|
||||
self._sub.update(0)
|
||||
self._refresh_tunables(tick)
|
||||
|
||||
vehicle_speed = max(self._sub["carState"].vEgo, 0.0)
|
||||
lat_horizon = self._runtime.lat_delay + self._runtime.LAT_SMOOTH_SECONDS + self._lat_smooth_extra_sec + DT_MDL
|
||||
long_horizon = self._long_action_delay + DT_MDL
|
||||
|
||||
main_warp, extra_warp, live_calib_seen = self._warps.refresh(
|
||||
self._sub, self._cameras.layout.main_is_wide, self._cameras.layout.dual_camera
|
||||
)
|
||||
dropped_frames, frame_drop_ratio, prepare_only = self._frame_meter.sample(main_stamp.frame_id)
|
||||
|
||||
vision_bufs = {
|
||||
stream_name: extra_buf if "big" in stream_name else main_buf
|
||||
for stream_name in self._runtime.model_runner.vision_input_names
|
||||
}
|
||||
warp_map = {
|
||||
stream_name: extra_warp if "big" in stream_name else main_warp
|
||||
for stream_name in self._runtime.model_runner.vision_input_names
|
||||
}
|
||||
fresh_inputs = self._compose_inputs(vehicle_speed, lat_horizon, long_horizon)
|
||||
|
||||
started_at = time.perf_counter()
|
||||
outputs = self._runtime.run(vision_bufs, warp_map, fresh_inputs)
|
||||
execution_time = time.perf_counter() - started_at
|
||||
execution_us = int(execution_time * 1_000_000)
|
||||
|
||||
sample = PerfSample(
|
||||
frame_id=main_stamp.frame_id,
|
||||
model_eval_us=execution_us,
|
||||
model_dropped_frames=dropped_frames,
|
||||
model_backlog=max(0, dropped_frames),
|
||||
)
|
||||
self._perf_ring.push(sample)
|
||||
if dropped_frames > 0 or execution_us >= IQMODEL_EVAL_WARN_US:
|
||||
severity = "warning"
|
||||
if dropped_frames > 0 or execution_us >= IQMODEL_EVAL_ERROR_US:
|
||||
severity = "error"
|
||||
self._perf.emit(
|
||||
"iqmodeld_dropped_frames" if dropped_frames > 0 else "iqmodeld_slow_eval",
|
||||
severity=severity,
|
||||
frame_id=main_stamp.frame_id,
|
||||
total_time_us=execution_us,
|
||||
dropped_frames=dropped_frames,
|
||||
backlog=max(0, dropped_frames),
|
||||
samples=self._perf_ring.snapshot(),
|
||||
detail=(
|
||||
f"model_eval_us={execution_us} dropped_frames={dropped_frames} prepare_only={int(prepare_only)} "
|
||||
f"road_frame_id={self._sub['roadCameraState'].frameId}"
|
||||
),
|
||||
min_interval_s=0.25,
|
||||
)
|
||||
|
||||
if outputs is not None:
|
||||
self._publish(
|
||||
outputs,
|
||||
main_stamp,
|
||||
extra_stamp,
|
||||
self._sub["roadCameraState"].frameId,
|
||||
frame_drop_ratio,
|
||||
dropped_frames,
|
||||
execution_time,
|
||||
live_calib_seen,
|
||||
lat_horizon,
|
||||
long_horizon,
|
||||
vehicle_speed,
|
||||
)
|
||||
|
||||
self._frame_meter.commit(main_stamp.frame_id)
|
||||
tick += 1
|
||||
|
||||
|
||||
def main(demo: bool = False, channel_path: str | None = "auto"):
|
||||
if channel_path == "auto":
|
||||
channel_path = None
|
||||
params = Params()
|
||||
from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected
|
||||
if params.get_bool("IQEmacEnabled") or egpu_selected(params):
|
||||
from iqpilot.selfdrive.iqmodeld.model_channel import SMALL_CHANNEL
|
||||
channel_path = SMALL_CHANNEL
|
||||
InferenceDaemon(demo=demo, channel_path=channel_path).serve()
|
||||
|
||||
|
||||
__all__ = [
|
||||
"PROCESS_NAME",
|
||||
"IQP_NAV_MODEL_INFLUENCE_ENABLED",
|
||||
"TurnDirection",
|
||||
"CaptureStamp",
|
||||
"ReplayLedger",
|
||||
"NeuralEngineState",
|
||||
"CameraIngress",
|
||||
"CalibrationAtlas",
|
||||
"FrameDropMeter",
|
||||
"InferenceDaemon",
|
||||
"main",
|
||||
]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
import argparse
|
||||
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--demo", action="store_true", help="Run iqmodeld in demo mode.")
|
||||
args = parser.parse_args()
|
||||
main(demo=args.demo)
|
||||
except KeyboardInterrupt:
|
||||
cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
|
||||
except Exception:
|
||||
sentry.capture_exception()
|
||||
raise
|
||||
+46
@@ -0,0 +1,46 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.cereal import log
|
||||
|
||||
from iqpilot.selfdrive.controls.lib.drive_helpers import smooth_value
|
||||
|
||||
LAT_SMOOTH_SECONDS = 0.0
|
||||
LONG_SMOOTH_SECONDS = 0.3
|
||||
MIN_LAT_CONTROL_SPEED = 0.3
|
||||
DESIRE_LEN = 8
|
||||
|
||||
|
||||
def get_action_from_model(outputs: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
v_ego: float, lat_action_t: float, long_action_t: float,
|
||||
lat_smooth_seconds: float | None = None) -> log.ModelDataV2.Action:
|
||||
if "action" in outputs:
|
||||
desired_accel = float(outputs["action"][0, 1])
|
||||
desired_curvature = float(outputs["action"][0, 0]) / (max(1.0, v_ego)) ** 2
|
||||
should_stop = bool(v_ego < 0.3 and desired_accel < 0.1)
|
||||
else:
|
||||
from iqpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan
|
||||
from iqpilot.selfdrive.iqmodeld.config import ModelConstants, Plan
|
||||
plan = outputs["plan"][0]
|
||||
desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0],
|
||||
plan[:, Plan.ACCELERATION][:, 0],
|
||||
ModelConstants.T_IDXS,
|
||||
action_t=long_action_t)
|
||||
desired_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2],
|
||||
plan[:, Plan.ORIENTATION_RATE][:, 2],
|
||||
ModelConstants.T_IDXS, v_ego, lat_action_t)
|
||||
desired_accel, should_stop = float(desired_accel), bool(should_stop)
|
||||
desired_curvature = float(desired_curvature)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
|
||||
if v_ego > MIN_LAT_CONTROL_SPEED:
|
||||
lat_smooth = LAT_SMOOTH_SECONDS if lat_smooth_seconds is None else lat_smooth_seconds
|
||||
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, lat_smooth)
|
||||
else:
|
||||
desired_curvature = prev_action.desiredCurvature
|
||||
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
|
||||
desiredAcceleration=float(desired_accel),
|
||||
shouldStop=should_stop)
|
||||
+158
@@ -0,0 +1,158 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import os
|
||||
import urllib.request
|
||||
from pathlib import Path
|
||||
|
||||
from iqpilot.system.hardware.usb import egpu_dock_ready
|
||||
|
||||
USB_SYSFS_ROOT = "/sys/bus/usb/devices"
|
||||
|
||||
COMMA_LFS_BATCH_URL = "https://gitlab.com/commaai/openpilot-lfs.git/info/lfs/objects/batch"
|
||||
|
||||
DOWNLOAD_CHUNK = 4 * 1024 * 1024
|
||||
|
||||
|
||||
def usbgpu_present(sysfs_root: str = USB_SYSFS_ROOT) -> bool:
|
||||
return egpu_dock_ready(Path(sysfs_root))
|
||||
|
||||
|
||||
def egpu_present_consented(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool:
|
||||
try:
|
||||
if params is not None and params.get_bool("IQEgpuDisabled"):
|
||||
return False
|
||||
except Exception:
|
||||
pass
|
||||
return usbgpu_present(sysfs_root)
|
||||
|
||||
|
||||
def egpu_selected(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool:
|
||||
try:
|
||||
if params is not None and params.get_bool("IQEgpuDisabled"):
|
||||
return False
|
||||
if params is not None and params.get_bool("IQEgpuEnabled"):
|
||||
return True
|
||||
except Exception:
|
||||
pass
|
||||
return usbgpu_present(sysfs_root)
|
||||
|
||||
|
||||
def resolve_backend(emac_enabled: bool, egpu_enabled: bool, egpu_present: bool = False) -> str | None:
|
||||
if egpu_present:
|
||||
return "egpu"
|
||||
if emac_enabled:
|
||||
return "emac"
|
||||
if egpu_enabled:
|
||||
return "egpu"
|
||||
return None
|
||||
|
||||
|
||||
def egpu_pkl_path(meta: dict) -> str:
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_tinygrad.pkl")
|
||||
|
||||
|
||||
def onnx_cache_path(meta: dict) -> str:
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
return os.path.join(Paths.model_root(), f"{meta['model_name']}_{meta['sha256'][:8]}.onnx")
|
||||
|
||||
|
||||
def _sha256_file(path: str) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with open(path, "rb") as f:
|
||||
while chunk := f.read(DOWNLOAD_CHUNK):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def quarantine_artifact(path: str, why: str) -> None:
|
||||
try:
|
||||
if os.path.isfile(path):
|
||||
os.replace(path, path + ".unusable")
|
||||
except OSError:
|
||||
try:
|
||||
os.remove(path)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def local_onnx(meta: dict) -> str | None:
|
||||
path = onnx_cache_path(meta)
|
||||
if not os.path.isfile(path):
|
||||
return None
|
||||
size = int(meta.get("download", {}).get("size", 0))
|
||||
if size and os.path.getsize(path) != size:
|
||||
quarantine_artifact(path, "onnx size mismatch")
|
||||
return None
|
||||
if _sha256_file(path) != meta["sha256"]:
|
||||
quarantine_artifact(path, "onnx sha256 mismatch")
|
||||
return None
|
||||
return path
|
||||
|
||||
|
||||
def resolve_download_url(download_url: str, sha256: str, size: int, timeout: float = 30.0) -> str:
|
||||
if download_url.startswith("commalfs:"):
|
||||
oid = download_url.split(":", 1)[1]
|
||||
body = json.dumps({"operation": "download", "transfers": ["basic"],
|
||||
"objects": [{"oid": oid, "size": size}]}).encode()
|
||||
req = urllib.request.Request(COMMA_LFS_BATCH_URL, data=body, headers={
|
||||
"Accept": "application/vnd.git-lfs+json", "Content-Type": "application/vnd.git-lfs+json"})
|
||||
with urllib.request.urlopen(req, timeout=timeout) as r:
|
||||
d = json.load(r)
|
||||
return d["objects"][0]["actions"]["download"]["href"]
|
||||
return download_url
|
||||
|
||||
|
||||
def download_onnx(meta: dict, progress_cb=None) -> str:
|
||||
from iqpilot.selfdrive.iqmodeld.egpu_model import download_descriptor
|
||||
download_url, size = download_descriptor(meta)
|
||||
if not download_url:
|
||||
raise RuntimeError(f"model {meta['key']} has no download source; stage the onnx at {onnx_cache_path(meta)}")
|
||||
|
||||
url = resolve_download_url(download_url, meta["sha256"], size)
|
||||
path = onnx_cache_path(meta)
|
||||
os.makedirs(os.path.dirname(path), exist_ok=True)
|
||||
tmp = path + ".part"
|
||||
digest = hashlib.sha256()
|
||||
got = 0
|
||||
with urllib.request.urlopen(url, timeout=60) as r, open(tmp, "wb") as f:
|
||||
while chunk := r.read(DOWNLOAD_CHUNK):
|
||||
f.write(chunk)
|
||||
digest.update(chunk)
|
||||
got += len(chunk)
|
||||
if progress_cb is not None and size:
|
||||
progress_cb(got / size)
|
||||
if size and got != size:
|
||||
os.remove(tmp)
|
||||
raise RuntimeError(f"onnx download truncated: {got}/{size} bytes")
|
||||
if digest.hexdigest() != meta["sha256"]:
|
||||
os.remove(tmp)
|
||||
raise RuntimeError(f"onnx sha256 mismatch for {meta['key']}")
|
||||
os.replace(tmp, path)
|
||||
return path
|
||||
|
||||
|
||||
def patch_tinygrad_fetch_fw() -> None:
|
||||
import pathlib
|
||||
|
||||
import zstandard
|
||||
from tinygrad import helpers
|
||||
if getattr(helpers.fetch_fw, "_iq_patched", False):
|
||||
return
|
||||
_orig = helpers.fetch_fw
|
||||
|
||||
def fetch_fw(path, name, sha256):
|
||||
p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
|
||||
if p.is_file():
|
||||
blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
|
||||
if hashlib.sha256(blob).hexdigest() == sha256:
|
||||
return blob
|
||||
return _orig(path, name, sha256)
|
||||
|
||||
fetch_fw._iq_patched = True
|
||||
helpers.fetch_fw = fetch_fw
|
||||
+100
@@ -0,0 +1,100 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
EGPU_MODELS: dict[str, dict] = {
|
||||
"lebrowski": {
|
||||
"model_name": "big_driving_supercombo",
|
||||
"sha256": "a501760a9d1d5fef0eab2b8c5d122d06124fc26dc8e0782e0aa94b82a208f0ff",
|
||||
"output_len": 2580,
|
||||
"frame_skip": 4,
|
||||
"download": {"kind": "comma_lfs", "size": 1757355221},
|
||||
"input_shapes": {
|
||||
"img": (1, 12, 128, 256),
|
||||
"big_img": (1, 12, 128, 256),
|
||||
"desire_pulse": (1, 25, 8),
|
||||
"traffic_convention": (1, 2),
|
||||
"action_t": (1, 2),
|
||||
"features_buffer": (1, 24, 512),
|
||||
},
|
||||
"output_slices": {
|
||||
"lane_lines": slice(0, 528),
|
||||
"lane_lines_prob": slice(528, 536),
|
||||
"road_edges": slice(536, 800),
|
||||
"meta": slice(800, 855),
|
||||
"desire_pred": slice(855, 887),
|
||||
"pose": slice(887, 899),
|
||||
"wide_from_device_euler": slice(899, 905),
|
||||
"road_transform": slice(905, 917),
|
||||
"plan": slice(917, 1907),
|
||||
"lead": slice(1907, 2051),
|
||||
"lead_prob": slice(2051, 2054),
|
||||
"desire_state": slice(2054, 2062),
|
||||
"action": slice(2062, 2066),
|
||||
"hidden_state": slice(2066, 2578),
|
||||
"pad": slice(-2, None),
|
||||
},
|
||||
},
|
||||
"comma_small": {
|
||||
"model_name": "driving_supercombo",
|
||||
"sha256": "659727c4d4839adc4992a254409a54259a8756a743f2d567bf5fdc6579f8009b",
|
||||
"output_len": 2576,
|
||||
"frame_skip": 4,
|
||||
"download": {"kind": "comma_lfs", "size": 60881999},
|
||||
"output_slices": {
|
||||
"meta": slice(0, 55),
|
||||
"desire_pred": slice(55, 87),
|
||||
"pose": slice(87, 99),
|
||||
"wide_from_device_euler": slice(99, 105),
|
||||
"road_transform": slice(105, 117),
|
||||
"lane_lines": slice(117, 645),
|
||||
"lane_lines_prob": slice(645, 653),
|
||||
"road_edges": slice(653, 917),
|
||||
"lead": slice(917, 1061),
|
||||
"lead_prob": slice(1061, 1064),
|
||||
"hidden_state": slice(1064, 1576),
|
||||
"plan": slice(1576, 2566),
|
||||
"desire_state": slice(2566, 2574),
|
||||
"pad": slice(-2, None),
|
||||
},
|
||||
},
|
||||
}
|
||||
|
||||
DEFAULT_EGPU_MODEL = "lebrowski"
|
||||
|
||||
BIG_MODEL_NAME = "big_driving_supercombo"
|
||||
|
||||
|
||||
def get_egpu_model(key: str | None = None) -> dict:
|
||||
name = key or DEFAULT_EGPU_MODEL
|
||||
if name not in EGPU_MODELS:
|
||||
raise KeyError(f"unknown eGPU model {name!r}; known: {sorted(EGPU_MODELS)}")
|
||||
return {"key": name, **EGPU_MODELS[name]}
|
||||
|
||||
|
||||
def resolve_egpu_model(params, selected: str | bytes | None = None, allow_refresh: bool = True) -> dict | None:
|
||||
name = selected if selected is not None else (params.get("IQEmacModel") if params is not None else None)
|
||||
name = name.decode() if isinstance(name, bytes) else (name or "")
|
||||
if not name or name == DEFAULT_EGPU_MODEL:
|
||||
return get_egpu_model()
|
||||
from iqpilot.selfdrive.iqmodeld.big_catalog import cached_catalog, refresh_catalog
|
||||
catalog = cached_catalog(params)
|
||||
if name not in catalog and params is not None and allow_refresh:
|
||||
refresh_catalog(params)
|
||||
catalog = cached_catalog(params)
|
||||
meta = catalog.get(name)
|
||||
if meta is None or meta.get("model_name") != BIG_MODEL_NAME:
|
||||
return None
|
||||
return {"key": name, **meta}
|
||||
|
||||
|
||||
def download_descriptor(meta: dict) -> tuple[str, int]:
|
||||
dl = meta.get("download")
|
||||
if not dl:
|
||||
return "", 0
|
||||
if dl["kind"] == "comma_lfs":
|
||||
return f"commalfs:{meta['sha256']}", int(dl["size"])
|
||||
if dl["kind"] == "url":
|
||||
return str(dl["url"]), int(dl.get("size", 0))
|
||||
return "", 0
|
||||
+9
@@ -0,0 +1,9 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
|
||||
|
||||
try:
|
||||
load_private_module(__name__, "iqpilot_private.models.emac_model_meta")
|
||||
except ProprietaryModuleMissing:
|
||||
from iqpilot.models_private_src.emac_model_meta import *
|
||||
+269
@@ -0,0 +1,269 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
import capnp
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.cereal import log
|
||||
from iqpilot.selfdrive.iqmodeld.models.helpers import plan_x_idxs_helper
|
||||
from iqpilot.selfdrive.iqmodeld.config import ModelConstants, Plan
|
||||
from iqpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
|
||||
|
||||
SEND_RAW_PRED = os.getenv("SEND_RAW_PRED")
|
||||
ConfidenceClass = log.ModelDataV2.ConfidenceClass
|
||||
|
||||
|
||||
def pick_curvature(outputs: dict[str, np.ndarray], plan_rows: np.ndarray, vehicle_speed: float,
|
||||
action_horizon: float, synthetic_lane_logic: bool) -> float:
|
||||
direct_signal = None if synthetic_lane_logic else outputs.get("desired_curvature")
|
||||
if direct_signal is not None:
|
||||
return float(direct_signal[0, 0])
|
||||
|
||||
yaw_track = plan_rows[:, Plan.T_FROM_CURRENT_EULER][:, 2]
|
||||
yaw_rate_track = plan_rows[:, Plan.ORIENTATION_RATE][:, 2]
|
||||
return float(get_curvature_from_plan(yaw_track, yaw_rate_track, ModelConstants.T_IDXS, vehicle_speed, action_horizon))
|
||||
|
||||
|
||||
@dataclass
|
||||
class DrivePacketMemory:
|
||||
disengage_rollup: np.ndarray = field(default_factory=lambda: np.zeros(
|
||||
ModelConstants.CONFIDENCE_BUFFER_LEN * ModelConstants.DISENGAGE_WIDTH, dtype=np.float32))
|
||||
brake_watch_5: np.ndarray = field(default_factory=lambda: np.zeros(
|
||||
ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32))
|
||||
brake_watch_3: np.ndarray = field(default_factory=lambda: np.zeros(
|
||||
ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32))
|
||||
|
||||
|
||||
def _assign_xyz(builder, t_points, x_track, y_track, z_track,
|
||||
x_std=None, y_std=None, z_std=None) -> None:
|
||||
builder.t = t_points
|
||||
builder.x = x_track.tolist()
|
||||
builder.y = y_track.tolist()
|
||||
builder.z = z_track.tolist()
|
||||
if x_std is not None:
|
||||
builder.xStd = x_std.tolist()
|
||||
if y_std is not None:
|
||||
builder.yStd = y_std.tolist()
|
||||
if z_std is not None:
|
||||
builder.zStd = z_std.tolist()
|
||||
|
||||
|
||||
def _assign_xyva(builder, t_points, x_track, y_track, v_track, a_track,
|
||||
x_std=None, y_std=None, v_std=None, a_std=None) -> None:
|
||||
builder.t = t_points
|
||||
builder.x = x_track.tolist()
|
||||
builder.y = y_track.tolist()
|
||||
builder.v = v_track.tolist()
|
||||
builder.a = a_track.tolist()
|
||||
if x_std is not None:
|
||||
builder.xStd = x_std.tolist()
|
||||
if y_std is not None:
|
||||
builder.yStd = y_std.tolist()
|
||||
if v_std is not None:
|
||||
builder.vStd = v_std.tolist()
|
||||
if a_std is not None:
|
||||
builder.aStd = a_std.tolist()
|
||||
|
||||
|
||||
def fill_xyz_poly(builder, degree: int, x_track: np.ndarray, y_track: np.ndarray, z_track: np.ndarray) -> None:
|
||||
stacked = np.stack([x_track, y_track, z_track], axis=1)
|
||||
coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, stacked, deg=degree)
|
||||
builder.xCoefficients = coeffs[:, 0].tolist()
|
||||
builder.yCoefficients = coeffs[:, 1].tolist()
|
||||
builder.zCoefficients = coeffs[:, 2].tolist()
|
||||
|
||||
|
||||
def fill_lane_line_meta(builder, lane_lines, lane_probs: list[float]) -> None:
|
||||
builder.leftY = lane_lines[1].y[0]
|
||||
builder.leftProb = lane_probs[1]
|
||||
builder.rightY = lane_lines[2].y[0]
|
||||
builder.rightProb = lane_probs[2]
|
||||
|
||||
|
||||
def _roll_brake_watch(outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout) -> bool:
|
||||
memory.brake_watch_5[:-1] = memory.brake_watch_5[1:]
|
||||
memory.brake_watch_5[-1] = outputs["meta"][0, meta_layout.HARD_BRAKE_5][0]
|
||||
memory.brake_watch_3[:-1] = memory.brake_watch_3[1:]
|
||||
memory.brake_watch_3[-1] = outputs["meta"][0, meta_layout.HARD_BRAKE_3][0]
|
||||
return bool(
|
||||
(memory.brake_watch_5 > ModelConstants.FCW_THRESHOLDS_5MS2).all()
|
||||
and (memory.brake_watch_3 > ModelConstants.FCW_THRESHOLDS_3MS2).all()
|
||||
)
|
||||
|
||||
|
||||
def _confidence_bucket(outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout, frame_id: int):
|
||||
width = ModelConstants.DISENGAGE_WIDTH
|
||||
if frame_id % (2 * ModelConstants.MODEL_FREQ) == 0:
|
||||
brake_probs = outputs["meta"][0, meta_layout.BRAKE_DISENGAGE]
|
||||
gas_probs = outputs["meta"][0, meta_layout.GAS_DISENGAGE]
|
||||
steer_probs = outputs["meta"][0, meta_layout.STEER_OVERRIDE]
|
||||
takeover_curve = 1 - ((1 - brake_probs) * (1 - gas_probs) * (1 - steer_probs))
|
||||
independent = np.r_[takeover_curve[0], np.diff(takeover_curve) / (1 - takeover_curve[:-1])]
|
||||
memory.disengage_rollup[:-width] = memory.disengage_rollup[width:]
|
||||
memory.disengage_rollup[-width:] = independent
|
||||
|
||||
score = 0.0
|
||||
for idx in range(width):
|
||||
score += memory.disengage_rollup[idx * width + width - 1 - idx].item() / width
|
||||
|
||||
if score < ModelConstants.RYG_GREEN:
|
||||
return ConfidenceClass.green
|
||||
if score < ModelConstants.RYG_YELLOW:
|
||||
return ConfidenceClass.yellow
|
||||
return ConfidenceClass.red
|
||||
|
||||
|
||||
def _write_plan_family(model_packet, driving_packet, outputs: dict[str, np.ndarray]) -> None:
|
||||
plan_rows = outputs["plan"][0]
|
||||
plan_stds = outputs["plan_stds"][0]
|
||||
_assign_xyz(model_packet.position, ModelConstants.T_IDXS, *plan_rows[:, Plan.POSITION].T, *plan_stds[:, Plan.POSITION].T)
|
||||
_assign_xyz(model_packet.velocity, ModelConstants.T_IDXS, *plan_rows[:, Plan.VELOCITY].T)
|
||||
_assign_xyz(model_packet.acceleration, ModelConstants.T_IDXS, *plan_rows[:, Plan.ACCELERATION].T)
|
||||
_assign_xyz(model_packet.orientation, ModelConstants.T_IDXS, *plan_rows[:, Plan.T_FROM_CURRENT_EULER].T)
|
||||
_assign_xyz(model_packet.orientationRate, ModelConstants.T_IDXS, *plan_rows[:, Plan.ORIENTATION_RATE].T)
|
||||
fill_xyz_poly(driving_packet.path, ModelConstants.POLY_PATH_DEGREE, *plan_rows[:, Plan.POSITION].T)
|
||||
|
||||
|
||||
def _write_temporal_pose(model_packet, outputs: dict[str, np.ndarray]) -> None:
|
||||
pose_packet = model_packet.temporalPoseDEPRECATED
|
||||
if "sim_pose" in outputs:
|
||||
half_width = ModelConstants.POSE_WIDTH // 2
|
||||
pose_packet.trans = outputs["sim_pose"][0, :half_width].tolist()
|
||||
pose_packet.transStd = outputs["sim_pose_stds"][0, :half_width].tolist()
|
||||
pose_packet.rot = outputs["sim_pose"][0, half_width:].tolist()
|
||||
pose_packet.rotStd = outputs["sim_pose_stds"][0, half_width:].tolist()
|
||||
return
|
||||
|
||||
pose_packet.trans = outputs["plan"][0, 0, Plan.VELOCITY].tolist()
|
||||
pose_packet.transStd = outputs["plan_stds"][0, 0, Plan.VELOCITY].tolist()
|
||||
pose_packet.rot = outputs["plan"][0, 0, Plan.ORIENTATION_RATE].tolist()
|
||||
pose_packet.rotStd = outputs["plan_stds"][0, 0, Plan.ORIENTATION_RATE].tolist()
|
||||
|
||||
|
||||
def _write_lane_family(model_packet, driving_packet, outputs: dict[str, np.ndarray]) -> None:
|
||||
time_axis = plan_x_idxs_helper(ModelConstants, Plan, outputs)
|
||||
model_packet.init("laneLines", 4)
|
||||
for lane_idx in range(4):
|
||||
lane_builder = model_packet.laneLines[lane_idx]
|
||||
_assign_xyz(
|
||||
lane_builder,
|
||||
time_axis,
|
||||
np.array(ModelConstants.X_IDXS),
|
||||
outputs["lane_lines"][0, lane_idx, :, 0],
|
||||
outputs["lane_lines"][0, lane_idx, :, 1],
|
||||
)
|
||||
model_packet.laneLineStds = outputs["lane_lines_stds"][0, :, 0, 0].tolist()
|
||||
model_packet.laneLineProbs = outputs["lane_lines_prob"][0, 1::2].tolist()
|
||||
fill_lane_line_meta(driving_packet.laneLineMeta, model_packet.laneLines, model_packet.laneLineProbs)
|
||||
|
||||
model_packet.init("roadEdges", 2)
|
||||
for edge_idx in range(2):
|
||||
edge_builder = model_packet.roadEdges[edge_idx]
|
||||
_assign_xyz(
|
||||
edge_builder,
|
||||
time_axis,
|
||||
np.array(ModelConstants.X_IDXS),
|
||||
outputs["road_edges"][0, edge_idx, :, 0],
|
||||
outputs["road_edges"][0, edge_idx, :, 1],
|
||||
)
|
||||
model_packet.roadEdgeStds = outputs["road_edges_stds"][0, :, 0, 0].tolist()
|
||||
|
||||
|
||||
def _write_leads(model_packet, outputs: dict[str, np.ndarray]) -> None:
|
||||
model_packet.init("leadsV3", 3)
|
||||
for lead_idx in range(3):
|
||||
lead_builder = model_packet.leadsV3[lead_idx]
|
||||
_assign_xyva(
|
||||
lead_builder,
|
||||
ModelConstants.LEAD_T_IDXS,
|
||||
*outputs["lead"][0, lead_idx].T,
|
||||
*outputs["lead_stds"][0, lead_idx].T,
|
||||
)
|
||||
lead_builder.prob = outputs["lead_prob"][0, lead_idx].tolist()
|
||||
lead_builder.probTime = ModelConstants.LEAD_T_OFFSETS[lead_idx]
|
||||
|
||||
|
||||
def _write_meta(model_packet, outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout, frame_id: int) -> None:
|
||||
meta = model_packet.meta
|
||||
meta.desireState = outputs["desire_state"][0].reshape(-1).tolist()
|
||||
meta.desirePrediction = outputs["desire_pred"][0].reshape(-1).tolist()
|
||||
meta.engagedProb = outputs["meta"][0, meta_layout.ENGAGED].item()
|
||||
meta.init("disengagePredictions")
|
||||
|
||||
pred = meta.disengagePredictions
|
||||
pred.t = ModelConstants.META_T_IDXS
|
||||
pred.brakeDisengageProbs = outputs["meta"][0, meta_layout.BRAKE_DISENGAGE].tolist()
|
||||
pred.gasDisengageProbs = outputs["meta"][0, meta_layout.GAS_DISENGAGE].tolist()
|
||||
pred.steerOverrideProbs = outputs["meta"][0, meta_layout.STEER_OVERRIDE].tolist()
|
||||
pred.brake3MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_3].tolist()
|
||||
pred.brake4MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_4].tolist()
|
||||
pred.brake5MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_5].tolist()
|
||||
|
||||
if hasattr(meta_layout, "GAS_PRESS") and hasattr(meta_layout, "BRAKE_PRESS"):
|
||||
pred.gasPressProbs = outputs["meta"][0, meta_layout.GAS_PRESS].tolist()
|
||||
pred.brakePressProbs = outputs["meta"][0, meta_layout.BRAKE_PRESS].tolist()
|
||||
|
||||
meta.hardBrakePredicted = _roll_brake_watch(outputs, memory, meta_layout)
|
||||
model_packet.confidence = _confidence_bucket(outputs, memory, meta_layout, frame_id)
|
||||
|
||||
|
||||
def populate_drive_messages(primary_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
|
||||
outputs: dict[str, np.ndarray], action: log.ModelDataV2.Action,
|
||||
memory: DrivePacketMemory, vipc_frame_id: int, vipc_frame_id_extra: int,
|
||||
frame_id: int, frame_drop: float, timestamp_eof: int,
|
||||
model_execution_time: float, valid: bool, meta_layout) -> None:
|
||||
frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
|
||||
frame_drop_percent = frame_drop * 100
|
||||
primary_msg.valid = valid
|
||||
extended_msg.valid = valid
|
||||
|
||||
driving_packet = primary_msg.drivingModelData
|
||||
driving_packet.frameId = vipc_frame_id
|
||||
driving_packet.frameIdExtra = vipc_frame_id_extra
|
||||
driving_packet.frameDropPerc = frame_drop_percent
|
||||
driving_packet.modelExecutionTime = model_execution_time
|
||||
driving_packet.action = action
|
||||
|
||||
model_packet = extended_msg.modelV2
|
||||
model_packet.frameId = vipc_frame_id
|
||||
model_packet.frameIdExtra = vipc_frame_id_extra
|
||||
model_packet.frameAge = frame_age
|
||||
model_packet.frameDropPerc = frame_drop_percent
|
||||
model_packet.timestampEof = timestamp_eof
|
||||
model_packet.modelExecutionTime = model_execution_time
|
||||
model_packet.action = action
|
||||
|
||||
_write_plan_family(model_packet, driving_packet, outputs)
|
||||
_write_temporal_pose(model_packet, outputs)
|
||||
_write_lane_family(model_packet, driving_packet, outputs)
|
||||
_write_leads(model_packet, outputs)
|
||||
_write_meta(model_packet, outputs, memory, meta_layout, vipc_frame_id)
|
||||
|
||||
if SEND_RAW_PRED:
|
||||
model_packet.rawPredictions = outputs["raw_pred"].tobytes()
|
||||
|
||||
|
||||
def populate_odometry_message(msg: capnp._DynamicStructBuilder, outputs: dict[str, np.ndarray],
|
||||
vipc_frame_id: int, vipc_dropped_frames: int,
|
||||
timestamp_eof: int, live_calib_seen: bool) -> None:
|
||||
msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
|
||||
odo = msg.cameraOdometry
|
||||
odo.frameId = vipc_frame_id
|
||||
odo.timestampEof = timestamp_eof
|
||||
odo.trans = outputs["pose"][0, :3].tolist()
|
||||
odo.rot = outputs["pose"][0, 3:].tolist()
|
||||
odo.wideFromDeviceEuler = outputs["wide_from_device_euler"][0, :].tolist()
|
||||
odo.roadTransformTrans = outputs["road_transform"][0, :3].tolist()
|
||||
odo.transStd = outputs["pose_stds"][0, :3].tolist()
|
||||
odo.rotStd = outputs["pose_stds"][0, 3:].tolist()
|
||||
odo.wideFromDeviceEulerStd = outputs["wide_from_device_euler_stds"][0, :].tolist()
|
||||
odo.roadTransformTransStd = outputs["road_transform_stds"][0, :3].tolist()
|
||||
|
||||
__all__ = [
|
||||
"DrivePacketMemory",
|
||||
"pick_curvature",
|
||||
"populate_drive_messages",
|
||||
"populate_odometry_message",
|
||||
]
|
||||
+96
@@ -0,0 +1,96 @@
|
||||
#!/usr/bin/env python3
|
||||
import codecs
|
||||
import pathlib
|
||||
import pickle
|
||||
import sys
|
||||
from collections.abc import Iterable
|
||||
from typing import Any
|
||||
|
||||
from iqpilot.cereal import custom
|
||||
from tinygrad.nn.onnx import OnnxPBParser
|
||||
|
||||
from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
|
||||
from iqpilot.selfdrive.iqmodeld.config import Meta
|
||||
|
||||
|
||||
ModelBundle = custom.IQModelManager.ModelBundle
|
||||
|
||||
|
||||
def _blank_proto_doc() -> dict[str, Any]:
|
||||
return {"graph": {"input": [], "output": []}, "metadata_props": []}
|
||||
|
||||
|
||||
class TelemetryEnvelopeParser(OnnxPBParser):
|
||||
def _parse_ModelProto(self) -> dict:
|
||||
envelope = _blank_proto_doc()
|
||||
for fid, wire_type in self._parse_message(self.reader.len):
|
||||
if fid == 7:
|
||||
envelope["graph"] = self._parse_GraphProto()
|
||||
elif fid == 14:
|
||||
envelope["metadata_props"].append(self._parse_StringStringEntryProto())
|
||||
else:
|
||||
self.reader.skip_field(wire_type)
|
||||
return envelope
|
||||
|
||||
|
||||
def _shape_fingerprint(value_info: dict[str, Any]) -> tuple[str, tuple[int, ...]]:
|
||||
resolved = []
|
||||
for axis in value_info["parsed_type"].shape:
|
||||
resolved.append(int(axis) if isinstance(axis, int) else 0)
|
||||
return value_info["name"], tuple(resolved)
|
||||
|
||||
|
||||
def _lookup_metadata(props: Iterable[dict[str, Any]], wanted_key: str) -> str | Any:
|
||||
for entry in props:
|
||||
if entry["key"] == wanted_key:
|
||||
return entry["value"]
|
||||
return None
|
||||
|
||||
|
||||
class Meta20hz(Meta):
|
||||
ENGAGED = slice(0, 1)
|
||||
GAS_DISENGAGE = slice(1, 31, 6)
|
||||
BRAKE_DISENGAGE = slice(2, 31, 6)
|
||||
STEER_OVERRIDE = slice(3, 31, 6)
|
||||
HARD_BRAKE_3 = slice(4, 31, 6)
|
||||
HARD_BRAKE_4 = slice(5, 31, 6)
|
||||
HARD_BRAKE_5 = slice(6, 31, 6)
|
||||
GAS_PRESS = slice(31, 55, 4)
|
||||
BRAKE_PRESS = slice(32, 55, 4)
|
||||
LEFT_BLINKER = slice(33, 55, 4)
|
||||
RIGHT_BLINKER = slice(34, 55, 4)
|
||||
|
||||
|
||||
def select_meta_layout():
|
||||
active_bundle = get_active_bundle()
|
||||
return Meta20hz if active_bundle is not None and active_bundle.is20hz else Meta
|
||||
|
||||
|
||||
def _decoded_slices(props: Iterable[dict[str, Any]]):
|
||||
encoded = _lookup_metadata(props, "output_slices")
|
||||
assert encoded is not None, "output_slices not found in metadata"
|
||||
return pickle.loads(codecs.decode(encoded.encode(), "base64"))
|
||||
|
||||
|
||||
def _graph_shape_table(graph_doc: dict[str, Any], field_name: str) -> dict[str, tuple[int, ...]]:
|
||||
return dict(_shape_fingerprint(item) for item in graph_doc[field_name])
|
||||
|
||||
|
||||
def build_metadata_record(model_path):
|
||||
parsed = TelemetryEnvelopeParser(model_path).parse()
|
||||
props = parsed["metadata_props"]
|
||||
graph = parsed["graph"]
|
||||
return {
|
||||
"model_checkpoint": _lookup_metadata(props, "model_checkpoint"),
|
||||
"output_slices": _decoded_slices(props),
|
||||
"input_shapes": _graph_shape_table(graph, "input"),
|
||||
"output_shapes": _graph_shape_table(graph, "output"),
|
||||
}
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
model_path = pathlib.Path(sys.argv[1])
|
||||
metadata_path = model_path.parent / f"{model_path.stem}_metadata.pkl"
|
||||
with open(metadata_path, "wb") as handle:
|
||||
pickle.dump(build_metadata_record(model_path), handle)
|
||||
print(f"saved metadata to {metadata_path}")
|
||||
+58
@@ -0,0 +1,58 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import mmap
|
||||
import os
|
||||
import pickle
|
||||
import struct
|
||||
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
|
||||
SMALL_CHANNEL = "/dev/shm/iqpilot_smallmodel"
|
||||
BIG_CHANNEL = "/dev/shm/iqpilot_bigmodel"
|
||||
SHM_SIZE = 8 * 1024 * 1024
|
||||
HEADER = struct.Struct("<QqQ")
|
||||
|
||||
|
||||
class ModelChannel:
|
||||
def __init__(self, path: str, create: bool):
|
||||
if create:
|
||||
fd = os.open(path, os.O_CREAT | os.O_RDWR, 0o600)
|
||||
os.ftruncate(fd, SHM_SIZE)
|
||||
else:
|
||||
fd = os.open(path, os.O_RDWR)
|
||||
self.mm = mmap.mmap(fd, SHM_SIZE)
|
||||
os.close(fd)
|
||||
if create:
|
||||
self.mm[:HEADER.size] = HEADER.pack(0, -1, 0)
|
||||
|
||||
def write(self, frame_id: int, payload: dict) -> None:
|
||||
data = pickle.dumps(payload, protocol=pickle.HIGHEST_PROTOCOL)
|
||||
if HEADER.size + len(data) > SHM_SIZE:
|
||||
cloudlog.error(f"model payload {len(data)} bytes exceeds shm {SHM_SIZE}, dropping frame {frame_id}")
|
||||
return
|
||||
seq = HEADER.unpack(self.mm[:HEADER.size])[0]
|
||||
HEADER.pack_into(self.mm, 0, seq + 1, frame_id, len(data))
|
||||
self.mm[HEADER.size:HEADER.size + len(data)] = data
|
||||
HEADER.pack_into(self.mm, 0, seq + 2, frame_id, len(data))
|
||||
|
||||
def peek_frame_id(self) -> int | None:
|
||||
seq, frame_id, length = HEADER.unpack(self.mm[:HEADER.size])
|
||||
if seq == 0 or seq % 2 != 0 or length == 0:
|
||||
return None
|
||||
return frame_id
|
||||
|
||||
def read(self) -> tuple[int, dict] | None:
|
||||
seq1, frame_id, length = HEADER.unpack(self.mm[:HEADER.size])
|
||||
if seq1 == 0 or seq1 % 2 != 0 or length == 0:
|
||||
return None
|
||||
data = bytes(self.mm[HEADER.size:HEADER.size + length])
|
||||
seq2 = HEADER.unpack(self.mm[:HEADER.size])[0]
|
||||
if seq1 != seq2:
|
||||
return None
|
||||
try:
|
||||
return frame_id, pickle.loads(data)
|
||||
except Exception:
|
||||
return None
|
||||
+80
@@ -0,0 +1,80 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import pickle
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
def _load_bundle(pkl_path: str, cam_w: int, cam_h: int, frame_skip: int) -> dict:
|
||||
with open(pkl_path, "rb") as f:
|
||||
bundle = pickle.load(f)
|
||||
if bundle.get("frame_skip") != frame_skip:
|
||||
raise RuntimeError(f"frame_skip {bundle.get('frame_skip')} != {frame_skip}")
|
||||
if (cam_w, cam_h) not in bundle:
|
||||
raise RuntimeError(f"missing {cam_w}x{cam_h}; has {[k for k in bundle if isinstance(k, tuple)]}")
|
||||
_verify_selftest(bundle, cam_w, cam_h)
|
||||
return bundle
|
||||
|
||||
|
||||
def _verify_selftest(bundle: dict, cam_w: int, cam_h: int) -> None:
|
||||
want = bundle.get("selftest")
|
||||
if not want:
|
||||
raise RuntimeError("warp artifact predates the self-test; recompiling")
|
||||
from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from iqpilot.selfdrive.iqmodeld.tools.compile_warp import selftest_digest
|
||||
nv12_size = get_nv12_info(cam_w, cam_h)[3]
|
||||
got = selftest_digest(bundle[(cam_w, cam_h)], cam_w, cam_h, nv12_size)
|
||||
if got != want:
|
||||
raise RuntimeError(f"warp self-test {got[:12]} != {want[:12]}; artifact computes differently here")
|
||||
|
||||
|
||||
class FrameWarp:
|
||||
|
||||
def __init__(self, cam_w: int, cam_h: int, frame_skip: int):
|
||||
from tinygrad.tensor import Tensor
|
||||
|
||||
pkl_path = os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl")
|
||||
bundle = None
|
||||
if os.path.isfile(pkl_path):
|
||||
try:
|
||||
bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip)
|
||||
except Exception as e:
|
||||
cloudlog.warning(f"warp artifact unusable ({e}); discarding and recompiling")
|
||||
os.remove(pkl_path)
|
||||
if bundle is None:
|
||||
cloudlog.warning(f"warp artifact missing; compiling for {cam_w}x{cam_h} (one-time)")
|
||||
from iqpilot.selfdrive.iqmodeld.tools.compile_warp import compile_warp
|
||||
compile_warp(cam_w, cam_h, pkl_path, frame_skip=frame_skip)
|
||||
cloudlog.warning(f"warp compiled -> {pkl_path}")
|
||||
bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip)
|
||||
self._jit = bundle[(cam_w, cam_h)]
|
||||
|
||||
self._npy = {"tfm": np.zeros((3, 3), dtype=np.float32), "big_tfm": np.zeros((3, 3), dtype=np.float32)}
|
||||
self._tensors = {k: Tensor(v, device="NPY").realize() for k, v in self._npy.items()}
|
||||
self._blob_cache: dict[tuple[str, int], object] = {}
|
||||
self._Tensor = Tensor
|
||||
|
||||
def _frame_tensor(self, key: str, buf):
|
||||
from tinygrad.device import Device
|
||||
arr = np.frombuffer(buf.data, dtype=np.uint8)
|
||||
ck = (key, arr.ctypes.data)
|
||||
t = self._blob_cache.get(ck)
|
||||
if t is None:
|
||||
t = self._Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype="uint8", device=Device.DEFAULT)
|
||||
self._blob_cache[ck] = t
|
||||
return t
|
||||
|
||||
def run(self, main_buf, extra_buf, main_tfm: np.ndarray, extra_tfm: np.ndarray) -> np.ndarray:
|
||||
self._npy["tfm"][:] = main_tfm
|
||||
self._npy["big_tfm"][:] = extra_tfm
|
||||
warped = self._jit(tfm=self._tensors["tfm"], big_tfm=self._tensors["big_tfm"],
|
||||
frame=self._frame_tensor("img", main_buf),
|
||||
big_frame=self._frame_tensor("big_img", extra_buf))
|
||||
return warped.numpy().astype(np.uint8, copy=False)
|
||||
+3
@@ -0,0 +1,3 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
+100
@@ -0,0 +1,100 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import re
|
||||
from pathlib import Path
|
||||
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
_MODEL_ROOT = Path(Paths.model_root())
|
||||
_OVERRIDE_KEYS = (
|
||||
"combinedRuntimeArtifact",
|
||||
"combinedSplitArtifact",
|
||||
"iqCombinedArtifact",
|
||||
)
|
||||
_SPLIT_ROLE_PATTERN = re.compile(r"^driving_(vision|policy|off_policy|on_policy)_(.+)_tinygrad\.pkl$")
|
||||
|
||||
|
||||
def _bundle_models(bundle) -> list:
|
||||
models = getattr(bundle, "models", None)
|
||||
return list(models) if models is not None else []
|
||||
|
||||
|
||||
def _bundle_override_map(bundle) -> dict[str, str]:
|
||||
result: dict[str, str] = {}
|
||||
for override in getattr(bundle, "overrides", None) or []:
|
||||
key = getattr(override, "key", None)
|
||||
value = getattr(override, "value", None)
|
||||
if key and value:
|
||||
result[str(key)] = str(value)
|
||||
return result
|
||||
|
||||
|
||||
def _artifact_name(model) -> str:
|
||||
return getattr(getattr(model, "artifact", None), "fileName", "") or ""
|
||||
|
||||
|
||||
def _split_suffixes(bundle) -> list[str]:
|
||||
suffixes: list[str] = []
|
||||
for model in _bundle_models(bundle):
|
||||
match = _SPLIT_ROLE_PATTERN.match(_artifact_name(model))
|
||||
if match:
|
||||
suffixes.append(match.group(2))
|
||||
return suffixes
|
||||
|
||||
|
||||
def _derived_candidates(bundle) -> list[str]:
|
||||
seen: set[str] = set()
|
||||
candidates: list[str] = []
|
||||
|
||||
for suffix in _split_suffixes(bundle):
|
||||
for candidate in (
|
||||
f"driving_combined_{suffix}.pkl",
|
||||
f"iqmodeld_combined_{suffix}.pkl",
|
||||
):
|
||||
if candidate not in seen:
|
||||
seen.add(candidate)
|
||||
candidates.append(candidate)
|
||||
|
||||
ref = getattr(bundle, "ref", None)
|
||||
if ref:
|
||||
short_ref = str(ref)[:8]
|
||||
for candidate in (
|
||||
f"driving_combined_{short_ref}.pkl",
|
||||
f"iqmodeld_combined_{short_ref}.pkl",
|
||||
):
|
||||
if candidate not in seen:
|
||||
seen.add(candidate)
|
||||
candidates.append(candidate)
|
||||
|
||||
return candidates
|
||||
|
||||
|
||||
def combined_split_artifact_candidates(bundle) -> list[Path]:
|
||||
explicit_env = os.getenv("IQMODEL_COMBINED_PKL")
|
||||
if explicit_env:
|
||||
explicit_path = Path(explicit_env)
|
||||
return [explicit_path if explicit_path.is_absolute() else _MODEL_ROOT / explicit_path]
|
||||
|
||||
overrides = _bundle_override_map(bundle)
|
||||
explicit_names = [overrides[key] for key in _OVERRIDE_KEYS if key in overrides]
|
||||
if explicit_names:
|
||||
return [_MODEL_ROOT / name for name in explicit_names]
|
||||
|
||||
return [_MODEL_ROOT / name for name in _derived_candidates(bundle)]
|
||||
|
||||
|
||||
def resolve_combined_split_artifact(bundle) -> Path | None:
|
||||
for candidate in combined_split_artifact_candidates(bundle):
|
||||
if candidate.is_file():
|
||||
return candidate
|
||||
return None
|
||||
|
||||
|
||||
def has_combined_split_artifact(bundle) -> bool:
|
||||
return resolve_combined_split_artifact(bundle) is not None
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
|
||||
|
||||
try:
|
||||
load_private_module(__name__, "iqpilot_private.models.fetcher")
|
||||
except ProprietaryModuleMissing:
|
||||
from iqpilot.models_private_src.fetcher import * # noqa: F403
|
||||
+313
@@ -0,0 +1,313 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import json
|
||||
import os
|
||||
import shutil
|
||||
from pathlib import Path
|
||||
|
||||
from iqpilot.cereal import custom
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
|
||||
try:
|
||||
load_private_module(__name__, "iqpilot_private.models.helpers")
|
||||
except ProprietaryModuleMissing:
|
||||
try:
|
||||
from iqpilot.models_private_src.helpers import * # noqa: F403
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
|
||||
ModelBundle = custom.IQModelManager.ModelBundle
|
||||
Runner = custom.IQModelManager.Runner
|
||||
_MODEL_ROOT = Path(Paths.model_root())
|
||||
_ACTIVE_BUNDLE_KEY = "ModelManager_ActiveBundle"
|
||||
_MODELS_CACHE_KEY = "ModelManager_ModelsCache"
|
||||
_RUNNER_CACHE_KEY = "ModelRunnerTypeCache"
|
||||
_DOWNLOAD_INDEX_KEY = "ModelManager_DownloadIndex"
|
||||
_PENDING_INDEX_KEY = "ModelManager_PendingIndex"
|
||||
_PENDING_MODEL_RESTORE_FILE = "/data/k3_pending_model_restore"
|
||||
_STOCK_RUNNER = int(Runner.stock)
|
||||
_TINYGRAD_RUNNER = int(Runner.tinygrad)
|
||||
_SNPE_RUNNER = int(Runner.snpe)
|
||||
|
||||
_DEFAULT_MODEL_DIR = Path(__file__).resolve().parents[1] / "default_model"
|
||||
_DEFAULT_BUNDLE_JSON = _DEFAULT_MODEL_DIR / "bundle.json"
|
||||
_DEFAULT_BUNDLE_REF = "default"
|
||||
|
||||
|
||||
def get_default_model_bundle(_bundles):
|
||||
return None
|
||||
|
||||
|
||||
def _coerce_runner_value(value) -> int | None:
|
||||
raw = getattr(value, "raw", value)
|
||||
try:
|
||||
return int(raw)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
def _bundle_models(bundle) -> list:
|
||||
models = getattr(bundle, "models", None)
|
||||
return list(models) if models is not None else []
|
||||
|
||||
|
||||
def _bundle_needs_runtime_upgrade(bundle) -> bool:
|
||||
if bundle is None:
|
||||
return False
|
||||
|
||||
if _coerce_runner_value(getattr(bundle, "runner", None)) == _SNPE_RUNNER:
|
||||
return True
|
||||
|
||||
for model in _bundle_models(bundle):
|
||||
file_name = getattr(getattr(model, "artifact", None), "fileName", "") or ""
|
||||
if file_name.endswith(".thneed"):
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
|
||||
def _load_cached_manifest_bundles(params: Params):
|
||||
cached = params.get(_MODELS_CACHE_KEY) or {}
|
||||
bundles = []
|
||||
for raw_bundle in cached.get("bundles", []):
|
||||
try:
|
||||
min_selector_version = int(raw_bundle.get("minimumSelectorVersion", raw_bundle.get("minimum_selector_version", 0)))
|
||||
compatibility_view = dict(raw_bundle)
|
||||
compatibility_view["minimumSelectorVersion"] = min_selector_version
|
||||
is_compatible = globals().get("is_bundle_version_compatible")
|
||||
if is_compatible is not None and not is_compatible(compatibility_view):
|
||||
continue
|
||||
|
||||
if "short_name" in raw_bundle:
|
||||
from iqpilot.selfdrive.iqmodeld.models.fetcher import ManifestDecoder
|
||||
bundles.append(ManifestDecoder._decode_bundle(raw_bundle))
|
||||
continue
|
||||
|
||||
if "internalName" in raw_bundle:
|
||||
bundles.append(ModelBundle(**raw_bundle))
|
||||
continue
|
||||
|
||||
bundle = ModelBundle()
|
||||
bundle.index = int(raw_bundle["index"])
|
||||
bundle.internalName = raw_bundle.get("short_name")
|
||||
bundle.displayName = raw_bundle.get("display_name")
|
||||
bundle.status = 0
|
||||
bundle.generation = int(raw_bundle["generation"])
|
||||
bundle.environment = raw_bundle["environment"]
|
||||
bundle.runner = raw_bundle.get("runner", Runner.tinygrad)
|
||||
bundle.is20hz = raw_bundle.get("is_20hz", False)
|
||||
bundle.minimumSelectorVersion = int(min_selector_version)
|
||||
bundle.ref = raw_bundle.get("ref")
|
||||
bundle.overrides = []
|
||||
for key, value in raw_bundle.get("overrides", {}).items():
|
||||
override = custom.IQModelManager.Override()
|
||||
override.key = key
|
||||
override.value = value
|
||||
bundle.overrides.append(override)
|
||||
|
||||
bundle.models = []
|
||||
for raw_model in raw_bundle.get("models", []):
|
||||
model = custom.IQModelManager.Model()
|
||||
model.type = raw_model.get("type")
|
||||
for attr_name in ("artifact", "metadata"):
|
||||
raw_artifact = raw_model.get(attr_name)
|
||||
if not raw_artifact:
|
||||
continue
|
||||
artifact = custom.IQModelManager.Artifact()
|
||||
artifact.fileName = raw_artifact.get("file_name")
|
||||
download_uri = custom.IQModelManager.DownloadUri()
|
||||
download_uri.uri = raw_artifact.get("download_uri", {}).get("url")
|
||||
download_uri.sha256 = raw_artifact.get("download_uri", {}).get("sha256")
|
||||
artifact.downloadUri = download_uri
|
||||
setattr(model, attr_name, artifact)
|
||||
bundle.models.append(model)
|
||||
|
||||
bundles.append(bundle)
|
||||
except Exception:
|
||||
continue
|
||||
return bundles
|
||||
|
||||
|
||||
def _bundle_match_key(bundle) -> tuple[str | None, str | None, str | None]:
|
||||
return (
|
||||
getattr(bundle, "ref", None),
|
||||
getattr(bundle, "internalName", None),
|
||||
getattr(bundle, "displayName", None),
|
||||
)
|
||||
|
||||
|
||||
def _find_runtime_upgrade(bundle, params: Params, available_bundles=None):
|
||||
if not _bundle_needs_runtime_upgrade(bundle):
|
||||
return bundle
|
||||
|
||||
candidate_bundles = available_bundles if available_bundles is not None else _load_cached_manifest_bundles(params)
|
||||
ref, internal_name, display_name = _bundle_match_key(bundle)
|
||||
|
||||
for candidate in candidate_bundles:
|
||||
if getattr(candidate, "ref", None) and getattr(candidate, "ref", None) == ref:
|
||||
return candidate
|
||||
|
||||
for candidate in candidate_bundles:
|
||||
if getattr(candidate, "internalName", None) == internal_name:
|
||||
return candidate
|
||||
|
||||
for candidate in candidate_bundles:
|
||||
if getattr(candidate, "displayName", None) == display_name:
|
||||
return candidate
|
||||
|
||||
return None
|
||||
|
||||
|
||||
def bundle_files_ready(bundle) -> bool:
|
||||
if bundle is None:
|
||||
return False
|
||||
|
||||
for model in _bundle_models(bundle):
|
||||
artifact = getattr(model, "artifact", None)
|
||||
metadata = getattr(model, "metadata", None)
|
||||
for file_name in (getattr(metadata, "fileName", None), getattr(artifact, "fileName", None)):
|
||||
if file_name and not (_MODEL_ROOT / file_name).is_file():
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def persist_active_bundle(params: Params, bundle) -> None:
|
||||
params.put(_ACTIVE_BUNDLE_KEY, bundle.to_dict())
|
||||
params.remove(_RUNNER_CACHE_KEY)
|
||||
|
||||
|
||||
def _load_default_bundle_dict() -> dict:
|
||||
return json.loads(_DEFAULT_BUNDLE_JSON.read_text())
|
||||
|
||||
|
||||
def _default_bundle_filenames(bundle_dict: dict) -> list[str]:
|
||||
names = []
|
||||
for model in bundle_dict.get("models", []):
|
||||
for artifact in (model.get("metadata"), model.get("artifact")):
|
||||
file_name = artifact.get("fileName", "") if isinstance(artifact, dict) else ""
|
||||
if file_name:
|
||||
names.append(file_name)
|
||||
return names
|
||||
|
||||
|
||||
def is_default_bundle(bundle) -> bool:
|
||||
return bool(bundle is not None and getattr(bundle, "ref", None) == _DEFAULT_BUNDLE_REF)
|
||||
|
||||
|
||||
def ensure_default_model_files(bundle_dict: dict = None) -> None:
|
||||
bundle_dict = bundle_dict if bundle_dict is not None else _load_default_bundle_dict()
|
||||
try:
|
||||
_MODEL_ROOT.mkdir(parents=True, exist_ok=True)
|
||||
except OSError as e:
|
||||
cloudlog.exception(f"default_model: cannot create model root: {e}")
|
||||
return
|
||||
for file_name in _default_bundle_filenames(bundle_dict):
|
||||
src = _DEFAULT_MODEL_DIR / file_name
|
||||
dst = _MODEL_ROOT / file_name
|
||||
if not src.is_file():
|
||||
cloudlog.error(f"default_model: shipped asset missing {src}")
|
||||
continue
|
||||
if dst.is_file() and dst.stat().st_size == src.stat().st_size:
|
||||
continue
|
||||
try:
|
||||
shutil.copy2(src, dst)
|
||||
cloudlog.warning(f"default_model: staged {file_name} into model root")
|
||||
except OSError as e:
|
||||
cloudlog.exception(f"default_model: failed staging {file_name}: {e}")
|
||||
|
||||
|
||||
def select_default_model(params: Params = None) -> None:
|
||||
params = Params() if params is None else params
|
||||
bundle_dict = _load_default_bundle_dict()
|
||||
ensure_default_model_files(bundle_dict)
|
||||
params.remove(_DOWNLOAD_INDEX_KEY)
|
||||
params.remove(_PENDING_INDEX_KEY)
|
||||
params.put(_ACTIVE_BUNDLE_KEY, bundle_dict)
|
||||
params.remove(_RUNNER_CACHE_KEY)
|
||||
params.put(_RUNNER_CACHE_KEY, _TINYGRAD_RUNNER)
|
||||
try:
|
||||
if os.path.isfile(_PENDING_MODEL_RESTORE_FILE):
|
||||
os.remove(_PENDING_MODEL_RESTORE_FILE)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def seed_default_bundle_if_unset(params: Params = None) -> None:
|
||||
params = Params() if params is None else params
|
||||
if params.get(_ACTIVE_BUNDLE_KEY):
|
||||
return
|
||||
queued_download = params.get(_DOWNLOAD_INDEX_KEY)
|
||||
try:
|
||||
select_default_model(params)
|
||||
if queued_download is not None:
|
||||
params.put(_DOWNLOAD_INDEX_KEY, queued_download)
|
||||
cloudlog.warning("default_model: seeded Default (CD210) as active bundle")
|
||||
except Exception as e:
|
||||
cloudlog.exception(f"default_model: failed to seed default bundle: {e}")
|
||||
|
||||
|
||||
def get_runtime_bundle_upgrade(bundle, params: Params = None, available_bundles=None):
|
||||
params = Params() if params is None else params
|
||||
return _find_runtime_upgrade(bundle, params, available_bundles)
|
||||
|
||||
|
||||
def get_active_bundle(params: Params = None):
|
||||
params = Params() if params is None else params
|
||||
|
||||
try:
|
||||
active_bundle = params.get(_ACTIVE_BUNDLE_KEY) or {}
|
||||
if not active_bundle:
|
||||
return None
|
||||
is_compatible = globals().get("is_bundle_version_compatible")
|
||||
if is_compatible is not None and not is_compatible(active_bundle):
|
||||
return None
|
||||
bundle = ModelBundle(**active_bundle)
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
replacement = _find_runtime_upgrade(bundle, params)
|
||||
if replacement is not None and replacement is not bundle and bundle_files_ready(replacement):
|
||||
persist_active_bundle(params, replacement)
|
||||
return replacement
|
||||
|
||||
return bundle
|
||||
|
||||
|
||||
def get_active_model_runner(params: Params = None, force_check=False):
|
||||
params = Params() if params is None else params
|
||||
|
||||
active_bundle = get_active_bundle(params)
|
||||
if not active_bundle:
|
||||
seed_default_bundle_if_unset(params)
|
||||
active_bundle = get_active_bundle(params)
|
||||
if not active_bundle:
|
||||
if params.get(_RUNNER_CACHE_KEY) != str(_TINYGRAD_RUNNER):
|
||||
params.put(_RUNNER_CACHE_KEY, _TINYGRAD_RUNNER)
|
||||
return _TINYGRAD_RUNNER
|
||||
|
||||
cached_runner_type = params.get(_RUNNER_CACHE_KEY)
|
||||
if cached_runner_type and not force_check and isinstance(cached_runner_type, str) and cached_runner_type.isdigit():
|
||||
return int(cached_runner_type)
|
||||
|
||||
runner_type = _coerce_runner_value(active_bundle.runner)
|
||||
if runner_type == _SNPE_RUNNER:
|
||||
replacement = _find_runtime_upgrade(active_bundle, params)
|
||||
if replacement is not None and replacement is not active_bundle and bundle_files_ready(replacement):
|
||||
persist_active_bundle(params, replacement)
|
||||
runner_type = _coerce_runner_value(replacement.runner)
|
||||
else:
|
||||
if replacement is not None and getattr(replacement, "index", None) is not None and params.get(_DOWNLOAD_INDEX_KEY) is None:
|
||||
params.put(_DOWNLOAD_INDEX_KEY, int(replacement.index))
|
||||
cloudlog.warning(f"Queued tinygrad migration for retired bundle {getattr(active_bundle, 'internalName', '<unknown>')}")
|
||||
runner_type = _TINYGRAD_RUNNER
|
||||
|
||||
if cached_runner_type != runner_type:
|
||||
params.put(_RUNNER_CACHE_KEY, int(runner_type))
|
||||
|
||||
return runner_type
|
||||
+9
@@ -0,0 +1,9 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from iqpilot.common.steer_delay import cached_steer_delay
|
||||
|
||||
|
||||
class InferenceStateBase:
|
||||
def __init__(self):
|
||||
self.lat_delay = cached_steer_delay()
|
||||
+231
@@ -0,0 +1,231 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import os
|
||||
import pickle as _pk
|
||||
from dataclasses import dataclass, field
|
||||
from typing import TYPE_CHECKING, Any
|
||||
|
||||
import numpy as np
|
||||
from iqpilot.cereal import custom
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.system.hardware import TICI
|
||||
from iqpilot.system.hardware.hw import Paths as _hw_paths
|
||||
from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle as _fetch_bundle
|
||||
from iqpilot.selfdrive.iqmodeld.models.combined_artifact import has_combined_split_artifact
|
||||
|
||||
# ---- runtime type surface (native OpenCL/frame handles resolve to Any off-device) ----
|
||||
if TYPE_CHECKING:
|
||||
from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import GpuMemorySlot, RoadProjector
|
||||
else:
|
||||
def _resolve_native_types() -> tuple[Any, Any]:
|
||||
try:
|
||||
from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import GpuMemorySlot as iq_clmem
|
||||
from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector as iq_frame
|
||||
return iq_clmem, iq_frame
|
||||
except (ModuleNotFoundError, ImportError):
|
||||
return Any, Any
|
||||
|
||||
GpuMemorySlot, RoadProjector = _resolve_native_types()
|
||||
|
||||
NumpyDict = dict[str, np.ndarray]
|
||||
ShapeDict = dict[str, tuple[int, ...]]
|
||||
SliceDict = dict[str, slice]
|
||||
CLMemDict = dict[str, GpuMemorySlot]
|
||||
FrameDict = dict[str, RoadProjector]
|
||||
|
||||
ModelType = custom.IQModelManager.Model.Type
|
||||
Model = custom.IQModelManager.Model
|
||||
|
||||
SEND_RAW_PRED = os.getenv("SEND_RAW_PRED")
|
||||
CUSTOM_MODEL_PATH = _hw_paths.model_root()
|
||||
|
||||
_META_FIELDS = ("input_shapes", "output_slices")
|
||||
|
||||
USBGPU = "USBGPU" in os.environ
|
||||
|
||||
|
||||
def _configure_accelerator():
|
||||
"""Point tinygrad at the right backend. Must run before tinygrad is imported,
|
||||
which is why it fires at module import."""
|
||||
backend, extra = ("QCOM" if TICI else "CPU"), {}
|
||||
if USBGPU:
|
||||
backend, extra = "AMD", {"AMD_IFACE": "USB"}
|
||||
elif TICI:
|
||||
extra = {"QCOM_PRIORITY": "8"}
|
||||
os.environ["DEV"] = backend
|
||||
os.environ.update(extra)
|
||||
|
||||
|
||||
_configure_accelerator()
|
||||
|
||||
|
||||
# real metadata pkls are a few KB; anything bigger is a model artifact wrongly
|
||||
# referenced as metadata (pre-fix manifests self-referenced the artifact), and
|
||||
# unpickling it here double-loads the model onto the GPU
|
||||
_META_MAX_BYTES = 1 << 20
|
||||
|
||||
|
||||
def load_artifact_metadata(metadata_filename):
|
||||
"""Read one artifact's metadata pkl: (input shapes, output slices)."""
|
||||
try:
|
||||
path = os.path.join(CUSTOM_MODEL_PATH, metadata_filename)
|
||||
if os.path.getsize(path) > _META_MAX_BYTES:
|
||||
cloudlog.error(f"metadata pkl {metadata_filename} is artifact-sized, refusing to unpickle it")
|
||||
return tuple({} for _ in _META_FIELDS)
|
||||
with open(path, 'rb') as fh:
|
||||
blob = _pk.load(fh)
|
||||
return tuple(blob.get(field, {}) for field in _META_FIELDS)
|
||||
except Exception:
|
||||
cloudlog.exception(f"unreadable metadata pkl {metadata_filename}, continuing without it")
|
||||
return tuple({} for _ in _META_FIELDS)
|
||||
|
||||
|
||||
@dataclass
|
||||
class ArtifactSpec:
|
||||
"""One model of the active bundle plus its unpacked metadata."""
|
||||
model: Any
|
||||
metadata: Any = None
|
||||
input_shapes: ShapeDict = field(default_factory=dict)
|
||||
output_slices: SliceDict = field(default_factory=dict)
|
||||
|
||||
def __post_init__(self):
|
||||
self.metadata = self.model.metadata
|
||||
if self.metadata:
|
||||
self.input_shapes, self.output_slices = load_artifact_metadata(self.metadata.fileName)
|
||||
|
||||
|
||||
# kept name: some runners annotate against the old alias
|
||||
ModelData = ArtifactSpec
|
||||
|
||||
|
||||
class RunnerRoot:
|
||||
"""Shared root of the runner hierarchy.
|
||||
|
||||
Both ModelRunner and the per-model parser mixins (model_types.py) inherit
|
||||
this, so the concrete `TinygradRunner(ModelRunner, *Tinygrad)` diamond keeps
|
||||
one consistent parser registry + slice implementation.
|
||||
"""
|
||||
|
||||
parser_method_dict: dict
|
||||
_model_data: "ArtifactSpec | None"
|
||||
|
||||
def _slice_outputs(self, model_outputs):
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
class ModelRunner(RunnerRoot):
|
||||
"""Base for the tinygrad/ONNX runners.
|
||||
|
||||
Owns the active bundle's ArtifactSpecs and the shared slice/parse plumbing;
|
||||
subclasses provide input staging (prepare_inputs) and execution (_run_model).
|
||||
"""
|
||||
|
||||
# False for fused runners, which warp + manage temporal buffers inside the JIT
|
||||
uses_opencl_warp = True
|
||||
|
||||
def __init__(self):
|
||||
active = _fetch_bundle()
|
||||
if not active:
|
||||
raise ValueError("runner started without an active model bundle")
|
||||
|
||||
self.models = {spec.type.raw: ArtifactSpec(spec) for spec in _qcom_models(active)}
|
||||
self.is_20hz_3d = False
|
||||
self.is_20hz = active.is20hz
|
||||
self.inputs = {}
|
||||
self.parser_method_dict = {}
|
||||
self._model_data = None # active spec for the current operation
|
||||
self._parser = self._constants = None
|
||||
|
||||
def _active_spec(self):
|
||||
spec = self._model_data
|
||||
if spec is None:
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
return spec
|
||||
|
||||
# views proxied straight off the active artifact spec; kept out of the class
|
||||
# body (served via __getattr__) so the read surface stays data-driven
|
||||
_SPEC_VIEW = frozenset(("input_shapes", "output_slices"))
|
||||
|
||||
def __getattr__(self, name):
|
||||
if name == "constants":
|
||||
return self._constants
|
||||
if name == "vision_input_names":
|
||||
return list(self._active_spec().input_shapes)
|
||||
if name in ModelRunner._SPEC_VIEW:
|
||||
return getattr(self._active_spec(), name)
|
||||
raise AttributeError(name)
|
||||
|
||||
def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
|
||||
"""Stage image + numpy inputs for inference; implemented per backend."""
|
||||
raise NotImplementedError
|
||||
|
||||
def _run_model(self):
|
||||
"""Execute inference over the staged inputs; implemented per backend."""
|
||||
raise NotImplementedError
|
||||
|
||||
def run_model(self):
|
||||
# parsing happens inside each backend's _run_model
|
||||
return self._run_model()
|
||||
|
||||
def _slice_outputs(self, model_outputs):
|
||||
"""Split the flat output vector into named views per the artifact's slice table."""
|
||||
sliced = {}
|
||||
for tag, span in self._active_spec().output_slices.items():
|
||||
sliced[tag] = model_outputs[np.newaxis, span]
|
||||
if SEND_RAW_PRED:
|
||||
sliced["raw_pred"] = model_outputs.copy()
|
||||
return sliced
|
||||
|
||||
|
||||
# ---- runner selection (which backend to build for the active bundle) ----------
|
||||
|
||||
def _qcom_models(bundle) -> list:
|
||||
# usbeMac artifacts ride along in a bundle for the eGPU host; they are never
|
||||
# loaded on QCOM and must not affect runner classification
|
||||
return [m for m in bundle.models if m.type.raw != ModelType.usbeMac]
|
||||
|
||||
|
||||
def _single_artifact_prefix(bundle, prefix: str) -> bool:
|
||||
models = _qcom_models(bundle)
|
||||
return len(models) == 1 and models[0].artifact.fileName.startswith(prefix)
|
||||
|
||||
|
||||
def _is_fused_bundle(bundle) -> bool:
|
||||
return _single_artifact_prefix(bundle, "driving_fused_")
|
||||
|
||||
|
||||
def _is_supercombo_bundle(bundle) -> bool:
|
||||
return _single_artifact_prefix(bundle, "driving_supercombo_")
|
||||
|
||||
|
||||
def _is_split_bundle(bundle) -> bool:
|
||||
present = {m.type.raw for m in _qcom_models(bundle)}
|
||||
split_kinds = {ModelType.vision, ModelType.policy, ModelType.offPolicy, ModelType.onPolicy}
|
||||
return not present.isdisjoint(split_kinds)
|
||||
|
||||
|
||||
def get_model_runner() -> "ModelRunner":
|
||||
"""Build the runner backend that fits the active bundle (supercombo / fused /
|
||||
combined-split / split / single). Concrete runners are imported lazily so one
|
||||
backend failing to load can't take down the others at import time."""
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.tinygrad_runner import (TinygradRunner,
|
||||
TinygradSplitRunner)
|
||||
bundle = _fetch_bundle()
|
||||
# an eMac-only bundle (no QCOM-loadable models) runs the stock default on
|
||||
# device; the big host serves the bundle's precompiled artifact
|
||||
if not (bundle and bundle.models and _qcom_models(bundle)):
|
||||
return TinygradRunner(ModelType.supercombo)
|
||||
|
||||
if _is_supercombo_bundle(bundle):
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.supercombo_runner import TinygradSupercomboRunner
|
||||
return TinygradSupercomboRunner()
|
||||
if _is_fused_bundle(bundle):
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.fused_runner import TinygradFusedRunner
|
||||
return TinygradFusedRunner()
|
||||
if _is_split_bundle(bundle) and has_combined_split_artifact(bundle):
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.combined_split_runner import TinygradCombinedSplitRunner
|
||||
return TinygradCombinedSplitRunner()
|
||||
if _is_split_bundle(bundle):
|
||||
return TinygradSplitRunner()
|
||||
return TinygradRunner(_qcom_models(bundle)[0].type.raw)
|
||||
+245
@@ -0,0 +1,245 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import pickle
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.selfdrive.iqmodeld.models.combined_artifact import resolve_combined_split_artifact
|
||||
from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import NumpyDict, ShapeDict, SliceDict
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
|
||||
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
|
||||
from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
|
||||
|
||||
|
||||
def _tinygrad_imports():
|
||||
from tinygrad.device import Device
|
||||
from tinygrad.tensor import Tensor
|
||||
return Tensor, Device
|
||||
|
||||
|
||||
def _phase_roles(meta_by_role: dict[str, dict]) -> list[str]:
|
||||
return [name for name in meta_by_role if name != "vision"]
|
||||
|
||||
|
||||
def _phase_desire_key(policy_shapes: dict[str, tuple[int, ...]]) -> str:
|
||||
for key in policy_shapes:
|
||||
if key.startswith("desire"):
|
||||
return key
|
||||
raise KeyError("No desire-like key found in policy inputs")
|
||||
|
||||
|
||||
def _phase_image_keys(vision_shapes: dict[str, tuple[int, ...]]) -> tuple[str, str]:
|
||||
names = sorted(name for name in vision_shapes if "img" in name)
|
||||
road_key = next((name for name in names if "big" not in name), None)
|
||||
wide_key = next((name for name in names if "big" in name), None)
|
||||
if road_key is None or wide_key is None:
|
||||
raise ValueError(f"Unable to resolve road/wide image keys from {list(vision_shapes)}")
|
||||
return road_key, wide_key
|
||||
|
||||
|
||||
def _base_policy_keys(policy_shapes: dict[str, tuple[int, ...]]) -> set[str]:
|
||||
desired_key = _phase_desire_key(policy_shapes)
|
||||
return {desired_key, "features_buffer", "traffic_convention", "action_t"}
|
||||
|
||||
|
||||
def _slice_map(raw_blob: np.ndarray, slices: dict[str, slice]) -> NumpyDict:
|
||||
return {name: raw_blob[np.newaxis, section] for name, section in slices.items() if name != "pad"}
|
||||
|
||||
|
||||
class TinygradCombinedSplitRunner(ModelRunner):
|
||||
uses_opencl_warp: bool = False
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self._constants = SplitModelConstants
|
||||
self._parser = PhaseParser()
|
||||
self._bundle = get_active_bundle()
|
||||
self._artifact_path = resolve_combined_split_artifact(self._bundle)
|
||||
if self._artifact_path is None:
|
||||
raise FileNotFoundError("No IQ combined split artifact is available for the active bundle")
|
||||
|
||||
with open(self._artifact_path, "rb") as artifact:
|
||||
runtime_package: dict[Any, Any] = pickle.load(artifact)
|
||||
|
||||
self._meta_by_role = runtime_package.get("meta_by_role", runtime_package.get("metadata", {}))
|
||||
self._policy_roles = runtime_package.get("roles", _phase_roles(self._meta_by_role))
|
||||
self._camera_programs = {
|
||||
camera_key: spec
|
||||
for camera_key, spec in runtime_package.items()
|
||||
if isinstance(camera_key, tuple) and isinstance(spec, dict)
|
||||
}
|
||||
self._execute_bundle = runtime_package.get("execute_bundle", runtime_package.get("run_policy"))
|
||||
self._frame_stride = int(runtime_package.get("frame_stride", runtime_package.get("frame_skip", 1)))
|
||||
|
||||
if "vision" not in self._meta_by_role:
|
||||
raise ValueError("Combined split artifact is missing vision metadata")
|
||||
if not self._policy_roles:
|
||||
raise ValueError("Combined split artifact is missing policy roles")
|
||||
if self._execute_bundle is None:
|
||||
raise ValueError("Combined split artifact is missing execute_bundle")
|
||||
|
||||
self._vision_meta = self._meta_by_role["vision"]
|
||||
self._primary_policy_meta = self._meta_by_role[self._policy_roles[0]]
|
||||
self._desired_key = _phase_desire_key(self._primary_policy_meta["input_shapes"])
|
||||
self._road_key, self._wide_key = _phase_image_keys(self._vision_meta["input_shapes"])
|
||||
self._extra_policy_keys = [
|
||||
key for key in self._primary_policy_meta["input_shapes"]
|
||||
if key not in _base_policy_keys(self._primary_policy_meta["input_shapes"])
|
||||
]
|
||||
|
||||
self._queue_tensors: dict[str, Any] | None = None
|
||||
self._numpy_state: dict[str, np.ndarray] | None = None
|
||||
self._camera_shape: tuple[int, int] | None = None
|
||||
self._blob_cache: dict[tuple[str, int], Any] = {}
|
||||
self._last_desire = np.zeros(self._primary_policy_meta["input_shapes"][self._desired_key][2], dtype=np.float32)
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
return [self._road_key, self._wide_key]
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
merged: ShapeDict = dict(self._vision_meta["input_shapes"])
|
||||
for role in self._policy_roles:
|
||||
merged.update(self._meta_by_role[role]["input_shapes"])
|
||||
return merged
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
merged: SliceDict = dict(self._vision_meta["output_slices"])
|
||||
for role in self._policy_roles:
|
||||
merged.update(self._meta_by_role[role]["output_slices"])
|
||||
return merged
|
||||
|
||||
def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
|
||||
raise RuntimeError("Combined split runner manages its own warp + queue state; use run_fused()")
|
||||
|
||||
def _frame_blob(self, stream_name: str, buf):
|
||||
Tensor, Device = _tinygrad_imports()
|
||||
raw_frame = np.frombuffer(buf.data, dtype=np.uint8)
|
||||
cache_key = (stream_name, raw_frame.ctypes.data)
|
||||
tensor = self._blob_cache.get(cache_key)
|
||||
if tensor is None:
|
||||
tensor = Tensor.from_blob(raw_frame.ctypes.data, (raw_frame.size,), dtype="uint8", device=Device.DEFAULT)
|
||||
self._blob_cache[cache_key] = tensor
|
||||
return tensor
|
||||
|
||||
def _allocate_runtime_state(self, camera_width: int, camera_height: int) -> None:
|
||||
if self._queue_tensors is not None and self._camera_shape == (camera_width, camera_height):
|
||||
return
|
||||
if (camera_width, camera_height) not in self._camera_programs:
|
||||
raise RuntimeError(f"No combined split kernels available for {camera_width}x{camera_height}")
|
||||
|
||||
Tensor, Device = _tinygrad_imports()
|
||||
vision_shapes = self._vision_meta["input_shapes"]
|
||||
policy_shapes = self._primary_policy_meta["input_shapes"]
|
||||
|
||||
image_shape = vision_shapes[self._road_key]
|
||||
frame_history = image_shape[1] // 6
|
||||
queue_depth = self._frame_stride * (frame_history - 1) + 1
|
||||
frame_queue_shape = (queue_depth, 6, image_shape[2], image_shape[3])
|
||||
|
||||
feature_shape = policy_shapes["features_buffer"]
|
||||
desired_shape = policy_shapes[self._desired_key]
|
||||
traffic_shape = policy_shapes["traffic_convention"]
|
||||
action_shape = policy_shapes.get("action_t", traffic_shape)
|
||||
|
||||
numpy_state = {
|
||||
"tfm": np.zeros((3, 3), dtype=np.float32),
|
||||
"big_tfm": np.zeros((3, 3), dtype=np.float32),
|
||||
"desire": np.zeros(desired_shape[2], dtype=np.float32),
|
||||
"traffic_convention": np.zeros(traffic_shape, dtype=np.float32),
|
||||
"action_t": np.zeros(action_shape, dtype=np.float32),
|
||||
}
|
||||
for key in self._extra_policy_keys:
|
||||
numpy_state[key] = np.zeros(policy_shapes[key], dtype=np.float32)
|
||||
|
||||
queue_tensors = {
|
||||
"img_q": Tensor(np.zeros(frame_queue_shape, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize(),
|
||||
"big_img_q": Tensor(np.zeros(frame_queue_shape, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize(),
|
||||
"feat_q": Tensor(
|
||||
np.zeros((self._frame_stride * (feature_shape[1] - 1) + 1, feature_shape[0], feature_shape[2]), dtype=np.float32),
|
||||
device=Device.DEFAULT,
|
||||
).contiguous().realize(),
|
||||
"desire_q": Tensor(
|
||||
np.zeros((self._frame_stride * desired_shape[1], desired_shape[0], desired_shape[2]), dtype=np.float32),
|
||||
device=Device.DEFAULT,
|
||||
).contiguous().realize(),
|
||||
**{name: Tensor(value, device="NPY").realize() for name, value in numpy_state.items()},
|
||||
}
|
||||
|
||||
self._queue_tensors = queue_tensors
|
||||
self._numpy_state = numpy_state
|
||||
self._camera_shape = (camera_width, camera_height)
|
||||
|
||||
def _policy_inputs(self) -> dict[str, Any]:
|
||||
assert self._queue_tensors is not None
|
||||
tensor_names = ["feat_q", "desire_q", "desire", "traffic_convention", "action_t", *self._extra_policy_keys]
|
||||
return {name: self._queue_tensors[name] for name in tensor_names if name in self._queue_tensors}
|
||||
|
||||
def _merge_policy_outputs(self, raw_outputs: tuple[Any, ...]) -> NumpyDict:
|
||||
outputs = self._parser.parse_vision_outputs(
|
||||
_slice_map(raw_outputs[0].numpy().flatten(), self._vision_meta["output_slices"])
|
||||
)
|
||||
|
||||
has_on_policy = any(role == "on_policy" for role in self._policy_roles)
|
||||
for role_name, tensor_out in zip(self._policy_roles, raw_outputs[1:], strict=True):
|
||||
parsed = self._parser.parse_policy_outputs(
|
||||
_slice_map(tensor_out.numpy().flatten(), self._meta_by_role[role_name]["output_slices"])
|
||||
)
|
||||
if role_name == "off_policy" and has_on_policy:
|
||||
parsed.pop("plan", None)
|
||||
outputs.update(parsed)
|
||||
|
||||
if "planplus" in outputs and "plan" in outputs:
|
||||
outputs["plan"] = outputs["plan"] + outputs["planplus"]
|
||||
return outputs
|
||||
|
||||
def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
|
||||
main_buf = bufs[self._road_key]
|
||||
self._allocate_runtime_state(main_buf.width, main_buf.height)
|
||||
assert self._queue_tensors is not None and self._numpy_state is not None and self._camera_shape is not None
|
||||
|
||||
self._numpy_state["tfm"][:] = transforms[self._road_key]
|
||||
self._numpy_state["big_tfm"][:] = transforms[self._wide_key]
|
||||
|
||||
current_desire = numpy_inputs[self._desired_key].copy()
|
||||
current_desire[0] = 0
|
||||
self._numpy_state["desire"][:] = np.where(current_desire - self._last_desire > 0.99, current_desire, 0)
|
||||
self._last_desire[:] = current_desire
|
||||
|
||||
if "traffic_convention" in numpy_inputs:
|
||||
self._numpy_state["traffic_convention"][:] = numpy_inputs["traffic_convention"]
|
||||
if "action_t" in numpy_inputs:
|
||||
self._numpy_state["action_t"][:] = numpy_inputs["action_t"]
|
||||
for key in self._extra_policy_keys:
|
||||
if key in numpy_inputs:
|
||||
self._numpy_state[key][:] = numpy_inputs[key]
|
||||
|
||||
stage_inputs = self._camera_programs[self._camera_shape].get("stage_inputs", self._camera_programs[self._camera_shape].get("warp_enqueue"))
|
||||
if stage_inputs is None:
|
||||
raise RuntimeError("Combined split artifact camera entry is missing stage_inputs")
|
||||
|
||||
staged_main, staged_wide = stage_inputs(
|
||||
img_q=self._queue_tensors["img_q"],
|
||||
big_img_q=self._queue_tensors["big_img_q"],
|
||||
tfm=self._queue_tensors["tfm"],
|
||||
big_tfm=self._queue_tensors["big_tfm"],
|
||||
frame=self._frame_blob(self._road_key, bufs[self._road_key]),
|
||||
big_frame=self._frame_blob(self._wide_key, bufs[self._wide_key]),
|
||||
)
|
||||
raw_outputs = self._execute_bundle(img=staged_main, big_img=staged_wide, **self._policy_inputs())
|
||||
if not isinstance(raw_outputs, tuple):
|
||||
raw_outputs = (raw_outputs,)
|
||||
return self._merge_policy_outputs(raw_outputs)
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
raise RuntimeError("Combined split runner executes through run_fused()")
|
||||
+178
@@ -0,0 +1,178 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import pickle
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import (
|
||||
CUSTOM_MODEL_PATH, NumpyDict, ShapeDict, SliceDict,
|
||||
)
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
|
||||
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
|
||||
from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
|
||||
|
||||
|
||||
def _tinygrad_imports():
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.device import Device
|
||||
return Tensor, Device
|
||||
|
||||
|
||||
WARP_DEV = os.getenv('WARP_DEV')
|
||||
|
||||
|
||||
class TinygradFusedRunner(ModelRunner):
|
||||
uses_opencl_warp: bool = False
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
|
||||
self._constants = SplitModelConstants
|
||||
self._parser = PhaseParser()
|
||||
|
||||
if len(self.models) != 1:
|
||||
raise ValueError(f"fused bundle must have exactly one artifact, got {list(self.models)}")
|
||||
self._model_data = next(iter(self.models.values()))
|
||||
|
||||
pkl_path = os.path.join(CUSTOM_MODEL_PATH, self._model_data.model.artifact.fileName)
|
||||
with open(pkl_path, 'rb') as f:
|
||||
self._fused: dict[Any, Any] = pickle.load(f)
|
||||
|
||||
self._vision_meta = self._fused['metadata']['vision']
|
||||
self._on_meta = self._fused['metadata']['on_policy']
|
||||
self._off_meta = self._fused['metadata']['off_policy']
|
||||
self._run_policy = self._fused['run_policy']
|
||||
self._warp_jits: dict[tuple[int, int], Any] = {k: v for k, v in self._fused.items() if isinstance(k, tuple)}
|
||||
if not self._warp_jits:
|
||||
raise ValueError("fused pkl has no warp JITs")
|
||||
|
||||
self._frame_skip: int = int(self._fused.get('frame_skip', 4))
|
||||
|
||||
self._queues: dict[str, Any] | None = None
|
||||
self._npy_buffers: dict[str, np.ndarray] | None = None
|
||||
self._cam_resolution: tuple[int, int] | None = None
|
||||
self._blob_cache: dict[tuple[str, int], Any] = {}
|
||||
|
||||
def _frame_tensor(self, key, buf):
|
||||
Tensor, Device = _tinygrad_imports()
|
||||
arr = np.frombuffer(buf.data, dtype=np.uint8)
|
||||
ck = (key, arr.ctypes.data)
|
||||
t = self._blob_cache.get(ck)
|
||||
if t is None:
|
||||
t = Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype='uint8', device=Device.DEFAULT)
|
||||
self._blob_cache[ck] = t
|
||||
return t
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
return ['img', 'big_img']
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
return {**self._vision_meta['input_shapes'], **self._on_meta['input_shapes']}
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
merged: SliceDict = {}
|
||||
for src in (self._vision_meta['output_slices'], self._on_meta['output_slices'], self._off_meta['output_slices']):
|
||||
merged.update({k: v for k, v in src.items() if k != 'pad'})
|
||||
return merged
|
||||
|
||||
def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
|
||||
raise RuntimeError("fused runner has no OpenCL path; use run_fused()")
|
||||
|
||||
def _ensure_queues(self, cam_w: int, cam_h: int) -> None:
|
||||
if self._queues is not None and self._cam_resolution == (cam_w, cam_h):
|
||||
return
|
||||
if (cam_w, cam_h) not in self._warp_jits:
|
||||
raise RuntimeError(f"no warp JIT for {cam_w}x{cam_h}; have {sorted(self._warp_jits)}")
|
||||
|
||||
Tensor, Device = _tinygrad_imports()
|
||||
img_shape = self._vision_meta['input_shapes']['img']
|
||||
fb = self._on_meta['input_shapes']['features_buffer']
|
||||
dp = self._on_meta['input_shapes']['desire_pulse']
|
||||
n_frames = img_shape[1] // 6
|
||||
img_buf_shape = (self._frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
|
||||
|
||||
zeros_u8 = lambda shp: Tensor(np.zeros(shp, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize()
|
||||
zeros_f32 = lambda shp: Tensor(np.zeros(shp, dtype=np.float32), device=Device.DEFAULT).contiguous().realize()
|
||||
|
||||
self._queues = {
|
||||
'img_q': zeros_u8(img_buf_shape),
|
||||
'big_img_q': zeros_u8(img_buf_shape),
|
||||
'feat_q': zeros_f32((self._frame_skip * (fb[1] - 1) + 1, fb[0], fb[2])),
|
||||
'desire_q': zeros_f32((self._frame_skip * dp[1], dp[0], dp[2])),
|
||||
}
|
||||
on_shapes = self._on_meta['input_shapes']
|
||||
captured = self._run_policy.captured
|
||||
jit_shapes = {
|
||||
name: tuple(int(s) for s in view.shape)
|
||||
for name, (view, _vars, _dtype, _device) in zip(captured.expected_names, captured.expected_input_info)
|
||||
}
|
||||
|
||||
def policy_input_shape(name):
|
||||
shape = on_shapes.get(name, jit_shapes.get(name))
|
||||
if shape is None:
|
||||
raise ValueError(f"fused pkl declares no shape for policy input {name}")
|
||||
return shape
|
||||
|
||||
self._npy_buffers = {
|
||||
'desire': np.zeros(dp[2], dtype=np.float32),
|
||||
'traffic_convention': np.zeros(policy_input_shape('traffic_convention'), dtype=np.float32),
|
||||
'tfm': np.zeros((3, 3), dtype=np.float32),
|
||||
'big_tfm': np.zeros((3, 3), dtype=np.float32),
|
||||
}
|
||||
if 'action_t' in jit_shapes:
|
||||
self._npy_buffers['action_t'] = np.zeros(policy_input_shape('action_t'), dtype=np.float32)
|
||||
self._cam_resolution = (cam_w, cam_h)
|
||||
|
||||
def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
|
||||
Tensor, Device = _tinygrad_imports()
|
||||
|
||||
main_buf = bufs['img']
|
||||
self._ensure_queues(main_buf.width, main_buf.height)
|
||||
assert self._queues is not None and self._npy_buffers is not None
|
||||
|
||||
desire_key = next((k for k in numpy_inputs if k.startswith('desire')), None)
|
||||
if desire_key is not None:
|
||||
self._npy_buffers['desire'][:] = numpy_inputs[desire_key]
|
||||
if 'traffic_convention' in numpy_inputs:
|
||||
self._npy_buffers['traffic_convention'][:] = numpy_inputs['traffic_convention']
|
||||
if 'action_t' in numpy_inputs and 'action_t' in self._npy_buffers:
|
||||
self._npy_buffers['action_t'][:] = numpy_inputs['action_t']
|
||||
self._npy_buffers['tfm'][:] = transforms['img']
|
||||
self._npy_buffers['big_tfm'][:] = transforms['big_img']
|
||||
|
||||
npy = lambda key: Tensor(self._npy_buffers[key], device='NPY')
|
||||
|
||||
frame = self._frame_tensor('img', bufs['img'])
|
||||
big_frame = self._frame_tensor('big_img', bufs['big_img'])
|
||||
|
||||
warp_jit = self._warp_jits[self._cam_resolution]
|
||||
img, big_img = warp_jit(img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'],
|
||||
tfm=npy('tfm'), big_tfm=npy('big_tfm'), frame=frame, big_frame=big_frame)
|
||||
|
||||
policy_inputs = dict(
|
||||
img=img, big_img=big_img, feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'],
|
||||
desire=npy('desire'), traffic_convention=npy('traffic_convention'))
|
||||
if 'action_t' in self._npy_buffers:
|
||||
policy_inputs['action_t'] = npy('action_t')
|
||||
vision_out_t, on_out_t, off_out_t = self._run_policy(**policy_inputs)
|
||||
|
||||
def _slice(tensor_out, meta) -> NumpyDict:
|
||||
flat = tensor_out.numpy().flatten()
|
||||
return {k: flat[np.newaxis, sl] for k, sl in meta['output_slices'].items() if k != 'pad'}
|
||||
|
||||
parsed: NumpyDict = {}
|
||||
parsed.update(self._parser.parse_vision_outputs(_slice(vision_out_t, self._vision_meta)))
|
||||
parsed.update(self._parser.parse_policy_outputs(_slice(off_out_t, self._off_meta)))
|
||||
parsed.update(self._parser.parse_policy_outputs(_slice(on_out_t, self._on_meta)))
|
||||
return parsed
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
raise RuntimeError("fused path goes through run_fused(), not _run_model()")
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from abc import ABC
|
||||
from collections.abc import Callable
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelType, NumpyDict
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import RunnerRoot
|
||||
from iqpilot.selfdrive.iqmodeld.parser import ArchiveParser, PhaseParser
|
||||
|
||||
|
||||
class _ParserRole(RunnerRoot, ABC):
|
||||
def _bind_parser_role(self,
|
||||
selector: int,
|
||||
parser_builder: Callable[[], object],
|
||||
projector: Callable[[object, NumpyDict], NumpyDict]) -> None:
|
||||
parser = parser_builder()
|
||||
self.parser_method_dict[selector] = lambda model_blob: projector(parser, self._slice_outputs(model_blob))
|
||||
|
||||
|
||||
def _phase_policy(parser: PhaseParser, sliced_outputs: NumpyDict) -> NumpyDict:
|
||||
return parser.parse_policy_outputs(sliced_outputs)
|
||||
|
||||
|
||||
def _phase_vision(parser: PhaseParser, sliced_outputs: NumpyDict) -> NumpyDict:
|
||||
return parser.parse_vision_outputs(sliced_outputs)
|
||||
|
||||
|
||||
def _archive_combined(parser: ArchiveParser, sliced_outputs: NumpyDict) -> NumpyDict:
|
||||
return parser.parse_outputs(sliced_outputs)
|
||||
|
||||
|
||||
class OffPolicyTinygrad(_ParserRole, ABC):
|
||||
def __init__(self):
|
||||
self._bind_parser_role(ModelType.offPolicy, PhaseParser, _phase_policy)
|
||||
|
||||
|
||||
class OnPolicyTinygrad(_ParserRole, ABC):
|
||||
def __init__(self):
|
||||
self._bind_parser_role(ModelType.onPolicy, PhaseParser, _phase_policy)
|
||||
|
||||
|
||||
class PolicyTinygrad(_ParserRole, ABC):
|
||||
def __init__(self):
|
||||
self._bind_parser_role(ModelType.policy, PhaseParser, _phase_policy)
|
||||
|
||||
|
||||
class VisionTinygrad(_ParserRole, ABC):
|
||||
def __init__(self):
|
||||
self._bind_parser_role(ModelType.vision, PhaseParser, _phase_vision)
|
||||
|
||||
|
||||
class SupercomboTinygrad(_ParserRole, ABC):
|
||||
def __init__(self):
|
||||
self._bind_parser_role(ModelType.supercombo, ArchiveParser, _archive_combined)
|
||||
+339
@@ -0,0 +1,339 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import math
|
||||
import os
|
||||
import pickle
|
||||
import re
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import CUSTOM_MODEL_PATH, NumpyDict, ShapeDict, SliceDict
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
|
||||
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
|
||||
from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
|
||||
|
||||
|
||||
def _tinygrad_imports():
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.device import Device
|
||||
return Tensor, Device
|
||||
|
||||
|
||||
def _captured_queue_depth(warp_jit: Any) -> int | None:
|
||||
captured = getattr(warp_jit, "captured", None)
|
||||
infos = getattr(captured, "expected_input_info", None)
|
||||
if not infos or len(infos) < 2:
|
||||
return None
|
||||
|
||||
view_repr = repr(infos[1][0])
|
||||
dims = [int(val) for val in re.findall(r"arg=(\d+)", view_repr)]
|
||||
return dims[0] if len(dims) >= 4 else None
|
||||
|
||||
|
||||
def _captured_devices(warp_jit: Any) -> set[str]:
|
||||
captured = getattr(warp_jit, "captured", None)
|
||||
infos = getattr(captured, "expected_input_info", None)
|
||||
if not infos:
|
||||
return set()
|
||||
|
||||
devices: set[str] = set()
|
||||
for info in infos:
|
||||
if isinstance(info, tuple) and len(info) >= 4 and isinstance(info[3], str):
|
||||
devices.add(info[3])
|
||||
return devices
|
||||
|
||||
|
||||
def _captured_expected_names(jit_obj: Any) -> list[str]:
|
||||
captured = getattr(jit_obj, "captured", None)
|
||||
names = getattr(captured, "expected_names", None)
|
||||
return list(names) if names else []
|
||||
|
||||
|
||||
def _file_sha256(path: str) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with open(path, "rb") as f:
|
||||
for chunk in iter(lambda: f.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _is_jit_arg_mismatch(err: BaseException) -> bool:
|
||||
return "args mismatch in JIT" in str(err)
|
||||
|
||||
|
||||
class TinygradSupercomboRunner(ModelRunner):
|
||||
uses_opencl_warp: bool = False
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self._constants = SplitModelConstants
|
||||
self._parser = PhaseParser()
|
||||
|
||||
if len(self.models) != 1:
|
||||
raise ValueError(f"supercombo bundle must have exactly one artifact, got {list(self.models)}")
|
||||
self._model_data = next(iter(self.models.values()))
|
||||
|
||||
pkl_path = os.path.join(CUSTOM_MODEL_PATH, self._model_data.model.artifact.fileName)
|
||||
self._pkl_path = pkl_path
|
||||
self._expected_sha256 = getattr(getattr(self._model_data.model.artifact, "downloadUri", None), "sha256", "") or ""
|
||||
self._verify_artifact_file()
|
||||
with open(pkl_path, 'rb') as f:
|
||||
self._m: dict[Any, Any] = pickle.load(f)
|
||||
|
||||
self._meta = self._m['metadata']
|
||||
self._ish = self._meta['input_shapes']
|
||||
self._slices = {k: v for k, v in self._meta['output_slices'].items() if k != 'pad'}
|
||||
self._hidden_slice = self._meta['output_slices']['hidden_state']
|
||||
self._run_policy = self._m['run_policy']
|
||||
self._warp_jits: dict[tuple[int, int], Any] = {k: v for k, v in self._m.items() if isinstance(k, tuple)}
|
||||
if not self._warp_jits:
|
||||
raise ValueError("supercombo pkl has no warp JITs")
|
||||
self._frame_skip = int(self._m.get('frame_skip', 4))
|
||||
self._validate_warp_jits(pkl_path)
|
||||
self._validate_jit_names()
|
||||
|
||||
self._queues: dict[str, Any] | None = None
|
||||
self._npy: dict[str, np.ndarray] | None = None
|
||||
self._cam: tuple[int, int] | None = None
|
||||
self._prev_desire = np.zeros(self._ish['desire_pulse'][2], dtype=np.float32)
|
||||
self._blob_cache: dict[tuple[str, int], Any] = {}
|
||||
|
||||
def _verify_artifact_file(self) -> None:
|
||||
if not self._expected_sha256:
|
||||
return
|
||||
|
||||
actual_sha256 = _file_sha256(self._pkl_path)
|
||||
if actual_sha256 == self._expected_sha256:
|
||||
return
|
||||
|
||||
try:
|
||||
os.remove(self._pkl_path)
|
||||
except OSError:
|
||||
pass
|
||||
redownload_msg = self._schedule_active_bundle_redownload()
|
||||
|
||||
raise RuntimeError(
|
||||
"supercombo artifact SHA mismatch: "
|
||||
f"expected {self._expected_sha256}, got {actual_sha256} for {self._pkl_path}. "
|
||||
f"Deleted the stale cached file{redownload_msg}."
|
||||
)
|
||||
|
||||
def _validate_warp_jits(self, pkl_path: str) -> None:
|
||||
img = self._ish['img']
|
||||
n_frames = img[1] // 6
|
||||
expected_depth = self._frame_skip * (n_frames - 1) + 1
|
||||
expected_device = os.getenv('DEV')
|
||||
|
||||
mismatches: list[str] = []
|
||||
for cam, warp_jit in sorted(self._warp_jits.items()):
|
||||
captured_depth = _captured_queue_depth(warp_jit)
|
||||
captured_devices = _captured_devices(warp_jit)
|
||||
if captured_depth is not None and captured_depth != expected_depth:
|
||||
mismatches.append(
|
||||
f"{cam[0]}x{cam[1]} queue-depth captured={captured_depth} expected={expected_depth}"
|
||||
)
|
||||
if expected_device and captured_devices and expected_device not in captured_devices:
|
||||
mismatches.append(
|
||||
f"{cam[0]}x{cam[1]} device captured={sorted(captured_devices)} expected={expected_device}"
|
||||
)
|
||||
|
||||
if mismatches:
|
||||
details = "; ".join(mismatches)
|
||||
raise RuntimeError(
|
||||
"supercombo warp JIT compatibility mismatch: "
|
||||
f"{details}. Bundle {pkl_path} was compiled with the wrong backend, frame_skip, or queue shape; "
|
||||
"re-download or rebuild this model artifact."
|
||||
)
|
||||
|
||||
def _validate_jit_names(self) -> None:
|
||||
expected_warp_names = ['big_frame', 'big_tfm', 'frame', 'tfm']
|
||||
expected_policy_names = ['big_img_q', 'desire_q', 'feat_q', 'img_q', 'packed_npy_inputs', 'warped']
|
||||
|
||||
mismatches: list[str] = []
|
||||
|
||||
policy_names = sorted(_captured_expected_names(self._run_policy))
|
||||
if policy_names and policy_names != expected_policy_names:
|
||||
mismatches.append(f"run_policy captured={policy_names} expected={expected_policy_names}")
|
||||
|
||||
for cam, warp_jit in sorted(self._warp_jits.items()):
|
||||
warp_names = sorted(_captured_expected_names(warp_jit))
|
||||
if warp_names and warp_names != expected_warp_names:
|
||||
mismatches.append(f"{cam[0]}x{cam[1]} warp captured={warp_names} expected={expected_warp_names}")
|
||||
|
||||
if mismatches:
|
||||
details = "; ".join(mismatches)
|
||||
actual_sha = None
|
||||
try:
|
||||
actual_sha = _file_sha256(self._pkl_path)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
if actual_sha and self._expected_sha256 and actual_sha != self._expected_sha256:
|
||||
try:
|
||||
os.remove(self._pkl_path)
|
||||
except OSError:
|
||||
pass
|
||||
redownload_msg = self._schedule_active_bundle_redownload()
|
||||
raise RuntimeError(
|
||||
"supercombo artifact contract mismatch with stale cached SHA: "
|
||||
f"{details}. Expected SHA {self._expected_sha256}, got {actual_sha}. "
|
||||
f"Deleted the stale cached file{redownload_msg}."
|
||||
)
|
||||
|
||||
raise RuntimeError(
|
||||
"supercombo artifact JIT argument mismatch: "
|
||||
f"{details}. This model file does not match the current IQPilot runtime contract. "
|
||||
"Re-download or rebuild this model artifact."
|
||||
)
|
||||
|
||||
def _handle_runtime_jit_mismatch(self, err: BaseException) -> None:
|
||||
if not _is_jit_arg_mismatch(err):
|
||||
raise err
|
||||
|
||||
actual_sha = None
|
||||
try:
|
||||
actual_sha = _file_sha256(self._pkl_path)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
if actual_sha and self._expected_sha256 and actual_sha != self._expected_sha256:
|
||||
try:
|
||||
os.remove(self._pkl_path)
|
||||
except OSError:
|
||||
pass
|
||||
redownload_msg = self._schedule_active_bundle_redownload()
|
||||
raise RuntimeError(
|
||||
"supercombo artifact runtime JIT mismatch with stale cached SHA: "
|
||||
f"expected {self._expected_sha256}, got {actual_sha} for {self._pkl_path}. "
|
||||
f"Deleted the stale cached file{redownload_msg}."
|
||||
) from err
|
||||
|
||||
raise RuntimeError(
|
||||
"supercombo artifact runtime JIT mismatch: "
|
||||
f"{err}. This model file does not match the current IQPilot runtime contract. "
|
||||
"Re-download or rebuild this model artifact."
|
||||
) from err
|
||||
|
||||
def _schedule_active_bundle_redownload(self) -> str:
|
||||
try:
|
||||
params = Params()
|
||||
active_bundle = params.get("ModelManager_ActiveBundle") or {}
|
||||
index = active_bundle.get("index") if isinstance(active_bundle, dict) else None
|
||||
if isinstance(index, str) and index.isdigit():
|
||||
index = int(index)
|
||||
if isinstance(index, int) and index >= 0:
|
||||
params.put("ModelManager_DownloadIndex", str(index))
|
||||
params.remove("ModelRunnerTypeCache")
|
||||
return "; scheduled automatic re-download of the active model"
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return "; unable to schedule automatic re-download"
|
||||
|
||||
def _frame_tensor(self, key: str, buf):
|
||||
Tensor, Device = _tinygrad_imports()
|
||||
arr = np.frombuffer(buf.data, dtype=np.uint8)
|
||||
ck = (key, arr.ctypes.data)
|
||||
t = self._blob_cache.get(ck)
|
||||
if t is None:
|
||||
t = Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype='uint8', device=Device.DEFAULT)
|
||||
self._blob_cache[ck] = t
|
||||
return t
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
return ['img', 'big_img']
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
return dict(self._ish)
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
return dict(self._slices)
|
||||
|
||||
def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
|
||||
raise RuntimeError("supercombo runner has no OpenCL path; use run_fused()")
|
||||
|
||||
def _ensure_queues(self, cam_w: int, cam_h: int) -> None:
|
||||
if self._queues is not None and self._cam == (cam_w, cam_h):
|
||||
return
|
||||
if (cam_w, cam_h) not in self._warp_jits:
|
||||
raise RuntimeError(f"no warp JIT for {cam_w}x{cam_h}; have {sorted(self._warp_jits)}")
|
||||
|
||||
Tensor, Device = _tinygrad_imports()
|
||||
fs = self._frame_skip
|
||||
img = self._ish['img']
|
||||
n_frames = img[1] // 6
|
||||
img_buf = (fs * (n_frames - 1) + 1, 6, img[2], img[3])
|
||||
fb = self._ish['features_buffer']
|
||||
dp = self._ish['desire_pulse']
|
||||
tc = self._ish['traffic_convention']
|
||||
at = self._ish['action_t']
|
||||
|
||||
zeros_u8 = lambda s: Tensor(np.zeros(s, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize()
|
||||
zeros_f32 = lambda s: Tensor(np.zeros(s, dtype=np.float32), device=Device.DEFAULT).contiguous().realize()
|
||||
|
||||
shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])}
|
||||
sizes = [math.prod(s) for s in shapes.values()]
|
||||
packed = np.zeros(sum(sizes), dtype=np.float32)
|
||||
views = {k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed, np.cumsum(sizes[:-1])), strict=True)}
|
||||
|
||||
self._npy = {'tfm': np.zeros((3, 3), dtype=np.float32), 'big_tfm': np.zeros((3, 3), dtype=np.float32), **views}
|
||||
self._queues = {
|
||||
'img_q': zeros_u8(img_buf),
|
||||
'big_img_q': zeros_u8(img_buf),
|
||||
'feat_q': zeros_f32((fs * fb[1], fb[0], fb[2])),
|
||||
'desire_q': zeros_f32((fs * dp[1], dp[0], dp[2])),
|
||||
'tfm': Tensor(self._npy['tfm'], device='NPY'),
|
||||
'big_tfm': Tensor(self._npy['big_tfm'], device='NPY'),
|
||||
'packed_npy_inputs': Tensor(packed, device='NPY'),
|
||||
}
|
||||
self._cam = (cam_w, cam_h)
|
||||
|
||||
def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
|
||||
Tensor, Device = _tinygrad_imports()
|
||||
main_buf = bufs['img']
|
||||
self._ensure_queues(main_buf.width, main_buf.height)
|
||||
assert self._queues is not None and self._npy is not None
|
||||
|
||||
self._npy['tfm'][:] = transforms['img']
|
||||
self._npy['big_tfm'][:] = transforms['big_img']
|
||||
|
||||
desire_key = next((k for k in numpy_inputs if k.startswith('desire')), None)
|
||||
cur = numpy_inputs[desire_key].copy() if desire_key is not None else np.zeros_like(self._prev_desire)
|
||||
cur[0] = 0
|
||||
self._npy['desire'][:] = np.where(cur - self._prev_desire > .99, cur, 0)
|
||||
self._prev_desire[:] = cur
|
||||
if 'traffic_convention' in numpy_inputs:
|
||||
self._npy['traffic_convention'][:] = numpy_inputs['traffic_convention']
|
||||
if 'action_t' in numpy_inputs:
|
||||
self._npy['action_t'][:] = numpy_inputs['action_t']
|
||||
|
||||
frame = self._frame_tensor('img', bufs['img'])
|
||||
big_frame = self._frame_tensor('big_img', bufs['big_img'])
|
||||
|
||||
warp = self._warp_jits[self._cam]
|
||||
try:
|
||||
warped = warp(tfm=self._queues['tfm'], big_tfm=self._queues['big_tfm'], frame=frame, big_frame=big_frame)
|
||||
out, = self._run_policy(warped=warped, img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'],
|
||||
feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'],
|
||||
packed_npy_inputs=self._queues['packed_npy_inputs'])
|
||||
except Exception as err:
|
||||
self._handle_runtime_jit_mismatch(err)
|
||||
raise
|
||||
flat = out.numpy().flatten()
|
||||
|
||||
self._npy['prev_feat'][:] = flat[self._hidden_slice].reshape(self._npy['prev_feat'].shape)
|
||||
|
||||
sliced = {k: flat[np.newaxis, sl] for k, sl in self._slices.items()}
|
||||
return self._parser.parse_vision_outputs(sliced)
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
raise RuntimeError("supercombo path goes through run_fused(), not _run_model()")
|
||||
+190
@@ -0,0 +1,190 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import pickle
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from tinygrad.dtype import dtypes
|
||||
from tinygrad.tensor import Tensor
|
||||
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import (
|
||||
CLMemDict,
|
||||
CUSTOM_MODEL_PATH,
|
||||
FrameDict,
|
||||
ModelType,
|
||||
NumpyDict,
|
||||
ShapeDict,
|
||||
SliceDict,
|
||||
)
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
|
||||
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.model_types import (
|
||||
OffPolicyTinygrad,
|
||||
OnPolicyTinygrad,
|
||||
PolicyTinygrad,
|
||||
SupercomboTinygrad,
|
||||
VisionTinygrad,
|
||||
)
|
||||
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
|
||||
from iqpilot.selfdrive.iqmodeld.config import ModelConstants
|
||||
from iqpilot.selfdrive.iqmodeld.runtime.tinygrad import qcom_tensor_from_opencl_address
|
||||
from iqpilot.system.hardware import TICI
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _TensorShapePlan:
|
||||
dtype: object
|
||||
device: str
|
||||
|
||||
|
||||
def _artifact_path(filename: str) -> str:
|
||||
return f"{CUSTOM_MODEL_PATH}/{filename}"
|
||||
|
||||
|
||||
def _load_program_blob(filename: str):
|
||||
with open(_artifact_path(filename), "rb") as artifact:
|
||||
try:
|
||||
return pickle.load(artifact)
|
||||
except FileNotFoundError as exc:
|
||||
assert "/dev/kgsl-3d0" not in str(exc), "Model was built on C3 or C3X, but is being loaded on PC"
|
||||
raise
|
||||
|
||||
|
||||
def _compile_input_plan(captured) -> dict[str, _TensorShapePlan]:
|
||||
plan: dict[str, _TensorShapePlan] = {}
|
||||
for name, info in zip(captured.expected_names, captured.expected_input_info, strict=True):
|
||||
plan[name] = _TensorShapePlan(dtype=info[2], device=info[3])
|
||||
return plan
|
||||
|
||||
|
||||
def _merge_step_outputs(output_groups: list[NumpyDict]) -> NumpyDict:
|
||||
stitched: NumpyDict = {}
|
||||
for payload in output_groups:
|
||||
stitched.update(payload)
|
||||
if "planplus" in stitched and "plan" in stitched:
|
||||
stitched["plan"] = stitched["plan"] + stitched["planplus"]
|
||||
return stitched
|
||||
|
||||
|
||||
class TinygradRunner(ModelRunner, SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad):
|
||||
def __init__(self, model_type: int = ModelType.supercombo):
|
||||
ModelRunner.__init__(self)
|
||||
for initializer in (SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad):
|
||||
initializer.__init__(self)
|
||||
|
||||
self._constants = ModelConstants
|
||||
self._model_data = self.models.get(model_type)
|
||||
if self._model_data is None or self._model_data.model is None:
|
||||
raise ValueError(f"Model data for type {model_type} not available.")
|
||||
|
||||
asset_name = self._model_data.model.artifact.fileName
|
||||
assert asset_name.endswith("_tinygrad.pkl"), f"Invalid model file {asset_name} for TinygradRunner"
|
||||
|
||||
self.model_run = _load_program_blob(asset_name)
|
||||
self._input_plan = _compile_input_plan(self.model_run.captured)
|
||||
for name, spec in self._input_plan.items():
|
||||
if "img" in name and spec.dtype is not dtypes.uint8:
|
||||
raise ValueError(f"{asset_name}: image input {name} expects {spec.dtype}, incompatible with uint8 warp buffer")
|
||||
self.input_to_dtype = {name: spec.dtype for name, spec in self._input_plan.items()}
|
||||
self.input_to_device = {name: spec.device for name, spec in self._input_plan.items()}
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
return [stream_name for stream_name in self.input_shapes if "img" in stream_name]
|
||||
|
||||
def _attach_vision_tensor(self, stream_name: str, frame_buffers: CLMemDict, frame_views: FrameDict) -> None:
|
||||
spec = self._input_plan[stream_name]
|
||||
frame_buffer = frame_buffers[stream_name]
|
||||
if TICI:
|
||||
self.inputs[stream_name] = qcom_tensor_from_opencl_address(frame_buffer.mem_address,
|
||||
self.input_shapes[stream_name],
|
||||
dtype=spec.dtype)
|
||||
return
|
||||
|
||||
mirrored = frame_views[stream_name].as_numpy(frame_buffer).reshape(self.input_shapes[stream_name])
|
||||
self.inputs[stream_name] = Tensor(mirrored, device=spec.device, dtype=spec.dtype).realize()
|
||||
|
||||
def _attach_state_tensor(self, tensor_name: str, tensor_value: np.ndarray) -> None:
|
||||
spec = self._input_plan[tensor_name]
|
||||
self.inputs[tensor_name] = Tensor(tensor_value, device=spec.device, dtype=spec.dtype).realize()
|
||||
|
||||
def prepare_vision_inputs(self, imgs_cl: CLMemDict, frames: FrameDict):
|
||||
for stream_name in imgs_cl:
|
||||
if stream_name not in self.inputs or not TICI:
|
||||
self._attach_vision_tensor(stream_name, imgs_cl, frames)
|
||||
|
||||
def prepare_policy_inputs(self, numpy_inputs: NumpyDict):
|
||||
for tensor_name, tensor_value in numpy_inputs.items():
|
||||
self._attach_state_tensor(tensor_name, tensor_value)
|
||||
|
||||
def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
|
||||
self.prepare_vision_inputs(imgs_cl, frames)
|
||||
self.prepare_policy_inputs(numpy_inputs)
|
||||
return self.inputs
|
||||
|
||||
def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
if self._model_data is None:
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
return self.parser_method_dict[self._model_data.model.type.raw](model_outputs)
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
raw_output = self.model_run(**self.inputs).numpy().reshape(-1)
|
||||
return self._parse_outputs(raw_output)
|
||||
|
||||
|
||||
class TinygradSplitRunner(ModelRunner):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.is_20hz_3d = True
|
||||
self._constants = SplitModelConstants
|
||||
self.vision_runner = TinygradRunner(ModelType.vision)
|
||||
self.policy_runner = TinygradRunner(ModelType.policy) if self.models.get(ModelType.policy) else None
|
||||
self.off_policy_runner = TinygradRunner(ModelType.offPolicy) if self.models.get(ModelType.offPolicy) else None
|
||||
self.on_policy_runner = TinygradRunner(ModelType.onPolicy) if self.models.get(ModelType.onPolicy) else None
|
||||
|
||||
def _policy_units(self) -> list[TinygradRunner]:
|
||||
return [runner for runner in (self.policy_runner, self.off_policy_runner, self.on_policy_runner) if runner is not None]
|
||||
|
||||
def run_vision(self) -> NumpyDict:
|
||||
return self.vision_runner.run_model()
|
||||
|
||||
def run_policy(self) -> NumpyDict:
|
||||
return _merge_step_outputs([runner.run_model() for runner in self._policy_units()])
|
||||
|
||||
def refresh_policy_features(self, features_buffer: np.ndarray) -> None:
|
||||
for runner in self._policy_units():
|
||||
if "features_buffer" in runner._input_plan:
|
||||
runner._attach_state_tensor("features_buffer", features_buffer)
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
return _merge_step_outputs([self.run_vision(), self.run_policy()])
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
return list(self.vision_runner.vision_input_names)
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
composite: ShapeDict = dict(self.vision_runner.input_shapes)
|
||||
for runner in self._policy_units():
|
||||
composite.update(runner.input_shapes)
|
||||
return composite
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
composite: SliceDict = dict(self.vision_runner.output_slices)
|
||||
for runner in self._policy_units():
|
||||
composite.update(runner.output_slices)
|
||||
return composite
|
||||
|
||||
def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
|
||||
self.vision_runner.prepare_vision_inputs(imgs_cl, frames)
|
||||
assembled_inputs = dict(self.vision_runner.inputs)
|
||||
for runner in self._policy_units():
|
||||
runner.prepare_policy_inputs(numpy_inputs)
|
||||
assembled_inputs.update(runner.inputs)
|
||||
self.inputs = assembled_inputs
|
||||
return assembled_inputs
|
||||
+89
@@ -0,0 +1,89 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
def index_function(idx, max_val=192, max_idx=32):
|
||||
return max_val * ((idx/max_idx)**2)
|
||||
|
||||
|
||||
class SplitModelConstants:
|
||||
IDX_N = 33
|
||||
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
|
||||
X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
|
||||
LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
|
||||
LEAD_T_OFFSETS = [0., 2., 4.]
|
||||
META_T_IDXS = [2., 4., 6., 8., 10.]
|
||||
|
||||
MODEL_FREQ = 20
|
||||
HISTORY_FREQ = 5
|
||||
HISTORY_LEN_SECONDS = 5
|
||||
TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ
|
||||
FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS
|
||||
INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS
|
||||
|
||||
FEATURE_LEN = 512
|
||||
|
||||
DESIRE_LEN = 8
|
||||
TRAFFIC_CONVENTION_LEN = 2
|
||||
LAT_PLANNER_STATE_LEN = 4
|
||||
LATERAL_CONTROL_PARAMS_LEN = 2
|
||||
PREV_DESIRED_CURV_LEN = 1
|
||||
|
||||
FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
|
||||
FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
|
||||
FCW_5MS2_PROBS_WIDTH = 5
|
||||
FCW_3MS2_PROBS_WIDTH = 2
|
||||
|
||||
DISENGAGE_WIDTH = 5
|
||||
POSE_WIDTH = 6
|
||||
WIDE_FROM_DEVICE_WIDTH = 3
|
||||
LEAD_WIDTH = 4
|
||||
LANE_LINES_WIDTH = 2
|
||||
ROAD_EDGES_WIDTH = 2
|
||||
PLAN_WIDTH = 15
|
||||
DESIRE_PRED_WIDTH = 8
|
||||
LAT_PLANNER_SOLUTION_WIDTH = 4
|
||||
DESIRED_CURV_WIDTH = 1
|
||||
ACTION_WIDTH = 2
|
||||
|
||||
NUM_LANE_LINES = 4
|
||||
NUM_ROAD_EDGES = 2
|
||||
|
||||
LEAD_TRAJ_LEN = 6
|
||||
DESIRE_PRED_LEN = 4
|
||||
|
||||
PLAN_MHP_N = 5
|
||||
LEAD_MHP_N = 2
|
||||
PLAN_MHP_SELECTION = 1
|
||||
LEAD_MHP_SELECTION = 3
|
||||
|
||||
FCW_THRESHOLD_5MS2_HIGH = 0.15
|
||||
FCW_THRESHOLD_5MS2_LOW = 0.05
|
||||
FCW_THRESHOLD_3MS2 = 0.7
|
||||
|
||||
CONFIDENCE_BUFFER_LEN = 5
|
||||
RYG_GREEN = 0.01165
|
||||
RYG_YELLOW = 0.06157
|
||||
|
||||
POLY_PATH_DEGREE = 4
|
||||
|
||||
|
||||
class Plan:
|
||||
POSITION = slice(0, 3)
|
||||
VELOCITY = slice(3, 6)
|
||||
ACCELERATION = slice(6, 9)
|
||||
T_FROM_CURRENT_EULER = slice(9, 12)
|
||||
ORIENTATION_RATE = slice(12, 15)
|
||||
|
||||
|
||||
class Meta:
|
||||
ENGAGED = slice(0, 1)
|
||||
GAS_DISENGAGE = slice(1, 31, 6)
|
||||
BRAKE_DISENGAGE = slice(2, 31, 6)
|
||||
STEER_OVERRIDE = slice(3, 31, 6)
|
||||
HARD_BRAKE_3 = slice(4, 31, 6)
|
||||
HARD_BRAKE_4 = slice(5, 31, 6)
|
||||
HARD_BRAKE_5 = slice(6, 31, 6)
|
||||
GAS_PRESS = slice(31, 55, 4)
|
||||
BRAKE_PRESS = slice(32, 55, 4)
|
||||
LEFT_BLINKER = slice(33, 55, 4)
|
||||
RIGHT_BLINKER = slice(34, 55, 4)
|
||||
+219
@@ -0,0 +1,219 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
|
||||
from iqpilot.selfdrive.iqmodeld.config import ModelConstants
|
||||
|
||||
|
||||
def safe_exp(values, out=None):
|
||||
return np.exp(np.clip(values, -np.inf, 11), out=out)
|
||||
|
||||
|
||||
def sigmoid(values):
|
||||
return 1.0 / (1.0 + safe_exp(-values))
|
||||
|
||||
|
||||
def _softmax_last(values, axis=-1):
|
||||
values -= np.max(values, axis=axis, keepdims=True)
|
||||
if values.dtype in (np.float32, np.float64):
|
||||
safe_exp(values, out=values)
|
||||
else:
|
||||
values = safe_exp(values)
|
||||
values /= np.sum(values, axis=axis, keepdims=True)
|
||||
return values
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _MixtureRecipe:
|
||||
input_heads: int
|
||||
output_heads: int
|
||||
final_shape: tuple[int, ...]
|
||||
|
||||
|
||||
class _TensorKitchen:
|
||||
def __init__(self, ignore_missing: bool = False):
|
||||
self.ignore_missing = ignore_missing
|
||||
|
||||
def _grab(self, outputs: dict[str, np.ndarray], tensor_name: str) -> np.ndarray | None:
|
||||
if tensor_name not in outputs:
|
||||
if not self.ignore_missing:
|
||||
raise ValueError(f"Missing output {tensor_name}")
|
||||
return
|
||||
return outputs[tensor_name]
|
||||
|
||||
def categorical(self, outputs: dict[str, np.ndarray], tensor_name: str, shape=None) -> None:
|
||||
raw = self._grab(outputs, tensor_name)
|
||||
if raw is None:
|
||||
return
|
||||
if shape is not None:
|
||||
raw = raw.reshape((raw.shape[0],) + shape)
|
||||
outputs[tensor_name] = _softmax_last(raw, axis=-1)
|
||||
|
||||
def binary(self, outputs: dict[str, np.ndarray], tensor_name: str) -> None:
|
||||
raw = self._grab(outputs, tensor_name)
|
||||
if raw is None:
|
||||
return
|
||||
outputs[tensor_name] = sigmoid(raw)
|
||||
|
||||
def mixture(self, outputs: dict[str, np.ndarray], tensor_name: str, recipe: _MixtureRecipe) -> None:
|
||||
raw = self._grab(outputs, tensor_name)
|
||||
if raw is None:
|
||||
return
|
||||
|
||||
reshaped = raw.reshape((raw.shape[0], max(recipe.input_heads, 1), -1))
|
||||
value_count = (reshaped.shape[2] - recipe.output_heads) // 2
|
||||
means = reshaped[:, :, :value_count]
|
||||
stds = safe_exp(reshaped[:, :, value_count:2 * value_count])
|
||||
|
||||
if recipe.input_heads > 1:
|
||||
weights = np.zeros((reshaped.shape[0], recipe.input_heads, recipe.output_heads), dtype=reshaped.dtype)
|
||||
for output_idx in range(recipe.output_heads):
|
||||
weights[:, :, output_idx - recipe.output_heads] = _softmax_last(
|
||||
reshaped[:, :, output_idx - recipe.output_heads], axis=-1
|
||||
)
|
||||
|
||||
if recipe.output_heads == 1:
|
||||
for batch_idx in range(weights.shape[0]):
|
||||
order = np.argsort(weights[batch_idx][:, 0])[::-1]
|
||||
weights[batch_idx] = weights[batch_idx][order]
|
||||
means[batch_idx] = means[batch_idx][order]
|
||||
stds[batch_idx] = stds[batch_idx][order]
|
||||
|
||||
hypothesis_shape = (reshaped.shape[0], recipe.input_heads, *recipe.final_shape)
|
||||
outputs[f"{tensor_name}_weights"] = weights
|
||||
outputs[f"{tensor_name}_hypotheses"] = means.reshape(hypothesis_shape)
|
||||
outputs[f"{tensor_name}_stds_hypotheses"] = stds.reshape(hypothesis_shape)
|
||||
|
||||
picked_means = np.zeros((reshaped.shape[0], recipe.output_heads, value_count), dtype=reshaped.dtype)
|
||||
picked_stds = np.zeros((reshaped.shape[0], recipe.output_heads, value_count), dtype=reshaped.dtype)
|
||||
for batch_idx in range(weights.shape[0]):
|
||||
for output_idx in range(recipe.output_heads):
|
||||
order = np.argsort(weights[batch_idx, :, output_idx])[::-1]
|
||||
picked_means[batch_idx, output_idx] = means[batch_idx, order[0]]
|
||||
picked_stds[batch_idx, output_idx] = stds[batch_idx, order[0]]
|
||||
else:
|
||||
picked_means = means
|
||||
picked_stds = stds
|
||||
|
||||
final_shape = ((reshaped.shape[0], recipe.output_heads, *recipe.final_shape)
|
||||
if recipe.output_heads > 1 else (reshaped.shape[0], *recipe.final_shape))
|
||||
outputs[tensor_name] = picked_means.reshape(final_shape)
|
||||
outputs[f"{tensor_name}_stds"] = picked_stds.reshape(final_shape)
|
||||
|
||||
|
||||
class ArchiveParser(_TensorKitchen):
|
||||
def __init__(self, ignore_missing: bool = False):
|
||||
super().__init__(ignore_missing=ignore_missing)
|
||||
self._c = ModelConstants
|
||||
|
||||
def _recipes(self) -> list[tuple[str, _MixtureRecipe]]:
|
||||
c = self._c
|
||||
return [
|
||||
("plan", _MixtureRecipe(c.PLAN_MHP_N, c.PLAN_MHP_SELECTION, (c.IDX_N, c.PLAN_WIDTH))),
|
||||
("lane_lines", _MixtureRecipe(0, 0, (c.NUM_LANE_LINES, c.IDX_N, c.LANE_LINES_WIDTH))),
|
||||
("road_edges", _MixtureRecipe(0, 0, (c.NUM_ROAD_EDGES, c.IDX_N, c.LANE_LINES_WIDTH))),
|
||||
("pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))),
|
||||
("road_transform", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))),
|
||||
("wide_from_device_euler", _MixtureRecipe(0, 0, (c.WIDE_FROM_DEVICE_WIDTH,))),
|
||||
("lead", _MixtureRecipe(c.LEAD_MHP_N, c.LEAD_MHP_SELECTION, (c.LEAD_TRAJ_LEN, c.LEAD_WIDTH))),
|
||||
]
|
||||
|
||||
def parse_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
c = self._c
|
||||
for tensor_name, recipe in self._recipes():
|
||||
self.mixture(outputs, tensor_name, recipe)
|
||||
if "sim_pose" in outputs:
|
||||
self.mixture(outputs, "sim_pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
|
||||
if "lat_planner_solution" in outputs:
|
||||
self.mixture(outputs, "lat_planner_solution", _MixtureRecipe(0, 0, (c.IDX_N, c.LAT_PLANNER_SOLUTION_WIDTH)))
|
||||
if "desired_curvature" in outputs:
|
||||
self.mixture(outputs, "desired_curvature", _MixtureRecipe(0, 0, (c.DESIRED_CURV_WIDTH,)))
|
||||
for name in ("lead_prob", "lane_lines_prob", "meta"):
|
||||
self.binary(outputs, name)
|
||||
self.categorical(outputs, "desire_state", shape=(c.DESIRE_PRED_WIDTH,))
|
||||
self.categorical(outputs, "desire_pred", shape=(c.DESIRE_PRED_LEN, c.DESIRE_PRED_WIDTH))
|
||||
return outputs
|
||||
|
||||
|
||||
class PhaseParser(_TensorKitchen):
|
||||
def __init__(self, ignore_missing: bool = False):
|
||||
super().__init__(ignore_missing=ignore_missing)
|
||||
self._c = SplitModelConstants
|
||||
|
||||
def _has_mixture_heads(self, outputs: dict[str, np.ndarray], tensor_name: str, flat_width: int) -> bool:
|
||||
raw = self._grab(outputs, tensor_name)
|
||||
if raw is None:
|
||||
return False
|
||||
return raw.shape[1] != 2 * flat_width
|
||||
|
||||
def _decode_dynamic_family(self, outputs: dict[str, np.ndarray]) -> None:
|
||||
c = self._c
|
||||
if "lead" in outputs:
|
||||
uses_heads = self._has_mixture_heads(outputs, "lead", c.LEAD_MHP_SELECTION * c.LEAD_TRAJ_LEN * c.LEAD_WIDTH)
|
||||
self.mixture(outputs, "lead", _MixtureRecipe(
|
||||
c.LEAD_MHP_N if uses_heads else 0,
|
||||
c.LEAD_MHP_SELECTION if uses_heads else 0,
|
||||
(c.LEAD_TRAJ_LEN, c.LEAD_WIDTH) if uses_heads else (c.LEAD_MHP_SELECTION, c.LEAD_TRAJ_LEN, c.LEAD_WIDTH),
|
||||
))
|
||||
|
||||
if "plan" in outputs:
|
||||
uses_heads = self._has_mixture_heads(outputs, "plan", c.IDX_N * c.PLAN_WIDTH)
|
||||
self.mixture(outputs, "plan", _MixtureRecipe(
|
||||
c.PLAN_MHP_N if uses_heads else 0,
|
||||
c.PLAN_MHP_SELECTION if uses_heads else 0,
|
||||
(c.IDX_N, c.PLAN_WIDTH),
|
||||
))
|
||||
|
||||
if "planplus" in outputs:
|
||||
self.mixture(outputs, "planplus", _MixtureRecipe(0, 0, (c.IDX_N, c.PLAN_WIDTH)))
|
||||
|
||||
def _decode_policy_family(self, outputs: dict[str, np.ndarray]) -> None:
|
||||
c = self._c
|
||||
if "action" in outputs:
|
||||
self.mixture(outputs, "action", _MixtureRecipe(0, 0, (c.ACTION_WIDTH,)))
|
||||
if "desired_curvature" in outputs:
|
||||
self.mixture(outputs, "desired_curvature", _MixtureRecipe(0, 0, (c.DESIRED_CURV_WIDTH,)))
|
||||
if "desire_pred" in outputs:
|
||||
self.categorical(outputs, "desire_pred", shape=(c.DESIRE_PRED_LEN, c.DESIRE_PRED_WIDTH))
|
||||
if "desire_state" in outputs:
|
||||
self.categorical(outputs, "desire_state", shape=(c.DESIRE_PRED_WIDTH,))
|
||||
if "lane_lines" in outputs:
|
||||
self.mixture(outputs, "lane_lines", _MixtureRecipe(0, 0, (c.NUM_LANE_LINES, c.IDX_N, c.LANE_LINES_WIDTH)))
|
||||
if "lane_lines_prob" in outputs:
|
||||
self.binary(outputs, "lane_lines_prob")
|
||||
if "lead_prob" in outputs:
|
||||
self.binary(outputs, "lead_prob")
|
||||
if "lat_planner_solution" in outputs:
|
||||
self.mixture(outputs, "lat_planner_solution", _MixtureRecipe(0, 0, (c.IDX_N, c.LAT_PLANNER_SOLUTION_WIDTH)))
|
||||
if "meta" in outputs:
|
||||
self.binary(outputs, "meta")
|
||||
if "road_edges" in outputs:
|
||||
self.mixture(outputs, "road_edges", _MixtureRecipe(0, 0, (c.NUM_ROAD_EDGES, c.IDX_N, c.LANE_LINES_WIDTH)))
|
||||
if "sim_pose" in outputs:
|
||||
self.mixture(outputs, "sim_pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
|
||||
|
||||
def parse_vision_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
c = self._c
|
||||
self.mixture(outputs, "pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
|
||||
self.mixture(outputs, "wide_from_device_euler", _MixtureRecipe(0, 0, (c.WIDE_FROM_DEVICE_WIDTH,)))
|
||||
self.mixture(outputs, "road_transform", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
|
||||
self._decode_dynamic_family(outputs)
|
||||
self._decode_policy_family(outputs)
|
||||
return outputs
|
||||
|
||||
def parse_policy_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
self._decode_dynamic_family(outputs)
|
||||
self._decode_policy_family(outputs)
|
||||
return outputs
|
||||
|
||||
def parse_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
return self.parse_policy_outputs(self.parse_vision_outputs(outputs))
|
||||
|
||||
|
||||
__all__ = [
|
||||
"ArchiveParser",
|
||||
"PhaseParser",
|
||||
]
|
||||
+23
@@ -0,0 +1,23 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.helpers import to_mv
|
||||
|
||||
_PTR_STRIDE = 8
|
||||
_RAW_GPU_PTR_SLOT = 20
|
||||
_RAW_GPU_PTR_VIEW_BYTES = 0x100
|
||||
|
||||
|
||||
def _descriptor_pointer(opencl_address: int) -> int:
|
||||
return to_mv(opencl_address, _PTR_STRIDE).cast("Q")[0]
|
||||
|
||||
|
||||
def _raw_gpu_pointer(descriptor_pointer: int) -> int:
|
||||
return to_mv(descriptor_pointer, _RAW_GPU_PTR_VIEW_BYTES).cast("Q")[_RAW_GPU_PTR_SLOT]
|
||||
|
||||
|
||||
def qcom_tensor_from_opencl_address(opencl_address, shape, dtype):
|
||||
descriptor_pointer = _descriptor_pointer(opencl_address)
|
||||
device_pointer = _raw_gpu_pointer(descriptor_pointer)
|
||||
return Tensor.from_blob(device_pointer, shape, dtype=dtype, device="QCOM")
|
||||
+131
@@ -0,0 +1,131 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
DEFAULT_FRAME_SKIP = 4
|
||||
|
||||
MODEL_INPUT_SPEC: dict[str, tuple[tuple[int, ...], str]] = {
|
||||
"img": ((1, 12, 128, 256), "uint8"),
|
||||
"big_img": ((1, 12, 128, 256), "uint8"),
|
||||
"desire_pulse": ((1, 25, 8), "float32"),
|
||||
"traffic_convention": ((1, 2), "float32"),
|
||||
"features_buffer": ((1, 24, 512), "float32"),
|
||||
"action_t": ((1, 2), "float32"),
|
||||
}
|
||||
|
||||
|
||||
def spec_from_meta(meta: dict) -> dict[str, tuple[tuple[int, ...], str]] | None:
|
||||
shapes = meta.get("input_shapes")
|
||||
if not shapes:
|
||||
return None
|
||||
return {name: (tuple(shape), "uint8" if name in ("img", "big_img") else "float32")
|
||||
for name, shape in shapes.items()}
|
||||
|
||||
|
||||
class TemporalInputState:
|
||||
def __init__(self, frame_skip: int, spec: dict[str, tuple[tuple[int, ...], str]] = MODEL_INPUT_SPEC):
|
||||
self.frame_skip = frame_skip
|
||||
img = spec["img"][0]
|
||||
fb = spec["features_buffer"][0]
|
||||
dp = spec["desire_pulse"][0]
|
||||
|
||||
self.n_frames = img[1] // 6
|
||||
img_q_shape = (frame_skip * (self.n_frames - 1) + 1, 6, img[2], img[3])
|
||||
self._img_shape = img
|
||||
self._fb_shape = fb
|
||||
self._dp_shape = dp
|
||||
feat_dim = math.prod(fb[2:])
|
||||
|
||||
self.img_q = np.zeros(img_q_shape, dtype=np.uint8)
|
||||
self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8)
|
||||
self.feat_q = np.zeros((frame_skip * fb[1], fb[0], feat_dim), dtype=np.float32)
|
||||
self.desire_q = np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32)
|
||||
self.prev_desire = np.zeros(dp[2], dtype=np.float32)
|
||||
self.prev_feat = np.zeros((fb[0], feat_dim), dtype=np.float32)
|
||||
|
||||
@staticmethod
|
||||
def _shift_append(q: np.ndarray, new_val: np.ndarray) -> None:
|
||||
q[:-1] = q[1:]
|
||||
q[-1] = new_val
|
||||
|
||||
def push_and_materialize(self, warped: np.ndarray, desire_pulse: np.ndarray,
|
||||
traffic_convention: np.ndarray, action_t: np.ndarray,
|
||||
) -> dict[str, np.ndarray]:
|
||||
fs = self.frame_skip
|
||||
|
||||
cur = desire_pulse.astype(np.float32).copy()
|
||||
cur[0] = 0
|
||||
pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32)
|
||||
self.prev_desire[:] = cur
|
||||
|
||||
self._shift_append(self.img_q, warped[0])
|
||||
self._shift_append(self.big_img_q, warped[1])
|
||||
self._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2]))
|
||||
self._shift_append(self.feat_q, self.prev_feat)
|
||||
|
||||
dp = self._dp_shape
|
||||
return {
|
||||
"img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape),
|
||||
"big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape),
|
||||
"features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape),
|
||||
"desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp),
|
||||
"traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1),
|
||||
"action_t": action_t.astype(np.float32).reshape(1, -1),
|
||||
}
|
||||
|
||||
def note_hidden_state(self, model_output: np.ndarray, hidden_slice: slice) -> None:
|
||||
self.prev_feat[:] = model_output[hidden_slice].reshape(self.prev_feat.shape)
|
||||
|
||||
|
||||
class SplitTemporalState:
|
||||
|
||||
def __init__(self, frame_skip: int, img_shape: tuple[int, ...],
|
||||
feature_shape: tuple[int, ...], desire_shape: tuple[int, ...]):
|
||||
self.frame_skip = frame_skip
|
||||
self._img_shape = tuple(img_shape)
|
||||
self._fb_shape = tuple(feature_shape)
|
||||
self._dp_shape = tuple(desire_shape)
|
||||
|
||||
n_frames = img_shape[1] // 6
|
||||
img_q_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
|
||||
self.img_q = np.zeros(img_q_shape, dtype=np.uint8)
|
||||
self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8)
|
||||
self.feat_q = np.zeros((frame_skip * (feature_shape[1] - 1) + 1, feature_shape[0], feature_shape[2]),
|
||||
dtype=np.float32)
|
||||
self.desire_q = np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]), dtype=np.float32)
|
||||
self.prev_desire = np.zeros(desire_shape[2], dtype=np.float32)
|
||||
|
||||
def materialize_vision(self, warped: np.ndarray, desire: np.ndarray) -> dict[str, np.ndarray]:
|
||||
fs = self.frame_skip
|
||||
cur = desire.astype(np.float32).copy()
|
||||
cur[0] = 0
|
||||
pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32)
|
||||
self.prev_desire[:] = cur
|
||||
|
||||
TemporalInputState._shift_append(self.img_q, warped[0])
|
||||
TemporalInputState._shift_append(self.big_img_q, warped[1])
|
||||
TemporalInputState._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2]))
|
||||
return {
|
||||
"img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape),
|
||||
"big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape),
|
||||
}
|
||||
|
||||
def materialize_policy(self, vision_feature: np.ndarray, traffic_convention: np.ndarray,
|
||||
action_t: np.ndarray | None = None) -> dict[str, np.ndarray]:
|
||||
fs = self.frame_skip
|
||||
TemporalInputState._shift_append(self.feat_q, vision_feature.reshape(self._fb_shape[0], self._fb_shape[2]))
|
||||
dp = self._dp_shape
|
||||
out = {
|
||||
"features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape),
|
||||
"desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp),
|
||||
"traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1),
|
||||
}
|
||||
if action_t is not None:
|
||||
out["action_t"] = action_t.astype(np.float32).reshape(1, -1)
|
||||
return out
|
||||
+422
@@ -0,0 +1,422 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import atexit
|
||||
import math
|
||||
import os
|
||||
import pickle
|
||||
import re
|
||||
import tempfile
|
||||
import time
|
||||
from functools import partial
|
||||
from collections import namedtuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
def _patch_tinygrad_fetch_fw():
|
||||
import hashlib
|
||||
import pathlib
|
||||
import zstandard
|
||||
from tinygrad import helpers
|
||||
_orig = helpers.fetch_fw
|
||||
def fetch_fw(path, name, sha256):
|
||||
p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
|
||||
if p.is_file():
|
||||
blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
|
||||
if hashlib.sha256(blob).hexdigest() == sha256:
|
||||
return blob
|
||||
return _orig(path, name, sha256)
|
||||
helpers.fetch_fw = fetch_fw
|
||||
_patch_tinygrad_fetch_fw()
|
||||
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.helpers import Context
|
||||
from tinygrad.device import Device
|
||||
from tinygrad.engine.jit import TinyJit
|
||||
|
||||
|
||||
NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
|
||||
WARP_INPUTS = ['tfm', 'big_tfm']
|
||||
POLICY_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
|
||||
|
||||
UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32)
|
||||
UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX)
|
||||
|
||||
WARP_DEV = os.getenv('WARP_DEV')
|
||||
|
||||
|
||||
def make_random_images(keys, shape, device=None):
|
||||
return {k: Tensor.randint(shape, low=0, high=256, dtype='uint8', device=device).realize() for k in keys}
|
||||
|
||||
|
||||
def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
|
||||
w_dst, h_dst = dst_shape
|
||||
h_src, w_src = src_shape
|
||||
|
||||
x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1)
|
||||
y = Tensor.arange(h_dst).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1)
|
||||
|
||||
src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2]
|
||||
src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2]
|
||||
src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2]
|
||||
|
||||
src_x = src_x / src_w
|
||||
src_y = src_y / src_w
|
||||
|
||||
x_round = Tensor.round(src_x)
|
||||
y_round = Tensor.round(src_y)
|
||||
x_nn_clipped = x_round.clip(0, w_src - 1).cast('int')
|
||||
y_nn_clipped = y_round.clip(0, h_src - 1).cast('int')
|
||||
idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped
|
||||
sampled = src_flat[idx]
|
||||
|
||||
if border_fill_val is None:
|
||||
return sampled
|
||||
|
||||
in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) &
|
||||
(y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype)
|
||||
return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds)
|
||||
|
||||
|
||||
def frames_to_tensor(frames):
|
||||
H = (frames.shape[0] * 2) // 3
|
||||
W = frames.shape[1]
|
||||
in_img1 = Tensor.cat(frames[0:H:2, 0::2],
|
||||
frames[1:H:2, 0::2],
|
||||
frames[0:H:2, 1::2],
|
||||
frames[1:H:2, 1::2],
|
||||
frames[H:H+H//4].reshape((H//2, W//2)),
|
||||
frames[H+H//4:H+H//2].reshape((H//2, W//2)), dim=0).reshape((6, H//2, W//2))
|
||||
return in_img1
|
||||
|
||||
|
||||
def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
|
||||
cam_w, cam_h, stride, y_height, uv_height, _ = nv12
|
||||
uv_offset = stride * y_height
|
||||
stride_pad = stride - cam_w
|
||||
|
||||
def frame_prepare_tinygrad(input_frame, M_inv):
|
||||
M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV)
|
||||
uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride)
|
||||
with Context(SPLIT_REDUCEOP=0):
|
||||
y = warp_perspective_tinygrad(input_frame[:cam_h*stride],
|
||||
M_inv, (model_w, model_h),
|
||||
(cam_h, cam_w), stride_pad).realize()
|
||||
u = warp_perspective_tinygrad(uv[:cam_h//2, :cam_w:2].flatten(),
|
||||
M_inv_uv, (model_w//2, model_h//2),
|
||||
(cam_h//2, cam_w//2), 0).realize()
|
||||
v = warp_perspective_tinygrad(uv[:cam_h//2, 1:cam_w:2].flatten(),
|
||||
M_inv_uv, (model_w//2, model_h//2),
|
||||
(cam_h//2, cam_w//2), 0).realize()
|
||||
yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w))
|
||||
tensor = frames_to_tensor(yuv)
|
||||
return tensor
|
||||
return frame_prepare_tinygrad
|
||||
|
||||
|
||||
def make_warp_input_queues(vision_input_shapes, frame_skip, device):
|
||||
img = vision_input_shapes['img'] # (1, 12, 128, 256)
|
||||
n_frames = img[1] // 6
|
||||
img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
|
||||
|
||||
npy = {
|
||||
'tfm': np.zeros((3, 3), dtype=np.float32),
|
||||
'big_tfm': np.zeros((3, 3), dtype=np.float32),
|
||||
}
|
||||
input_queues = {
|
||||
'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
|
||||
'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
|
||||
**{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
|
||||
}
|
||||
return input_queues, npy
|
||||
|
||||
|
||||
def get_policy_npy_shapes(input_shapes):
|
||||
dp = input_shapes['desire_pulse'] # (1, 25, 8)
|
||||
tc = input_shapes['traffic_convention'] # (1, 2)
|
||||
at = input_shapes['action_t'] # (1, 2)
|
||||
fb = input_shapes['features_buffer'] # (1, 24, 512)
|
||||
shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])}
|
||||
return shapes, [math.prod(s) for s in shapes.values()]
|
||||
|
||||
|
||||
def make_input_queues(input_shapes, frame_skip, device):
|
||||
input_queues, npy = make_warp_input_queues(input_shapes, frame_skip, device)
|
||||
|
||||
fb = input_shapes['features_buffer'] # (1, 24, 512), past features only; the model appends the current frame's feature
|
||||
dp = input_shapes['desire_pulse'] # (1, 25, 8)
|
||||
|
||||
shapes, sizes = get_policy_npy_shapes(input_shapes)
|
||||
packed_npy_inputs = np.zeros(sum(sizes), dtype=np.float32)
|
||||
npy.update({k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed_npy_inputs, np.cumsum(sizes[:-1])), strict=True)})
|
||||
input_queues.update({
|
||||
'feat_q': Tensor(np.zeros((frame_skip * fb[1], fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(),
|
||||
'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
|
||||
'packed_npy_inputs': Tensor(packed_npy_inputs, device='NPY').realize(),
|
||||
})
|
||||
return input_queues, npy
|
||||
|
||||
|
||||
def shift_and_sample(buf, new_val, sample_fn):
|
||||
buf.assign(buf[1:].cat(new_val, dim=0).contiguous())
|
||||
return sample_fn(buf)
|
||||
|
||||
|
||||
def sample_skip(buf, frame_skip):
|
||||
return buf[::frame_skip].contiguous().flatten(0, 1).unsqueeze(0)
|
||||
|
||||
|
||||
def sample_desire(buf, frame_skip):
|
||||
return buf.reshape(-1, frame_skip, *buf.shape[1:]).max(1).flatten(0, 1).unsqueeze(0)
|
||||
|
||||
|
||||
def make_warp(nv12, model_w, model_h, frame_skip):
|
||||
frame_prepare = make_frame_prepare(nv12, model_w, model_h)
|
||||
|
||||
def warp(tfm, big_tfm, frame, big_frame):
|
||||
tfm = tfm.to(WARP_DEV)
|
||||
big_tfm = big_tfm.to(WARP_DEV)
|
||||
Tensor.realize(tfm, big_tfm)
|
||||
|
||||
warped_frame = frame_prepare(frame, tfm).unsqueeze(0)
|
||||
warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0)
|
||||
return Tensor.cat(warped_frame, warped_big_frame)
|
||||
|
||||
return warp
|
||||
|
||||
|
||||
def make_run_policy(model_runner, model_metadata, frame_skip):
|
||||
sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
|
||||
sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
|
||||
npy_shapes, npy_sizes = get_policy_npy_shapes(model_metadata['input_shapes'])
|
||||
|
||||
def run_policy(warped, img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
|
||||
packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT)
|
||||
warped = warped.to(Device.DEFAULT)
|
||||
Tensor.realize(packed_npy_inputs, warped)
|
||||
|
||||
img = shift_and_sample(img_q, warped[0:1], sample_skip_fn)
|
||||
big_img = shift_and_sample(big_img_q, warped[1:2], sample_skip_fn)
|
||||
|
||||
desire, traffic_convention, action_t, prev_feat = (t.reshape(s) for t, s in zip(packed_npy_inputs.split(npy_sizes), npy_shapes.values(), strict=True))
|
||||
desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
|
||||
feat_buf = shift_and_sample(feat_q, prev_feat.reshape(1, 1, -1), sample_skip_fn)
|
||||
|
||||
inputs = {
|
||||
'img': img,
|
||||
'big_img': big_img,
|
||||
'features_buffer': feat_buf,
|
||||
'desire_pulse': desire_buf,
|
||||
'traffic_convention': traffic_convention,
|
||||
'action_t': action_t,
|
||||
}
|
||||
out = next(iter(model_runner(inputs).values())).cast('float32')
|
||||
return out,
|
||||
return run_policy
|
||||
|
||||
|
||||
def compile_jit(jit, make_random_inputs, input_keys, make_queues):
|
||||
SEED = 42
|
||||
def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True):
|
||||
input_queues, npy = make_queues(Device.DEFAULT)
|
||||
np.random.seed(seed)
|
||||
Tensor.manual_seed(seed)
|
||||
|
||||
testing = test_val is not None or test_buffers is not None
|
||||
n_runs = 1 if testing else 3
|
||||
|
||||
for i in range(n_runs):
|
||||
for v in npy.values():
|
||||
v[:] = np.random.randn(*v.shape).astype(v.dtype)
|
||||
Device.default.synchronize()
|
||||
random_inputs = make_random_inputs()
|
||||
st = time.perf_counter()
|
||||
outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs)
|
||||
mt = time.perf_counter()
|
||||
Device.default.synchronize()
|
||||
et = time.perf_counter()
|
||||
print(f" [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
|
||||
|
||||
if i == 0:
|
||||
val = [np.copy(v.numpy()) for v in outs]
|
||||
buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()]
|
||||
|
||||
if test_val is not None:
|
||||
match = all(np.array_equal(a, b) for a, b in zip(val, test_val, strict=True))
|
||||
assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})"
|
||||
if test_buffers is not None:
|
||||
match = all(np.array_equal(a, b) for a, b in zip(buffers, test_buffers, strict=True))
|
||||
assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})"
|
||||
return val, buffers
|
||||
|
||||
print('capture + replay')
|
||||
test_val, test_buffers = random_inputs_run(jit, SEED)
|
||||
print('pickle round trip')
|
||||
jit = pickle.loads(pickle.dumps(jit))
|
||||
random_inputs_run(jit, SEED, test_val, test_buffers, expect_match=True)
|
||||
random_inputs_run(jit, SEED+1, test_val, test_buffers, expect_match=False)
|
||||
return jit
|
||||
|
||||
|
||||
def _captured_devices(jit) -> set[str]:
|
||||
captured = getattr(jit, 'captured', None)
|
||||
infos = getattr(captured, 'expected_input_info', None)
|
||||
if not infos:
|
||||
return set()
|
||||
|
||||
devices: set[str] = set()
|
||||
for info in infos:
|
||||
if isinstance(info, tuple) and len(info) >= 4 and isinstance(info[3], str):
|
||||
devices.add(info[3])
|
||||
return devices
|
||||
|
||||
|
||||
def _slice_outputs(model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]:
|
||||
return {name: model_outputs[np.newaxis, tensor_slice] for name, tensor_slice in output_slices.items() if name != 'pad'}
|
||||
|
||||
|
||||
def _validate_pose_outputs(parsed_outputs: dict[str, np.ndarray]) -> None:
|
||||
from iqpilot.selfdrive.locationd.locationd import MIN_STD_SANITY_CHECK, ROTATION_SANITY_CHECK, TRANS_SANITY_CHECK
|
||||
|
||||
required = (
|
||||
'pose', 'pose_stds', 'wide_from_device_euler', 'wide_from_device_euler_stds',
|
||||
'road_transform', 'road_transform_stds',
|
||||
)
|
||||
missing = [name for name in required if name not in parsed_outputs]
|
||||
if missing:
|
||||
raise AssertionError(f"parsed supercombo outputs missing required odometry tensors: {missing}")
|
||||
|
||||
for name in required:
|
||||
values = parsed_outputs[name]
|
||||
if not np.isfinite(values).all():
|
||||
raise AssertionError(f"parsed supercombo output {name} contains non-finite values")
|
||||
|
||||
pose = parsed_outputs['pose'][0]
|
||||
pose_stds = parsed_outputs['pose_stds'][0]
|
||||
road_transform_stds = parsed_outputs['road_transform_stds'][0]
|
||||
wide_stds = parsed_outputs['wide_from_device_euler_stds'][0]
|
||||
|
||||
if pose_stds.min() <= MIN_STD_SANITY_CHECK:
|
||||
raise AssertionError(f"pose_stds min {pose_stds.min()} <= {MIN_STD_SANITY_CHECK}")
|
||||
if road_transform_stds.min() <= MIN_STD_SANITY_CHECK:
|
||||
raise AssertionError(f"road_transform_stds min {road_transform_stds.min()} <= {MIN_STD_SANITY_CHECK}")
|
||||
if wide_stds.min() <= MIN_STD_SANITY_CHECK:
|
||||
raise AssertionError(f"wide_from_device_euler_stds min {wide_stds.min()} <= {MIN_STD_SANITY_CHECK}")
|
||||
|
||||
if np.linalg.norm(pose[:3]) > TRANS_SANITY_CHECK:
|
||||
raise AssertionError(f"pose translation norm {np.linalg.norm(pose[:3])} exceeds {TRANS_SANITY_CHECK}")
|
||||
if np.linalg.norm(pose[3:]) > ROTATION_SANITY_CHECK:
|
||||
raise AssertionError(f"pose rotation norm {np.linalg.norm(pose[3:])} exceeds {ROTATION_SANITY_CHECK}")
|
||||
if np.linalg.norm(pose_stds[:3]) > 10 * TRANS_SANITY_CHECK:
|
||||
raise AssertionError(
|
||||
f"pose translation std norm {np.linalg.norm(pose_stds[:3])} exceeds {10 * TRANS_SANITY_CHECK}"
|
||||
)
|
||||
if np.linalg.norm(pose_stds[3:]) > 10 * ROTATION_SANITY_CHECK:
|
||||
raise AssertionError(
|
||||
f"pose rotation std norm {np.linalg.norm(pose_stds[3:])} exceeds {10 * ROTATION_SANITY_CHECK}"
|
||||
)
|
||||
|
||||
|
||||
def validate_supercombo_release(run_policy_jit, model_runner, model_metadata, frame_skip, expected_device: str) -> None:
|
||||
from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
|
||||
|
||||
direct_fn = make_run_policy(model_runner, model_metadata, frame_skip)
|
||||
parser = PhaseParser()
|
||||
queue_factory = partial(make_input_queues, model_metadata['input_shapes'], frame_skip)
|
||||
image_shape = model_metadata['input_shapes']['img']
|
||||
|
||||
jit_queues, jit_npy = queue_factory(Device.DEFAULT)
|
||||
direct_queues, direct_npy = queue_factory(Device.DEFAULT)
|
||||
|
||||
for payload in (jit_npy, direct_npy):
|
||||
for name, value in payload.items():
|
||||
value[:] = 0 if value.dtype.kind in ('i', 'u') else 0.0
|
||||
|
||||
zero_inputs = {
|
||||
'warped': Tensor(np.zeros((2, 6, *image_shape[2:]), dtype=np.uint8), device=Device.DEFAULT).realize(),
|
||||
}
|
||||
|
||||
direct_outs, = direct_fn(**{k: direct_queues[k] for k in POLICY_INPUTS}, **zero_inputs)
|
||||
jit_outs, = run_policy_jit(**{k: jit_queues[k] for k in POLICY_INPUTS}, **zero_inputs)
|
||||
|
||||
direct_flat = direct_outs.numpy().astype(np.float32).reshape(-1)
|
||||
jit_flat = jit_outs.numpy().astype(np.float32).reshape(-1)
|
||||
|
||||
if not np.allclose(direct_flat, jit_flat, atol=1e-4, rtol=1e-4):
|
||||
max_delta = float(np.max(np.abs(direct_flat - jit_flat)))
|
||||
raise AssertionError(f"JIT supercombo output diverges from direct ONNX execution; max abs delta {max_delta}")
|
||||
|
||||
parsed = parser.parse_vision_outputs(_slice_outputs(jit_flat, model_metadata['output_slices']))
|
||||
_validate_pose_outputs(parsed)
|
||||
|
||||
captured_devices = _captured_devices(run_policy_jit)
|
||||
if expected_device and captured_devices and expected_device not in captured_devices:
|
||||
raise AssertionError(
|
||||
f"compiled run_policy backend mismatch: captured {sorted(captured_devices)} expected {expected_device}"
|
||||
)
|
||||
|
||||
|
||||
def _parse_size(s):
|
||||
w, h = s.lower().split('x')
|
||||
return int(w), int(h)
|
||||
|
||||
|
||||
def read_file_chunked_to_shm(path):
|
||||
from iqpilot.common.file_chunker import read_file_chunked
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
with tempfile.NamedTemporaryFile(prefix='compile_modeld_', dir=Paths.shm_path(), delete=False) as f:
|
||||
f.write(read_file_chunked(path))
|
||||
tmp_path = f.name
|
||||
atexit.register(lambda: os.path.exists(tmp_path) and os.remove(tmp_path))
|
||||
return tmp_path
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
from tinygrad.nn.onnx import OnnxRunner
|
||||
from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from iqpilot.selfdrive.iqmodeld.metadata import build_metadata_record
|
||||
p = argparse.ArgumentParser()
|
||||
p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
|
||||
p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True,
|
||||
help='camera resolutions WxH (one or more)')
|
||||
p.add_argument('--onnx', required=True)
|
||||
p.add_argument('--output', required=True)
|
||||
p.add_argument('--frame-skip', type=int, required=True)
|
||||
p.add_argument('--expected-device', default='QCOM', help='expected tinygrad backend baked into the JIT')
|
||||
args = p.parse_args()
|
||||
|
||||
model_path = read_file_chunked_to_shm(args.onnx)
|
||||
model_w, model_h = args.model_size
|
||||
|
||||
model_runner = OnnxRunner(model_path)
|
||||
out = {
|
||||
'metadata': build_metadata_record(model_path),
|
||||
'frame_skip': args.frame_skip,
|
||||
}
|
||||
|
||||
run_policy_jit = TinyJit(make_run_policy(model_runner, out['metadata'], args.frame_skip), prune=True)
|
||||
|
||||
make_policy_queues = partial(make_input_queues, out['metadata']['input_shapes'], args.frame_skip)
|
||||
make_random_model_inputs = partial(make_random_images, keys=['warped'], shape=(2, 6, *out['metadata']['input_shapes']['img'][2:]))
|
||||
out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS,
|
||||
make_policy_queues)
|
||||
validate_supercombo_release(out['run_policy'], model_runner, out['metadata'], args.frame_skip, args.expected_device)
|
||||
|
||||
for cam_w, cam_h in args.camera_resolutions:
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV)
|
||||
warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True)
|
||||
make_warp_queues = partial(make_warp_input_queues, out['metadata']['input_shapes'], args.frame_skip)
|
||||
out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)
|
||||
captured_devices = _captured_devices(out[(cam_w,cam_h)])
|
||||
if args.expected_device and captured_devices and args.expected_device not in captured_devices:
|
||||
raise AssertionError(
|
||||
f"compiled warp backend mismatch for {cam_w}x{cam_h}: captured {sorted(captured_devices)} expected {args.expected_device}"
|
||||
)
|
||||
|
||||
with open(args.output, "wb") as f:
|
||||
pickle.dump(out, f)
|
||||
print(f"Saved JITs to {args.output} ({os.path.getsize(args.output) / 1e6:.2f} MB)")
|
||||
+119
@@ -0,0 +1,119 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
|
||||
Compile the backend-neutral warp-only artifact: NV12 camera frames + 3x3
|
||||
transforms -> (2, 6, model_h/2, model_w/2) uint8 warped tensor, on the device
|
||||
GPU (QCOM). maciqmodeld runs this locally
|
||||
and feed the output to their backend, so the big model's image pipeline is
|
||||
bit-identical to comma's fused pkl warp stage.
|
||||
|
||||
Run ON the device (needs the QCOM backend):
|
||||
cd /data/openpilot && DEV=QCOM WARP_DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 \
|
||||
python3 iqpilot/selfdrive/iqmodeld/tools/compile_warp.py \
|
||||
--camera-resolutions 1928x1208 --output /data/models/emac_warp.pkl
|
||||
The artifact is then split per-resolution into Paths.model_root().
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import os
|
||||
import pickle
|
||||
from functools import partial
|
||||
|
||||
import numpy as np
|
||||
|
||||
SELFTEST_SEED = 20260817
|
||||
|
||||
from iqpilot.selfdrive.iqmodeld.temporal_state import DEFAULT_FRAME_SKIP, MODEL_INPUT_SPEC
|
||||
from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import (
|
||||
NV12Frame, WARP_INPUTS, compile_jit, make_random_images, make_warp, make_warp_input_queues,
|
||||
)
|
||||
|
||||
MODEL_SIZE = (MODEL_INPUT_SPEC["img"][0][3] * 2, MODEL_INPUT_SPEC["img"][0][2] * 2) # (512, 256)
|
||||
|
||||
|
||||
def _parse_size(s: str) -> tuple[int, int]:
|
||||
w, h = s.lower().split("x")
|
||||
return int(w), int(h)
|
||||
|
||||
|
||||
def compile_warp(cam_w: int, cam_h: int, out_path: str | None = None,
|
||||
frame_skip: int = DEFAULT_FRAME_SKIP) -> str:
|
||||
"""Compile the warp-only QCOM JIT for one camera resolution and write the pkl.
|
||||
Returns the artifact path. Callable from the workers so a fresh device
|
||||
self-provisions the warp instead of erroring — needs the QCOM backend."""
|
||||
# the QCOM warp env must be set before tinygrad is imported here
|
||||
os.environ.setdefault("DEV", "QCOM")
|
||||
os.environ.setdefault("WARP_DEV", "QCOM")
|
||||
os.environ.setdefault("IMAGE", "1")
|
||||
os.environ.setdefault("FLOAT16", "1")
|
||||
os.environ.setdefault("NOLOCALS", "1")
|
||||
os.environ.setdefault("JIT_BATCH_SIZE", "0")
|
||||
from tinygrad.engine.jit import TinyJit
|
||||
from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
|
||||
model_w, model_h = MODEL_SIZE
|
||||
input_shapes = {name: shape for name, (shape, _) in MODEL_INPUT_SPEC.items()}
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
make_random_warp_inputs = partial(make_random_images, keys=["frame", "big_frame"],
|
||||
shape=nv12.size, device=os.getenv("WARP_DEV"))
|
||||
warp_jit = TinyJit(make_warp(nv12, model_w, model_h, frame_skip), prune=True)
|
||||
make_warp_queues = partial(make_warp_input_queues, input_shapes, frame_skip)
|
||||
compiled = compile_jit(warp_jit, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)
|
||||
|
||||
# historical artifact name: already-provisioned devices keep their warp
|
||||
out_path = out_path or os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl")
|
||||
os.makedirs(os.path.dirname(out_path), exist_ok=True)
|
||||
tmp = out_path + ".part"
|
||||
bundle = {(cam_w, cam_h): compiled, "frame_skip": frame_skip, "model_size": MODEL_SIZE}
|
||||
bundle["selftest"] = selftest_digest(compiled, cam_w, cam_h, nv12.size)
|
||||
with open(tmp, "wb") as f:
|
||||
pickle.dump(bundle, f)
|
||||
os.replace(tmp, out_path) # atomic: a reader never sees a half-written pkl
|
||||
return out_path
|
||||
|
||||
|
||||
def main() -> None:
|
||||
p = argparse.ArgumentParser()
|
||||
p.add_argument("--camera-resolutions", type=_parse_size, nargs="+", default=[(1928, 1208)])
|
||||
p.add_argument("--output", default=None)
|
||||
p.add_argument("--frame-skip", type=int, default=DEFAULT_FRAME_SKIP)
|
||||
args = p.parse_args()
|
||||
for cam_w, cam_h in args.camera_resolutions:
|
||||
out = compile_warp(cam_w, cam_h, args.output, frame_skip=args.frame_skip)
|
||||
print(f"saved warp JIT to {out} ({os.path.getsize(out) / 1e6:.2f} MB)")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
|
||||
def selftest_inputs(cam_w: int, cam_h: int, nv12_size: int):
|
||||
"""A fixed synthetic frame pair and pair of matrices. Deterministic so the
|
||||
digest is reproducible on the device that compiled the artifact."""
|
||||
rng = np.random.default_rng(SELFTEST_SEED)
|
||||
frame = rng.integers(0, 256, nv12_size, dtype=np.uint8)
|
||||
big_frame = rng.integers(0, 256, nv12_size, dtype=np.uint8)
|
||||
tfm = np.array([[0.7, 0.02, 300.0], [0.01, 0.7, 240.0], [0.0, 0.0, 1.0]], dtype=np.float32)
|
||||
big_tfm = np.array([[0.5, 0.01, 380.0], [0.02, 0.5, 300.0], [0.0, 0.0, 1.0]], dtype=np.float32)
|
||||
return frame, big_frame, tfm, big_tfm
|
||||
|
||||
|
||||
def selftest_digest(compiled, cam_w: int, cam_h: int, nv12_size: int) -> str:
|
||||
"""Hash the warp's output for a fixed input.
|
||||
|
||||
A warp artifact pinned to one tinygrad can still unpickle under another and
|
||||
then compute silently wrong, which reaches the model as a garbage image and
|
||||
looks like a bad model rather than a stale artifact. A version string cannot
|
||||
see that; running it can."""
|
||||
from tinygrad.tensor import Tensor
|
||||
frame, big_frame, tfm, big_tfm = selftest_inputs(cam_w, cam_h, nv12_size)
|
||||
dev = os.getenv("WARP_DEV") or "QCOM"
|
||||
out = compiled(tfm=Tensor(tfm, device="NPY").realize(),
|
||||
big_tfm=Tensor(big_tfm, device="NPY").realize(),
|
||||
frame=Tensor(frame, device=dev).realize(),
|
||||
big_frame=Tensor(big_frame, device=dev).realize())
|
||||
return hashlib.sha256(out.numpy().astype(np.uint8).tobytes()).hexdigest()
|
||||
+32
@@ -0,0 +1,32 @@
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
from iqpilot.cereal import log
|
||||
|
||||
from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT, HEIGHT_SANE_MIN, HEIGHT_SANE_MAX
|
||||
|
||||
|
||||
def get_calibrated_rpy(live_calib: log.ExtrinsicsCalibration) -> np.ndarray | None:
|
||||
if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
|
||||
return None
|
||||
|
||||
if len(live_calib.rpyCalib) != 3:
|
||||
return None
|
||||
|
||||
calib_rpy = np.asarray(live_calib.rpyCalib, dtype=np.float32)
|
||||
return calib_rpy if np.isfinite(calib_rpy).all() else None
|
||||
|
||||
|
||||
def get_render_path_height(live_calib: log.ExtrinsicsCalibration) -> float:
|
||||
if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
|
||||
return float(HEIGHT_INIT[0])
|
||||
|
||||
if len(live_calib.height) != 1:
|
||||
return float(HEIGHT_INIT[0])
|
||||
|
||||
height = float(live_calib.height[0])
|
||||
if not math.isfinite(height):
|
||||
return float(HEIGHT_INIT[0])
|
||||
if not (HEIGHT_SANE_MIN <= height <= HEIGHT_SANE_MAX):
|
||||
return float(HEIGHT_INIT[0])
|
||||
return height
|
||||
+367
@@ -0,0 +1,367 @@
|
||||
#!/usr/bin/env python3
|
||||
'''
|
||||
This process finds calibration values. More info on what these calibration values
|
||||
are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations
|
||||
While the roll calibration is a real value that can be estimated, here we assume it's zero,
|
||||
and the image input into the neural network is not corrected for roll.
|
||||
'''
|
||||
|
||||
import os
|
||||
import capnp
|
||||
import numpy as np
|
||||
from typing import NoReturn
|
||||
|
||||
from iqpilot.cereal import log, car
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.common.constants import CV
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.issue_debug import log_issue_limited
|
||||
from iqpilot.common.realtime import config_realtime_process
|
||||
from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.system.hardware import HARDWARE
|
||||
|
||||
MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
|
||||
MAX_VEL_ANGLE_STD = np.radians(0.25)
|
||||
MAX_YAW_RATE_FILTER = np.radians(2) # per second
|
||||
|
||||
MAX_HEIGHT_STD = np.exp(-3.5)
|
||||
|
||||
# This is at model frequency, blocks needed for efficiency
|
||||
SMOOTH_CYCLES = 10
|
||||
BLOCK_SIZE = 100
|
||||
INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration
|
||||
INPUTS_WANTED = 50 # We want a little bit more than we need for stability
|
||||
MAX_ALLOWED_YAW_SPREAD = np.radians(2)
|
||||
MAX_ALLOWED_PITCH_SPREAD = np.radians(4)
|
||||
TICI_FAMILY_PITCH_SPREAD_RESET = np.radians(3)
|
||||
RPY_INIT = np.array([0.0,0.0,0.0])
|
||||
WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0])
|
||||
HEIGHT_INIT = np.array([1.22])
|
||||
HEIGHT_SANE_MIN, HEIGHT_SANE_MAX = 0.9, 2.0
|
||||
DEVICE_IS_TICI_FAMILY = HARDWARE.get_device_type() in ("tici", "tizi")
|
||||
|
||||
# These values are needed to accommodate the model frame in the narrow cam
|
||||
if HARDWARE.get_device_type() == 'mici':
|
||||
PITCH_LIMITS = np.array([-0.143101, 0.22235988])
|
||||
else:
|
||||
PITCH_LIMITS = np.array([-0.09074112085129739, 0.17])
|
||||
YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
|
||||
DEBUG = os.getenv("DEBUG") is not None
|
||||
|
||||
def is_calibration_valid(rpy: np.ndarray) -> bool:
|
||||
return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
|
||||
|
||||
|
||||
def sanity_clip(rpy: np.ndarray) -> np.ndarray:
|
||||
if np.isnan(rpy).any():
|
||||
rpy = RPY_INIT
|
||||
return np.array([rpy[0],
|
||||
np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005),
|
||||
np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
|
||||
|
||||
def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx: int, block_size: float) -> np.ndarray:
|
||||
return (idx*prev_mean + (block_size - idx) * new_val) / block_size
|
||||
|
||||
class Calibrator:
|
||||
def __init__(self, param_put: bool = False):
|
||||
self.param_put = param_put
|
||||
|
||||
self.not_car = False
|
||||
self.stable_rpy = RPY_INIT.copy()
|
||||
self.stable_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
|
||||
self.stable_height = HEIGHT_INIT.copy()
|
||||
self.has_stable_snapshot = False
|
||||
|
||||
# Read saved calibration
|
||||
self.params = Params()
|
||||
calibration_params = self.params.get("CalibrationParams")
|
||||
rpy_init = RPY_INIT
|
||||
wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
|
||||
height = HEIGHT_INIT
|
||||
valid_blocks = 0
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
|
||||
|
||||
if param_put and calibration_params:
|
||||
try:
|
||||
with log.Event.from_bytes(calibration_params) as msg:
|
||||
rpy_init = np.array(msg.extrinsicsCalibration.rpyCalib)
|
||||
valid_blocks = msg.extrinsicsCalibration.validBlocks
|
||||
wide_from_device_euler = np.array(msg.extrinsicsCalibration.wideFromDeviceEuler)
|
||||
height = np.array(msg.extrinsicsCalibration.height)
|
||||
except Exception:
|
||||
cloudlog.exception("Error reading cached CalibrationParams")
|
||||
|
||||
self.reset(rpy_init, valid_blocks, wide_from_device_euler, height)
|
||||
self.update_status()
|
||||
|
||||
# If saved calibration is immediately invalid (e.g. bad params from a previous
|
||||
# bootstrap bug or device remount), auto-clear it so we recalibrate from scratch
|
||||
# instead of getting permanently stuck in the "Calibration Invalid" state.
|
||||
if self.cal_status == log.ExtrinsicsCalibration.Status.invalid:
|
||||
cloudlog.warning("calibrationd: saved CalibrationParams are invalid, clearing and starting fresh")
|
||||
if param_put:
|
||||
self.params.remove("CalibrationParams")
|
||||
self.reset()
|
||||
self.update_status()
|
||||
|
||||
def _remember_stable_solution(self) -> None:
|
||||
self.stable_rpy = self.rpy.copy()
|
||||
self.stable_wide_from_device_euler = self.wide_from_device_euler.copy()
|
||||
self.stable_height = self.height.copy()
|
||||
self.has_stable_snapshot = True
|
||||
|
||||
def reset(self, rpy_init: np.ndarray = RPY_INIT,
|
||||
valid_blocks: int = 0,
|
||||
wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
|
||||
height_init: np.ndarray = HEIGHT_INIT,
|
||||
smooth_from: np.ndarray | None = None) -> None:
|
||||
if not np.isfinite(rpy_init).all():
|
||||
self.rpy = RPY_INIT.copy()
|
||||
else:
|
||||
self.rpy = rpy_init.copy()
|
||||
|
||||
if not np.isfinite(height_init).all() or len(height_init) != 1:
|
||||
self.height = HEIGHT_INIT.copy()
|
||||
else:
|
||||
self.height = height_init.copy()
|
||||
|
||||
if not np.isfinite(wide_from_device_euler_init).all() or len(wide_from_device_euler_init) != 3:
|
||||
self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
|
||||
else:
|
||||
self.wide_from_device_euler = wide_from_device_euler_init.copy()
|
||||
|
||||
if not np.isfinite(valid_blocks) or valid_blocks < 0:
|
||||
self.valid_blocks = 0
|
||||
else:
|
||||
self.valid_blocks = valid_blocks
|
||||
|
||||
self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1))
|
||||
self.wide_from_device_eulers = np.tile(self.wide_from_device_euler, (INPUTS_WANTED, 1))
|
||||
self.heights = np.tile(self.height, (INPUTS_WANTED, 1))
|
||||
|
||||
self.idx = 0
|
||||
self.block_idx = 0
|
||||
self.v_ego = 0.0
|
||||
|
||||
if smooth_from is None:
|
||||
self.old_rpy = RPY_INIT
|
||||
self.old_rpy_weight = 0.0
|
||||
else:
|
||||
self.old_rpy = smooth_from
|
||||
self.old_rpy_weight = 1.0
|
||||
|
||||
def get_valid_idxs(self) -> list[int]:
|
||||
# exclude current block_idx from validity window
|
||||
before_current = list(range(self.block_idx))
|
||||
after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks))
|
||||
return before_current + after_current
|
||||
|
||||
def update_status(self) -> None:
|
||||
valid_idxs = self.get_valid_idxs()
|
||||
if valid_idxs:
|
||||
self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0)
|
||||
self.height = np.mean(self.heights[valid_idxs], axis=0)
|
||||
rpys = self.rpys[valid_idxs]
|
||||
self.rpy = np.mean(rpys, axis=0)
|
||||
max_rpy_calib = np.array(np.max(rpys, axis=0))
|
||||
min_rpy_calib = np.array(np.min(rpys, axis=0))
|
||||
self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
|
||||
else:
|
||||
self.calib_spread = np.zeros(3)
|
||||
|
||||
if self.valid_blocks < INPUTS_NEEDED:
|
||||
if self.cal_status == log.ExtrinsicsCalibration.Status.recalibrating:
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
|
||||
else:
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
|
||||
elif is_calibration_valid(self.rpy):
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.calibrated
|
||||
else:
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.invalid
|
||||
|
||||
# If spread is too high, assume mounting was changed and reset to last block.
|
||||
# Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
|
||||
# TODO: add height spread check with smooth transition too
|
||||
pitch_spread_limit = TICI_FAMILY_PITCH_SPREAD_RESET if DEVICE_IS_TICI_FAMILY else MAX_ALLOWED_PITCH_SPREAD
|
||||
spread_too_high = self.calib_spread[1] > pitch_spread_limit or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD
|
||||
if self.cal_status == log.ExtrinsicsCalibration.Status.calibrated and not spread_too_high:
|
||||
self._remember_stable_solution()
|
||||
|
||||
if spread_too_high and self.cal_status == log.ExtrinsicsCalibration.Status.calibrated:
|
||||
use_stable_snapshot = DEVICE_IS_TICI_FAMILY and self.has_stable_snapshot
|
||||
if use_stable_snapshot:
|
||||
reset_rpy = self.stable_rpy
|
||||
reset_wide = self.stable_wide_from_device_euler
|
||||
reset_height = self.stable_height
|
||||
else:
|
||||
reset_rpy = self.rpys[self.block_idx - 1]
|
||||
reset_wide = self.wide_from_device_eulers[self.block_idx - 1]
|
||||
reset_height = self.heights[self.block_idx - 1]
|
||||
|
||||
log_issue_limited(
|
||||
"calibrationd_reset_spread",
|
||||
"calibration",
|
||||
f"calibrationd reset unstable solution pitchSpread={self.calib_spread[1]:.6f} "
|
||||
f"yawSpread={self.calib_spread[2]:.6f} pitchLimit={pitch_spread_limit:.6f} "
|
||||
f"use_stable_snapshot={use_stable_snapshot} rpy={self.rpy.tolist()}",
|
||||
interval_sec=0.5,
|
||||
)
|
||||
self.reset(reset_rpy, valid_blocks=1, wide_from_device_euler_init=reset_wide,
|
||||
height_init=reset_height, smooth_from=self.stable_rpy if use_stable_snapshot else self.rpy)
|
||||
self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
|
||||
|
||||
write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
|
||||
if self.param_put and write_this_cycle:
|
||||
self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes())
|
||||
|
||||
def handle_v_ego(self, v_ego: float) -> None:
|
||||
self.v_ego = v_ego
|
||||
|
||||
def get_smooth_rpy(self) -> np.ndarray:
|
||||
if self.old_rpy_weight > 0:
|
||||
return self.old_rpy_weight * self.old_rpy + (1.0 - self.old_rpy_weight) * self.rpy
|
||||
else:
|
||||
return self.rpy
|
||||
|
||||
def handle_cam_odom(self, trans: list[float],
|
||||
rot: list[float],
|
||||
wide_from_device_euler: list[float],
|
||||
trans_std: list[float],
|
||||
road_transform_trans: list[float],
|
||||
road_transform_trans_std: list[float]) -> np.ndarray | None:
|
||||
self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
|
||||
|
||||
fast_enough = self.v_ego > MIN_SPEED_FILTER
|
||||
motion_speed = max(float(self.v_ego), float(trans[0]))
|
||||
cam_fast_enough = motion_speed > MIN_SPEED_FILTER
|
||||
yaw_ok = abs(rot[2]) < MAX_YAW_RATE_FILTER
|
||||
straight_and_fast = fast_enough and cam_fast_enough and yaw_ok
|
||||
angle_std_threshold = MAX_VEL_ANGLE_STD
|
||||
height_std_threshold = MAX_HEIGHT_STD
|
||||
rpy_certain = np.arctan2(trans_std[1], motion_speed) < angle_std_threshold
|
||||
if len(road_transform_trans_std) == 3:
|
||||
height_certain = road_transform_trans_std[2] < height_std_threshold
|
||||
else:
|
||||
height_certain = True
|
||||
|
||||
certain_if_calib = rpy_certain
|
||||
if not (straight_and_fast and certain_if_calib):
|
||||
log_issue_limited(
|
||||
"calibrationd_rejected_sample",
|
||||
"calibration",
|
||||
f"calibrationd rejected sample vEgo={self.v_ego:.2f} trans0={trans[0]:.2f} yawRate={rot[2]:.4f} "
|
||||
f"fast_enough={fast_enough} cam_fast_enough={cam_fast_enough} motion_speed={motion_speed:.2f} yaw_ok={yaw_ok} "
|
||||
f"rpy_certain={rpy_certain} height_certain={height_certain} valid_blocks={self.valid_blocks} idx={self.idx}",
|
||||
interval_sec=1.0,
|
||||
)
|
||||
return None
|
||||
|
||||
observed_rpy = np.array([0,
|
||||
-np.arctan2(trans[2], trans[0]),
|
||||
np.arctan2(trans[1], trans[0])])
|
||||
new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy)))
|
||||
new_rpy = sanity_clip(new_rpy)
|
||||
|
||||
if len(wide_from_device_euler) == 3:
|
||||
new_wide_from_device_euler = np.array(wide_from_device_euler)
|
||||
else:
|
||||
new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
|
||||
|
||||
if len(road_transform_trans) == 3 and HEIGHT_SANE_MIN <= road_transform_trans[2] <= HEIGHT_SANE_MAX:
|
||||
new_height = np.array([road_transform_trans[2]])
|
||||
else:
|
||||
new_height = HEIGHT_INIT
|
||||
|
||||
self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE))
|
||||
self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx],
|
||||
new_wide_from_device_euler, self.idx, float(BLOCK_SIZE))
|
||||
self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE))
|
||||
|
||||
self.idx = (self.idx + 1) % BLOCK_SIZE
|
||||
if self.idx == 0:
|
||||
self.block_idx += 1
|
||||
self.valid_blocks = max(self.block_idx, self.valid_blocks)
|
||||
self.block_idx = self.block_idx % INPUTS_WANTED
|
||||
|
||||
self.update_status()
|
||||
|
||||
if self.idx == 0:
|
||||
log_issue_limited(
|
||||
"calibrationd_progress_block",
|
||||
"calibration",
|
||||
f"calibrationd progress status={int(self.cal_status)} valid_blocks={self.valid_blocks} "
|
||||
f"calPerc={min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)} "
|
||||
f"rpy={self.rpy.tolist()} spread={self.calib_spread.tolist()}",
|
||||
interval_sec=0.5,
|
||||
)
|
||||
|
||||
return new_rpy
|
||||
|
||||
def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder:
|
||||
smooth_rpy = self.get_smooth_rpy()
|
||||
|
||||
msg = messaging.new_message('extrinsicsCalibration')
|
||||
msg.valid = valid
|
||||
|
||||
extrinsicsCalibration = msg.extrinsicsCalibration
|
||||
extrinsicsCalibration.validBlocks = self.valid_blocks
|
||||
extrinsicsCalibration.calStatus = self.cal_status
|
||||
extrinsicsCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
|
||||
extrinsicsCalibration.rpyCalib = smooth_rpy.tolist()
|
||||
extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist()
|
||||
extrinsicsCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
|
||||
extrinsicsCalibration.height = self.height.tolist()
|
||||
|
||||
return msg
|
||||
|
||||
def send_data(self, pm: messaging.PubMaster, valid: bool) -> None:
|
||||
pm.send('extrinsicsCalibration', self.get_msg(valid))
|
||||
|
||||
|
||||
def main() -> NoReturn:
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
pm = messaging.PubMaster(['extrinsicsCalibration'])
|
||||
sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry')
|
||||
|
||||
params_reader = Params()
|
||||
CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
|
||||
|
||||
calibrator = Calibrator(param_put=True)
|
||||
calibrator.not_car = CP.notCar
|
||||
|
||||
while 1:
|
||||
timeout = 0 if sm.frame == -1 else 100
|
||||
sm.update(timeout)
|
||||
|
||||
if sm.updated['cameraOdometry']:
|
||||
calibrator.handle_v_ego(sm['carState'].vEgo)
|
||||
new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
|
||||
sm['cameraOdometry'].rot,
|
||||
sm['cameraOdometry'].wideFromDeviceEuler,
|
||||
sm['cameraOdometry'].transStd,
|
||||
sm['cameraOdometry'].roadTransformTrans,
|
||||
sm['cameraOdometry'].roadTransformTransStd)
|
||||
|
||||
if DEBUG and new_rpy is not None:
|
||||
print('got new rpy', new_rpy)
|
||||
|
||||
# 4Hz driven by cameraOdometry
|
||||
if sm.frame % 5 == 0:
|
||||
checks_ok = sm.all_checks()
|
||||
if not checks_ok:
|
||||
ft = sm.freq_tracker
|
||||
recv_hz = {s: (round(1.0 / ft[s].avg_dt.get_average(), 2) if ft[s].avg_dt.count else None) for s in sm.services}
|
||||
log_issue_limited(
|
||||
"calibrationd_checks_failed",
|
||||
"calibration",
|
||||
f"calibrationd all_checks failed alive={sm.alive} freq_ok={sm.freq_ok} valid={sm.valid} "
|
||||
f"seen={sm.seen} recv_hz={recv_hz}",
|
||||
interval_sec=5.0,
|
||||
)
|
||||
calibrator.send_data(pm, checks_ok)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+188
@@ -0,0 +1,188 @@
|
||||
import numpy as np
|
||||
from typing import Any
|
||||
from functools import cache
|
||||
|
||||
from iqpilot.cereal import log
|
||||
from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
|
||||
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
|
||||
|
||||
|
||||
@cache
|
||||
def fft_next_good_size(n: int) -> int:
|
||||
"""
|
||||
smallest composite of 2, 3, 5, 7, 11 that is >= n
|
||||
inspired by pocketfft
|
||||
"""
|
||||
if n <= 6:
|
||||
return n
|
||||
best, f2 = 2 * n, 1
|
||||
while f2 < best:
|
||||
f23 = f2
|
||||
while f23 < best:
|
||||
f235 = f23
|
||||
while f235 < best:
|
||||
f2357 = f235
|
||||
while f2357 < best:
|
||||
f235711 = f2357
|
||||
while f235711 < best:
|
||||
best = f235711 if f235711 >= n else best
|
||||
f235711 *= 11
|
||||
f2357 *= 7
|
||||
f235 *= 5
|
||||
f23 *= 3
|
||||
f2 *= 2
|
||||
return best
|
||||
|
||||
|
||||
def parabolic_peak_interp(R, max_index):
|
||||
if max_index == 0 or max_index == len(R) - 1:
|
||||
return max_index
|
||||
|
||||
y_m1, y_0, y_p1 = R[max_index - 1], R[max_index], R[max_index + 1]
|
||||
offset = 0.5 * (y_p1 - y_m1) / (2 * y_0 - y_p1 - y_m1)
|
||||
|
||||
return max_index + offset
|
||||
|
||||
|
||||
def rotate_cov(rot_matrix, cov_in):
|
||||
return rot_matrix @ cov_in @ rot_matrix.T
|
||||
|
||||
|
||||
def rotate_std(rot_matrix, std_in):
|
||||
return np.sqrt(np.diag(rotate_cov(rot_matrix, np.diag(std_in**2))))
|
||||
|
||||
|
||||
class NPQueue:
|
||||
def __init__(self, maxlen: int, rowsize: int) -> None:
|
||||
self.maxlen = maxlen
|
||||
self.arr = np.empty((0, rowsize))
|
||||
|
||||
def __len__(self) -> int:
|
||||
return len(self.arr)
|
||||
|
||||
def append(self, pt: list[float]) -> None:
|
||||
if len(self.arr) < self.maxlen:
|
||||
self.arr = np.append(self.arr, [pt], axis=0)
|
||||
else:
|
||||
self.arr[:-1] = self.arr[1:]
|
||||
self.arr[-1] = pt
|
||||
|
||||
|
||||
class PointBuckets:
|
||||
def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None:
|
||||
self.x_bounds = x_bounds
|
||||
self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds}
|
||||
self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True))
|
||||
self.min_points_total = min_points_total
|
||||
|
||||
def __len__(self) -> int:
|
||||
return sum([len(v) for v in self.buckets.values()])
|
||||
|
||||
def is_valid(self) -> bool:
|
||||
individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True))
|
||||
total_points_valid = self.__len__() >= self.min_points_total
|
||||
return individual_buckets_valid and total_points_valid
|
||||
|
||||
def get_valid_percent(self) -> int:
|
||||
total_points_perc = min(self.__len__() / self.min_points_total * 100, 100)
|
||||
individual_buckets_perc = min(min(len(v) / min_pts * 100 for v, min_pts in
|
||||
zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)), 100)
|
||||
return int((total_points_perc + individual_buckets_perc) / 2)
|
||||
|
||||
def is_calculable(self) -> bool:
|
||||
return all(len(v) > 0 for v in self.buckets.values())
|
||||
|
||||
def add_point(self, x: float, y: float) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
def get_points(self, num_points: int | None = None) -> Any:
|
||||
points = np.vstack([x.arr for x in self.buckets.values()])
|
||||
if num_points is None:
|
||||
return points
|
||||
return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
|
||||
|
||||
def load_points(self, points: list[list[float]]) -> None:
|
||||
for point in points:
|
||||
self.add_point(*point)
|
||||
|
||||
|
||||
class ParameterEstimator:
|
||||
""" Base class for parameter estimators """
|
||||
def reset(self) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
def handle_log(self, t: int, which: str, msg: log.Event) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
def get_msg(self, valid: bool, with_points: bool) -> log.Event:
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
class Measurement:
|
||||
x, y, z = (property(lambda self: self.xyz[0]), property(lambda self: self.xyz[1]), property(lambda self: self.xyz[2]))
|
||||
x_std, y_std, z_std = (property(lambda self: self.xyz_std[0]), property(lambda self: self.xyz_std[1]), property(lambda self: self.xyz_std[2]))
|
||||
roll, pitch, yaw = x, y, z
|
||||
roll_std, pitch_std, yaw_std = x_std, y_std, z_std
|
||||
|
||||
def __init__(self, xyz: np.ndarray, xyz_std: np.ndarray):
|
||||
self.xyz: np.ndarray = xyz
|
||||
self.xyz_std: np.ndarray = xyz_std
|
||||
|
||||
@classmethod
|
||||
def from_measurement_xyz(cls, measurement: log.DeviceMotion.XYZMeasurement) -> 'Measurement':
|
||||
return cls(
|
||||
xyz=np.array([measurement.x, measurement.y, measurement.z]),
|
||||
xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd])
|
||||
)
|
||||
|
||||
|
||||
class Pose:
|
||||
def __init__(self, orientation: Measurement, velocity: Measurement, acceleration: Measurement, angular_velocity: Measurement):
|
||||
self.orientation = orientation
|
||||
self.velocity = velocity
|
||||
self.acceleration = acceleration
|
||||
self.angular_velocity = angular_velocity
|
||||
|
||||
@classmethod
|
||||
def from_live_pose(cls, live_pose: log.DeviceMotion) -> 'Pose':
|
||||
return Pose(
|
||||
orientation=Measurement.from_measurement_xyz(live_pose.orientationNED),
|
||||
velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice),
|
||||
acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice),
|
||||
angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice)
|
||||
)
|
||||
|
||||
|
||||
class PoseCalibrator:
|
||||
def __init__(self):
|
||||
self.calib_valid = False
|
||||
self.calib_from_device = np.eye(3)
|
||||
|
||||
def _transform_calib_from_device(self, meas: Measurement):
|
||||
new_xyz = self.calib_from_device @ meas.xyz
|
||||
new_xyz_std = rotate_std(self.calib_from_device, meas.xyz_std)
|
||||
return Measurement(new_xyz, new_xyz_std)
|
||||
|
||||
def _ned_from_calib(self, orientation: Measurement):
|
||||
ned_from_device = rot_from_euler(orientation.xyz)
|
||||
ned_from_calib = ned_from_device @ self.calib_from_device.T
|
||||
ned_from_calib_euler_meas = Measurement(euler_from_rot(ned_from_calib), np.full(3, np.nan))
|
||||
return ned_from_calib_euler_meas
|
||||
|
||||
def build_calibrated_pose(self, pose: Pose) -> Pose:
|
||||
ned_from_calib_euler = self._ned_from_calib(pose.orientation)
|
||||
angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity)
|
||||
acceleration_calib = self._transform_calib_from_device(pose.acceleration)
|
||||
velocity_calib = self._transform_calib_from_device(pose.velocity)
|
||||
|
||||
return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib)
|
||||
|
||||
def feed_live_calib(self, live_calib: log.ExtrinsicsCalibration):
|
||||
calib_rpy = get_calibrated_rpy(live_calib)
|
||||
if calib_rpy is not None:
|
||||
self.calib_from_device = rot_from_euler(calib_rpy).T
|
||||
self.calib_valid = True
|
||||
else:
|
||||
if not self.calib_valid:
|
||||
self.calib_from_device = np.eye(3)
|
||||
self.calib_valid = False
|
||||
+336
@@ -0,0 +1,336 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import capnp
|
||||
import numpy as np
|
||||
from enum import Enum
|
||||
from collections import defaultdict
|
||||
|
||||
from iqpilot.cereal import log, messaging
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
from iqpilot.common.transformations.orientation import rot_from_euler
|
||||
from iqpilot.common.realtime import config_realtime_process
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
|
||||
from iqpilot.selfdrive.locationd.helpers import rotate_std
|
||||
from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
|
||||
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
|
||||
|
||||
ACCEL_SANITY_CHECK = 100.0 # m/s^2
|
||||
ROTATION_SANITY_CHECK = 10.0 # rad/s
|
||||
TRANS_SANITY_CHECK = 200.0 # m/s
|
||||
CALIB_RPY_SANITY_CHECK = 0.5 # rad (+- 30 deg)
|
||||
MIN_STD_SANITY_CHECK = 1e-5 # m or rad
|
||||
MAX_FILTER_REWIND_TIME = 0.8 # s
|
||||
MAX_SENSOR_TIME_DIFF = 0.1 # s
|
||||
YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
|
||||
INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
|
||||
INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
|
||||
POSENET_STD_INITIAL_VALUE = 10.0
|
||||
POSENET_STD_HIST_HALF = 20
|
||||
|
||||
|
||||
def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
|
||||
return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency))
|
||||
|
||||
|
||||
def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool):
|
||||
assert len(values) == len(stds) == 3
|
||||
measurement.x, measurement.y, measurement.z = map(float, values)
|
||||
measurement.xStd, measurement.yStd, measurement.zStd = map(float, stds)
|
||||
measurement.valid = valid
|
||||
|
||||
|
||||
class HandleLogResult(Enum):
|
||||
SUCCESS = 0
|
||||
TIMING_INVALID = 1
|
||||
INPUT_INVALID = 2
|
||||
SENSOR_SOURCE_INVALID = 3
|
||||
|
||||
|
||||
class LocationEstimator:
|
||||
def __init__(self, debug: bool):
|
||||
self.kf = PoseKalman(MAX_FILTER_REWIND_TIME)
|
||||
|
||||
self.debug = debug
|
||||
|
||||
self.posenet_stds = np.array([POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2))
|
||||
self.car_speed = 0.0
|
||||
self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std
|
||||
self.device_from_calib = np.eye(3)
|
||||
|
||||
obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION]
|
||||
self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
|
||||
self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
|
||||
|
||||
def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P):
|
||||
self.kf.init_state(x_initial, covs=P_initial, filter_time=t)
|
||||
|
||||
def _validate_sensor_source(self, source: log.SensorEventData.SensorSource):
|
||||
# some segments have two IMUs, ignore the second one
|
||||
return source != log.SensorEventData.SensorSource.bmx055
|
||||
|
||||
def _validate_sensor_time(self, sensor_time: float, t: float):
|
||||
# ignore empty readings
|
||||
if sensor_time == 0:
|
||||
return False
|
||||
|
||||
# sensor time and log time should be close
|
||||
sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF
|
||||
if sensor_time_invalid:
|
||||
cloudlog.warning("Sensor reading ignored, sensor timestamp more than 100ms off from log time")
|
||||
return not sensor_time_invalid
|
||||
|
||||
def _validate_timestamp(self, t: float):
|
||||
kf_t = self.kf.t
|
||||
invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
|
||||
if invalid:
|
||||
cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter")
|
||||
return not invalid
|
||||
|
||||
def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray):
|
||||
all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all()
|
||||
if not all_finite:
|
||||
cloudlog.error("Non-finite values detected, kalman reset")
|
||||
self.reset(t)
|
||||
|
||||
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult:
|
||||
new_x, new_P = None, None
|
||||
if which == "accelerometer" and msg.which() == "acceleration":
|
||||
sensor_time = msg.timestamp * 1e-9
|
||||
|
||||
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
|
||||
return HandleLogResult.TIMING_INVALID
|
||||
|
||||
if not self._validate_sensor_source(msg.source):
|
||||
return HandleLogResult.SENSOR_SOURCE_INVALID
|
||||
|
||||
v = msg.acceleration.v
|
||||
meas = np.array([-v[2], -v[1], -v[0]])
|
||||
if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK:
|
||||
return HandleLogResult.INPUT_INVALID
|
||||
|
||||
acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas)
|
||||
if acc_res is not None:
|
||||
_, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res
|
||||
self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err)
|
||||
self.observations[ObservationKind.PHONE_ACCEL] = meas
|
||||
|
||||
elif which == "gyroscope" and msg.which() == "gyroUncalibrated":
|
||||
sensor_time = msg.timestamp * 1e-9
|
||||
|
||||
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
|
||||
return HandleLogResult.TIMING_INVALID
|
||||
|
||||
if not self._validate_sensor_source(msg.source):
|
||||
return HandleLogResult.SENSOR_SOURCE_INVALID
|
||||
|
||||
v = msg.gyroUncalibrated.v
|
||||
meas = np.array([-v[2], -v[1], -v[0]])
|
||||
|
||||
gyro_bias = self.kf.x[States.GYRO_BIAS]
|
||||
gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0])
|
||||
gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1]
|
||||
gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold
|
||||
|
||||
if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid:
|
||||
return HandleLogResult.INPUT_INVALID
|
||||
|
||||
gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas)
|
||||
if gyro_res is not None:
|
||||
_, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res
|
||||
self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err)
|
||||
self.observations[ObservationKind.PHONE_GYRO] = meas
|
||||
|
||||
elif which == "carState":
|
||||
self.car_speed = abs(msg.vEgo)
|
||||
|
||||
elif which == "extrinsicsCalibration":
|
||||
# Note that we use this message during calibration
|
||||
calib = get_calibrated_rpy(msg)
|
||||
if calib is None and len(msg.rpyCalib) > 0:
|
||||
calib = np.array(msg.rpyCalib)
|
||||
|
||||
if calib is not None:
|
||||
if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK:
|
||||
return HandleLogResult.INPUT_INVALID
|
||||
|
||||
self.device_from_calib = rot_from_euler(calib)
|
||||
|
||||
elif which == "cameraOdometry":
|
||||
if not self._validate_timestamp(t):
|
||||
return HandleLogResult.TIMING_INVALID
|
||||
|
||||
rot_device = np.matmul(self.device_from_calib, np.array(msg.rot))
|
||||
trans_device = np.matmul(self.device_from_calib, np.array(msg.trans))
|
||||
|
||||
if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK:
|
||||
return HandleLogResult.INPUT_INVALID
|
||||
|
||||
rot_calib_std = np.array(msg.rotStd)
|
||||
trans_calib_std = np.array(msg.transStd)
|
||||
|
||||
if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK:
|
||||
return HandleLogResult.INPUT_INVALID
|
||||
|
||||
if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK:
|
||||
return HandleLogResult.INPUT_INVALID
|
||||
|
||||
self.posenet_stds = np.roll(self.posenet_stds, -1)
|
||||
self.posenet_stds[-1] = trans_calib_std[0]
|
||||
|
||||
# Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
|
||||
rot_calib_std *= 10
|
||||
trans_calib_std *= 2
|
||||
|
||||
rot_device_std = rotate_std(self.device_from_calib, rot_calib_std)
|
||||
trans_device_std = rotate_std(self.device_from_calib, trans_calib_std)
|
||||
rot_device_noise = rot_device_std ** 2
|
||||
trans_device_noise = trans_device_std ** 2
|
||||
|
||||
cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)]))
|
||||
cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)]))
|
||||
self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]])
|
||||
if cam_odo_rot_res is not None:
|
||||
_, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res
|
||||
self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err)
|
||||
self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device
|
||||
if cam_odo_trans_res is not None:
|
||||
_, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res
|
||||
self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err)
|
||||
self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device
|
||||
|
||||
if new_x is not None and new_P is not None:
|
||||
self._finite_check(t, new_x, new_P)
|
||||
return HandleLogResult.SUCCESS
|
||||
|
||||
def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool):
|
||||
state, cov = self.kf.x, self.kf.P
|
||||
std = np.sqrt(np.diag(cov))
|
||||
|
||||
orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION]
|
||||
velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY]
|
||||
angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY]
|
||||
acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION]
|
||||
|
||||
msg = messaging.new_message("deviceMotion")
|
||||
msg.valid = filter_valid
|
||||
|
||||
deviceMotion = msg.deviceMotion
|
||||
init_xyz_measurement(deviceMotion.orientationNED, orientation_ned, orientation_ned_std, filter_valid)
|
||||
init_xyz_measurement(deviceMotion.velocityDevice, velocity_device, velocity_device_std, filter_valid)
|
||||
init_xyz_measurement(deviceMotion.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid)
|
||||
init_xyz_measurement(deviceMotion.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid)
|
||||
if self.debug:
|
||||
deviceMotion.debugFilterState.value = state.tolist()
|
||||
deviceMotion.debugFilterState.std = std.tolist()
|
||||
deviceMotion.debugFilterState.valid = filter_valid
|
||||
deviceMotion.debugFilterState.observations = [
|
||||
{'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()}
|
||||
for k in self.observations.keys()
|
||||
]
|
||||
|
||||
old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF])
|
||||
new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:])
|
||||
std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0
|
||||
|
||||
deviceMotion.inputsOK = inputs_valid
|
||||
deviceMotion.posenetOK = not std_spike or self.car_speed <= 5.0
|
||||
deviceMotion.sensorsOK = sensors_valid
|
||||
|
||||
return msg
|
||||
|
||||
|
||||
def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation):
|
||||
cur_time = time.monotonic()
|
||||
for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]:
|
||||
if len(msgs) > 0:
|
||||
sensor_valid[which] = msgs[-1].valid
|
||||
sensor_recv_time[which] = cur_time
|
||||
|
||||
if not simulation:
|
||||
sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1
|
||||
else:
|
||||
sensor_alive[which] = len(msgs) > 0
|
||||
|
||||
return all(sensor_alive.values()) and all(sensor_valid.values())
|
||||
|
||||
|
||||
def main():
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
DEBUG = bool(int(os.getenv("DEBUG", "0")))
|
||||
SIMULATION = bool(int(os.getenv("SIMULATION", "0")))
|
||||
|
||||
pm = messaging.PubMaster(['deviceMotion'])
|
||||
sm = messaging.SubMaster(['carState', 'extrinsicsCalibration', 'cameraOdometry'], poll='cameraOdometry')
|
||||
# separate sensor sockets for efficiency
|
||||
sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']]
|
||||
sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float)
|
||||
|
||||
params = Params()
|
||||
|
||||
estimator = LocationEstimator(DEBUG)
|
||||
|
||||
filter_initialized = False
|
||||
critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
|
||||
observation_input_invalid = defaultdict(int)
|
||||
|
||||
input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
|
||||
input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
|
||||
input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services}
|
||||
|
||||
initial_pose_data = params.get("LocationFilterInitialState")
|
||||
if initial_pose_data is not None:
|
||||
with log.Event.from_bytes(initial_pose_data) as lp_msg:
|
||||
filter_state = lp_msg.deviceMotion.debugFilterState
|
||||
x_initial = np.array(filter_state.value, dtype=np.float64) if len(filter_state.value) != 0 else PoseKalman.initial_x
|
||||
P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P
|
||||
estimator.reset(None, x_initial, P_initial)
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets)
|
||||
|
||||
if filter_initialized:
|
||||
msgs = []
|
||||
for msg in acc_msgs + gyro_msgs:
|
||||
t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which())
|
||||
msgs.append((t, valid, which, data))
|
||||
for which, updated in sm.updated.items():
|
||||
if not updated:
|
||||
continue
|
||||
t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which]
|
||||
msgs.append((t, valid, which, data))
|
||||
|
||||
for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]):
|
||||
if valid:
|
||||
t = log_mono_time * 1e-9
|
||||
res = estimator.handle_log(t, which, msg)
|
||||
if which not in critcal_services:
|
||||
continue
|
||||
|
||||
if res == HandleLogResult.TIMING_INVALID:
|
||||
cloudlog.warning(f"Observation {which} ignored due to failed timing check")
|
||||
observation_input_invalid[which] += 1
|
||||
elif res == HandleLogResult.INPUT_INVALID:
|
||||
cloudlog.warning(f"Observation {which} ignored due to failed sanity check")
|
||||
observation_input_invalid[which] += 1
|
||||
elif res == HandleLogResult.SUCCESS:
|
||||
observation_input_invalid[which] *= input_invalid_decay[which]
|
||||
else:
|
||||
filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
|
||||
|
||||
if sm.updated["cameraOdometry"]:
|
||||
critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
|
||||
inputs_valid = sm.all_valid() and critical_service_inputs_valid
|
||||
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
|
||||
|
||||
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
|
||||
pm.send("deviceMotion", msg)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+88
@@ -0,0 +1,88 @@
|
||||
class ObservationKind:
|
||||
UNKNOWN = 0
|
||||
NO_OBSERVATION = 1
|
||||
GPS_NED = 2
|
||||
ODOMETRIC_SPEED = 3
|
||||
PHONE_GYRO = 4
|
||||
GPS_VEL = 5
|
||||
PSEUDORANGE_GPS = 6
|
||||
PSEUDORANGE_RATE_GPS = 7
|
||||
SPEED = 8
|
||||
NO_ROT = 9
|
||||
PHONE_ACCEL = 10
|
||||
ORB_POINT = 11
|
||||
ECEF_POS = 12
|
||||
CAMERA_ODO_TRANSLATION = 13
|
||||
CAMERA_ODO_ROTATION = 14
|
||||
ORB_FEATURES = 15
|
||||
MSCKF_TEST = 16
|
||||
FEATURE_TRACK_TEST = 17
|
||||
LANE_PT = 18
|
||||
IMU_FRAME = 19
|
||||
PSEUDORANGE_GLONASS = 20
|
||||
PSEUDORANGE_RATE_GLONASS = 21
|
||||
PSEUDORANGE = 22
|
||||
PSEUDORANGE_RATE = 23
|
||||
ECEF_VEL = 35
|
||||
ECEF_ORIENTATION_FROM_GPS = 32
|
||||
NO_ACCEL = 33
|
||||
ORB_FEATURES_WIDE = 34
|
||||
|
||||
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
|
||||
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
|
||||
STEER_ANGLE = 26 # [rad]
|
||||
ANGLE_OFFSET_FAST = 27 # [rad]
|
||||
STIFFNESS = 28 # [-]
|
||||
STEER_RATIO = 29 # [-]
|
||||
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
|
||||
ROAD_ROLL = 31 # [rad]
|
||||
|
||||
names = [
|
||||
'Unknown',
|
||||
'No observation',
|
||||
'GPS NED',
|
||||
'Odometric speed',
|
||||
'Phone gyro',
|
||||
'GPS velocity',
|
||||
'GPS pseudorange',
|
||||
'GPS pseudorange rate',
|
||||
'Speed',
|
||||
'No rotation',
|
||||
'Phone acceleration',
|
||||
'ORB point',
|
||||
'ECEF pos',
|
||||
'camera odometric translation',
|
||||
'camera odometric rotation',
|
||||
'ORB features',
|
||||
'MSCKF test',
|
||||
'Feature track test',
|
||||
'Lane ecef point',
|
||||
'imu frame eulers',
|
||||
'GLONASS pseudorange',
|
||||
'GLONASS pseudorange rate',
|
||||
'pseudorange',
|
||||
'pseudorange rate',
|
||||
|
||||
'Road Frame x,y speed',
|
||||
'Road Frame yaw rate',
|
||||
'Steer Angle',
|
||||
'Fast Angle Offset',
|
||||
'Stiffness',
|
||||
'Steer Ratio',
|
||||
'Road Frame x speed',
|
||||
'Road Roll',
|
||||
'ECEF orientation from GPS',
|
||||
'NO accel',
|
||||
'ORB features wide camera',
|
||||
'ECEF_VEL',
|
||||
]
|
||||
|
||||
@classmethod
|
||||
def to_string(cls, kind):
|
||||
return cls.names[kind]
|
||||
|
||||
|
||||
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GPS,
|
||||
ObservationKind.PSEUDORANGE_GLONASS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GLONASS]
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user