mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-03 17:19:24 +08:00
Compare commits
64 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 289171fef8 | |||
| 5305655f78 | |||
| 1a07e47228 | |||
| 50b860c928 | |||
| 978ec800fe | |||
| d733058dee | |||
| ce488dff50 | |||
| 2e87e08ba6 | |||
| 785a6baa3d | |||
| c22f58dae3 | |||
| 3c9dd2e590 | |||
| d086f83aed | |||
| eef36d8b54 | |||
| 62e0462662 | |||
| ee9ee3a6fc | |||
| c4a3854dc5 | |||
| ad516e619b | |||
| 8c3cd2575f | |||
| 3ec226474a | |||
| 273eb9f997 | |||
| e143888eb1 | |||
| 8092f13438 | |||
| a87788b65c | |||
| 4cbe97fd20 | |||
| 98254867a9 | |||
| bee1cdd45d | |||
| 91e40b80d8 | |||
| 110568a9d1 | |||
| bb1b9a27d8 | |||
| 76b21c72cf | |||
| 3a05c03079 | |||
| fd22de1c9a | |||
| ee3583df33 | |||
| 7801bdf0cc | |||
| 70424bd661 | |||
| d215eab1d4 | |||
| 91316c8cb5 | |||
| fa284be7e6 | |||
| 6e7d9e5e52 | |||
| a4a7c2335d | |||
| ae573c7c3f | |||
| 7d7b6ee306 | |||
| 6a4c59c3e0 | |||
| fb5cb7a1cc | |||
| 049dfd2eaa | |||
| be20848487 | |||
| cdd232b606 | |||
| b21c70b1ba | |||
| 67e5bd3c1e | |||
| 74692d0b5f | |||
| dd35c27981 | |||
| 159140e64e | |||
| f1ab6c8dfb | |||
| e1fe30fd3e | |||
| fba521dcff | |||
| a8ef55bfaa | |||
| a232f54e2d | |||
| 2697008aa7 | |||
| ad5abd242a | |||
| 6c1e0f370b | |||
| 1083f5bf21 | |||
| dc5116c718 | |||
| 8611e08dc6 | |||
| dc0f73c63b |
@@ -12,11 +12,11 @@ on:
|
||||
required: false
|
||||
type: string
|
||||
recompiled_dir:
|
||||
description: 'Existing recompiled directory number (e.g. 3 for recompiled3)'
|
||||
description: 'Existing recompiled directory number (e.g. 1 for recompiled1)'
|
||||
required: true
|
||||
type: string
|
||||
json_version:
|
||||
description: 'driving_models version number to update (e.g. 5 for driving_models_v5.json)'
|
||||
description: 'driving_models version number to update (e.g. 18 for driving_models_v18.json)'
|
||||
required: true
|
||||
type: string
|
||||
artifact_suffix:
|
||||
@@ -63,12 +63,11 @@ on:
|
||||
default: 'None'
|
||||
options:
|
||||
- None
|
||||
- Simple Plan Models
|
||||
- Space Lab Models
|
||||
- TR Models
|
||||
- DTR Models
|
||||
- Master Models
|
||||
- Release Models
|
||||
- 2025 World Models
|
||||
- 2026 World Models
|
||||
- Custom Merge Models
|
||||
- FOF series models
|
||||
- Other
|
||||
custom_model_folder:
|
||||
description: 'Custom model folder name (if "Other" selected)'
|
||||
|
||||
@@ -1,39 +0,0 @@
|
||||
name: prebuilt
|
||||
on:
|
||||
schedule:
|
||||
- cron: '0 * * * *'
|
||||
workflow_dispatch:
|
||||
|
||||
env:
|
||||
DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }}
|
||||
BUILD: release/ci/docker_build_sp.sh
|
||||
|
||||
jobs:
|
||||
build_prebuilt:
|
||||
name: build prebuilt
|
||||
runs-on: ubuntu-latest
|
||||
if: github.repository == 'sunnypilot/sunnypilot'
|
||||
env:
|
||||
PUSH_IMAGE: true
|
||||
permissions:
|
||||
checks: read
|
||||
contents: read
|
||||
packages: write
|
||||
steps:
|
||||
- name: Wait for green check mark
|
||||
if: ${{ github.event_name != 'workflow_dispatch' }}
|
||||
uses: lewagon/wait-on-check-action@ccfb013c15c8afb7bf2b7c028fb74dc5a068cccc
|
||||
with:
|
||||
ref: master
|
||||
wait-interval: 30
|
||||
running-workflow-name: 'build prebuilt'
|
||||
repo-token: ${{ secrets.GITHUB_TOKEN }}
|
||||
check-regexp: ^((?!.*(build master-ci|create badges).*).)*$
|
||||
- uses: actions/checkout@v6
|
||||
with:
|
||||
submodules: true
|
||||
- run: git lfs pull
|
||||
- name: Build and Push docker image
|
||||
run: |
|
||||
$DOCKER_LOGIN
|
||||
eval "$BUILD"
|
||||
@@ -30,6 +30,11 @@ on:
|
||||
required: false
|
||||
type: string
|
||||
default: ''
|
||||
target_hardware:
|
||||
description: 'Hardware target to compile for (qcom or usbgpu)'
|
||||
required: false
|
||||
type: string
|
||||
default: 'qcom'
|
||||
workflow_dispatch:
|
||||
inputs:
|
||||
upstream_branch:
|
||||
@@ -46,6 +51,14 @@ on:
|
||||
required: false
|
||||
type: boolean
|
||||
default: true
|
||||
target_hardware:
|
||||
description: 'Hardware target to compile for'
|
||||
required: true
|
||||
type: choice
|
||||
options:
|
||||
- qcom
|
||||
- usbgpu
|
||||
default: 'qcom'
|
||||
|
||||
|
||||
run-name: Build model [${{ inputs.custom_name || inputs.upstream_branch }}] from ref [${{ inputs.upstream_branch }}]
|
||||
@@ -161,19 +174,30 @@ jobs:
|
||||
name: models-${{ env.REF }}${{ inputs.artifact_suffix }}
|
||||
path: ${{ env.MODELS_DIR }}
|
||||
- run: |
|
||||
rm -f ${{ env.MODELS_DIR }}/{dmonitoring_model,big_driving_policy,big_driving_vision}.onnx
|
||||
rm -f ${{ env.MODELS_DIR }}/{dmonitoring_model,big_driving_policy,big_driving_vision,big_driving_supercombo}.onnx
|
||||
|
||||
- name: Build Model
|
||||
run: |
|
||||
source /etc/profile
|
||||
export UV_PROJECT_ENVIRONMENT=${HOME}/venv
|
||||
export VIRTUAL_ENV=$UV_PROJECT_ENVIRONMENT
|
||||
source ${UV_PROJECT_ENVIRONMENT}/bin/activate
|
||||
export PYTHONPATH="${PYTHONPATH}:${{ env.TINYGRAD_PATH }}:${{ github.workspace }}"
|
||||
|
||||
COMPILE_MODELD="${{ github.workspace }}/openpilot/sunnypilot/modeld_v2/compile_modeld.py"
|
||||
MODEL_SIZE=$(python3 -c "from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE as s; print(f'{s[0]}x{s[1]}')")
|
||||
CAMERA_RES=$(python3 -c "from openpilot.common.transformations.camera import _ar_ox_fisheye as a, _os_fisheye as o; print(f'{a.width}x{a.height} {o.width}x{o.height}')")
|
||||
TG_FLAGS="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
|
||||
|
||||
if [ "${{ inputs.target_hardware }}" == "usbgpu" ]; then
|
||||
echo "USBGPU build"
|
||||
export USBGPU=1
|
||||
TG_FLAGS="DEV=AMD USBGPU=1 IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
|
||||
OUTPUT_PKL="${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl"
|
||||
else
|
||||
echo "QCOM build"
|
||||
TG_FLAGS="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
|
||||
OUTPUT_PKL="${{ env.MODELS_DIR }}/driving_tinygrad.pkl"
|
||||
fi
|
||||
|
||||
# Generate metadata for all ONNX files
|
||||
find "${{ env.MODELS_DIR }}" -maxdepth 1 -name '*.onnx' | while IFS= read -r onnx_file; do
|
||||
@@ -186,7 +210,13 @@ jobs:
|
||||
POLICY_ONNX="${{ env.MODELS_DIR }}/driving_policy.onnx"
|
||||
OFF_POLICY_ONNX="${{ env.MODELS_DIR }}/driving_off_policy.onnx"
|
||||
ON_POLICY_ONNX="${{ env.MODELS_DIR }}/driving_on_policy.onnx"
|
||||
SUPERCOMBO_ONNX="${{ env.MODELS_DIR }}/supercombo.onnx"
|
||||
SUPERCOMBO_ONNX=""
|
||||
for f in "${{ env.MODELS_DIR }}/supercombo.onnx" "${{ env.MODELS_DIR }}/driving_supercombo.onnx"; do
|
||||
if [ -f "$f" ]; then
|
||||
SUPERCOMBO_ONNX="$f"
|
||||
break
|
||||
fi
|
||||
done
|
||||
|
||||
MODEL_TYPE="" ONNX_ARGS="" OUTPUT_NAME=""
|
||||
if [ -f "$VISION_ONNX" ]; then
|
||||
@@ -207,24 +237,15 @@ jobs:
|
||||
fi
|
||||
|
||||
if [ -n "$MODEL_TYPE" ]; then
|
||||
echo "Detected: $MODEL_TYPE -> driving_tinygrad.pkl"
|
||||
echo "Detected: $MODEL_TYPE -> $OUTPUT_PKL"
|
||||
env ${TG_FLAGS} python3 "$COMPILE_MODELD" \
|
||||
--model-type $MODEL_TYPE \
|
||||
--model-size $MODEL_SIZE \
|
||||
--camera-resolutions $CAMERA_RES \
|
||||
$ONNX_ARGS \
|
||||
--output "${{ env.MODELS_DIR }}/driving_tinygrad.pkl"
|
||||
--output "$OUTPUT_PKL"
|
||||
fi
|
||||
|
||||
- name: Validate Model Outputs
|
||||
run: |
|
||||
source /etc/profile
|
||||
export UV_PROJECT_ENVIRONMENT=${HOME}/venv
|
||||
export VIRTUAL_ENV=$UV_PROJECT_ENVIRONMENT
|
||||
python3 "${{ github.workspace }}/release/ci/model_generator.py" \
|
||||
--validate-only \
|
||||
--model-dir "${{ env.MODELS_DIR }}"
|
||||
|
||||
- name: Prepare Output
|
||||
run: |
|
||||
sudo rm -rf ${{ env.OUTPUT_DIR }}
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: a63c0cf016...4c64e8a95b
@@ -69,6 +69,8 @@ struct LeadData {
|
||||
struct SelfdriveStateSP @0x81c2f05a394cf4af {
|
||||
mads @0 :ModularAssistiveDrivingSystem;
|
||||
intelligentCruiseButtonManagement @1 :IntelligentCruiseButtonManagement;
|
||||
buttonsPressed @2 :UInt16;
|
||||
buttonsReleaseToggle @3 :UInt16;
|
||||
|
||||
enum AudibleAlert {
|
||||
none @0;
|
||||
@@ -137,10 +139,16 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
eta @2 :UInt32;
|
||||
}
|
||||
|
||||
struct Chunk {
|
||||
fileName @0 :Text;
|
||||
sha256 @1 :Text;
|
||||
}
|
||||
|
||||
struct Artifact {
|
||||
fileName @0 :Text;
|
||||
downloadUri @1 :DownloadUri;
|
||||
downloadProgress @2 :DownloadProgress;
|
||||
chunks @3 :List(Chunk);
|
||||
}
|
||||
|
||||
struct Model {
|
||||
@@ -155,6 +163,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
policy @3;
|
||||
offPolicy @4;
|
||||
onPolicy @5;
|
||||
chunked @6;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -155,7 +155,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"CustomAccShortPressIncrement", {PERSISTENT | BACKUP, INT, "1"}},
|
||||
{"DeviceBootMode", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"DrawRadarTracks", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
|
||||
{"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
|
||||
{"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
@@ -179,7 +178,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"OnroadUploads", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
{"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RadarTracks", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
@@ -224,6 +222,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TeslaCoopSteering", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TeslaMadsScreenButton", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
|
||||
@@ -182,8 +182,6 @@ class Car:
|
||||
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode")
|
||||
self.radar_tracks = int(self.params.get("RadarTracks", return_default=True))
|
||||
self._applied_radar_tracks = getattr(self.RI, "radar_mode", self.radar_tracks)
|
||||
|
||||
# card is driven by can recv, expected at 100Hz
|
||||
self.rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
@@ -201,8 +199,6 @@ class Car:
|
||||
CS, CS_SP = self.CI.update(can_list)
|
||||
CS_SP = convert_to_capnp(CS_SP)
|
||||
|
||||
self._update_radar_tracks()
|
||||
|
||||
# Update radar tracks from CAN
|
||||
RD: structs.RadarDataT | None = self.RI.update(can_list)
|
||||
|
||||
@@ -229,18 +225,6 @@ class Car:
|
||||
|
||||
return CS, CS_SP, RD
|
||||
|
||||
def _update_radar_tracks(self) -> None:
|
||||
if self.CP.brand != "hyundai" or self.radar_tracks == self._applied_radar_tracks:
|
||||
return
|
||||
|
||||
if self.RI.set_radar_mode(self.radar_tracks):
|
||||
self.CP_SP_capnp = convert_to_capnp(self.CP_SP)
|
||||
cp_sp_bytes = self.CP_SP_capnp.to_bytes()
|
||||
self.params.put("CarParamsSP", cp_sp_bytes)
|
||||
self.params.put("CarParamsSPCache", cp_sp_bytes)
|
||||
self.params.put("CarParamsSPPersistent", cp_sp_bytes)
|
||||
self._applied_radar_tracks = self.radar_tracks
|
||||
|
||||
def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, RD: structs.RadarDataT | None):
|
||||
"""carState and carParams publish loop"""
|
||||
|
||||
@@ -319,7 +303,6 @@ class Car:
|
||||
while not evt.is_set():
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
|
||||
self.radar_tracks = int(self.params.get("RadarTracks", return_default=True))
|
||||
|
||||
# sunnypilot
|
||||
self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
|
||||
|
||||
@@ -158,7 +158,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
def publish(self, sm, pm):
|
||||
plan_send = messaging.new_message('longitudinalPlan')
|
||||
|
||||
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState', 'radarState'])
|
||||
plan_send.valid = sm.all_checks()
|
||||
|
||||
longitudinalPlan = plan_send.longitudinalPlan
|
||||
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
||||
|
||||
@@ -23,24 +23,25 @@ def main():
|
||||
cloudlog.info("plannerd got CarParamsSP")
|
||||
|
||||
gps_location_service = get_gps_location_service(params)
|
||||
ignore_services = ["liveMapDataSP", gps_location_service]
|
||||
|
||||
ldw = LaneDepartureWarning()
|
||||
longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
|
||||
'liveMapDataSP', 'carStateSP', gps_location_service],
|
||||
poll='carState')
|
||||
'liveMapDataSP', 'carStateSP', 'selfdriveStateSP', gps_location_service],
|
||||
poll='modelV2', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
longitudinal_planner.sla.update_car_state(sm['carState'])
|
||||
longitudinal_planner.sla.update_buttons(sm['selfdriveStateSP'].buttonsReleaseToggle)
|
||||
if sm.updated['modelV2']:
|
||||
longitudinal_planner.update(sm)
|
||||
longitudinal_planner.publish(sm, pm)
|
||||
|
||||
ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl'])
|
||||
msg = messaging.new_message('driverAssistance')
|
||||
msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2', 'liveParameters'])
|
||||
msg.valid = sm.all_checks()
|
||||
msg.driverAssistance.leftLaneDeparture = ldw.left
|
||||
msg.driverAssistance.rightLaneDeparture = ldw.right
|
||||
pm.send('driverAssistance', msg)
|
||||
|
||||
@@ -26,8 +26,6 @@ SPEED, ACCEL = 0, 1 # Kalman filter states enum
|
||||
|
||||
# stationary qualification parameters
|
||||
V_EGO_STATIONARY = 4. # no stationary object flag below this speed
|
||||
DBC_MOTION_STATIONARY = 1
|
||||
DBC_MOTION_MOVING = 2
|
||||
|
||||
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame
|
||||
|
||||
@@ -193,14 +191,8 @@ def get_custom_yrel(CP: structs.CarParams, CP_SP: structs.CarParamsSP, lead_dict
|
||||
return lead_dict
|
||||
|
||||
|
||||
def radar_point_eligible_for_fusion(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
point: car.RadarData.RadarPoint) -> bool:
|
||||
use_dbc_motion = CP.brand == "hyundai" and CP_SP.flags & HyundaiFlagsSP.RADAR_FULL_RADAR
|
||||
return not use_dbc_motion or point.motionState in (DBC_MOTION_STATIONARY, DBC_MOTION_MOVING)
|
||||
|
||||
|
||||
class RadarD:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, delay: float = 0.0):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams, delay: float = 0.0):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
@@ -228,11 +220,7 @@ class RadarD:
|
||||
self.v_ego_hist.append(self.v_ego)
|
||||
self.last_v_ego_frame = sm.recv_frame['carState']
|
||||
|
||||
ar_pts = {
|
||||
pt.trackId: [pt.dRel, pt.yRel, pt.vRel]
|
||||
for pt in rr.points
|
||||
if radar_point_eligible_for_fusion(self.CP, self.CP_SP, pt)
|
||||
}
|
||||
ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel] for pt in rr.points}
|
||||
|
||||
# *** remove missing points from meta data ***
|
||||
for ids in list(self.tracks.keys()):
|
||||
|
||||
@@ -1,29 +0,0 @@
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.selfdrive.controls.radard import radar_point_eligible_for_fusion
|
||||
|
||||
|
||||
def radar_point(motion_state: int):
|
||||
point = car.RadarData.RadarPoint.new_message()
|
||||
point.motionState = motion_state
|
||||
return point
|
||||
|
||||
|
||||
def test_hyundai_full_radar_fuses_only_classified_points():
|
||||
CP = structs.CarParams(brand="hyundai")
|
||||
CP_SP = custom.CarParamsSP.new_message()
|
||||
CP_SP.flags = HyundaiFlagsSP.RADAR_FULL_RADAR.value
|
||||
|
||||
assert radar_point_eligible_for_fusion(CP, CP_SP, radar_point(1))
|
||||
assert radar_point_eligible_for_fusion(CP, CP_SP, radar_point(2))
|
||||
assert not radar_point_eligible_for_fusion(CP, CP_SP, radar_point(0))
|
||||
assert not radar_point_eligible_for_fusion(CP, CP_SP, radar_point(255))
|
||||
|
||||
|
||||
def test_radar_motion_filter_does_not_affect_other_modes_or_brands():
|
||||
CP_SP = custom.CarParamsSP.new_message()
|
||||
|
||||
assert radar_point_eligible_for_fusion(structs.CarParams(brand="hyundai"), CP_SP, radar_point(0))
|
||||
assert radar_point_eligible_for_fusion(structs.CarParams(brand="toyota"), CP_SP, radar_point(0))
|
||||
@@ -30,6 +30,7 @@ from openpilot.sunnypilot import get_sanitize_int_param
|
||||
from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
|
||||
from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
|
||||
from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
@@ -177,6 +178,7 @@ class SelfdriveD(CruiseHelper):
|
||||
self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP)
|
||||
|
||||
CruiseHelper.__init__(self, self.CP)
|
||||
self.button_state_tracker = ButtonStateTracker()
|
||||
|
||||
def update_events(self, CS):
|
||||
"""Compute onroadEvents from carState"""
|
||||
@@ -597,6 +599,8 @@ class SelfdriveD(CruiseHelper):
|
||||
icbm.sendButton = self.icbm.cruise_button
|
||||
icbm.vTarget = self.icbm.v_target
|
||||
|
||||
self.button_state_tracker.publish(ss_sp)
|
||||
|
||||
self.pm.send('selfdriveStateSP', ss_sp_msg)
|
||||
|
||||
# onroadEventsSP - logged every second or on change
|
||||
@@ -616,6 +620,7 @@ class SelfdriveD(CruiseHelper):
|
||||
self.mads.update(CS)
|
||||
self.update_alerts(CS)
|
||||
|
||||
self.button_state_tracker.update(CS)
|
||||
self.publish_selfdriveState(CS)
|
||||
|
||||
self.CS_prev = CS
|
||||
|
||||
@@ -37,7 +37,7 @@ class MainLayout(Widget):
|
||||
self._layouts: dict[MainState, Widget] = {
|
||||
MainState.HOME: self._home_layout,
|
||||
MainState.SETTINGS: SettingsLayout(),
|
||||
MainState.ONROAD: AugmentedRoadView(radar_tracks_settings_callback=lambda: self.open_settings(PanelType.TOGGLES)),
|
||||
MainState.ONROAD: AugmentedRoadView(),
|
||||
}
|
||||
|
||||
self._sidebar_rect = rl.Rectangle(0, 0, 0, 0)
|
||||
|
||||
@@ -106,24 +106,7 @@ class TogglesLayout(Widget):
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._radar_tracks_setting = None
|
||||
self._toggles = {}
|
||||
if gui_app.sunnypilot_ui():
|
||||
self._radar_tracks_setting = multiple_button_item(
|
||||
lambda: tr("Radar Tracks"),
|
||||
"",
|
||||
buttons=[lambda: tr("Off"), lambda: tr("Lead Only"), lambda: tr("Full Radar")],
|
||||
button_width=250,
|
||||
callback=self._set_radar_tracks,
|
||||
selected_index=self._params.get("RadarTracks", return_default=True),
|
||||
)
|
||||
self._toggles["RadarTracks"] = self._radar_tracks_setting
|
||||
self._toggle_defs["DrawRadarTracks"] = (
|
||||
lambda: tr("Draw Radar Tracks"),
|
||||
tr_noop("Show radar tracks on the driving screen. Disabling this does not disable radar processing."),
|
||||
"",
|
||||
False,
|
||||
)
|
||||
self._locked_toggles = set()
|
||||
for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
|
||||
toggle = toggle_item(
|
||||
@@ -220,8 +203,6 @@ class TogglesLayout(Widget):
|
||||
# refresh toggles from params to mirror external changes
|
||||
for param in self._toggle_defs:
|
||||
self._toggles[param].action_item.set_state(self._params.get_bool(param))
|
||||
if self._radar_tracks_setting is not None:
|
||||
self._radar_tracks_setting.action_item.set_selected_button(self._params.get("RadarTracks", return_default=True))
|
||||
|
||||
# these toggles need restart, block while engaged
|
||||
for toggle_def in self._toggle_defs:
|
||||
@@ -266,6 +247,3 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _set_longitudinal_personality(self, button_index: int):
|
||||
self._params.put("LongitudinalPersonality", button_index, block=True)
|
||||
|
||||
def _set_radar_tracks(self, button_index: int):
|
||||
self._params.put("RadarTracks", button_index, block=True)
|
||||
|
||||
@@ -32,8 +32,7 @@ class MiciMainLayout(Scroller):
|
||||
self._home_layout = MiciHomeLayout()
|
||||
self._alerts_layout = MiciOffroadAlerts()
|
||||
self._settings_layout = SettingsLayout()
|
||||
self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked,
|
||||
radar_tracks_settings_callback=self._settings_layout.open_toggles)
|
||||
self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
|
||||
self._body_onroad_layout = BodyLayout()
|
||||
|
||||
# Initialize widget rects
|
||||
|
||||
@@ -20,9 +20,9 @@ class SettingsLayout(NavScroller):
|
||||
super().__init__()
|
||||
self._params = Params()
|
||||
|
||||
self._toggles_panel = TogglesLayoutMici()
|
||||
toggles_panel = TogglesLayoutMici()
|
||||
toggles_btn = SettingsBigButton("toggles", "", gui_app.texture("icons_mici/settings.png", 64, 64))
|
||||
toggles_btn.set_click_callback(lambda: gui_app.push_widget(self._toggles_panel))
|
||||
toggles_btn.set_click_callback(lambda: gui_app.push_widget(toggles_panel))
|
||||
|
||||
network_panel = NetworkLayoutMici()
|
||||
network_btn = SettingsBigButton("network", "", gui_app.texture("icons_mici/settings/network/wifi_strength_full.png", 76, 56))
|
||||
@@ -56,8 +56,3 @@ class SettingsLayout(NavScroller):
|
||||
])
|
||||
|
||||
self._font_medium = gui_app.font(FontWeight.MEDIUM)
|
||||
|
||||
def open_toggles(self) -> None:
|
||||
if not gui_app.widget_in_stack(self):
|
||||
gui_app.push_widget(self)
|
||||
gui_app.push_widget(self._toggles_panel)
|
||||
|
||||
@@ -21,12 +21,8 @@ class TogglesLayoutMici(NavScroller):
|
||||
record_front = BigParamControl("record & upload driver camera", "RecordFront", toggle_callback=restart_needed_callback)
|
||||
record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
|
||||
enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
|
||||
radar_tracks = BigMultiParamToggle("radar tracks", "RadarTracks", ["off", "lead only", "full radar"])
|
||||
draw_radar_tracks = BigParamControl("draw radar tracks", "DrawRadarTracks")
|
||||
|
||||
self._scroller.add_widgets([
|
||||
radar_tracks,
|
||||
draw_radar_tracks,
|
||||
self._personality_toggle,
|
||||
self._experimental_btn,
|
||||
is_metric_toggle,
|
||||
@@ -39,8 +35,6 @@ class TogglesLayoutMici(NavScroller):
|
||||
|
||||
# Toggle lists
|
||||
self._refresh_toggles = (
|
||||
("RadarTracks", radar_tracks),
|
||||
("DrawRadarTracks", draw_radar_tracks),
|
||||
("ExperimentalMode", self._experimental_btn),
|
||||
("IsMetric", is_metric_toggle),
|
||||
("IsLdwEnabled", ldw_toggle),
|
||||
|
||||
@@ -11,7 +11,6 @@ from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.model_renderer import ModelRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.confidence_ball import ConfidenceBall
|
||||
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracksStatus
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
@@ -135,11 +134,9 @@ class BookmarkIcon(Widget):
|
||||
|
||||
|
||||
class AugmentedRoadView(CameraView):
|
||||
def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD,
|
||||
radar_tracks_settings_callback=None):
|
||||
def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD):
|
||||
super().__init__("camerad", stream_type)
|
||||
self._bookmark_callback = bookmark_callback
|
||||
self._radar_tracks_status = RadarTracksStatus(radar_tracks_settings_callback)
|
||||
self._set_placeholder_color(rl.BLACK)
|
||||
|
||||
self.device_camera: DeviceCameraConfig | None = None
|
||||
@@ -173,13 +170,6 @@ class AugmentedRoadView(CameraView):
|
||||
def _update_state(self):
|
||||
super()._update_state()
|
||||
|
||||
if ui_state.sm.updated["liveTracks"]:
|
||||
self._radar_tracks_status.update(
|
||||
ui_state.sm["liveTracks"], ui_state.sm.valid["liveTracks"], ui_state.radar_tracks, ui_state.sm["carState"].vEgo,
|
||||
)
|
||||
elif not ui_state.sm.alive["liveTracks"]:
|
||||
self._radar_tracks_status.reset()
|
||||
|
||||
# update offroad label
|
||||
if ui_state.panda_type == log.PandaState.PandaType.unknown:
|
||||
self._offroad_label.set_text("system booting")
|
||||
@@ -189,9 +179,6 @@ class AugmentedRoadView(CameraView):
|
||||
self._offroad_label.set_text("start the car to\nuse sunnypilot")
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos):
|
||||
if self._radar_tracks_status.handle_mouse(mouse_pos):
|
||||
return
|
||||
|
||||
# Don't trigger click callback if bookmark was triggered
|
||||
if not self._bookmark_icon.interacting():
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
@@ -234,8 +221,6 @@ class AugmentedRoadView(CameraView):
|
||||
# Fade out bottom of overlays for looks
|
||||
rl.draw_texture_ex(self._fade_texture, rl.Vector2(self._content_rect.x, self._content_rect.y), 0.0, 1.0, rl.WHITE)
|
||||
|
||||
self._radar_tracks_status.render(self._content_rect)
|
||||
|
||||
alert_to_render, not_animating_out = self._alert_renderer.will_render()
|
||||
|
||||
# Hide DMoji when disengaged unless AlwaysOnDM is enabled
|
||||
|
||||
@@ -14,7 +14,6 @@ from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.model_renderer import LANE_LINE_COLORS_SP, ModelRendererSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, radar_lead_track_colors
|
||||
|
||||
CLIP_MARGIN = 500
|
||||
MIN_DRAW_DISTANCE = 10.0
|
||||
@@ -39,6 +38,7 @@ LANE_LINE_COLORS = {
|
||||
**LANE_LINE_COLORS_SP,
|
||||
}
|
||||
|
||||
|
||||
@dataclass
|
||||
class ModelPoints:
|
||||
raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32))
|
||||
@@ -50,9 +50,6 @@ class LeadVehicle:
|
||||
glow: list[tuple[float, float]] = field(default_factory=list)
|
||||
chevron: list[tuple[float, float]] = field(default_factory=list)
|
||||
fill_alpha: int = 0
|
||||
position: tuple[float, float] | None = None
|
||||
radar_track_id: int = -1
|
||||
radar: bool = False
|
||||
|
||||
|
||||
class ModelRenderer(Widget, ModelRendererSP):
|
||||
@@ -135,12 +132,11 @@ class ModelRenderer(Widget, ModelRendererSP):
|
||||
model = sm['modelV2']
|
||||
radar_state = sm['radarState'] if sm.valid['radarState'] else None
|
||||
lead_one = radar_state.leadOne if radar_state else None
|
||||
render_lead_indicator = ui_state.draw_radar_tracks and ui_state.radar_tracks != 0 and radar_state is not None
|
||||
render_lead_indicator = self._longitudinal_control and radar_state is not None
|
||||
|
||||
# Update model data when needed
|
||||
model_updated = sm.updated['modelV2']
|
||||
transform_updated = self._transform_dirty
|
||||
if model_updated or sm.updated['radarState'] or transform_updated:
|
||||
if model_updated or sm.updated['radarState'] or self._transform_dirty:
|
||||
if model_updated:
|
||||
self._update_raw_points(model)
|
||||
|
||||
@@ -158,27 +154,8 @@ class ModelRenderer(Widget, ModelRendererSP):
|
||||
self._draw_lane_lines()
|
||||
self._draw_path(sm)
|
||||
|
||||
if ui_state.draw_radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
|
||||
if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or transform_updated or
|
||||
not self.radar_tracks.projection_initialized):
|
||||
self.radar_tracks.update_radar_tracks(
|
||||
sm['liveTracks'], self._map_to_screen, self._path_offset_z,
|
||||
)
|
||||
highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {}
|
||||
matched_positions = self.radar_tracks.draw_cached_radar_tracks(
|
||||
screen_offset=(self._rect.x, self._rect.y),
|
||||
highlighted_tracks=highlighted_tracks,
|
||||
)
|
||||
if render_lead_indicator:
|
||||
draw_radar_lead_connectors(
|
||||
self._lead_vehicles, matched_positions, highlighted_tracks,
|
||||
screen_offset=(self._rect.x, self._rect.y),
|
||||
)
|
||||
else:
|
||||
self.radar_tracks.clear_projection()
|
||||
|
||||
if render_lead_indicator:
|
||||
self._draw_lead_indicator()
|
||||
# if render_lead_indicator and radar_state:
|
||||
# self._draw_lead_indicator()
|
||||
|
||||
def _update_raw_points(self, model):
|
||||
"""Update raw 3D points from model data"""
|
||||
@@ -208,11 +185,7 @@ class ModelRenderer(Widget, ModelRendererSP):
|
||||
z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
|
||||
point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
|
||||
if point:
|
||||
lead_vehicle = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
|
||||
lead_vehicle.position = lead_vehicle.chevron[1]
|
||||
lead_vehicle.radar_track_id = int(lead_data.radarTrackId)
|
||||
lead_vehicle.radar = lead_data.radar
|
||||
self._lead_vehicles[i] = lead_vehicle
|
||||
self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
|
||||
|
||||
def _update_model(self, lead, path_x_array):
|
||||
"""Update model visualization data based on model message"""
|
||||
@@ -407,10 +380,8 @@ class ModelRenderer(Widget, ModelRendererSP):
|
||||
if not lead.glow or not lead.chevron:
|
||||
continue
|
||||
|
||||
offset_glow = [(x + self._rect.x, y + self._rect.y) for x, y in lead.glow]
|
||||
offset_chevron = [(x + self._rect.x, y + self._rect.y) for x, y in lead.chevron]
|
||||
rl.draw_triangle_fan(offset_glow, len(offset_glow), rl.Color(218, 202, 37, 255))
|
||||
rl.draw_triangle_fan(offset_chevron, len(offset_chevron), rl.Color(201, 34, 49, lead.fill_alpha))
|
||||
rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255))
|
||||
rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha))
|
||||
|
||||
@staticmethod
|
||||
def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int:
|
||||
|
||||
@@ -18,7 +18,6 @@ if gui_app.sunnypilot_ui():
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.augmented_road_view import BORDER_COLORS_SP, AugmentedRoadViewSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.driver_state import DriverStateRendererSP as DriverStateRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.hud_renderer import HudRendererSP as HudRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracksStatus
|
||||
from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
|
||||
|
||||
OpState = log.SelfdriveState.OpenpilotState
|
||||
@@ -40,7 +39,7 @@ INF_POINT = np.array([1000.0, 0.0, 0.0])
|
||||
|
||||
|
||||
class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
|
||||
def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD, radar_tracks_settings_callback=None):
|
||||
def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD):
|
||||
CameraView.__init__(self, "camerad", stream_type)
|
||||
AugmentedRoadViewSP.__init__(self)
|
||||
self._set_placeholder_color(BORDER_COLORS[UIStatus.DISENGAGED])
|
||||
@@ -52,24 +51,12 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
|
||||
self._matrix_cache_key = (0, 0.0, 0.0, stream_type)
|
||||
self._cached_matrix: np.ndarray | None = None
|
||||
self._content_rect = rl.Rectangle()
|
||||
self._radar_tracks_status = RadarTracksStatus(radar_tracks_settings_callback, right_margin=240) if gui_app.sunnypilot_ui() else None
|
||||
|
||||
self.model_renderer = ModelRenderer()
|
||||
self._hud_renderer = HudRenderer()
|
||||
self.alert_renderer = AlertRenderer()
|
||||
self.driver_state_renderer = DriverStateRenderer()
|
||||
|
||||
def _update_state(self):
|
||||
super()._update_state()
|
||||
if self._radar_tracks_status is None:
|
||||
return
|
||||
if ui_state.sm.updated["liveTracks"]:
|
||||
self._radar_tracks_status.update(
|
||||
ui_state.sm["liveTracks"], ui_state.sm.valid["liveTracks"], ui_state.radar_tracks, ui_state.sm["carState"].vEgo,
|
||||
)
|
||||
elif not ui_state.sm.alive["liveTracks"]:
|
||||
self._radar_tracks_status.reset()
|
||||
|
||||
def _render(self, rect):
|
||||
# Only render when system is started to avoid invalid data access
|
||||
if not ui_state.started:
|
||||
@@ -103,8 +90,6 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
|
||||
# Draw all UI overlays
|
||||
self.model_renderer.render(self._content_rect)
|
||||
AugmentedRoadViewSP.update_fade_out_bottom_overlay(self, self._content_rect)
|
||||
if self._radar_tracks_status is not None:
|
||||
self._radar_tracks_status.render(self._content_rect)
|
||||
self._hud_renderer.render(self._content_rect)
|
||||
self.alert_renderer.render(self._content_rect)
|
||||
self.driver_state_renderer.render(self._content_rect)
|
||||
@@ -118,9 +103,7 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
|
||||
# Draw colored border based on driving state
|
||||
self._draw_border(rect)
|
||||
|
||||
def _handle_mouse_press(self, mouse_pos):
|
||||
if self._radar_tracks_status is not None and self._radar_tracks_status.handle_mouse(mouse_pos):
|
||||
return
|
||||
def _handle_mouse_press(self, _):
|
||||
if not self._hud_renderer.user_interacting() and self._click_callback is not None:
|
||||
self._click_callback()
|
||||
|
||||
|
||||
@@ -13,7 +13,6 @@ from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.model_renderer import ChevronMetrics, ModelRendererSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, radar_lead_track_colors
|
||||
|
||||
CLIP_MARGIN = 500
|
||||
MIN_DRAW_DISTANCE = 10.0
|
||||
@@ -43,9 +42,6 @@ class LeadVehicle:
|
||||
glow: list[tuple[float, float]] = field(default_factory=list)
|
||||
chevron: list[tuple[float, float]] = field(default_factory=list)
|
||||
fill_alpha: int = 0
|
||||
position: tuple[float, float] | None = None
|
||||
radar_track_id: int = -1
|
||||
radar: bool = False
|
||||
|
||||
|
||||
class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
@@ -119,12 +115,11 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
model = sm['modelV2']
|
||||
radar_state = sm['radarState'] if sm.valid['radarState'] else None
|
||||
lead_one = radar_state.leadOne if radar_state else None
|
||||
render_lead_indicator = ui_state.draw_radar_tracks and ui_state.radar_tracks != 0 and radar_state is not None
|
||||
render_lead_indicator = self._longitudinal_control and radar_state is not None
|
||||
|
||||
# Update model data when needed
|
||||
model_updated = sm.updated['modelV2']
|
||||
transform_updated = self._transform_dirty
|
||||
if model_updated or sm.updated['radarState'] or transform_updated:
|
||||
if model_updated or sm.updated['radarState'] or self._transform_dirty:
|
||||
if model_updated:
|
||||
self._update_raw_points(model)
|
||||
|
||||
@@ -141,21 +136,6 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
self._draw_lane_lines()
|
||||
self._draw_path(sm)
|
||||
|
||||
if ui_state.draw_radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
|
||||
if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or transform_updated or
|
||||
not self.radar_tracks.projection_initialized):
|
||||
self.radar_tracks.update_radar_tracks(
|
||||
sm['liveTracks'], self._map_to_screen, self._path_offset_z,
|
||||
)
|
||||
highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {}
|
||||
matched_positions = self.radar_tracks.draw_cached_radar_tracks(
|
||||
highlighted_tracks=highlighted_tracks,
|
||||
)
|
||||
if render_lead_indicator:
|
||||
draw_radar_lead_connectors(self._lead_vehicles, matched_positions, highlighted_tracks)
|
||||
else:
|
||||
self.radar_tracks.clear_projection()
|
||||
|
||||
if render_lead_indicator and radar_state:
|
||||
self._draw_lead_indicator()
|
||||
self.chevron_metrics.draw_lead_status(sm, radar_state, self._rect, self._lead_vehicles)
|
||||
@@ -188,11 +168,7 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
|
||||
point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
|
||||
if point:
|
||||
lead_vehicle = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
|
||||
lead_vehicle.position = lead_vehicle.chevron[1]
|
||||
lead_vehicle.radar_track_id = int(lead_data.radarTrackId)
|
||||
lead_vehicle.radar = lead_data.radar
|
||||
self._lead_vehicles[i] = lead_vehicle
|
||||
self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
|
||||
|
||||
def _update_model(self, lead, path_x_array):
|
||||
"""Update model visualization data based on model message"""
|
||||
|
||||
@@ -43,7 +43,7 @@ class ModelsLayout(Widget):
|
||||
self._initialize_items()
|
||||
|
||||
self.clear_cache_item.action_item.set_value(f"{self.calculate_cache_size():.2f} MB")
|
||||
for ctrl, key in [(self.lane_turn_value_control, "LaneTurnValue"), (self.delay_control, "LagdToggleDelay")]:
|
||||
for ctrl, key in [(self.lane_turn_value_control, "LaneTurnValue"), (self.delay_control, "LagdToggleDelay"), (self.camera_offset, "CameraOffset")]:
|
||||
ctrl.action_item.set_value(int(float(ui_state.params.get(key, return_default=True)) * 100))
|
||||
|
||||
self._scroller = Scroller(self.items, line_separator=True, spacing=0)
|
||||
@@ -93,9 +93,14 @@ class ModelsLayout(Widget):
|
||||
|
||||
self.lagd_toggle = toggle_item_sp(tr("Live Learning Steer Delay"), "", param="LagdToggle")
|
||||
|
||||
self.camera_offset = option_item_sp(tr("Adjust Camera Offset"), "CameraOffset", -35, 35,
|
||||
tr("Virtually shift camera's perspective to move model's center to Left(+ values) or Right (- values)"),
|
||||
1, None, True, "", style.BUTTON_ACTION_WIDTH, None, True,
|
||||
lambda v: f"{v / 100:.2f} m")
|
||||
|
||||
self.items = [self.current_model_item, self.cancel_download_item, self.supercombo_label, self.vision_label,
|
||||
self.policy_label, self.off_policy_label, self.on_policy_label, self.refresh_item, self.clear_cache_item, self.lane_turn_desire_toggle,
|
||||
self.lane_turn_value_control, self.lagd_toggle, self.delay_control]
|
||||
self.policy_label, self.off_policy_label, self.on_policy_label, self.refresh_item, self.clear_cache_item,
|
||||
self.lane_turn_desire_toggle, self.lane_turn_value_control, self.lagd_toggle, self.delay_control, self.camera_offset]
|
||||
|
||||
def _update_lagd_description(self, lagd_toggle: bool):
|
||||
desc = tr("Enable this for the car to learn and adapt its steering response time. Disable to use a fixed steering response time. " +
|
||||
@@ -232,6 +237,7 @@ class ModelsLayout(Widget):
|
||||
advanced_controls: bool = ui_state.params.get_bool("ShowAdvancedControls")
|
||||
turn_desire: bool = ui_state.params.get_bool("LaneTurnDesire")
|
||||
live_delay: bool = ui_state.params.get_bool("LagdToggle")
|
||||
camera_offset: bool = ui_state.params.get("ModelManager_ActiveBundle") is not None
|
||||
|
||||
self.lane_turn_desire_toggle.action_item.set_state(turn_desire)
|
||||
self.lane_turn_value_control.set_visible(turn_desire and advanced_controls)
|
||||
@@ -240,6 +246,7 @@ class ModelsLayout(Widget):
|
||||
new_step = int(round(100 / CV.MPH_TO_KPH)) if ui_state.is_metric else 100
|
||||
if self.lane_turn_value_control.action_item is not None and self.lane_turn_value_control.action_item.value_change_step != new_step:
|
||||
self.lane_turn_value_control.action_item.value_change_step = new_step
|
||||
self.camera_offset.set_visible(camera_offset)
|
||||
|
||||
self._update_lagd_description(live_delay)
|
||||
self.model_manager = ui_state.sm["modelManagerSP"]
|
||||
|
||||
+5
-2
@@ -7,7 +7,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
from collections.abc import Callable
|
||||
import pyray as rl
|
||||
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import MadsScreenButtonType, TeslaFlagsSP
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake
|
||||
from openpilot.system.ui.lib.multilang import tr, tr_noop
|
||||
@@ -96,7 +96,10 @@ class MadsSettingsLayout(Widget):
|
||||
if brand == "rivian":
|
||||
return True
|
||||
elif brand == "tesla":
|
||||
return not (ui_state.CP_SP is not None and ui_state.CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS)
|
||||
if ui_state.CP_SP is None or not ui_state.CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS:
|
||||
return True
|
||||
screen_button = int(ui_state.params.get("TeslaMadsScreenButton", return_default=True))
|
||||
return screen_button == MadsScreenButtonType.OFF
|
||||
return False
|
||||
|
||||
def _update_steering_mode_description(self, button_index: int):
|
||||
|
||||
@@ -4,10 +4,11 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.vehicle.brands.base import BrandSettings
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
|
||||
from openpilot.system.ui.sunnypilot.widgets.list_view import multiple_button_item_sp, toggle_item_sp
|
||||
|
||||
COOP_STEERING_MIN_KMH = 23
|
||||
OEM_STEERING_MIN_KMH = 48
|
||||
@@ -18,7 +19,14 @@ class TeslaSettings(BrandSettings):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.coop_steering_toggle = toggle_item_sp(tr("Cooperative Steering (Beta)"), "", param="TeslaCoopSteering")
|
||||
self.items = [self.coop_steering_toggle]
|
||||
self.mads_screen_button = multiple_button_item_sp(
|
||||
title=lambda: tr("MADS Screen Activation"),
|
||||
description="",
|
||||
buttons=[lambda: tr("Off"), lambda: tr("3-Finger"), lambda: tr("4-Finger"), lambda: tr("5-Finger")],
|
||||
param="TeslaMadsScreenButton",
|
||||
inline=False,
|
||||
)
|
||||
self.items = [self.coop_steering_toggle, self.mads_screen_button]
|
||||
|
||||
def update_settings(self):
|
||||
is_metric = ui_state.is_metric
|
||||
@@ -41,3 +49,18 @@ class TeslaSettings(BrandSettings):
|
||||
|
||||
self.coop_steering_toggle.set_description(coop_steering_desc)
|
||||
self.coop_steering_toggle.action_item.set_enabled(ui_state.is_offroad())
|
||||
|
||||
has_vehicle_bus = ui_state.CP_SP is not None and bool(ui_state.CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS)
|
||||
self.mads_screen_button.set_visible(has_vehicle_bus)
|
||||
|
||||
mads_screen_button_desc = (
|
||||
f"{tr('Use a multi-finger press on the infotainment screen to toggle MADS.')} " +
|
||||
f"{tr('This allows the use of full MADS functionality when enabled.')}<br><br>" +
|
||||
f"{tr('Selecting a higher finger count may reduce accidental activations.')}<br><br>" +
|
||||
f"<b>{tr('Note: Setting this to Off will reset your MADS settings to default.')}</b>"
|
||||
)
|
||||
if not ui_state.is_offroad():
|
||||
mads_screen_button_disabled_msg = tr("Enable \"Always Offroad\" in Device panel, or turn vehicle off to change.")
|
||||
mads_screen_button_desc = f"<b>{mads_screen_button_disabled_msg}</b><br><br>{mads_screen_button_desc}"
|
||||
self.mads_screen_button.set_description(mads_screen_button_desc)
|
||||
self.mads_screen_button.action_item.set_enabled(ui_state.is_offroad())
|
||||
|
||||
@@ -7,7 +7,6 @@ See the LICENSE.md file in the root directory for more details.
|
||||
import pyray as rl
|
||||
from openpilot.selfdrive.ui.ui_state import UIStatus
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracks
|
||||
|
||||
LANE_LINE_COLORS_SP = {
|
||||
UIStatus.LAT_ONLY: rl.Color(0, 255, 64, 255),
|
||||
@@ -18,4 +17,3 @@ LANE_LINE_COLORS_SP = {
|
||||
class ModelRendererSP:
|
||||
def __init__(self):
|
||||
self.rainbow_path = RainbowPath()
|
||||
self.radar_tracks = RadarTracks()
|
||||
|
||||
@@ -6,12 +6,9 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracks
|
||||
|
||||
|
||||
class ModelRendererSP:
|
||||
def __init__(self):
|
||||
self.rainbow_path = RainbowPath()
|
||||
self.chevron_metrics = ChevronMetrics()
|
||||
self.radar_tracks = RadarTracks()
|
||||
|
||||
|
||||
@@ -1,344 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
import pyray as rl
|
||||
from opendbc.car.hyundai.radar_interface import RADAR_235_248, RADAR_3A5_3C4
|
||||
from openpilot.system.ui.lib.application import FontWeight
|
||||
from openpilot.system.ui.widgets.label import UnifiedLabel
|
||||
|
||||
NEUTRAL_COLOR = (255, 255, 255)
|
||||
DBC_MOVING_COLOR = (190, 125, 255)
|
||||
DBC_UNKNOWN_COLOR = (154, 168, 184)
|
||||
DBC_MOTION_STATIONARY = 1
|
||||
DBC_MOTION_MOVING = 2
|
||||
LEAD_TRACK_COLORS = (
|
||||
rl.Color(255, 215, 0, 255),
|
||||
rl.Color(255, 140, 0, 220),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ProjectedRadarTrack:
|
||||
x: float
|
||||
y: float
|
||||
radius: float
|
||||
color: object
|
||||
source_index: int
|
||||
camera_object: bool
|
||||
track_id: int
|
||||
|
||||
|
||||
def is_preferred_radar_source(source) -> bool:
|
||||
return source.startAddress == RADAR_3A5_3C4.start_addr and source.endAddress == RADAR_3A5_3C4.end_addr
|
||||
|
||||
|
||||
def radar_source_sort_key(source) -> tuple[bool, int, int, int]:
|
||||
return (
|
||||
not is_preferred_radar_source(source),
|
||||
int(source.startAddress),
|
||||
int(source.endAddress),
|
||||
int(source.bus),
|
||||
)
|
||||
|
||||
|
||||
def sorted_radar_sources(live_tracks):
|
||||
return sorted(live_tracks.trackSources, key=radar_source_sort_key)
|
||||
|
||||
|
||||
def radar_track_source_index(track, sources) -> int:
|
||||
address = int(track.sourceAddress)
|
||||
bus = int(track.sourceBus)
|
||||
if address != 0:
|
||||
return next((
|
||||
index for index, source in enumerate(sources)
|
||||
if source.startAddress <= address <= source.endAddress and source.bus == bus
|
||||
), 0)
|
||||
return 0
|
||||
|
||||
|
||||
def is_camera_object_source(source) -> bool:
|
||||
return source.startAddress == RADAR_235_248.start_addr and source.endAddress == RADAR_235_248.end_addr
|
||||
|
||||
|
||||
def radar_track_source(track, sources):
|
||||
address = int(track.sourceAddress)
|
||||
bus = int(track.sourceBus)
|
||||
if address == 0:
|
||||
return None
|
||||
return next((
|
||||
source for source in sources
|
||||
if source.startAddress <= address <= source.endAddress and source.bus == bus
|
||||
), None)
|
||||
|
||||
|
||||
def radar_source_label(source) -> str:
|
||||
prefix = "CAM " if is_camera_object_source(source) else ""
|
||||
return f"{prefix}{source.startAddress:X}-{source.endAddress:X}"
|
||||
|
||||
|
||||
def draw_radar_source_marker(center: rl.Vector2, radius: float, color: rl.Color, source_index: int,
|
||||
camera_object: bool = False) -> None:
|
||||
if camera_object:
|
||||
rl.draw_poly(center, 3, radius, -90.0, color)
|
||||
return
|
||||
if source_index <= 0:
|
||||
rl.draw_circle(int(center.x), int(center.y), radius, color)
|
||||
return
|
||||
sides = (4, 3, 5, 6)[(source_index - 1) % 4]
|
||||
rotation = 45.0 if sides == 4 else -90.0
|
||||
rl.draw_poly(center, sides, radius, rotation, color)
|
||||
|
||||
|
||||
def radar_track_display(motion_state: int) -> tuple[rl.Color, bool]:
|
||||
"""Color tracks exclusively from the radar's DBC motion classification."""
|
||||
if motion_state == DBC_MOTION_STATIONARY:
|
||||
return rl.Color(*NEUTRAL_COLOR, 255), True
|
||||
if motion_state == DBC_MOTION_MOVING:
|
||||
return rl.Color(*DBC_MOVING_COLOR, 255), False
|
||||
return rl.Color(*DBC_UNKNOWN_COLOR, 255), False
|
||||
|
||||
|
||||
def radar_lead_track_colors(radar_state) -> dict[int, rl.Color]:
|
||||
highlighted_tracks = {}
|
||||
if radar_state is None:
|
||||
return highlighted_tracks
|
||||
|
||||
for lead, color in zip((radar_state.leadOne, radar_state.leadTwo), LEAD_TRACK_COLORS, strict=True):
|
||||
if lead.present and lead.radar and lead.radarTrackId >= 0:
|
||||
highlighted_tracks.setdefault(int(lead.radarTrackId), color)
|
||||
return highlighted_tracks
|
||||
|
||||
|
||||
def draw_radar_lead_connectors(lead_vehicles, matched_positions, highlighted_tracks, screen_offset=(0, 0)) -> None:
|
||||
for lead in lead_vehicles:
|
||||
if not lead.radar or lead.position is None or lead.radar_track_id not in matched_positions:
|
||||
continue
|
||||
|
||||
radar_position = matched_positions[lead.radar_track_id]
|
||||
lead_position = (lead.position[0] + screen_offset[0], lead.position[1] + screen_offset[1])
|
||||
if math.dist(lead_position, radar_position) < 4:
|
||||
continue
|
||||
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(*lead_position), rl.Vector2(*radar_position), 2,
|
||||
highlighted_tracks[lead.radar_track_id],
|
||||
)
|
||||
|
||||
|
||||
def format_radar_tracks_onroad_columns(live_tracks, v_ego: float = 0.0) -> tuple[str, str, str, str, str, str]:
|
||||
sources = sorted_radar_sources(live_tracks)
|
||||
if not sources:
|
||||
return "", "none", "", "", "", ""
|
||||
|
||||
range_text = "\n".join(radar_source_label(source) for source in sources)
|
||||
count_text = "\n".join(str(source.trackCount) for source in sources)
|
||||
motion_states = [int(track.motionState) for track in live_tracks.points]
|
||||
|
||||
moving_count = sum(state == DBC_MOTION_MOVING for state in motion_states)
|
||||
stationary_count = sum(state == DBC_MOTION_STATIONARY for state in motion_states)
|
||||
unknown_count = len(motion_states) - moving_count - stationary_count
|
||||
return range_text, count_text, str(moving_count), str(stationary_count), str(unknown_count), ""
|
||||
|
||||
|
||||
class RadarTracksStatus:
|
||||
HORIZONTAL_PADDING = 8
|
||||
COLUMN_GAP = 8
|
||||
SOURCE_MARKER_WIDTH = 18
|
||||
|
||||
def __init__(self, settings_callback=None, right_margin: int = 12):
|
||||
self._settings_callback = settings_callback
|
||||
self._right_margin = right_margin
|
||||
self._rect = rl.Rectangle()
|
||||
|
||||
def status_label(text: str, color: rl.Color) -> UnifiedLabel:
|
||||
return UnifiedLabel(
|
||||
text,
|
||||
font_size=26,
|
||||
font_weight=FontWeight.SEMI_BOLD,
|
||||
text_color=color,
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
|
||||
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
|
||||
wrap_text=False,
|
||||
)
|
||||
|
||||
self._labels = (
|
||||
status_label("", rl.Color(0, 255, 64, 255)),
|
||||
status_label("none", rl.Color(0, 255, 64, 255)),
|
||||
status_label("", rl.Color(*DBC_MOVING_COLOR, 255)),
|
||||
status_label("", rl.Color(*NEUTRAL_COLOR, 255)),
|
||||
status_label("", rl.Color(*DBC_UNKNOWN_COLOR, 255)),
|
||||
status_label("", rl.Color(*DBC_UNKNOWN_COLOR, 255)),
|
||||
)
|
||||
self._status = ("", "none", "", "", "", "")
|
||||
self._status_colors: tuple[tuple[int, int, int], ...] = ()
|
||||
self._source_count = 0
|
||||
self._layout_key: tuple[str, str, str, str, str, str, int] | None = None
|
||||
self._column_widths = [0, 36, 0, 0, 0, 0]
|
||||
self._width = 52
|
||||
self._height = 42
|
||||
|
||||
def update(self, live_tracks, valid: bool, radar_mode: int, v_ego: float = 0.0) -> None:
|
||||
if live_tracks.radarTracksAvailable and radar_mode != 2:
|
||||
status = ("", "radar detected\ntap to enable", "", "", "", "")
|
||||
status_colors = ()
|
||||
source_count = 0
|
||||
else:
|
||||
status = format_radar_tracks_onroad_columns(live_tracks, v_ego) if valid else ("", "none", "", "", "", "")
|
||||
status_colors = (DBC_MOVING_COLOR, NEUTRAL_COLOR, DBC_UNKNOWN_COLOR, DBC_UNKNOWN_COLOR)
|
||||
source_count = len(live_tracks.trackSources) if valid else 0
|
||||
self._set_status(status, status_colors, source_count)
|
||||
|
||||
def reset(self) -> None:
|
||||
self._set_status(("", "none", "", "", "", ""), (), 0)
|
||||
|
||||
def handle_mouse(self, mouse_pos) -> bool:
|
||||
if self._settings_callback is None or not rl.check_collision_point_rec(mouse_pos, self._rect):
|
||||
return False
|
||||
|
||||
self._settings_callback()
|
||||
return True
|
||||
|
||||
def render(self, content_rect: rl.Rectangle) -> None:
|
||||
self._update_layout(int(content_rect.width - 40))
|
||||
self._rect = rl.Rectangle(
|
||||
content_rect.x + content_rect.width - self._width - self._right_margin,
|
||||
content_rect.y + 8,
|
||||
self._width,
|
||||
self._height,
|
||||
)
|
||||
rl.draw_rectangle_rounded(self._rect, 0.5, 8, rl.Color(0, 0, 0, 170))
|
||||
x = self._rect.x + self.HORIZONTAL_PADDING
|
||||
active_columns = [
|
||||
(column_index, label, width)
|
||||
for column_index, (label, width) in enumerate(zip(self._labels, self._column_widths, strict=True))
|
||||
if width
|
||||
]
|
||||
for active_index, (column_index, label, width) in enumerate(active_columns):
|
||||
label_x = x
|
||||
label_width = width
|
||||
if column_index == 0 and self._source_count:
|
||||
marker_height = (self._rect.height - 10) / self._source_count
|
||||
for source_index in range(self._source_count):
|
||||
draw_radar_source_marker(
|
||||
rl.Vector2(x + 6, self._rect.y + 5 + marker_height * (source_index + 0.5)),
|
||||
5.0, rl.Color(0, 255, 64, 255), source_index,
|
||||
)
|
||||
label_x += self.SOURCE_MARKER_WIDTH
|
||||
label_width -= self.SOURCE_MARKER_WIDTH
|
||||
label.render(rl.Rectangle(label_x, self._rect.y + 5, label_width, self._rect.height - 10))
|
||||
x += width + (self.COLUMN_GAP if active_index < len(active_columns) - 1 else 0)
|
||||
|
||||
def _set_status(self, status: tuple[str, str, str, str, str, str],
|
||||
status_colors: tuple[tuple[int, int, int], ...], source_count: int) -> None:
|
||||
if status == self._status and status_colors == self._status_colors and source_count == self._source_count:
|
||||
return
|
||||
|
||||
self._status = status
|
||||
self._status_colors = status_colors
|
||||
self._source_count = source_count
|
||||
for label, text in zip(self._labels, status, strict=True):
|
||||
label.set_text(text)
|
||||
for label, color in zip(self._labels[2:], status_colors, strict=False):
|
||||
label.set_text_color(rl.Color(*color, 255))
|
||||
self._layout_key = None
|
||||
|
||||
def _update_layout(self, max_inner_width: int) -> None:
|
||||
layout_key = (*self._status, max_inner_width)
|
||||
if layout_key == self._layout_key:
|
||||
return
|
||||
|
||||
for label in self._labels:
|
||||
label.get_content_height(max_inner_width)
|
||||
self._column_widths = [
|
||||
math.ceil(label.text_width) if text else 0
|
||||
for label, text in zip(self._labels, self._status, strict=True)
|
||||
]
|
||||
if self._column_widths[0] and self._source_count:
|
||||
self._column_widths[0] += self.SOURCE_MARKER_WIDTH
|
||||
self._column_widths[1] = max(36, self._column_widths[1])
|
||||
for index in (2, 3, 4):
|
||||
if self._column_widths[index]:
|
||||
self._column_widths[index] = max(36, self._column_widths[index])
|
||||
active_widths = [width for width in self._column_widths if width]
|
||||
inner_width = sum(active_widths) + self.COLUMN_GAP * (len(active_widths) - 1)
|
||||
self._width = inner_width + self.HORIZONTAL_PADDING * 2
|
||||
self._height = max(
|
||||
42,
|
||||
*(label.get_content_height(max(width, 1)) + 10
|
||||
for label, width in zip(self._labels, self._column_widths, strict=True) if width),
|
||||
)
|
||||
self._layout_key = layout_key
|
||||
|
||||
|
||||
class RadarTracks:
|
||||
def __init__(self):
|
||||
self._projected_tracks: tuple[ProjectedRadarTrack, ...] = ()
|
||||
self._projection_initialized = False
|
||||
|
||||
@property
|
||||
def projection_initialized(self) -> bool:
|
||||
return self._projection_initialized
|
||||
|
||||
def clear_projection(self) -> None:
|
||||
self._projected_tracks = ()
|
||||
self._projection_initialized = False
|
||||
|
||||
def update_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7) -> None:
|
||||
projected_tracks = []
|
||||
sources = sorted_radar_sources(live_tracks)
|
||||
|
||||
for track in live_tracks.points:
|
||||
d_rel, y_rel, v_rel = track.dRel, track.yRel, track.vRel
|
||||
if not (math.isfinite(d_rel) and math.isfinite(y_rel) and math.isfinite(v_rel)):
|
||||
continue
|
||||
|
||||
motion_state = int(track.motionState)
|
||||
if motion_state not in (DBC_MOTION_STATIONARY, DBC_MOTION_MOVING):
|
||||
continue
|
||||
|
||||
pt = map_to_screen(d_rel, -y_rel, path_offset_z)
|
||||
if pt is None:
|
||||
continue
|
||||
|
||||
color, stationary = radar_track_display(motion_state)
|
||||
radius = max(1, track_size - 5) if stationary else track_size
|
||||
source = radar_track_source(track, sources)
|
||||
projected_tracks.append(ProjectedRadarTrack(
|
||||
x=pt[0],
|
||||
y=pt[1],
|
||||
radius=radius,
|
||||
color=color,
|
||||
source_index=radar_track_source_index(track, sources),
|
||||
camera_object=source is not None and is_camera_object_source(source),
|
||||
track_id=int(track.trackId),
|
||||
))
|
||||
|
||||
self._projected_tracks = tuple(projected_tracks)
|
||||
self._projection_initialized = True
|
||||
|
||||
def draw_cached_radar_tracks(self, screen_offset=(0, 0), highlighted_tracks=None):
|
||||
highlighted_tracks = highlighted_tracks or {}
|
||||
highlighted_positions = {}
|
||||
|
||||
for track in self._projected_tracks:
|
||||
x, y = track.x + screen_offset[0], track.y + screen_offset[1]
|
||||
highlight_color = highlighted_tracks.get(track.track_id)
|
||||
if highlight_color is not None:
|
||||
center = rl.Vector2(int(x), int(y))
|
||||
rl.draw_ring(center, track.radius + 2, track.radius + 5, 0, 360, 24, highlight_color)
|
||||
highlighted_positions[track.track_id] = (x, y)
|
||||
draw_radar_source_marker(
|
||||
rl.Vector2(x, y), track.radius, track.color, track.source_index, track.camera_object,
|
||||
)
|
||||
|
||||
return highlighted_positions
|
||||
|
||||
def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7, screen_offset=(0, 0), v_ego=0.0,
|
||||
highlighted_tracks=None):
|
||||
self.update_radar_tracks(live_tracks, map_to_screen, path_offset_z, track_size)
|
||||
return self.draw_cached_radar_tracks(screen_offset, highlighted_tracks)
|
||||
@@ -1,324 +0,0 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
from opendbc.car.structs import car
|
||||
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad import radar_tracks
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, format_radar_tracks_onroad_columns, \
|
||||
radar_lead_track_colors, radar_track_display
|
||||
|
||||
|
||||
def color_tuple(color):
|
||||
return color.r, color.g, color.b, color.a
|
||||
|
||||
|
||||
def test_dbc_motion_colors():
|
||||
assert color_tuple(radar_track_display(2)[0]) == (190, 125, 255, 255)
|
||||
assert not radar_track_display(2)[1]
|
||||
assert color_tuple(radar_track_display(1)[0]) == (255, 255, 255, 255)
|
||||
assert radar_track_display(1)[1]
|
||||
|
||||
|
||||
def test_unknown_dbc_motion_uses_neutral_dbc_color():
|
||||
color, stationary = radar_track_display(0)
|
||||
|
||||
assert color_tuple(color) == (*radar_tracks.DBC_UNKNOWN_COLOR, 255)
|
||||
assert not stationary
|
||||
|
||||
|
||||
def test_radar_lead_track_colors_only_highlight_radar_matches():
|
||||
radar_state = SimpleNamespace(
|
||||
leadOne=SimpleNamespace(present=True, radar=True, radarTrackId=7),
|
||||
leadTwo=SimpleNamespace(present=True, radar=False, radarTrackId=9),
|
||||
)
|
||||
|
||||
colors = radar_lead_track_colors(radar_state)
|
||||
|
||||
assert list(colors) == [7]
|
||||
assert color_tuple(colors[7]) == color_tuple(radar_tracks.LEAD_TRACK_COLORS[0])
|
||||
|
||||
|
||||
def test_draw_radar_lead_connectors_applies_screen_offset(monkeypatch):
|
||||
lead = SimpleNamespace(radar=True, position=(10, 20), radar_track_id=7)
|
||||
color = radar_tracks.LEAD_TRACK_COLORS[0]
|
||||
drawn = []
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl, "draw_line_ex",
|
||||
lambda start, end, width, line_color: drawn.append(
|
||||
((start.x, start.y), (end.x, end.y), width, color_tuple(line_color))
|
||||
),
|
||||
)
|
||||
|
||||
draw_radar_lead_connectors(
|
||||
[lead], {7: (120, 30)}, {7: color}, screen_offset=(100, 5),
|
||||
)
|
||||
|
||||
assert drawn == [((110, 25), (120, 30), 2, color_tuple(color))]
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_none():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("", "none", "", "", "", "")
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_range_and_count():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1, "trackCount": 2}]
|
||||
points = live_tracks.init("points", 2)
|
||||
points[0].motionState = 2
|
||||
points[1].motionState = 1
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("3A5-3C4", "2", "1", "1", "0", "")
|
||||
|
||||
|
||||
def test_format_camera_objects_are_not_labeled_as_radar():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x235, "endAddress": 0x248, "bus": 1, "trackCount": 3}]
|
||||
points = live_tracks.init("points", 3)
|
||||
for point in points:
|
||||
point.motionState = 2
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("CAM 235-248", "3", "3", "0", "0", "")
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_stacks_all_ranges_with_preferred_first():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [
|
||||
{"startAddress": 0x500, "endAddress": 0x51F, "bus": 2, "trackCount": 3},
|
||||
{"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1, "trackCount": 2},
|
||||
]
|
||||
points = live_tracks.init("points", 3)
|
||||
points[0].motionState = 2
|
||||
points[0].sourceAddress = 0x3A5
|
||||
points[1].motionState = 1
|
||||
points[1].sourceAddress = 0x3A6
|
||||
points[2].motionState = 0
|
||||
points[2].sourceAddress = 0x500
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == (
|
||||
"3A5-3C4\n500-51F",
|
||||
"2\n3",
|
||||
"1",
|
||||
"1",
|
||||
"1",
|
||||
"",
|
||||
)
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_shows_non_motion_source():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 4}]
|
||||
points = live_tracks.init("points", 4)
|
||||
for point, v_rel in zip(points, (-5.0, 0.2, -20.0, 5.0), strict=True):
|
||||
point.vRel = v_rel
|
||||
point.motionState = 0
|
||||
point.sourceAddress = 0x500
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks, v_ego=20.0) == ("500-51F", "4", "0", "0", "4", "")
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_shows_64_track_source():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x500, "endAddress": 0x53F, "bus": 1, "trackCount": 7}]
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("500-53F", "7", "0", "0", "0", "")
|
||||
|
||||
|
||||
def test_draw_radar_tracks_applies_screen_offset(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
points = live_tracks.init("points", 1)
|
||||
points[0].dRel = 10
|
||||
points[0].yRel = 1
|
||||
points[0].vRel = 2
|
||||
points[0].motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
drawn_circles = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_circles.append((x, y, size)))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks,
|
||||
lambda d_rel, y_rel, z: (20, 30),
|
||||
path_offset_z=1.2,
|
||||
track_size=3,
|
||||
screen_offset=(100, 7),
|
||||
)
|
||||
|
||||
assert drawn_circles == [(120, 37, 3)]
|
||||
|
||||
|
||||
def test_draw_radar_tracks_hides_unknown_motion(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = -5
|
||||
point.motionState = 0
|
||||
drawn_colors = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_colors.append(color_tuple(color)))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2)
|
||||
|
||||
assert drawn_colors == []
|
||||
|
||||
|
||||
def test_draw_radar_tracks_hides_unknown_motion_from_other_source(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = -5
|
||||
point.motionState = 0
|
||||
point.sourceAddress = 0x500
|
||||
drawn_circles = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda *args: drawn_circles.append(args))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2,
|
||||
)
|
||||
|
||||
assert drawn_circles == []
|
||||
|
||||
|
||||
def test_draw_radar_tracks_uses_source_shapes_with_preferred_circle(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [
|
||||
{"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 1},
|
||||
{"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1, "trackCount": 1},
|
||||
]
|
||||
points = live_tracks.init("points", 2)
|
||||
for point, address in zip(points, (0x500, 0x3A5), strict=True):
|
||||
point.dRel = address
|
||||
point.yRel = 1
|
||||
point.vRel = 2
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
point.sourceAddress = address
|
||||
point.sourceBus = 1
|
||||
|
||||
circles = []
|
||||
polygons = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, radius, color: circles.append((x, radius)))
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl, "draw_poly",
|
||||
lambda center, sides, radius, rotation, color: polygons.append((center.x, sides, radius, rotation)),
|
||||
)
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (d_rel, 30), path_offset_z=1.2, track_size=6,
|
||||
)
|
||||
|
||||
assert circles == [(0x3A5, 6)]
|
||||
assert polygons == [(0x500, 4, 6, 45.0)]
|
||||
|
||||
|
||||
def test_draw_camera_objects_uses_triangle(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x235, "endAddress": 0x248, "bus": 1, "trackCount": 1}]
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 25
|
||||
point.yRel = 1
|
||||
point.vRel = 2
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
point.sourceAddress = 0x235
|
||||
point.sourceBus = 1
|
||||
polygons = []
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl, "draw_poly",
|
||||
lambda center, sides, radius, rotation, color: polygons.append((center.x, sides, radius, rotation)),
|
||||
)
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (d_rel, 30), path_offset_z=1.2, track_size=6,
|
||||
)
|
||||
|
||||
assert polygons == [(25, 3, 6, -90.0)]
|
||||
|
||||
|
||||
def test_draw_radar_tracks_shrinks_stationary_dots(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = -20
|
||||
point.motionState = 1
|
||||
drawn_sizes = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_sizes.append(size))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2, track_size=6, v_ego=20,
|
||||
)
|
||||
|
||||
assert drawn_sizes == [1]
|
||||
|
||||
|
||||
def test_draw_radar_tracks_keeps_matched_speed_dots_large(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = 0.5
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
drawn_sizes = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_sizes.append(size))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2, track_size=6, v_ego=20,
|
||||
)
|
||||
|
||||
assert drawn_sizes == [6]
|
||||
|
||||
|
||||
def test_draw_radar_tracks_highlights_and_returns_matched_track(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
points = live_tracks.init("points", 2)
|
||||
for track_id, point in enumerate(points, start=10):
|
||||
point.trackId = track_id
|
||||
point.dRel = track_id
|
||||
point.yRel = 1
|
||||
point.vRel = 2
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
|
||||
highlight_color = radar_tracks.rl.Color(255, 215, 0, 255)
|
||||
drawn_rings = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda *args: None)
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl,
|
||||
"draw_ring",
|
||||
lambda center, inner, outer, start, end, segments, color: drawn_rings.append(
|
||||
((center.x, center.y), inner, outer, color_tuple(color))
|
||||
),
|
||||
)
|
||||
|
||||
matched_positions = radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (d_rel, 30), path_offset_z=1.2,
|
||||
screen_offset=(100, 7), highlighted_tracks={11: highlight_color},
|
||||
)
|
||||
|
||||
assert drawn_rings == [((111, 37), 9, 12, (255, 215, 0, 255))]
|
||||
assert matched_positions == {11: (111, 37)}
|
||||
|
||||
|
||||
def test_cached_radar_tracks_only_reproject_on_update(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.trackId = 7
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = 0
|
||||
point.motionState = radar_tracks.DBC_MOTION_MOVING
|
||||
projected = []
|
||||
drawn = []
|
||||
|
||||
def map_to_screen(d_rel, y_rel, z):
|
||||
projected.append((d_rel, y_rel, z))
|
||||
return (20, 30)
|
||||
|
||||
monkeypatch.setattr(
|
||||
radar_tracks.rl, "draw_circle",
|
||||
lambda x, y, radius, color: drawn.append((x, y)),
|
||||
)
|
||||
renderer = radar_tracks.RadarTracks()
|
||||
renderer.update_radar_tracks(live_tracks, map_to_screen, path_offset_z=1.2)
|
||||
renderer.draw_cached_radar_tracks(screen_offset=(100, 7))
|
||||
renderer.draw_cached_radar_tracks(screen_offset=(200, 9))
|
||||
|
||||
assert projected == [(10, -1, 1.2)]
|
||||
assert drawn == [(120, 37), (220, 39)]
|
||||
@@ -43,9 +43,7 @@ class UIStateSP:
|
||||
self.chevron_metrics = None
|
||||
self.custom_interactive_timeout: int = 0
|
||||
self.developer_ui = None
|
||||
self.draw_radar_tracks: bool = self.params.get_bool("DrawRadarTracks")
|
||||
self.hide_v_ego_ui: bool = False
|
||||
self.radar_tracks: int = self.params.get("RadarTracks", return_default=True)
|
||||
self.onroad_brightness: int = 0
|
||||
self.onroad_brightness_timer: int = 0
|
||||
self.onroad_brightness_timer_param: int = 0
|
||||
@@ -153,9 +151,7 @@ class UIStateSP:
|
||||
self.chevron_metrics = self.params.get("ChevronInfo")
|
||||
self.custom_interactive_timeout = self.params.get("InteractivityTimeout", return_default=True)
|
||||
self.developer_ui = self.params.get("DevUIInfo")
|
||||
self.draw_radar_tracks = self.params.get_bool("DrawRadarTracks")
|
||||
self.hide_v_ego_ui = self.params.get_bool("HideVEgoUI")
|
||||
self.radar_tracks = self.params.get("RadarTracks", return_default=True)
|
||||
self.onroad_brightness = int(float(self.params.get("OnroadScreenOffBrightness", return_default=True)))
|
||||
self.onroad_brightness_timer_param = self.params.get("OnroadScreenOffTimer", return_default=True)
|
||||
self.rainbow_path = self.params.get_bool("RainbowMode")
|
||||
|
||||
@@ -63,7 +63,6 @@ class UIState(UIStateSP):
|
||||
"liveParameters",
|
||||
"testJoystick",
|
||||
"rawAudioData",
|
||||
"liveTracks",
|
||||
] + self.sm_services_ext
|
||||
)
|
||||
|
||||
|
||||
@@ -9,7 +9,7 @@ from openpilot.common.params import Params
|
||||
from opendbc.car import structs
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import MadsScreenButtonType, TeslaFlagsSP
|
||||
|
||||
|
||||
MADS_NO_ACC_MAIN_BUTTON = ("rivian", "tesla")
|
||||
@@ -21,17 +21,20 @@ class MadsSteeringModeOnBrake:
|
||||
DISENGAGE = 2
|
||||
|
||||
|
||||
def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> bool:
|
||||
def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params) -> bool:
|
||||
if CP.brand == 'rivian':
|
||||
return True
|
||||
if CP.brand == 'tesla':
|
||||
return not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS
|
||||
if not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS:
|
||||
return True
|
||||
screen_button = int(params.get("TeslaMadsScreenButton", return_default=True))
|
||||
return screen_button == MadsScreenButtonType.OFF
|
||||
|
||||
return False
|
||||
|
||||
|
||||
def read_steering_mode_param(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params):
|
||||
if get_mads_limited_brands(CP, CP_SP):
|
||||
if get_mads_limited_brands(CP, CP_SP, params):
|
||||
return MadsSteeringModeOnBrake.DISENGAGE
|
||||
|
||||
return params.get("MadsSteeringMode", return_default=True)
|
||||
@@ -63,7 +66,7 @@ def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, p
|
||||
# MADS is currently partially supported for these platforms due to lack of consistent states to engage controls
|
||||
# Only MadsSteeringModeOnBrake.DISENGAGE is supported for these platforms
|
||||
# TODO-SP: To enable MADS full support for Rivian and most Tesla, identify consistent signals for MADS toggling
|
||||
mads_partial_support = get_mads_limited_brands(CP, CP_SP)
|
||||
mads_partial_support = get_mads_limited_brands(CP, CP_SP, params)
|
||||
if mads_partial_support:
|
||||
params.put("MadsSteeringMode", 2, block=True)
|
||||
params.put_bool("MadsUnifiedEngagementMode", True, block=True)
|
||||
|
||||
@@ -13,7 +13,7 @@ from openpilot.selfdrive.selfdrived.events import Events
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param
|
||||
from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import MadsScreenButtonType, TeslaFlagsSP
|
||||
|
||||
State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
|
||||
EventName = log.OnroadEvent.EventName
|
||||
@@ -38,6 +38,12 @@ def make_panda_state(mocker, controls_allowed_lateral=True):
|
||||
return ps
|
||||
|
||||
|
||||
def make_params_mock(mocker, values):
|
||||
params = mocker.MagicMock()
|
||||
params.get = mocker.MagicMock(side_effect=lambda k, **kwargs: values[k])
|
||||
return params
|
||||
|
||||
|
||||
def make_mads(mocker, steering_mode):
|
||||
sd = mocker.MagicMock()
|
||||
sd.CP = structs.CarParams()
|
||||
@@ -223,15 +229,27 @@ class TestBrandSteeringModeRestrictions:
|
||||
params = mocker.MagicMock()
|
||||
assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.DISENGAGE
|
||||
|
||||
def test_tesla_with_vehicle_bus_uses_param(self, mocker):
|
||||
@pytest.mark.parametrize("screen_button", [MadsScreenButtonType.THREE_FINGER,
|
||||
MadsScreenButtonType.FOUR_FINGER,
|
||||
MadsScreenButtonType.FIVE_FINGER])
|
||||
def test_tesla_with_vehicle_bus_uses_param(self, mocker, screen_button):
|
||||
CP = structs.CarParams()
|
||||
CP.brand = "tesla"
|
||||
CP_SP = structs.CarParamsSP()
|
||||
CP_SP.flags = TeslaFlagsSP.HAS_VEHICLE_BUS
|
||||
params = mocker.MagicMock()
|
||||
params.get = mocker.MagicMock(return_value=MadsSteeringModeOnBrake.REMAIN_ACTIVE)
|
||||
params = make_params_mock(mocker, {"TeslaMadsScreenButton": screen_button,
|
||||
"MadsSteeringMode": MadsSteeringModeOnBrake.REMAIN_ACTIVE})
|
||||
assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.REMAIN_ACTIVE
|
||||
|
||||
def test_tesla_with_vehicle_bus_screen_button_off_forced_to_disengage(self, mocker):
|
||||
CP = structs.CarParams()
|
||||
CP.brand = "tesla"
|
||||
CP_SP = structs.CarParamsSP()
|
||||
CP_SP.flags = TeslaFlagsSP.HAS_VEHICLE_BUS
|
||||
params = make_params_mock(mocker, {"TeslaMadsScreenButton": MadsScreenButtonType.OFF,
|
||||
"MadsSteeringMode": MadsSteeringModeOnBrake.REMAIN_ACTIVE})
|
||||
assert read_steering_mode_param(CP, CP_SP, params) == MadsSteeringModeOnBrake.DISENGAGE
|
||||
|
||||
@pytest.mark.parametrize("brand", ["hyundai", "toyota", "honda", "gm"])
|
||||
def test_other_brands_use_param(self, mocker, brand):
|
||||
CP = structs.CarParams()
|
||||
|
||||
@@ -10,471 +10,291 @@ import argparse
|
||||
import os
|
||||
import pickle
|
||||
import time
|
||||
from functools import partial
|
||||
from collections import defaultdict
|
||||
|
||||
from functools import partial
|
||||
import numpy as np
|
||||
from tinygrad.tensor import Tensor
|
||||
os.environ['GMMU'] = '0'
|
||||
|
||||
def _patch_tinygrad_fetch_fw():
|
||||
import hashlib
|
||||
import pathlib
|
||||
import zstandard
|
||||
from tinygrad import helpers
|
||||
_orig_fetch_fw = 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_fetch_fw(path, name, sha256)
|
||||
helpers.fetch_fw = fetch_fw
|
||||
_patch_tinygrad_fetch_fw()
|
||||
|
||||
from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare, sample_desire, sample_skip, shift_and_sample
|
||||
from tinygrad import dtypes
|
||||
from tinygrad.device import Device
|
||||
from tinygrad.engine.jit import TinyJit
|
||||
|
||||
from openpilot.selfdrive.modeld.compile_modeld import (
|
||||
NV12Frame, make_frame_prepare,
|
||||
shift_and_sample, sample_skip, sample_desire,
|
||||
)
|
||||
from tinygrad.tensor import Tensor
|
||||
|
||||
MODEL_TYPES = ('vision_policy', 'supercombo', 'vision_multi_policy')
|
||||
|
||||
|
||||
def _detect_desire_key(policy_input_shapes):
|
||||
for k in policy_input_shapes:
|
||||
if k.startswith('desire'):
|
||||
return k
|
||||
return None
|
||||
def _detect_desire_key(shapes: dict) -> str | None:
|
||||
return next((key for key in shapes if key.startswith('desire')), None)
|
||||
|
||||
|
||||
def _detect_vision_keys(vision_input_shapes):
|
||||
img_keys = sorted([k for k in vision_input_shapes if 'img' in k])
|
||||
road_key = next((k for k in img_keys if 'big' not in k), None)
|
||||
wide_key = next((k for k in img_keys if 'big' in k), None)
|
||||
if road_key is None or wide_key is None:
|
||||
raise ValueError(f"Cannot determine road/wide image keys from {list(vision_input_shapes.keys())}")
|
||||
return road_key, wide_key
|
||||
def _detect_vision_keys(shapes: dict) -> tuple[str | None, str | None]:
|
||||
img_keys = sorted(key for key in shapes if 'img' in key)
|
||||
return (
|
||||
next((key for key in img_keys if 'big' not in key), None),
|
||||
next((key for key in img_keys if 'big' in key), None)
|
||||
)
|
||||
|
||||
|
||||
def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
|
||||
road_key, _ = _detect_vision_keys(vision_input_shapes)
|
||||
img = vision_input_shapes[road_key]
|
||||
n_frames = img[1] // 6
|
||||
img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
|
||||
|
||||
fb = policy_input_shapes['features_buffer']
|
||||
desire_key = _detect_desire_key(policy_input_shapes)
|
||||
dp = policy_input_shapes[desire_key]
|
||||
tc = policy_input_shapes.get('traffic_convention', (1, 2))
|
||||
|
||||
npy = {
|
||||
'desire': np.zeros(dp[2], dtype=np.float32),
|
||||
'traffic_convention': np.zeros(tc, dtype=np.float32),
|
||||
'tfm': np.zeros((3, 3), dtype=np.float32),
|
||||
'big_tfm': np.zeros((3, 3), dtype=np.float32),
|
||||
}
|
||||
|
||||
handled = {'features_buffer', desire_key, 'traffic_convention'}
|
||||
for key, shape in policy_input_shapes.items():
|
||||
if key in handled:
|
||||
continue
|
||||
npy[key] = np.zeros(shape, 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(),
|
||||
'feat_q': Tensor(np.zeros((frame_skip * (fb[1] - 1) + 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(),
|
||||
**{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
|
||||
}
|
||||
return input_queues, npy
|
||||
def derive_frame_skip(vision_input_shapes: dict, policy_input_shapes: dict) -> int:
|
||||
features_buffer = policy_input_shapes.get('features_buffer')
|
||||
return 1 if not features_buffer or features_buffer[1] >= 99 else 4
|
||||
|
||||
|
||||
def make_run_split_policy(vision_runner, policy_runner, nv12: NV12Frame, model_w, model_h,
|
||||
vision_features_slice, frame_skip, desire_key, extra_policy_keys,
|
||||
vision_road_key, vision_wide_key, prepare_only=False):
|
||||
frame_prepare = make_frame_prepare(nv12, model_w, model_h)
|
||||
sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
|
||||
sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
|
||||
|
||||
def run_policy(img_q, big_img_q, feat_q, desire_q, desire, traffic_convention, tfm, big_tfm, frame, big_frame, **extra):
|
||||
npy_tensors = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT),
|
||||
desire.to(Device.DEFAULT), traffic_convention.to(Device.DEFAULT)]
|
||||
extra_device = {k: extra[k].to(Device.DEFAULT) for k in extra_policy_keys}
|
||||
Tensor.realize(*npy_tensors, *extra_device.values())
|
||||
tfm, big_tfm, desire, traffic_convention = npy_tensors
|
||||
|
||||
img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
|
||||
big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
|
||||
|
||||
if prepare_only:
|
||||
return img, big_img
|
||||
|
||||
vision_out = next(iter(vision_runner({vision_road_key: img, vision_wide_key: big_img}).values())).cast('float32')
|
||||
|
||||
new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
|
||||
feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
|
||||
desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
|
||||
|
||||
inputs = {'features_buffer': feat_buf, desire_key: desire_buf, 'traffic_convention': traffic_convention, **extra_device}
|
||||
policy_out = next(iter(policy_runner(inputs).values())).cast('float32')
|
||||
|
||||
return vision_out, policy_out
|
||||
return run_policy
|
||||
|
||||
|
||||
def compile_split_policy(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
|
||||
vision_runner, policy_runner, vision_metadata, policy_metadata):
|
||||
print(f"Compiling combined policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
|
||||
|
||||
vision_features_slice = vision_metadata['output_slices']['hidden_state']
|
||||
vision_input_shapes = vision_metadata['input_shapes']
|
||||
policy_input_shapes = policy_metadata['input_shapes']
|
||||
desire_key = _detect_desire_key(policy_input_shapes)
|
||||
extra_policy_keys = [k for k in policy_input_shapes if k not in ('features_buffer', desire_key, 'traffic_convention')]
|
||||
vision_road_key, vision_wide_key = _detect_vision_keys(vision_input_shapes)
|
||||
|
||||
_run = make_run_split_policy(vision_runner, policy_runner, nv12, model_w, model_h,
|
||||
vision_features_slice, frame_skip, desire_key, extra_policy_keys,
|
||||
vision_road_key, vision_wide_key, prepare_only)
|
||||
run_policy_jit = TinyJit(_run, prune=True)
|
||||
|
||||
SEED = 42
|
||||
|
||||
def random_inputs_run_fn(fn, seed, test_val=None, test_buffers=None, expect_match=True):
|
||||
input_queues, npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
|
||||
rng = np.random.default_rng(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):
|
||||
frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
|
||||
big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
|
||||
for v in npy.values():
|
||||
v[:] = rng.standard_normal(v.shape).astype(v.dtype)
|
||||
Device.default.synchronize()
|
||||
st = time.perf_counter()
|
||||
outs = fn(**input_queues, frame=frame, big_frame=big_frame)
|
||||
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 fn, val, buffers
|
||||
|
||||
print('capture + replay')
|
||||
run_policy_jit, test_val, test_buffers = random_inputs_run_fn(run_policy_jit, SEED)
|
||||
|
||||
print('pickle round trip')
|
||||
run_policy_jit = pickle.loads(pickle.dumps(run_policy_jit))
|
||||
random_inputs_run_fn(run_policy_jit, SEED, test_val, test_buffers, expect_match=True)
|
||||
random_inputs_run_fn(run_policy_jit, SEED+1, test_val, test_buffers, expect_match=False)
|
||||
return run_policy_jit
|
||||
|
||||
|
||||
def derive_frame_skip(vision_input_shapes, policy_input_shapes):
|
||||
fb = policy_input_shapes.get('features_buffer')
|
||||
if fb is None:
|
||||
return 1
|
||||
fb_history = fb[1]
|
||||
if fb_history >= 99:
|
||||
return 1
|
||||
return 4
|
||||
|
||||
|
||||
def make_supercombo_input_queues(input_shapes, frame_skip, device):
|
||||
img_shape = input_shapes.get('img', input_shapes.get('input_imgs'))
|
||||
if img_shape is None:
|
||||
raise ValueError("No img input found in model shapes")
|
||||
def generate_queues_and_npy(input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
|
||||
road_key, _ = _detect_vision_keys(input_shapes)
|
||||
if not road_key:
|
||||
raise ValueError("Vision road key missing from input shapes.")
|
||||
|
||||
img_shape = input_shapes[road_key]
|
||||
n_frames = img_shape[1] // 6
|
||||
img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
|
||||
|
||||
numpy_keys = {}
|
||||
queue_keys = {}
|
||||
desire_key = _detect_desire_key(input_shapes)
|
||||
if not desire_key:
|
||||
raise ValueError("Desire key missing from input shapes.")
|
||||
|
||||
desire_shape = input_shapes[desire_key]
|
||||
features_buffer = input_shapes.get('features_buffer')
|
||||
|
||||
npy_arrays = {
|
||||
'desire': np.zeros(desire_shape[2], dtype=np.float32),
|
||||
'tfm': np.zeros((3, 3), dtype=np.float32),
|
||||
'big_tfm': np.zeros((3, 3), dtype=np.float32)
|
||||
}
|
||||
|
||||
for key, shape in input_shapes.items():
|
||||
if 'img' in key:
|
||||
continue
|
||||
if len(shape) == 3 and shape[1] > 1:
|
||||
if key.startswith('desire'):
|
||||
numpy_keys[key] = np.zeros(shape[2], dtype=np.float32)
|
||||
queue_keys[f'{key}_q'] = Tensor(
|
||||
np.zeros((frame_skip * shape[1], shape[0], shape[2]), dtype=np.float32),
|
||||
device=device).contiguous().realize()
|
||||
elif key == 'features_buffer':
|
||||
queue_keys['feat_q'] = Tensor(
|
||||
np.zeros((frame_skip * (shape[1] - 1) + 1, shape[0], shape[2]), dtype=np.float32),
|
||||
device=device).contiguous().realize()
|
||||
else:
|
||||
numpy_keys[key] = np.zeros(shape, dtype=np.float32)
|
||||
elif len(shape) == 2:
|
||||
numpy_keys[key] = np.zeros(shape, dtype=np.float32)
|
||||
if key not in npy_arrays and 'img' not in key and key not in ('features_buffer', desire_key):
|
||||
npy_arrays[key] = np.zeros(shape, dtype=np.float32)
|
||||
|
||||
if 'traffic_convention' not in numpy_keys:
|
||||
tc_shape = input_shapes.get('traffic_convention', (1, 2))
|
||||
numpy_keys['traffic_convention'] = np.zeros(tc_shape, dtype=np.float32)
|
||||
|
||||
numpy_keys['tfm'] = np.zeros((3, 3), dtype=np.float32)
|
||||
numpy_keys['big_tfm'] = np.zeros((3, 3), dtype=np.float32)
|
||||
|
||||
input_queues = {
|
||||
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(),
|
||||
**queue_keys,
|
||||
**{k: Tensor(v, device='NPY').realize() for k, v in numpy_keys.items()},
|
||||
'desire_q': Tensor(np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]),
|
||||
dtype=np.float32), device=device).contiguous().realize()
|
||||
}
|
||||
return input_queues, numpy_keys
|
||||
|
||||
if features_buffer:
|
||||
queues['feat_q'] = Tensor(np.zeros((frame_skip * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]),
|
||||
dtype=np.float32), device=device).contiguous().realize()
|
||||
|
||||
queues.update({key: Tensor(value, device='NPY').realize() for key, value in npy_arrays.items()})
|
||||
return queues, npy_arrays
|
||||
|
||||
|
||||
def make_run_supercombo(model_runner, nv12: NV12Frame, model_w, model_h,
|
||||
features_slice, frame_skip, input_shapes, prepare_only=False):
|
||||
frame_prepare = make_frame_prepare(nv12, model_w, model_h)
|
||||
def make_split_input_queues(vision_input_shapes: dict, policy_input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
|
||||
return generate_queues_and_npy({**vision_input_shapes, **policy_input_shapes}, frame_skip, device)
|
||||
|
||||
|
||||
def make_supercombo_input_queues(input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
|
||||
return generate_queues_and_npy(input_shapes, frame_skip, device)
|
||||
|
||||
|
||||
def create_jit_runner(vision_runner, policy_runners: list, nv12: NV12Frame, model_size: tuple[int, int],
|
||||
features_slice: slice, frame_skip: int, input_shapes: dict, prepare_only: bool):
|
||||
frame_prepare = make_frame_prepare(nv12, *model_size)
|
||||
sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
|
||||
sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
|
||||
|
||||
desire_key = _detect_desire_key(input_shapes)
|
||||
if desire_key is None:
|
||||
raise ValueError(f"No desire* key found in input_shapes: {list(input_shapes.keys())}")
|
||||
road_img_key, wide_img_key = _detect_vision_keys(input_shapes)
|
||||
extra_policy_keys = [k for k in input_shapes
|
||||
if k not in (desire_key, 'features_buffer', 'traffic_convention')
|
||||
and 'img' not in k]
|
||||
road_key, wide_key = _detect_vision_keys(input_shapes)
|
||||
|
||||
def run_supercombo(img_q, big_img_q, feat_q, desire_q,
|
||||
frame, big_frame, **kwargs):
|
||||
desire = kwargs.get(desire_key)
|
||||
if not desire_key or not road_key or not wide_key:
|
||||
raise ValueError("Missing required vision or desire keys in input shapes.")
|
||||
|
||||
extra_keys = [key for key in input_shapes if key not in (desire_key, 'features_buffer', 'traffic_convention') and 'img' not in key]
|
||||
|
||||
def runner(img_q, big_img_q, feat_q, frame, big_frame, tfm, big_tfm, **kwargs):
|
||||
desire_q = kwargs['desire_q']
|
||||
desire = kwargs['desire']
|
||||
traffic_convention = kwargs.get('traffic_convention')
|
||||
tfm = kwargs['tfm']
|
||||
big_tfm = kwargs['big_tfm']
|
||||
|
||||
tfm = tfm.to(Device.DEFAULT)
|
||||
big_tfm = big_tfm.to(Device.DEFAULT)
|
||||
desire = desire.to(Device.DEFAULT)
|
||||
traffic_convention = traffic_convention.to(Device.DEFAULT)
|
||||
Tensor.realize(tfm, big_tfm, desire, traffic_convention)
|
||||
npys = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT), desire.to(Device.DEFAULT)]
|
||||
if traffic_convention is not None:
|
||||
npys.append(traffic_convention.to(Device.DEFAULT))
|
||||
|
||||
img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
|
||||
big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
|
||||
extra_tensors = {key: kwargs[key].to(Device.DEFAULT) for key in extra_keys if key in kwargs}
|
||||
Tensor.realize(*npys, *extra_tensors.values())
|
||||
|
||||
tfm_dev, big_tfm_dev, desire_dev = npys[:3]
|
||||
traffic_conv_dev = npys[3] if traffic_convention is not None else None
|
||||
|
||||
img = shift_and_sample(img_q, frame_prepare(frame, tfm_dev).unsqueeze(0), sample_skip_fn).realize()
|
||||
big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm_dev).unsqueeze(0), sample_skip_fn).realize()
|
||||
|
||||
if prepare_only:
|
||||
return img, big_img
|
||||
|
||||
desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
|
||||
feat_buf = sample_skip_fn(feat_q)
|
||||
desire_buf = shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn).realize()
|
||||
inputs = {desire_key: desire_buf, **extra_tensors}
|
||||
|
||||
inputs = {road_img_key: img, wide_img_key: big_img,
|
||||
desire_key: desire_buf, 'features_buffer': feat_buf,
|
||||
'traffic_convention': traffic_convention}
|
||||
for k in extra_policy_keys:
|
||||
if k in kwargs:
|
||||
inputs[k] = kwargs[k].to(Device.DEFAULT)
|
||||
if traffic_conv_dev is not None:
|
||||
inputs['traffic_convention'] = traffic_conv_dev
|
||||
|
||||
model_out = next(iter(model_runner(inputs).values())).cast('float32')
|
||||
if vision_runner:
|
||||
vision_out_cast = next(iter(vision_runner({road_key: img, wide_key: big_img}).values())).cast('float32').realize()
|
||||
new_feat = vision_out_cast[:, features_slice].reshape(1, -1).unsqueeze(0)
|
||||
inputs['features_buffer'] = shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
|
||||
policy_outs = [next(iter(pol_runner(inputs).values())).cast('float32').realize() for pol_runner in policy_runners]
|
||||
return (vision_out_cast, *policy_outs) if len(policy_outs) > 1 else (vision_out_cast, policy_outs[0])
|
||||
inputs.update({road_key: img, wide_key: big_img, 'features_buffer': sample_skip_fn(feat_q)})
|
||||
policy_out = next(iter(policy_runners[0](inputs).values())).cast('float32').realize()
|
||||
new_feat = policy_out[:, features_slice].reshape(1, -1).unsqueeze(0)
|
||||
shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
|
||||
return policy_out
|
||||
|
||||
new_feat = model_out[:, features_slice].reshape(1, -1).unsqueeze(0)
|
||||
shift_and_sample(feat_q, new_feat, sample_skip_fn)
|
||||
|
||||
return model_out
|
||||
|
||||
return run_supercombo
|
||||
return runner
|
||||
|
||||
|
||||
def make_run_vision_multi_policy(vision_runner, policy_runners, nv12: NV12Frame, model_w, model_h,
|
||||
vision_features_slice, frame_skip, desire_key, extra_policy_keys,
|
||||
vision_road_key, vision_wide_key, prepare_only=False):
|
||||
frame_prepare = make_frame_prepare(nv12, model_w, model_h)
|
||||
sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
|
||||
sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
|
||||
def compile_and_warmup(nv12: NV12Frame, model_size: tuple[int, int], prepare_only: bool, frame_skip: int, vision_runner, policy_runners: list, metadata: dict):
|
||||
print(f"Compiling combined JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
|
||||
|
||||
def run_multi_policy(img_q, big_img_q, feat_q, desire_q, desire,
|
||||
traffic_convention, tfm, big_tfm, frame, big_frame, **extra):
|
||||
npy_tensors = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT),
|
||||
desire.to(Device.DEFAULT), traffic_convention.to(Device.DEFAULT)]
|
||||
extra_device = {k: extra[k].to(Device.DEFAULT) for k in extra_policy_keys}
|
||||
Tensor.realize(*npy_tensors, *extra_device.values())
|
||||
tfm, big_tfm, desire, traffic_convention = npy_tensors
|
||||
all_shapes = {key: value for meta in metadata.values() for key, value in meta['input_shapes'].items()}
|
||||
|
||||
img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
|
||||
big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
|
||||
feat_meta = metadata.get('vision') or metadata.get('model') or metadata.get('policy')
|
||||
if not feat_meta:
|
||||
raise ValueError("Could not find vision, model, or policy metadata.")
|
||||
|
||||
if prepare_only:
|
||||
return img, big_img
|
||||
features_slice = feat_meta['output_slices']['hidden_state']
|
||||
WARP_DEV = 'CPU' if "USBGPU" in os.environ else Device.DEFAULT
|
||||
|
||||
vision_out = next(iter(vision_runner({vision_road_key: img, vision_wide_key: big_img}).values())).cast('float32')
|
||||
run_func = create_jit_runner(vision_runner, policy_runners, nv12, model_size, features_slice, frame_skip, all_shapes, prepare_only)
|
||||
run_jit = TinyJit(run_func, prune=True)
|
||||
queues, npy_arrays = generate_queues_and_npy(all_shapes, frame_skip, Device.DEFAULT)
|
||||
|
||||
new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
|
||||
feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
|
||||
desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
|
||||
|
||||
inputs = {'features_buffer': feat_buf, desire_key: desire_buf, 'traffic_convention': traffic_convention, **extra_device}
|
||||
|
||||
policy_outputs = []
|
||||
for runner in policy_runners:
|
||||
policy_out = next(iter(runner(inputs).values())).cast('float32')
|
||||
policy_outputs.append(policy_out)
|
||||
|
||||
return (vision_out, *policy_outputs)
|
||||
|
||||
return run_multi_policy
|
||||
|
||||
|
||||
def _warmup_and_serialize(run_jit, input_queues, npy, nv12):
|
||||
for i in range(3):
|
||||
rng = np.random.default_rng(42 + i)
|
||||
frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
|
||||
big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
|
||||
for v in npy.values():
|
||||
v[:] = rng.standard_normal(v.shape).astype(v.dtype)
|
||||
frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
|
||||
big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
|
||||
for arr in npy_arrays.values():
|
||||
arr[:] = rng.standard_normal(arr.shape).astype(arr.dtype)
|
||||
|
||||
Device.default.synchronize()
|
||||
st = time.perf_counter()
|
||||
run_jit(**input_queues, frame=frame, big_frame=big_frame)
|
||||
mt = time.perf_counter()
|
||||
start_time = time.perf_counter()
|
||||
run_jit(**queues, frame=frame, big_frame=big_frame)
|
||||
mid_time = time.perf_counter()
|
||||
Device.default.synchronize()
|
||||
et = time.perf_counter()
|
||||
print(f" [{i + 1}/3] enqueue {(mt - st) * 1e3:6.2f} ms -- total {(et - st) * 1e3:6.2f} ms")
|
||||
return pickle.loads(pickle.dumps(run_jit))
|
||||
print(f" [{i + 1}/3] enqueue {(mid_time - start_time) * 1e3:6.2f} ms -- total {(time.perf_counter() - start_time) * 1e3:6.2f} ms")
|
||||
|
||||
return pickle.loads(pickle.dumps(run_jit)) if not prepare_only else run_jit
|
||||
|
||||
|
||||
def compile_supercombo(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
|
||||
model_runner, metadata):
|
||||
print(f"Compiling combined supercombo JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
|
||||
|
||||
features_slice = metadata['output_slices']['hidden_state']
|
||||
input_shapes = metadata['input_shapes']
|
||||
|
||||
_run = make_run_supercombo(model_runner, nv12, model_w, model_h,
|
||||
features_slice, frame_skip, input_shapes, prepare_only)
|
||||
run_jit = TinyJit(_run, prune=True)
|
||||
|
||||
input_queues, npy = make_supercombo_input_queues(input_shapes, frame_skip, Device.DEFAULT)
|
||||
|
||||
run_jit = _warmup_and_serialize(run_jit, input_queues, npy, nv12)
|
||||
return run_jit
|
||||
def _parse_size(size_str: str) -> tuple[int, int]:
|
||||
width, height = size_str.lower().split('x')
|
||||
return int(width), int(height)
|
||||
|
||||
|
||||
def compile_multi_policy(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
|
||||
vision_runner, policy_runners, vision_metadata, policy_metadata):
|
||||
print(f"Compiling combined multi-policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
|
||||
|
||||
vision_features_slice = vision_metadata['output_slices']['hidden_state']
|
||||
vision_input_shapes = vision_metadata['input_shapes']
|
||||
policy_input_shapes = policy_metadata['input_shapes']
|
||||
desire_key = _detect_desire_key(policy_input_shapes)
|
||||
extra_policy_keys = [k for k in policy_input_shapes if k not in ('features_buffer', desire_key, 'traffic_convention')]
|
||||
vision_road_key, vision_wide_key = _detect_vision_keys(vision_input_shapes)
|
||||
|
||||
_run = make_run_vision_multi_policy(vision_runner, policy_runners, nv12, model_w, model_h,
|
||||
vision_features_slice, frame_skip, desire_key, extra_policy_keys,
|
||||
vision_road_key, vision_wide_key, prepare_only)
|
||||
run_jit = TinyJit(_run, prune=True)
|
||||
|
||||
input_queues, npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
|
||||
|
||||
run_jit = _warmup_and_serialize(run_jit, input_queues, npy, nv12)
|
||||
return run_jit
|
||||
def read_file_chunked_to_shm(path):
|
||||
if not path:
|
||||
return None
|
||||
import atexit
|
||||
import shutil
|
||||
from openpilot.common.file_chunker import open_file_chunked
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
shm_path = os.path.join(Paths.shm_path(), os.path.basename(path))
|
||||
atexit.register(lambda: os.path.exists(shm_path) and os.remove(shm_path))
|
||||
with open(shm_path, 'wb') as dst, open_file_chunked(path) as src:
|
||||
shutil.copyfileobj(src, dst)
|
||||
return shm_path
|
||||
|
||||
|
||||
def _parse_size(s):
|
||||
w, h = s.lower().split('x')
|
||||
return int(w), int(h)
|
||||
def _compile_for_resolutions(camera_resolutions: list, model_size: tuple[int, int], frame_skip: int,
|
||||
vision_runner, policy_runners: list, metadata: dict) -> dict:
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
return {
|
||||
(cam_w, cam_h): {
|
||||
name: compile_and_warmup(NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)), model_size, prepare_only,
|
||||
frame_skip, vision_runner, policy_runners, metadata)
|
||||
for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
|
||||
}
|
||||
for cam_w, cam_h in camera_resolutions
|
||||
}
|
||||
|
||||
|
||||
def _load_policy_runners(args: argparse.Namespace) -> tuple[list, list]:
|
||||
runners, keys = [], []
|
||||
for name, onnx_arg in [('policy', args.policy_onnx), ('off_policy', args.off_policy_onnx), ('on_policy', args.on_policy_onnx)]:
|
||||
if onnx_arg:
|
||||
runners.append(OnnxRunner(onnx_arg))
|
||||
keys.append(name)
|
||||
return runners, keys
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
from tinygrad.nn.onnx import OnnxRunner
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
|
||||
from tinygrad.nn.onnx import OnnxRunner
|
||||
|
||||
p = argparse.ArgumentParser(description="Compile combined JIT pkl for sunnypilot modeld_v2")
|
||||
p.add_argument('--model-type', choices=MODEL_TYPES, required=True)
|
||||
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)
|
||||
p.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
|
||||
p.add_argument('--output', required=True)
|
||||
parser = argparse.ArgumentParser(description="Compile combined JIT pkl for sunnypilot modeld_v2")
|
||||
parser.add_argument('--model-type', choices=MODEL_TYPES, required=True)
|
||||
parser.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
|
||||
parser.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True)
|
||||
parser.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
|
||||
parser.add_argument('--output', required=True)
|
||||
|
||||
p.add_argument('--vision-onnx', help='vision ONNX (for split models)')
|
||||
p.add_argument('--policy-onnx', help='policy ONNX (for vision_policy)')
|
||||
p.add_argument('--off-policy-onnx', help='off-policy ONNX (for vision_multi_policy)')
|
||||
p.add_argument('--on-policy-onnx', help='on-policy ONNX (for vision_multi_policy)')
|
||||
p.add_argument('--supercombo-onnx', help='supercombo ONNX (for supercombo)')
|
||||
parser.add_argument('--vision-onnx', help='vision ONNX (for split models)')
|
||||
parser.add_argument('--policy-onnx', help='policy ONNX (for vision_policy)')
|
||||
parser.add_argument('--off-policy-onnx', help='off-policy ONNX (for vision_multi_policy)')
|
||||
parser.add_argument('--on-policy-onnx', help='on-policy ONNX (for vision_multi_policy)')
|
||||
parser.add_argument('--supercombo-onnx', help='supercombo ONNX (for supercombo)')
|
||||
|
||||
args = p.parse_args()
|
||||
out = defaultdict(dict)
|
||||
args = parser.parse_args()
|
||||
output_data = defaultdict(dict)
|
||||
|
||||
args.vision_onnx = read_file_chunked_to_shm(args.vision_onnx)
|
||||
args.policy_onnx = read_file_chunked_to_shm(args.policy_onnx)
|
||||
args.off_policy_onnx = read_file_chunked_to_shm(args.off_policy_onnx)
|
||||
args.on_policy_onnx = read_file_chunked_to_shm(args.on_policy_onnx)
|
||||
args.supercombo_onnx = read_file_chunked_to_shm(args.supercombo_onnx)
|
||||
|
||||
vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
|
||||
|
||||
if args.model_type == 'vision_policy':
|
||||
assert args.vision_onnx and args.policy_onnx
|
||||
vision_runner = OnnxRunner(args.vision_onnx)
|
||||
policy_runner = OnnxRunner(args.policy_onnx)
|
||||
out['metadata']['vision'] = make_metadata_dict(args.vision_onnx)
|
||||
out['metadata']['policy'] = make_metadata_dict(args.policy_onnx)
|
||||
|
||||
frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip(out['metadata']['vision']['input_shapes'],
|
||||
out['metadata']['policy']['input_shapes'])
|
||||
|
||||
for cam_w, cam_h in args.camera_resolutions:
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
model_w, model_h = args.model_size
|
||||
out[(cam_w, cam_h)] = {
|
||||
name: compile_split_policy(nv12, model_w, model_h, prepare_only, frame_skip,
|
||||
vision_runner, policy_runner,
|
||||
out['metadata']['vision'], out['metadata']['policy'])
|
||||
for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
|
||||
}
|
||||
|
||||
assert vision_runner and args.policy_onnx
|
||||
policy_runners = [OnnxRunner(args.policy_onnx)]
|
||||
output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx), 'policy': make_metadata_dict(args.policy_onnx)}
|
||||
elif args.model_type == 'supercombo':
|
||||
assert args.supercombo_onnx
|
||||
model_runner = OnnxRunner(args.supercombo_onnx)
|
||||
out['metadata']['model'] = make_metadata_dict(args.supercombo_onnx)
|
||||
|
||||
frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip({}, out['metadata']['model']['input_shapes'])
|
||||
|
||||
for cam_w, cam_h in args.camera_resolutions:
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
model_w, model_h = args.model_size
|
||||
out[(cam_w, cam_h)] = {
|
||||
name: compile_supercombo(nv12, model_w, model_h, prepare_only, frame_skip,
|
||||
model_runner, out['metadata']['model'])
|
||||
for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
|
||||
}
|
||||
|
||||
policy_runners = [OnnxRunner(args.supercombo_onnx)]
|
||||
output_data['metadata'] = {'model': make_metadata_dict(args.supercombo_onnx)}
|
||||
elif args.model_type == 'vision_multi_policy':
|
||||
assert args.vision_onnx
|
||||
vision_runner = OnnxRunner(args.vision_onnx)
|
||||
out['metadata']['vision'] = make_metadata_dict(args.vision_onnx)
|
||||
assert vision_runner
|
||||
policy_runners, policy_names = _load_policy_runners(args)
|
||||
output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx)}
|
||||
for name in policy_names:
|
||||
runner_arg = getattr(args, f"{name}_onnx")
|
||||
output_data['metadata'][name] = make_metadata_dict(runner_arg)
|
||||
|
||||
policy_runners = []
|
||||
policy_onnxes = []
|
||||
if args.policy_onnx:
|
||||
policy_onnxes.append(('policy', args.policy_onnx))
|
||||
if args.off_policy_onnx:
|
||||
policy_onnxes.append(('off_policy', args.off_policy_onnx))
|
||||
if args.on_policy_onnx:
|
||||
policy_onnxes.append(('on_policy', args.on_policy_onnx))
|
||||
policy_keys = [key for key in output_data['metadata'].keys() if key != 'vision']
|
||||
first_policy_meta = output_data['metadata'][policy_keys[0]] if policy_keys else {}
|
||||
vision_meta = output_data['metadata'].get('vision', {})
|
||||
|
||||
for name, onnx_path in policy_onnxes:
|
||||
runner = OnnxRunner(onnx_path)
|
||||
policy_runners.append(runner)
|
||||
out['metadata'][name] = make_metadata_dict(onnx_path)
|
||||
derived_frame_skip = args.frame_skip or derive_frame_skip(vision_meta.get('input_shapes', {}), first_policy_meta.get('input_shapes', {}))
|
||||
output_data.update(_compile_for_resolutions(args.camera_resolutions, args.model_size, derived_frame_skip,
|
||||
vision_runner, policy_runners, output_data['metadata']))
|
||||
|
||||
first_policy_key = policy_onnxes[0][0]
|
||||
frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip(out['metadata']['vision']['input_shapes'],
|
||||
out['metadata'][first_policy_key]['input_shapes'])
|
||||
with open(args.output, "wb") as file:
|
||||
pickle.dump(output_data, file)
|
||||
|
||||
for cam_w, cam_h in args.camera_resolutions:
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
model_w, model_h = args.model_size
|
||||
out[(cam_w, cam_h)] = {
|
||||
name: compile_multi_policy(nv12, model_w, model_h, prepare_only, frame_skip,
|
||||
vision_runner, policy_runners,
|
||||
out['metadata']['vision'], out['metadata'][first_policy_key])
|
||||
for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
|
||||
}
|
||||
|
||||
with open(args.output, "wb") as f:
|
||||
pickle.dump(out, f)
|
||||
pkl_size = os.path.getsize(args.output)
|
||||
print(f"Saved combined JIT to {args.output} ({pkl_size / 1e6:.2f} MB)")
|
||||
|
||||
from openpilot.common.file_chunker import chunk_file, get_chunk_targets
|
||||
chunk_targets = get_chunk_targets(args.output, pkl_size)
|
||||
chunk_file(args.output, chunk_targets)
|
||||
num_chunks = len(chunk_targets) - 1
|
||||
print(f"Chunked into {num_chunks} file(s)")
|
||||
print(f"Chunked into {len(chunk_targets) - 1} file(s)")
|
||||
|
||||
@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import os
|
||||
os.environ['GMMU'] = '0'
|
||||
from openpilot.common.hardware import TICI
|
||||
os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
|
||||
USBGPU = "USBGPU" in os.environ
|
||||
@@ -23,6 +24,11 @@ from setproctitle import setproctitle
|
||||
from openpilot.cereal.messaging import PubMaster, SubMaster
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
|
||||
from opendbc.car.car_helpers import get_demo_car_params
|
||||
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.device import Device
|
||||
|
||||
from openpilot.common.file_chunker import open_file_chunked
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
@@ -30,6 +36,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.common.transformations.model import get_warp_matrix
|
||||
from openpilot.system import sentry
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
|
||||
|
||||
@@ -37,6 +44,7 @@ from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_p
|
||||
from openpilot.sunnypilot.modeld_v2.constants import Plan
|
||||
from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
|
||||
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues
|
||||
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
@@ -99,17 +107,12 @@ class ModelState(ModelStateBase):
|
||||
self._init_combined(pkl_path, cam_w, cam_h, model_bundle)
|
||||
|
||||
def _init_combined(self, pkl_path, cam_w, cam_h, bundle):
|
||||
from tinygrad.tensor import Tensor
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues
|
||||
from tinygrad.device import Device
|
||||
|
||||
from openpilot.common.file_chunker import open_file_chunked
|
||||
|
||||
cloudlog.warning(f"loading combined pkl: {pkl_path}")
|
||||
jits = pickle.load(open_file_chunked(pkl_path))
|
||||
|
||||
self.DEV = Device.DEFAULT
|
||||
self.WARP_DEV = 'CPU' if USBGPU else self.DEV
|
||||
self.QUEUE_DEV = self.DEV
|
||||
|
||||
metadata = jits['metadata']
|
||||
if 'model' in metadata:
|
||||
@@ -118,10 +121,10 @@ class ModelState(ModelStateBase):
|
||||
self.policy_output_slices = {}
|
||||
self._policy_slices_list = []
|
||||
self._combined_model_type = 'supercombo'
|
||||
self._vision_input_names = [k for k in model_metadata['input_shapes'] if 'img' in k]
|
||||
self._vision_input_names = [key for key in model_metadata['input_shapes'] if 'img' in key]
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import make_supercombo_input_queues
|
||||
frame_skip = derive_frame_skip({}, model_metadata['input_shapes'])
|
||||
self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'], frame_skip, device=self.DEV)
|
||||
self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'], frame_skip, device=self.QUEUE_DEV)
|
||||
else:
|
||||
vision_metadata = metadata['vision']
|
||||
policy_keys = [k for k in metadata if k != 'vision']
|
||||
@@ -139,11 +142,11 @@ class ModelState(ModelStateBase):
|
||||
policy_input_shapes = first_policy_metadata['input_shapes']
|
||||
self._vision_input_names = [k for k in vision_input_shapes if 'img' in k]
|
||||
frame_skip = derive_frame_skip(vision_input_shapes, policy_input_shapes)
|
||||
self.input_queues, self.numpy_inputs = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device=self.DEV)
|
||||
self.input_queues, self.numpy_inputs = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device=self.QUEUE_DEV)
|
||||
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
|
||||
self.parser = SplitParser() if self._combined_model_type != 'supercombo' else CombinedParser()
|
||||
self._desire_key = next(key for key in self.numpy_inputs if key.startswith('desire'))
|
||||
self._road_key = next(key for key in self._vision_input_names if 'big' not in key)
|
||||
self._wide_key = next(key for key in self._vision_input_names if 'big' in key)
|
||||
|
||||
is_20hz = bundle.is20hz if bundle else self._combined_model_type in ('split', 'multi_policy')
|
||||
if is_20hz:
|
||||
@@ -153,6 +156,13 @@ class ModelState(ModelStateBase):
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
self.constants = ModelConstants()
|
||||
|
||||
if self._combined_model_type != 'supercombo':
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
|
||||
self.parser = SplitParser()
|
||||
else:
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
|
||||
self.parser = CombinedParser()
|
||||
|
||||
self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32)
|
||||
self.full_frames: dict = {}
|
||||
self._blob_cache: dict = {}
|
||||
@@ -161,12 +171,11 @@ class ModelState(ModelStateBase):
|
||||
|
||||
self._run_policy = jits[(cam_w, cam_h)]['run_policy']
|
||||
self._warp_enqueue = jits[(cam_w, cam_h)]['warp_enqueue']
|
||||
road_name = next(k for k in self._vision_input_names if 'big' not in k)
|
||||
yuv_size = self.frame_buf_params[road_name][3]
|
||||
yuv_size = self.frame_buf_params[self._road_key][3]
|
||||
self._warp_enqueue(
|
||||
**self.input_queues,
|
||||
frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.DEV).contiguous().realize(),
|
||||
big_frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.DEV).contiguous().realize())
|
||||
frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize(),
|
||||
big_frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize())
|
||||
|
||||
|
||||
@property
|
||||
@@ -179,7 +188,7 @@ class ModelState(ModelStateBase):
|
||||
|
||||
@property
|
||||
def desire_key(self) -> str:
|
||||
return next(k for k in self.numpy_inputs if k.startswith('desire'))
|
||||
return self._desire_key
|
||||
|
||||
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
|
||||
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
|
||||
@@ -190,19 +199,19 @@ class ModelState(ModelStateBase):
|
||||
yuv_size = self.frame_buf_params[key][3]
|
||||
cache_key = (key, ptr)
|
||||
if cache_key not in self._blob_cache:
|
||||
self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8', device=self.DEV)
|
||||
self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8', device=self.WARP_DEV)
|
||||
self.full_frames[key] = self._blob_cache[cache_key]
|
||||
|
||||
desire_key = self.desire_key
|
||||
inputs[desire_key][0] = 0
|
||||
self.numpy_inputs[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
|
||||
self.prev_desire[:] = inputs[desire_key]
|
||||
for key in ('traffic_convention', 'lateral_control_params'):
|
||||
for key in ('traffic_convention', 'lateral_control_params', 'action_t'):
|
||||
if key in self.numpy_inputs and key in inputs:
|
||||
self.numpy_inputs[key][:] = inputs[key]
|
||||
|
||||
road_key = next(n for n in bufs if 'big' not in n)
|
||||
wide_key = next(n for n in bufs if 'big' in n)
|
||||
road_key = self._road_key
|
||||
wide_key = self._wide_key
|
||||
self.numpy_inputs['tfm'][:, :] = transforms[road_key].reshape(3, 3)
|
||||
self.numpy_inputs['big_tfm'][:, :] = transforms[wide_key].reshape(3, 3)
|
||||
|
||||
@@ -225,8 +234,12 @@ class ModelState(ModelStateBase):
|
||||
policy_output = raw_outputs[i + 1].numpy().flatten()
|
||||
policy_sliced = {k: policy_output[np.newaxis, v] for k, v in policy_slices.items()}
|
||||
parsed = self.parser.parse_policy_outputs(policy_sliced)
|
||||
if 'off' in self._policy_keys[i] and self._has_on_policy:
|
||||
if ('off' in self._policy_keys[i]
|
||||
and self._has_on_policy
|
||||
and any('plan' in self._policy_slices_list[j] for j, k in enumerate(self._policy_keys) if 'on' in k.lower())):
|
||||
|
||||
parsed.pop('plan', None)
|
||||
|
||||
outputs.update(parsed)
|
||||
|
||||
if 'planplus' in outputs and 'plan' in outputs:
|
||||
@@ -241,13 +254,20 @@ class ModelState(ModelStateBase):
|
||||
|
||||
def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
|
||||
lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
|
||||
action_t=long_action_t)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
if 'action' not in model_output:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
|
||||
action_t=long_action_t)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
|
||||
curvature_plan = (plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0]
|
||||
if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan)
|
||||
desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim)
|
||||
else:
|
||||
desired_accel = model_output['action'][0, 1]
|
||||
desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
|
||||
should_stop = (v_ego < 0.3 and desired_accel < 0.1)
|
||||
|
||||
curvature_plan = plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0] if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan
|
||||
desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim)
|
||||
if self.generation is not None and self.generation >= 10: # smooth curvature for post FOF models
|
||||
if v_ego > self.MIN_LAT_CONTROL_SPEED:
|
||||
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS)
|
||||
@@ -400,6 +420,12 @@ def main(demo=False):
|
||||
|
||||
bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
|
||||
transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
|
||||
|
||||
frame_delay = DT_MDL # compensate for time passed since the frame was captured: current_time - timestamp_eof is 50ms on average
|
||||
action_delay = DT_MDL / 2 # middle of the interval between model output (current state) and next frame (expected state)
|
||||
lat_action_t = lat_delay + frame_delay + action_delay
|
||||
long_action_t = long_delay + frame_delay + action_delay
|
||||
|
||||
inputs:dict[str, np.ndarray] = {
|
||||
model.desire_key: vec_desire,
|
||||
'traffic_convention': traffic_convention,
|
||||
@@ -408,6 +434,9 @@ def main(demo=False):
|
||||
if 'lateral_control_params' in model.numpy_inputs:
|
||||
inputs['lateral_control_params'] = np.array([v_ego, lat_delay], dtype=np.float32)
|
||||
|
||||
if 'action_t' in model.numpy_inputs:
|
||||
inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32)
|
||||
|
||||
mt1 = time.perf_counter()
|
||||
model_output = model.run(bufs, transforms, inputs, prepare_only)
|
||||
mt2 = time.perf_counter()
|
||||
@@ -419,7 +448,7 @@ def main(demo=False):
|
||||
posenet_send = messaging.new_message('cameraOdometry')
|
||||
mdv2sp_send = messaging.new_message('modelDataV2SP')
|
||||
|
||||
action = model.get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego)
|
||||
action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
|
||||
prev_action = action
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
|
||||
@@ -1,13 +1,16 @@
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
|
||||
|
||||
def safe_exp(x, out=None):
|
||||
# -11 is around 10**14, more causes float16 overflow
|
||||
return np.exp(np.clip(x, -np.inf, 11), out=out)
|
||||
|
||||
|
||||
def sigmoid(x):
|
||||
return 1. / (1. + safe_exp(-x))
|
||||
|
||||
|
||||
def softmax(x, axis=-1):
|
||||
x -= np.max(x, axis=axis, keepdims=True)
|
||||
if x.dtype == np.float32 or x.dtype == np.float64:
|
||||
@@ -17,6 +20,19 @@ def softmax(x, axis=-1):
|
||||
x /= np.sum(x, axis=axis, keepdims=True)
|
||||
return x
|
||||
|
||||
|
||||
def _infer_mhp(slice_size: int, prod_out_shape: int, max_in_n: int = 16, max_out_n: int = 6) -> tuple[int, int]:
|
||||
for out_n in range(max_out_n + 1):
|
||||
per = 2 * prod_out_shape + out_n
|
||||
if per <= 0:
|
||||
continue
|
||||
if slice_size % per == 0:
|
||||
in_n = slice_size // per
|
||||
if 1 <= in_n <= max_in_n:
|
||||
return in_n, out_n
|
||||
return 1, 0 # single hypothesis, no weights — matches a non-MDN output
|
||||
|
||||
|
||||
class Parser:
|
||||
def __init__(self, ignore_missing=False):
|
||||
self.ignore_missing = ignore_missing
|
||||
@@ -40,17 +56,22 @@ class Parser:
|
||||
raw = outs[name]
|
||||
outs[name] = sigmoid(raw)
|
||||
|
||||
def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
|
||||
def parse_mdn(self, name, outs, out_shape, in_N=0, out_N=0):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
|
||||
|
||||
if in_N == 0 and out_N == 0:
|
||||
prod = int(np.prod(out_shape))
|
||||
in_N, out_N = _infer_mhp(raw.shape[1], prod)
|
||||
|
||||
raw = raw.reshape((raw.shape[0], in_N, -1))
|
||||
|
||||
n_values = (raw.shape[2] - out_N)//2
|
||||
pred_mu = raw[:,:,:n_values]
|
||||
pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
|
||||
|
||||
if in_N > 1:
|
||||
if in_N > 1 and out_N > 0:
|
||||
weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
|
||||
for i in range(out_N):
|
||||
weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
|
||||
@@ -61,7 +82,6 @@ class Parser:
|
||||
weights[fidx] = weights[fidx][idxs]
|
||||
pred_mu[fidx] = pred_mu[fidx][idxs]
|
||||
pred_std[fidx] = pred_std[fidx][idxs]
|
||||
assert out_shape is not None
|
||||
full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
|
||||
outs[name + '_weights'] = weights
|
||||
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
|
||||
@@ -74,37 +94,43 @@ class Parser:
|
||||
idxs = np.argsort(weights[fidx,:,hidx])[::-1]
|
||||
pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
|
||||
pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
|
||||
elif in_N > 1 and out_N == 0:
|
||||
# MHP without weights: keep every hypothesis intact, surface them as
|
||||
# ``*_hypotheses`` and propagate the full multi-hypothesis tensor.
|
||||
full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
|
||||
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
|
||||
outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
|
||||
pred_mu_final = pred_mu
|
||||
pred_std_final = pred_std
|
||||
else:
|
||||
pred_mu_final = pred_mu
|
||||
pred_std_final = pred_std
|
||||
|
||||
if out_N > 1:
|
||||
assert out_shape is not None
|
||||
final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
|
||||
if out_N > 1 or (in_N > 1 and out_N == 0):
|
||||
n_selections = out_N if out_N > 1 else in_N
|
||||
final_shape = tuple([raw.shape[0], n_selections] + list(out_shape))
|
||||
else:
|
||||
assert out_shape is not None
|
||||
final_shape = tuple([raw.shape[0],] + list(out_shape))
|
||||
outs[name] = pred_mu_final.reshape(final_shape)
|
||||
outs[name + '_stds'] = pred_std_final.reshape(final_shape)
|
||||
|
||||
def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
|
||||
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
|
||||
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
# supercombo (4955 / 102) and newer variants (e.g. 990 / 144).
|
||||
self.parse_mdn('plan', outs, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
|
||||
self.parse_mdn('lane_lines', outs, out_shape=(ModelConstants.NUM_LANE_LINES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('road_edges', outs, out_shape=(ModelConstants.NUM_ROAD_EDGES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('pose', outs, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('road_transform', outs, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
if 'sim_pose' in outs:
|
||||
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
|
||||
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
|
||||
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
|
||||
self.parse_mdn('sim_pose', outs, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('wide_from_device_euler', outs, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
|
||||
self.parse_mdn('lead', outs, out_shape=(ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
|
||||
if 'lat_planner_solution' in outs:
|
||||
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
|
||||
self.parse_mdn('lat_planner_solution', outs, out_shape=(ModelConstants.IDX_N, ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
|
||||
if 'desired_curvature' in outs:
|
||||
self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
|
||||
self.parse_mdn('desired_curvature', outs, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
|
||||
for k in ['lead_prob', 'lane_lines_prob', 'meta']:
|
||||
self.parse_binary_crossentropy(k, outs)
|
||||
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
|
||||
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
|
||||
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN, ModelConstants.DESIRE_PRED_WIDTH))
|
||||
return outs
|
||||
|
||||
@@ -123,7 +123,7 @@ class Parser:
|
||||
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(SplitModelConstants.DESIRE_PRED_WIDTH,))
|
||||
if 'lane_lines' in outs:
|
||||
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0,
|
||||
out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
if 'lane_lines_prob' in outs:
|
||||
self.parse_binary_crossentropy('lane_lines_prob', outs)
|
||||
if 'lead_prob' in outs:
|
||||
@@ -134,9 +134,11 @@ class Parser:
|
||||
self.parse_binary_crossentropy('meta', outs)
|
||||
if 'road_edges' in outs:
|
||||
self.parse_mdn('road_edges', outs, in_N=0, out_N=0,
|
||||
out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
||||
if 'sim_pose' in outs:
|
||||
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
|
||||
if 'action' in outs:
|
||||
self.parse_mdn('action', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.ACTION_WIDTH,))
|
||||
|
||||
def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
||||
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
|
||||
|
||||
@@ -67,22 +67,14 @@ class TestStockEquivalence:
|
||||
state = model_state_factory(ARCHETYPES['vision_policy_split'])
|
||||
|
||||
frame_skip = derive_frame_skip(SPLIT_VISION_INPUT_SHAPES, SPLIT_POLICY_INPUT_SHAPES)
|
||||
# action_t is a deep-model prerequisite the SP loader doesn't provide yet; see skip_keys below
|
||||
stock_shapes = {**SPLIT_VISION_INPUT_SHAPES, **SPLIT_POLICY_INPUT_SHAPES, 'action_t': (1, 2)}
|
||||
stock_queues, stock_npy = make_input_queues(stock_shapes, frame_skip, device='NPY')
|
||||
|
||||
# TODO-SP: remove action_t skip once SP adds prerequisite for deep models (action_t input queue)
|
||||
# prev_feat is a stock QCOM corruption workaround handled inside the SP loader's JIT path
|
||||
skip_keys = {'action_t', 'prev_feat'}
|
||||
# stock packs the per-key policy inputs into packed_npy_inputs; the npy views carry the individual keys
|
||||
stock_queue_keys = set(stock_queues.keys())
|
||||
if 'packed_npy_inputs' in stock_queue_keys:
|
||||
stock_queue_keys.remove('packed_npy_inputs')
|
||||
stock_queue_keys |= set(stock_npy.keys())
|
||||
assert set(state.input_queues.keys()) == stock_queue_keys - skip_keys, \
|
||||
f"Queue keys differ: v2={set(state.input_queues.keys())}, stock={stock_queue_keys}"
|
||||
assert set(state.numpy_inputs.keys()) == set(stock_npy.keys()) - skip_keys, \
|
||||
f"Npy keys differ: v2={set(state.numpy_inputs.keys())}, stock={set(stock_npy.keys())}"
|
||||
assert set(state.input_queues.keys()) - {'desire', 'traffic_convention'} == \
|
||||
set(stock_queues.keys()) - {'packed_npy_inputs'}
|
||||
assert {'desire', 'traffic_convention'} <= set(state.input_queues.keys())
|
||||
# We generate action_t and prev_feat dynamically based on the metadata
|
||||
assert set(state.numpy_inputs.keys()) == set(stock_npy.keys()) - {'action_t', 'prev_feat'}
|
||||
|
||||
def test_split_queue_keys_work_with_desire_key(self, model_state_factory):
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues
|
||||
|
||||
@@ -1,103 +0,0 @@
|
||||
import os
|
||||
os.environ['DEV'] = 'CPU'
|
||||
import pytest
|
||||
import numpy as np
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.sunnypilot.modeld_v2.warp import CAMERA_CONFIGS
|
||||
from openpilot.sunnypilot.modeld_v2.warp import Warp, MODEL_W, MODEL_H
|
||||
|
||||
VISION_NAME_PAIRS = [ # needed to account for supercombos input_imgs
|
||||
('img', 'big_img'),
|
||||
('input_imgs', 'big_input_imgs'),
|
||||
]
|
||||
|
||||
|
||||
class MockVisionBuf:
|
||||
def __init__(self, w, h):
|
||||
self.width = w
|
||||
self.height = h
|
||||
_, _, _, yuv_size = get_nv12_info(w, h)
|
||||
self.data = np.zeros(yuv_size, dtype=np.uint8)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("buffer_length", [2, 5])
|
||||
def test_warp_initialization(buffer_length):
|
||||
warp = Warp(buffer_length)
|
||||
assert warp.buffer_length == buffer_length
|
||||
assert warp.img_buffer_shape == (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("buffer_length", [2, 5])
|
||||
@pytest.mark.parametrize("cam_w, cam_h", CAMERA_CONFIGS)
|
||||
@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
|
||||
def test_warp_process(buffer_length, cam_w, cam_h, road, wide):
|
||||
warp = Warp(buffer_length)
|
||||
mock_buf = MockVisionBuf(cam_w, cam_h)
|
||||
transform = np.eye(3, dtype=np.float32).flatten()
|
||||
bufs = {road: mock_buf, wide: mock_buf}
|
||||
transforms = {road: transform, wide: transform}
|
||||
|
||||
out = warp.process(bufs, transforms)
|
||||
assert isinstance(out, dict)
|
||||
assert road in out and wide in out
|
||||
assert out[road].shape == (1, 12, MODEL_H // 2, MODEL_W // 2)
|
||||
assert out[wide].shape == (1, 12, MODEL_H // 2, MODEL_W // 2)
|
||||
|
||||
key = (cam_w, cam_h)
|
||||
assert key in warp.jit_cache
|
||||
|
||||
out2 = warp.process(bufs, transforms)
|
||||
assert out2[road].shape == out[road].shape
|
||||
|
||||
|
||||
@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
|
||||
def test_warp_buffer_shift(road, wide):
|
||||
warp = Warp(2)
|
||||
cam_w, cam_h = CAMERA_CONFIGS[1]
|
||||
transform = np.eye(3, dtype=np.float32).flatten()
|
||||
|
||||
buf1 = MockVisionBuf(cam_w, cam_h)
|
||||
buf1.data[0] = 255
|
||||
bufs1 = {road: buf1, wide: buf1}
|
||||
transforms = {road: transform, wide: transform}
|
||||
out1 = warp.process(bufs1, transforms)
|
||||
road1 = out1[road].numpy().copy()
|
||||
|
||||
buf2 = MockVisionBuf(cam_w, cam_h)
|
||||
buf2.data[0] = 128
|
||||
bufs2 = {road: buf2, wide: buf2}
|
||||
out2 = warp.process(bufs2, transforms)
|
||||
assert not np.array_equal(road1, out2[road].numpy())
|
||||
|
||||
|
||||
@pytest.mark.parametrize("buffer_length", [2, 5])
|
||||
@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
|
||||
def test_warp_buffer_accumulation(buffer_length, road, wide):
|
||||
warp = Warp(buffer_length)
|
||||
cam_w, cam_h = CAMERA_CONFIGS[0]
|
||||
transform = np.eye(3, dtype=np.float32).flatten()
|
||||
transforms = {road: transform, wide: transform}
|
||||
outputs = []
|
||||
|
||||
for i in range(buffer_length + 1):
|
||||
buf = MockVisionBuf(cam_w, cam_h)
|
||||
buf.data[:] = i * 10
|
||||
out = warp.process({road: buf, wide: buf}, transforms)
|
||||
outputs.append(out[road].numpy().copy())
|
||||
|
||||
assert warp.full_buffers['img'].shape == (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
|
||||
for i in range(1, len(outputs)):
|
||||
assert not np.array_equal(outputs[i - 1], outputs[i])
|
||||
|
||||
|
||||
def test_warp_different_cameras_same_instance():
|
||||
warp = Warp(2)
|
||||
transform = np.eye(3, dtype=np.float32).flatten()
|
||||
|
||||
buf1 = MockVisionBuf(*CAMERA_CONFIGS[0])
|
||||
warp.process({'img': buf1, 'big_img': buf1}, {'img': transform, 'big_img': transform})
|
||||
assert len(warp.jit_cache) == 1
|
||||
|
||||
buf2 = MockVisionBuf(*CAMERA_CONFIGS[1])
|
||||
warp.process({'img': buf2, 'big_img': buf2}, {'img': transform, 'big_img': transform})
|
||||
assert len(warp.jit_cache) == 2
|
||||
@@ -1,171 +0,0 @@
|
||||
import pickle
|
||||
import time
|
||||
import numpy as np
|
||||
from pathlib import Path
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.engine.jit import TinyJit
|
||||
from tinygrad.device import Device
|
||||
|
||||
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
||||
from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE
|
||||
from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
|
||||
from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare as _make_frame_prepare
|
||||
|
||||
CAMERA_CONFIGS = [
|
||||
(_ar_ox_fisheye.width, _ar_ox_fisheye.height),
|
||||
(_os_fisheye.width, _os_fisheye.height),
|
||||
]
|
||||
|
||||
|
||||
def make_frame_prepare(cam_w, cam_h, model_w, model_h):
|
||||
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
|
||||
return _make_frame_prepare(nv12, model_w, model_h)
|
||||
|
||||
|
||||
def warp_pkl_path(w, h):
|
||||
from openpilot.selfdrive.modeld.helpers import MODELS_DIR
|
||||
return MODELS_DIR / f'warp_{w}x{h}_tinygrad.pkl'
|
||||
|
||||
|
||||
def make_update_img_input(frame_prepare, model_w, model_h):
|
||||
def update_img_input_tinygrad(tensor, frame, M_inv):
|
||||
M_inv = M_inv.to(Device.DEFAULT)
|
||||
new_img = frame_prepare(frame, M_inv)
|
||||
tensor.assign(tensor[6:].cat(new_img, dim=0).contiguous())
|
||||
return Tensor.cat(tensor[:6], tensor[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2)
|
||||
return update_img_input_tinygrad
|
||||
|
||||
|
||||
def make_update_both_imgs(frame_prepare, model_w, model_h):
|
||||
update_img = make_update_img_input(frame_prepare, model_w, model_h)
|
||||
def update_both_imgs_tinygrad(calib_img_buffer, new_img, M_inv,
|
||||
calib_big_img_buffer, new_big_img, M_inv_big):
|
||||
calib_img_pair = update_img(calib_img_buffer, new_img, M_inv)
|
||||
calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big)
|
||||
return calib_img_pair, calib_big_img_pair
|
||||
return update_both_imgs_tinygrad
|
||||
|
||||
MODELS_DIR = Path(__file__).parent / 'models'
|
||||
MODEL_W, MODEL_H = MEDMODEL_INPUT_SIZE
|
||||
UPSTREAM_BUFFER_LENGTH = 5
|
||||
|
||||
|
||||
def v2_warp_pkl_path(cam_w, cam_h, buffer_length):
|
||||
return MODELS_DIR / f'warp_{cam_w}x{cam_h}_b{buffer_length}_tinygrad.pkl'
|
||||
|
||||
|
||||
def compile_v2_warp(cam_w, cam_h, buffer_length):
|
||||
_, _, _, yuv_size = get_nv12_info(cam_w, cam_h)
|
||||
img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
|
||||
|
||||
print(f"Compiling v2 warp for {cam_w}x{cam_h} buffer_length={buffer_length}...")
|
||||
|
||||
frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
|
||||
update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
|
||||
update_img_jit = TinyJit(update_both_imgs, prune=True)
|
||||
|
||||
full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize()
|
||||
big_full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize()
|
||||
new_frame_np = np.random.default_rng(0).integers(0, 256, yuv_size, dtype=np.uint8)
|
||||
new_big_frame_np = np.random.default_rng(1).integers(0, 256, yuv_size, dtype=np.uint8)
|
||||
for i in range(10):
|
||||
img_inputs = [full_buffer,
|
||||
Tensor.from_blob(new_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
|
||||
Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
|
||||
big_img_inputs = [big_full_buffer,
|
||||
Tensor.from_blob(new_big_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
|
||||
Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
|
||||
inputs = img_inputs + big_img_inputs
|
||||
Device.default.synchronize()
|
||||
|
||||
st = time.perf_counter()
|
||||
_ = update_img_jit(*inputs)
|
||||
mt = time.perf_counter()
|
||||
Device.default.synchronize()
|
||||
et = time.perf_counter()
|
||||
print(f" [{i+1}/10] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
|
||||
|
||||
pkl_path = v2_warp_pkl_path(cam_w, cam_h, buffer_length)
|
||||
with open(pkl_path, "wb") as f:
|
||||
pickle.dump(update_img_jit, f)
|
||||
print(f" Saved to {pkl_path}")
|
||||
|
||||
jit = pickle.load(open(pkl_path, "rb"))
|
||||
verify_frame = np.random.default_rng(0).integers(0, 256, yuv_size, dtype=np.uint8)
|
||||
verify_big_frame = np.random.default_rng(1).integers(0, 256, yuv_size, dtype=np.uint8)
|
||||
fresh_inputs = [
|
||||
Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize(),
|
||||
Tensor.from_blob(verify_frame.ctypes.data, (yuv_size,), dtype='uint8').realize(),
|
||||
Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY'),
|
||||
Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize(),
|
||||
Tensor.from_blob(verify_big_frame.ctypes.data, (yuv_size,), dtype='uint8').realize(),
|
||||
Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY'),
|
||||
]
|
||||
jit(*fresh_inputs)
|
||||
|
||||
|
||||
class Warp:
|
||||
def __init__(self, buffer_length=2):
|
||||
self.buffer_length = buffer_length
|
||||
self.img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
|
||||
|
||||
self.jit_cache = {}
|
||||
self.full_buffers = {k: Tensor.zeros(self.img_buffer_shape, dtype='uint8').contiguous().realize() for k in ['img', 'big_img']}
|
||||
self._blob_cache: dict[int, Tensor] = {}
|
||||
self._nv12_cache: dict[tuple[int, int], int] = {}
|
||||
self.transforms_np = {k: np.zeros((3, 3), dtype=np.float32) for k in ['img', 'big_img']}
|
||||
self.transforms = {k: Tensor(v, device='NPY').realize() for k, v in self.transforms_np.items()}
|
||||
|
||||
def process(self, bufs, transforms):
|
||||
if not bufs:
|
||||
return {}
|
||||
road = next(n for n in bufs if 'big' not in n)
|
||||
wide = next(n for n in bufs if 'big' in n)
|
||||
cam_w, cam_h = bufs[road].width, bufs[road].height
|
||||
key = (cam_w, cam_h)
|
||||
|
||||
if key not in self.jit_cache:
|
||||
v2_pkl = v2_warp_pkl_path(cam_w, cam_h, self.buffer_length)
|
||||
if v2_pkl.exists():
|
||||
with open(v2_pkl, 'rb') as f:
|
||||
self.jit_cache[key] = pickle.load(f)
|
||||
elif self.buffer_length == UPSTREAM_BUFFER_LENGTH:
|
||||
upstream_pkl = warp_pkl_path(cam_w, cam_h)
|
||||
if upstream_pkl.exists():
|
||||
with open(upstream_pkl, 'rb') as f:
|
||||
self.jit_cache[key] = pickle.load(f)
|
||||
if key not in self.jit_cache:
|
||||
frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
|
||||
update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
|
||||
self.jit_cache[key] = TinyJit(update_both_imgs, prune=True)
|
||||
|
||||
if key not in self._nv12_cache:
|
||||
self._nv12_cache[key] = get_nv12_info(cam_w, cam_h)[3]
|
||||
yuv_size = self._nv12_cache[key]
|
||||
|
||||
road_ptr = bufs[road].data.ctypes.data
|
||||
wide_ptr = bufs[wide].data.ctypes.data
|
||||
if road_ptr not in self._blob_cache:
|
||||
self._blob_cache[road_ptr] = Tensor.from_blob(road_ptr, (yuv_size,), dtype='uint8')
|
||||
if wide_ptr not in self._blob_cache:
|
||||
self._blob_cache[wide_ptr] = Tensor.from_blob(wide_ptr, (yuv_size,), dtype='uint8')
|
||||
road_blob = self._blob_cache[road_ptr]
|
||||
wide_blob = self._blob_cache[wide_ptr] if wide_ptr != road_ptr else Tensor.from_blob(wide_ptr, (yuv_size,), dtype='uint8')
|
||||
np.copyto(self.transforms_np['img'], transforms[road].reshape(3, 3))
|
||||
np.copyto(self.transforms_np['big_img'], transforms[wide].reshape(3, 3))
|
||||
|
||||
Device.default.synchronize()
|
||||
res = self.jit_cache[key](
|
||||
self.full_buffers['img'], road_blob, self.transforms['img'],
|
||||
self.full_buffers['big_img'], wide_blob, self.transforms['big_img'],
|
||||
)
|
||||
out_road = res[0].realize()
|
||||
out_wide = res[1].realize()
|
||||
|
||||
return {road: out_road, wide: out_wide}
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
for cam_w, cam_h in CAMERA_CONFIGS:
|
||||
for bl in [2, 5]:
|
||||
compile_v2_warp(cam_w, cam_h, bl)
|
||||
@@ -6,11 +6,12 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import time
|
||||
|
||||
import os
|
||||
import requests
|
||||
from requests.exceptions import (SSLError, RequestException, HTTPError)
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
from openpilot.sunnypilot.models.helpers import is_bundle_version_compatible
|
||||
|
||||
from openpilot.cereal import custom
|
||||
@@ -26,11 +27,35 @@ class ModelParser:
|
||||
download_uri.sha256 = download_uri_data.get("sha256")
|
||||
return download_uri
|
||||
|
||||
@staticmethod
|
||||
def _parse_chunk(chunk_data) -> custom.ModelManagerSP.Chunk:
|
||||
chunk = custom.ModelManagerSP.Chunk()
|
||||
chunk.fileName = chunk_data.get("file_name")
|
||||
chunk.sha256 = chunk_data.get("sha256")
|
||||
return chunk
|
||||
|
||||
@staticmethod
|
||||
def _parse_artifact(artifact_data) -> custom.ModelManagerSP.Artifact:
|
||||
artifact = custom.ModelManagerSP.Artifact()
|
||||
artifact.fileName = artifact_data.get("file_name")
|
||||
artifact.downloadUri = ModelParser._parse_download_uri(artifact_data.get("download_uri", {}))
|
||||
|
||||
if "chunks" in artifact_data:
|
||||
artifact.chunks = [ModelParser._parse_chunk(chunk_data) for chunk_data in artifact_data["chunks"]]
|
||||
|
||||
try:
|
||||
model_dir = Paths.model_root()
|
||||
os.makedirs(model_dir, exist_ok=True)
|
||||
manifest_path = os.path.join(model_dir, f"{artifact.fileName}.chunkmanifest")
|
||||
num_chunks = str(len(artifact.chunks))
|
||||
|
||||
if not os.path.exists(manifest_path) or open(manifest_path).read().strip() != num_chunks:
|
||||
with open(manifest_path, "w") as f:
|
||||
f.write(num_chunks)
|
||||
cloudlog.info(f"Wrote chunk manifest for {artifact.fileName}: {num_chunks} chunks")
|
||||
except Exception as e:
|
||||
cloudlog.warning(f"Failed to write chunk manifest for {artifact.fileName}: {e}")
|
||||
|
||||
return artifact
|
||||
|
||||
@staticmethod
|
||||
@@ -39,8 +64,6 @@ class ModelParser:
|
||||
|
||||
model.type = model_data.get("type")
|
||||
model.artifact = ModelParser._parse_artifact(model_data.get("artifact", {}))
|
||||
if metadata := model_data.get("metadata"):
|
||||
model.metadata = ModelParser._parse_artifact(metadata)
|
||||
return model
|
||||
|
||||
@staticmethod
|
||||
@@ -116,7 +139,7 @@ class ModelCache:
|
||||
|
||||
class ModelFetcher:
|
||||
"""Handles fetching and caching of model data from remote source"""
|
||||
MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v17.json"
|
||||
MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v18.json"
|
||||
|
||||
def __init__(self, params: Params):
|
||||
self.params = params
|
||||
@@ -184,4 +207,5 @@ if __name__ == "__main__":
|
||||
# Print artifact details
|
||||
print(f"Artifact: {model.artifact.fileName}, Download URI: {model.artifact.downloadUri.uri}")
|
||||
# Print metadata details
|
||||
print(f"Metadata: {model.metadata.fileName}, Download URI: {model.metadata.downloadUri.uri}")
|
||||
if model.artifact.chunks:
|
||||
print(f"Contains {len(model.artifact.chunks)} chunks.")
|
||||
|
||||
@@ -18,7 +18,7 @@ from openpilot.sunnypilot.models.constants import Meta, MetaSimPose, MetaTombRai
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
|
||||
# SET ME TO THE EXACT JSON VERSION WE SET IN SUNNYPILOT_MODELS REPO
|
||||
REQUIRED_JSON_VERSION = 15
|
||||
REQUIRED_JSON_VERSION = 16
|
||||
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
|
||||
@@ -56,12 +56,20 @@ def is_bundle_version_compatible(bundle: dict) -> bool:
|
||||
|
||||
def _bundle_artifacts(bundle: custom.ModelManagerSP.ModelBundle) -> list[tuple[str, str]]:
|
||||
artifacts = []
|
||||
from openpilot.common.file_chunker import get_chunk_name
|
||||
for model in getattr(bundle, 'models', []) or []:
|
||||
for artifact in (getattr(model, 'artifact', None), getattr(model, 'metadata', None)):
|
||||
if artifact and getattr(artifact, 'fileName', None) and getattr(artifact, 'downloadUri', None):
|
||||
sha256 = getattr(artifact.downloadUri, 'sha256', None)
|
||||
if sha256:
|
||||
artifacts.append((artifact.fileName, sha256))
|
||||
for artifact in (getattr(model, 'artifact', None),):
|
||||
if artifact and getattr(artifact, 'fileName', None):
|
||||
if len(artifact.chunks) > 0:
|
||||
for i, chunk in enumerate(artifact.chunks):
|
||||
chunk_name = get_chunk_name(artifact.fileName, i, len(artifact.chunks))
|
||||
if getattr(chunk, 'sha256', None):
|
||||
artifacts.append((chunk_name, chunk.sha256))
|
||||
else:
|
||||
if getattr(artifact, 'downloadUri', None):
|
||||
sha256 = getattr(artifact.downloadUri, 'sha256', None)
|
||||
if sha256:
|
||||
artifacts.append((artifact.fileName, sha256))
|
||||
return artifacts
|
||||
|
||||
|
||||
@@ -156,8 +164,7 @@ def _get_model():
|
||||
|
||||
|
||||
def load_metadata():
|
||||
model = _get_model()
|
||||
metadata_path = f"{CUSTOM_MODEL_PATH}/{model.metadata.fileName}" if model else METADATA_PATH
|
||||
metadata_path = METADATA_PATH
|
||||
|
||||
with open(metadata_path, 'rb') as f:
|
||||
return pickle.load(f)
|
||||
|
||||
@@ -38,11 +38,11 @@ class ModelManagerSP:
|
||||
if not self.selected_bundle:
|
||||
return
|
||||
for model in self.selected_bundle.models:
|
||||
for artifact in (model.artifact, model.metadata):
|
||||
if artifact is not source_artifact and artifact.fileName == source_artifact.fileName:
|
||||
artifact.downloadProgress.status = source_artifact.downloadProgress.status
|
||||
artifact.downloadProgress.progress = source_artifact.downloadProgress.progress
|
||||
artifact.downloadProgress.eta = source_artifact.downloadProgress.eta
|
||||
artifact = model.artifact
|
||||
if artifact is not source_artifact and artifact.fileName == source_artifact.fileName:
|
||||
artifact.downloadProgress.status = source_artifact.downloadProgress.status
|
||||
artifact.downloadProgress.progress = source_artifact.downloadProgress.progress
|
||||
artifact.downloadProgress.eta = source_artifact.downloadProgress.eta
|
||||
|
||||
def _calculate_eta(self, filename: str, progress: float) -> int:
|
||||
"""Calculate ETA based on elapsed time and current progress"""
|
||||
@@ -89,20 +89,16 @@ class ModelManagerSP:
|
||||
del self._download_start_times[model.fileName]
|
||||
|
||||
async def _download_chunked(self, base_url: str, base_path: str, artifact) -> None:
|
||||
from openpilot.common.file_chunker import get_manifest_path, get_chunk_name
|
||||
manifest_url = get_manifest_path(base_url)
|
||||
from openpilot.common.file_chunker import get_chunk_name, get_manifest_path
|
||||
|
||||
num_chunks = len(artifact.chunks)
|
||||
if num_chunks == 0:
|
||||
raise ValueError("No chunks defined in artifact")
|
||||
|
||||
manifest_path = get_manifest_path(base_path)
|
||||
|
||||
async with aiohttp.ClientSession() as session:
|
||||
async with session.get(manifest_url) as resp:
|
||||
if resp.status == 404:
|
||||
raise FileNotFoundError
|
||||
resp.raise_for_status()
|
||||
num_chunks = int((await resp.read()).strip())
|
||||
|
||||
self._download_start_times[artifact.fileName] = time.monotonic()
|
||||
|
||||
for i in range(num_chunks):
|
||||
for i, _ in enumerate(artifact.chunks):
|
||||
chunk_url = get_chunk_name(base_url, i, num_chunks)
|
||||
chunk_path = get_chunk_name(base_path, i, num_chunks)
|
||||
chunk_downloaded = 0
|
||||
@@ -117,7 +113,7 @@ class ModelManagerSP:
|
||||
if self.params.get("ModelManager_DownloadIndex") is None:
|
||||
raise Exception("Download cancelled")
|
||||
intra = chunk_downloaded / max(chunk_size, 1)
|
||||
progress = min(99, (i + intra) / num_chunks * 100)
|
||||
progress = min(99.0, ((i + intra) / num_chunks) * 100)
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.downloading
|
||||
artifact.downloadProgress.progress = progress
|
||||
artifact.downloadProgress.eta = self._calculate_eta(artifact.fileName, progress)
|
||||
@@ -140,7 +136,22 @@ class ModelManagerSP:
|
||||
full_path = os.path.join(destination_path, filename)
|
||||
|
||||
try:
|
||||
if await verify_file(full_path, expected_hash):
|
||||
is_cached = False
|
||||
if len(artifact.chunks) > 0:
|
||||
from openpilot.common.file_chunker import get_chunk_name
|
||||
chunks_valid = True
|
||||
for i, chunk in enumerate(artifact.chunks):
|
||||
chunk_path = get_chunk_name(full_path, i, len(artifact.chunks))
|
||||
if not await verify_file(chunk_path, chunk.sha256):
|
||||
chunks_valid = False
|
||||
break
|
||||
if chunks_valid and len(artifact.chunks) > 0:
|
||||
is_cached = True
|
||||
else:
|
||||
if await verify_file(full_path, expected_hash):
|
||||
is_cached = True
|
||||
|
||||
if is_cached:
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.cached
|
||||
artifact.downloadProgress.progress = 100
|
||||
artifact.downloadProgress.eta = 0
|
||||
@@ -148,13 +159,17 @@ class ModelManagerSP:
|
||||
self._report_status()
|
||||
return
|
||||
|
||||
try:
|
||||
if len(artifact.chunks) > 0:
|
||||
await self._download_chunked(url, full_path, artifact)
|
||||
except (FileNotFoundError, aiohttp.ClientResponseError):
|
||||
from openpilot.common.file_chunker import get_chunk_name
|
||||
for i, chunk in enumerate(artifact.chunks):
|
||||
chunk_path = get_chunk_name(full_path, i, len(artifact.chunks))
|
||||
if not await verify_file(chunk_path, chunk.sha256):
|
||||
raise ValueError(f"Hash validation failed for chunk {i+1} of {filename}")
|
||||
else:
|
||||
await self._download_file(url, full_path, artifact)
|
||||
|
||||
if not await verify_file(full_path, expected_hash):
|
||||
raise ValueError(f"Hash validation failed for {filename}")
|
||||
if not await verify_file(full_path, expected_hash):
|
||||
raise ValueError(f"Hash validation failed for {filename}")
|
||||
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.downloaded
|
||||
artifact.downloadProgress.progress = 100
|
||||
@@ -170,18 +185,15 @@ class ModelManagerSP:
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.failed
|
||||
artifact.downloadProgress.eta = 0
|
||||
self._sync_artifact_progress(artifact)
|
||||
self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.failed
|
||||
if self.selected_bundle:
|
||||
self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.failed
|
||||
self._report_status()
|
||||
self._download_start_times.pop(artifact.fileName, None)
|
||||
raise
|
||||
|
||||
async def _process_model(self, model, destination_path: str) -> None:
|
||||
"""Processes a single model download including verification"""
|
||||
model_artifact = model.artifact
|
||||
metadata_artifact = model.metadata
|
||||
|
||||
await self._process_artifact(metadata_artifact, destination_path)
|
||||
await self._process_artifact(model_artifact, destination_path)
|
||||
await self._process_artifact(model.artifact, destination_path)
|
||||
|
||||
def _report_status(self) -> None:
|
||||
"""Reports current status through messaging system"""
|
||||
@@ -205,16 +217,16 @@ class ModelManagerSP:
|
||||
try:
|
||||
seen_artifacts: set[str] = set()
|
||||
for model in self.selected_bundle.models:
|
||||
for artifact in (model.metadata, model.artifact):
|
||||
if not artifact.fileName:
|
||||
continue
|
||||
if artifact.fileName in seen_artifacts:
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.cached
|
||||
artifact.downloadProgress.progress = 100
|
||||
artifact.downloadProgress.eta = 0
|
||||
else:
|
||||
seen_artifacts.add(artifact.fileName)
|
||||
await self._process_artifact(artifact, destination_path)
|
||||
artifact = model.artifact
|
||||
if not artifact.fileName:
|
||||
continue
|
||||
if artifact.fileName in seen_artifacts:
|
||||
artifact.downloadProgress.status = custom.ModelManagerSP.DownloadStatus.cached
|
||||
artifact.downloadProgress.progress = 100
|
||||
artifact.downloadProgress.eta = 0
|
||||
else:
|
||||
seen_artifacts.add(artifact.fileName)
|
||||
await self._process_artifact(artifact, destination_path)
|
||||
|
||||
self.active_bundle = self.selected_bundle
|
||||
self.active_bundle.status = custom.ModelManagerSP.DownloadStatus.downloaded
|
||||
@@ -275,8 +287,6 @@ class ModelManagerSP:
|
||||
for model in self.active_bundle.models:
|
||||
if hasattr(model, 'artifact') and model.artifact.fileName:
|
||||
active_files.append(model.artifact.fileName)
|
||||
if hasattr(model, 'metadata') and model.metadata.fileName:
|
||||
active_files.append(model.metadata.fileName)
|
||||
|
||||
# Remove all files except active ones (including their chunk files)
|
||||
model_dir = Paths.model_root()
|
||||
|
||||
@@ -1,28 +0,0 @@
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModelRunner
|
||||
from openpilot.sunnypilot.models.runners.tinygrad.tinygrad_runner import TinygradRunner, TinygradSplitRunner
|
||||
from openpilot.sunnypilot.models.runners.constants import ModelType
|
||||
|
||||
|
||||
def get_model_runner() -> ModelRunner:
|
||||
"""
|
||||
Factory function to create and return the appropriate ModelRunner instance.
|
||||
|
||||
Selects TinygradRunner, choosing TinygradSplitRunner if separate vision/policy
|
||||
models are detected in the active bundle.
|
||||
|
||||
:return: An instance of a ModelRunner subclass (ONNXRunner, TinygradRunner, or TinygradSplitRunner).
|
||||
"""
|
||||
bundle = get_active_bundle()
|
||||
if bundle and bundle.models:
|
||||
model_types = {m.type.raw for m in bundle.models}
|
||||
# Check if the bundle uses separate vision and policy models (legacy or new split format)
|
||||
split_types = {ModelType.vision, ModelType.policy, ModelType.offPolicy, ModelType.onPolicy}
|
||||
if model_types & split_types:
|
||||
return TinygradSplitRunner()
|
||||
# Otherwise, assume a single model (likely supercombo)
|
||||
if bundle.models:
|
||||
return TinygradRunner(bundle.models[0].type.raw)
|
||||
|
||||
# Default fallback to TinygradRunner with the supercombo type if bundle info is missing/incomplete
|
||||
return TinygradRunner(ModelType.supercombo)
|
||||
@@ -1,174 +0,0 @@
|
||||
from abc import abstractmethod, ABC
|
||||
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.sunnypilot.models.runners.constants import NumpyDict, ShapeDict, Model, SliceDict, SEND_RAW_PRED
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
import pickle
|
||||
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
|
||||
|
||||
class ModelData:
|
||||
"""
|
||||
Stores metadata and configuration for a specific machine learning model.
|
||||
|
||||
This class loads model metadata (like input shapes and output slices)
|
||||
from a pickle file associated with a model instance.
|
||||
|
||||
:param model: The machine learning model object containing metadata.
|
||||
"""
|
||||
def __init__(self, model: Model):
|
||||
self.model = model
|
||||
self.metadata = model.metadata
|
||||
self.input_shapes: ShapeDict = {}
|
||||
self.output_slices: SliceDict = {}
|
||||
if self.metadata:
|
||||
self._load_metadata()
|
||||
|
||||
def _load_metadata(self) -> None:
|
||||
"""Loads input shapes and output slices from the model's metadata pickle file."""
|
||||
metadata_path = f"{CUSTOM_MODEL_PATH}/{self.metadata.fileName}"
|
||||
with open(metadata_path, 'rb') as f:
|
||||
model_metadata = pickle.load(f)
|
||||
self.input_shapes = model_metadata.get('input_shapes', {})
|
||||
self.output_slices = model_metadata.get('output_slices', {})
|
||||
|
||||
|
||||
class ModularRunner(ABC):
|
||||
"""
|
||||
Represents a modular runner for handling and slicing model outputs.
|
||||
|
||||
This abstract base class is designed to provide an interface for modular
|
||||
parsing and processing of model outputs. Classes inheriting from it must
|
||||
implement the specified abstract methods, defining how model outputs
|
||||
should be handled and stored. The primary goal is to enable structured
|
||||
parsing of outputs through a dictionary-based method mapping.
|
||||
|
||||
:ivar parser_method_dict: Mapping dictionary containing parser methods
|
||||
for handling specific types of outputs.
|
||||
:type parser_method_dict: dict
|
||||
"""
|
||||
|
||||
@property
|
||||
@abstractmethod
|
||||
def parser_method_dict(self) -> dict:
|
||||
pass
|
||||
|
||||
@parser_method_dict.setter
|
||||
@abstractmethod
|
||||
def parser_method_dict(self, value: dict) -> None:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def _slice_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
pass
|
||||
|
||||
|
||||
class ModelRunner(ModularRunner):
|
||||
"""
|
||||
Abstract base class for managing and executing machine learning models.
|
||||
|
||||
Provides a common interface for loading models, preparing inputs, running
|
||||
inference, and slicing/parsing outputs based on model metadata. Derived
|
||||
classes implement the specifics of input preparation and model execution
|
||||
for different frameworks (e.g., Tinygrad, ONNX).
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
"""Initializes the model runner, loading the active model bundle."""
|
||||
self.is_20hz: bool | None = None
|
||||
self.is_20hz_3d: bool | None = None
|
||||
self.models: dict[int, ModelData] = {}
|
||||
self._model_data: ModelData | None = None # Active model data for current operation
|
||||
self._parser_method_dict: dict = {}
|
||||
self.inputs: dict = {}
|
||||
self._parser = None
|
||||
self._load_models()
|
||||
self._constants = None
|
||||
|
||||
@property
|
||||
def constants(self):
|
||||
return self._constants
|
||||
|
||||
@property
|
||||
def parser_method_dict(self) -> dict:
|
||||
"""Returns the dictionary mapping model types to their respective parsing methods."""
|
||||
return self._parser_method_dict
|
||||
|
||||
@parser_method_dict.setter
|
||||
def parser_method_dict(self, value: dict) -> None:
|
||||
"""Sets the dictionary mapping model types to their respective parsing methods."""
|
||||
self._parser_method_dict = value
|
||||
|
||||
def _load_models(self) -> None:
|
||||
"""Loads the active model bundle configuration and sets up ModelData."""
|
||||
bundle = get_active_bundle()
|
||||
if not bundle:
|
||||
raise ValueError("No active model bundle found, why are we being executed?")
|
||||
|
||||
self.models = {model.type.raw: ModelData(model) for model in bundle.models}
|
||||
self.is_20hz = bundle.is20hz
|
||||
self.is_20hz_3d = False
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
"""Returns the input shapes for the currently active model."""
|
||||
if self._model_data:
|
||||
return self._model_data.input_shapes
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
"""Returns the output slices for the currently active model."""
|
||||
if self._model_data:
|
||||
return self._model_data.output_slices
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
"""Returns the list of vision input names from the input shapes."""
|
||||
if self._model_data:
|
||||
return list(self._model_data.input_shapes.keys())
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
@abstractmethod
|
||||
def prepare_inputs(self, numpy_inputs: NumpyDict) -> dict:
|
||||
"""
|
||||
Abstract method to prepare inputs for model inference.
|
||||
|
||||
:param numpy_inputs: Dictionary of numpy arrays for non-image inputs.
|
||||
:return: Dictionary of prepared inputs ready for the model.
|
||||
"""
|
||||
raise NotImplementedError
|
||||
|
||||
@abstractmethod
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""
|
||||
Abstract method to execute model inference with prepared inputs.
|
||||
|
||||
:return: Dictionary containing the model's raw output arrays.
|
||||
"""
|
||||
raise NotImplementedError
|
||||
|
||||
def _slice_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""
|
||||
Slices the raw model output array based on the output_slices metadata.
|
||||
|
||||
:param model_outputs: The raw numpy array output from the model.
|
||||
:return: A dictionary where keys are output names and values are sliced numpy arrays.
|
||||
"""
|
||||
if not self._model_data:
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
sliced_outputs = {k: model_outputs[np.newaxis, v] for k, v in self._model_data.output_slices.items()}
|
||||
if SEND_RAW_PRED:
|
||||
sliced_outputs['raw_pred'] = model_outputs.copy() # Optionally include the full raw output
|
||||
return sliced_outputs
|
||||
|
||||
def run_model(self) -> NumpyDict:
|
||||
"""
|
||||
Executes the model inference pipeline: runs the model and parses outputs.
|
||||
|
||||
:return: Dictionary containing the final parsed model outputs.
|
||||
"""
|
||||
return self._run_model() # Parsing is handled within specific runner implementations
|
||||
@@ -1,91 +0,0 @@
|
||||
import os
|
||||
from abc import ABC
|
||||
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
|
||||
from openpilot.sunnypilot.models.runners.constants import ModelType, NumpyDict
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModularRunner
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
|
||||
|
||||
class OffPolicyTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for off-policy models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the off-policy part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._off_policy_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.offPolicy] = self._parse_off_policy_outputs
|
||||
|
||||
def _parse_off_policy_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses off-policy model outputs using SplitParser."""
|
||||
result: NumpyDict = self._off_policy_parser.parse_policy_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
|
||||
class OnPolicyTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for on-policy models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the on-policy part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._on_policy_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.onPolicy] = self._parse_on_policy_outputs
|
||||
|
||||
def _parse_on_policy_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses on-policy model outputs using SplitParser."""
|
||||
result: NumpyDict = self._on_policy_parser.parse_policy_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
|
||||
class PolicyTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for policy-only models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the policy part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._policy_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.policy] = self._parse_policy_outputs
|
||||
|
||||
def _parse_policy_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses policy model outputs using SplitParser."""
|
||||
result: NumpyDict = self._policy_parser.parse_policy_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
class VisionTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for vision-only models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the vision part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._vision_parser = SplitParser()
|
||||
self.parser_method_dict[ModelType.vision] = self._parse_vision_outputs
|
||||
|
||||
def _parse_vision_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses vision model outputs using SplitParser."""
|
||||
result: NumpyDict = self._vision_parser.parse_vision_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
|
||||
class SupercomboTinygrad(ModularRunner, ABC):
|
||||
"""
|
||||
A TinygradRunner specialized for vision-only models.
|
||||
|
||||
Uses a SplitParser to handle outputs specific to the vision part of a split model setup.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._supercombo_parser = CombinedParser()
|
||||
self.parser_method_dict[ModelType.supercombo] = self._parse_supercombo_outputs
|
||||
|
||||
def _parse_supercombo_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses vision model outputs using SplitParser."""
|
||||
result: NumpyDict = self._supercombo_parser.parse_outputs(self._slice_outputs(model_outputs))
|
||||
return result
|
||||
@@ -1,179 +0,0 @@
|
||||
import pickle
|
||||
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.models.runners.constants import NumpyDict, ModelType, ShapeDict, CUSTOM_MODEL_PATH, SliceDict
|
||||
from openpilot.sunnypilot.models.runners.model_runner import ModelRunner
|
||||
from openpilot.sunnypilot.models.runners.tinygrad.model_types import PolicyTinygrad, VisionTinygrad, SupercomboTinygrad, OffPolicyTinygrad, OnPolicyTinygrad
|
||||
from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
|
||||
|
||||
from tinygrad.tensor import Tensor
|
||||
|
||||
|
||||
class TinygradRunner(ModelRunner, SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad):
|
||||
"""
|
||||
A ModelRunner implementation for executing Tinygrad models.
|
||||
|
||||
Handles loading Tinygrad model artifacts (.pkl), preparing inputs as Tinygrad
|
||||
Tensors (potentially using QCOM extensions on TICI), running inference,
|
||||
and parsing the outputs.
|
||||
|
||||
:param model_type: The type of model (e.g., supercombo) to load and run.
|
||||
"""
|
||||
def __init__(self, model_type: int = ModelType.supercombo):
|
||||
ModelRunner.__init__(self)
|
||||
SupercomboTinygrad.__init__(self)
|
||||
PolicyTinygrad.__init__(self)
|
||||
VisionTinygrad.__init__(self)
|
||||
OffPolicyTinygrad.__init__(self)
|
||||
OnPolicyTinygrad.__init__(self)
|
||||
self._constants = ModelConstants
|
||||
self._model_data = self.models.get(model_type)
|
||||
if not self._model_data or not self._model_data.model:
|
||||
raise ValueError(f"Model data for type {model_type} not available.")
|
||||
|
||||
artifact_filename = self._model_data.model.artifact.fileName
|
||||
assert artifact_filename.endswith('_tinygrad.pkl'), \
|
||||
f"Invalid model file {artifact_filename} for TinygradRunner"
|
||||
|
||||
model_pkl_path = f"{CUSTOM_MODEL_PATH}/{artifact_filename}"
|
||||
with open(model_pkl_path, "rb") as f:
|
||||
try:
|
||||
# Load the compiled Tinygrad model runner function
|
||||
self.model_run = pickle.load(f)
|
||||
except FileNotFoundError as e:
|
||||
# Provide a helpful error message if the model was built for a different platform
|
||||
assert "/dev/kgsl-3d0" not in str(e), "Model was built on C3 or C3X, but is being loaded on PC"
|
||||
raise
|
||||
|
||||
# Map input names to their required dtype and device from the loaded model
|
||||
self.input_to_dtype = {}
|
||||
self.input_to_device = {}
|
||||
for idx, name in enumerate(self.model_run.captured.expected_names):
|
||||
info = self.model_run.captured.expected_input_info[idx]
|
||||
self.input_to_dtype[name] = info[2] # dtype
|
||||
self.input_to_device[name] = info[3] # device
|
||||
self._policy_cached = False
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
"""Returns the list of vision input names from the input shapes."""
|
||||
return [name for name in self.input_shapes.keys() if 'img' in name]
|
||||
|
||||
|
||||
def prepare_policy_inputs(self, numpy_inputs: NumpyDict):
|
||||
if not self._policy_cached:
|
||||
for key, value in numpy_inputs.items():
|
||||
self.inputs[key] = Tensor(value, device='NPY').realize()
|
||||
self._policy_cached = True
|
||||
|
||||
def prepare_inputs(self, numpy_inputs: NumpyDict) -> dict:
|
||||
"""Prepares all vision and policy inputs for the model."""
|
||||
self.prepare_policy_inputs(numpy_inputs)
|
||||
for key in self.vision_input_names:
|
||||
if key in self.inputs:
|
||||
self.inputs[key] = self.inputs[key].cast(self.input_to_dtype[key])
|
||||
return self.inputs
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""Runs the Tinygrad model inference and parses the outputs."""
|
||||
outputs = self.model_run(**self.inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
|
||||
return self._parse_outputs(outputs)
|
||||
|
||||
def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
|
||||
"""Parses the raw model outputs using the standard Parser."""
|
||||
if self._model_data is None:
|
||||
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
|
||||
|
||||
result: NumpyDict = self.parser_method_dict[self._model_data.model.type.raw](model_outputs)
|
||||
return result
|
||||
|
||||
|
||||
class TinygradSplitRunner(ModelRunner):
|
||||
"""
|
||||
A ModelRunner that coordinates separate TinygradVisionRunner and TinygradPolicyRunner instances.
|
||||
|
||||
Manages the execution of split vision and policy models, combining their inputs and outputs.
|
||||
"""
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.is_20hz_3d = True
|
||||
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
|
||||
self._constants = SplitModelConstants
|
||||
|
||||
def _run_model(self) -> NumpyDict:
|
||||
"""Runs both vision and policy models and merges their parsed outputs."""
|
||||
vision_output = self.vision_runner.run_model()
|
||||
outputs = {**vision_output}
|
||||
|
||||
if self.policy_runner:
|
||||
policy_output = self.policy_runner.run_model()
|
||||
outputs.update(policy_output)
|
||||
|
||||
if self.off_policy_runner:
|
||||
off_policy_output = self.off_policy_runner.run_model()
|
||||
if self.on_policy_runner:
|
||||
off_policy_output.pop('plan', None)
|
||||
outputs.update(off_policy_output)
|
||||
|
||||
if self.on_policy_runner:
|
||||
on_policy_output = self.on_policy_runner.run_model()
|
||||
outputs.update(on_policy_output)
|
||||
|
||||
if 'planplus' in outputs and 'plan' in outputs:
|
||||
outputs['plan'] = outputs['plan'] + outputs['planplus']
|
||||
|
||||
return outputs
|
||||
|
||||
@property
|
||||
def vision_input_names(self) -> list[str]:
|
||||
"""Returns the list of vision input names from the vision runner."""
|
||||
return list(self.vision_runner.vision_input_names)
|
||||
|
||||
@property
|
||||
def input_shapes(self) -> ShapeDict:
|
||||
"""Returns the combined input shapes from both vision and policy models."""
|
||||
shapes = {**self.vision_runner.input_shapes}
|
||||
if self.policy_runner:
|
||||
shapes.update(self.policy_runner.input_shapes)
|
||||
if self.off_policy_runner:
|
||||
shapes.update(self.off_policy_runner.input_shapes)
|
||||
if self.on_policy_runner:
|
||||
shapes.update(self.on_policy_runner.input_shapes)
|
||||
return shapes
|
||||
|
||||
@property
|
||||
def output_slices(self) -> SliceDict:
|
||||
"""Returns the combined output slices from both vision and policy models."""
|
||||
slices = {**self.vision_runner.output_slices}
|
||||
if self.policy_runner:
|
||||
slices.update(self.policy_runner.output_slices)
|
||||
if self.off_policy_runner:
|
||||
slices.update(self.off_policy_runner.output_slices)
|
||||
if self.on_policy_runner:
|
||||
slices.update(self.on_policy_runner.output_slices)
|
||||
return slices
|
||||
|
||||
def prepare_inputs(self, numpy_inputs: NumpyDict) -> dict:
|
||||
"""Prepares inputs for both vision and policy models."""
|
||||
if self.policy_runner:
|
||||
self.policy_runner.prepare_policy_inputs(numpy_inputs)
|
||||
|
||||
for key in self.vision_input_names:
|
||||
if key in self.inputs:
|
||||
self.vision_runner.inputs[key] = self.inputs[key].cast(self.vision_runner.input_to_dtype[key])
|
||||
|
||||
inputs = {**self.vision_runner.inputs}
|
||||
if self.policy_runner:
|
||||
inputs.update(self.policy_runner.inputs)
|
||||
|
||||
if self.off_policy_runner:
|
||||
self.off_policy_runner.prepare_policy_inputs(numpy_inputs)
|
||||
inputs.update(self.off_policy_runner.inputs)
|
||||
if self.on_policy_runner:
|
||||
self.on_policy_runner.prepare_policy_inputs(numpy_inputs)
|
||||
inputs.update(self.on_policy_runner.inputs)
|
||||
return inputs
|
||||
@@ -43,6 +43,7 @@ class SplitModelConstants:
|
||||
LANE_LINES_WIDTH = 2
|
||||
ROAD_EDGES_WIDTH = 2
|
||||
PLAN_WIDTH = 15
|
||||
ACTION_WIDTH = 2
|
||||
DESIRE_PRED_WIDTH = 8
|
||||
LAT_PLANNER_SOLUTION_WIDTH = 4
|
||||
DESIRED_CURV_WIDTH = 1
|
||||
|
||||
@@ -109,8 +109,6 @@ def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
|
||||
def initialize_params(params) -> list[dict[str, Any]]:
|
||||
keys: list = []
|
||||
|
||||
keys.append("RadarTracks")
|
||||
|
||||
# hyundai
|
||||
keys.extend([
|
||||
"HyundaiLongitudinalTuning",
|
||||
@@ -125,6 +123,7 @@ def initialize_params(params) -> list[dict[str, Any]]:
|
||||
# tesla
|
||||
keys.extend([
|
||||
"TeslaCoopSteering",
|
||||
"TeslaMadsScreenButton",
|
||||
])
|
||||
|
||||
# toyota
|
||||
|
||||
@@ -151,8 +151,8 @@ class SmartCruiseControlMap:
|
||||
a = 0.5 * TARGET_JERK
|
||||
b = self.a_ego
|
||||
c = self.v_ego - tv
|
||||
t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
|
||||
t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
|
||||
t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / (2 * a)
|
||||
t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / (2 * a)
|
||||
if not isinstance(t_a, complex) and t_a > 0:
|
||||
t = t_a
|
||||
else:
|
||||
|
||||
+18
-1
@@ -4,13 +4,17 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import json
|
||||
import math
|
||||
import platform
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import R, SmartCruiseControlMap
|
||||
|
||||
MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
|
||||
|
||||
@@ -55,4 +59,17 @@ class TestSmartCruiseControlMap:
|
||||
self.scc_m.update(True, False, 0., 0., 0.)
|
||||
assert self.scc_m.state == VisionState.enabled
|
||||
|
||||
def test_moderate_curve(self):
|
||||
# Regression: `... / 2 * a` parsed as `(.../2)*a` instead of `.../(2*a)`,
|
||||
# making max_d ~11x too small so the moderate-curve branch never tripped.
|
||||
# v_ego=25, a_ego=0, tv=24: fixed max_d≈45m vs buggy ≈4m at a 40m waypoint.
|
||||
waypoint_lon_deg = (40.0 / R) * (180.0 / math.pi)
|
||||
self.mem_params.put("LastGPSPosition", json.dumps({"latitude": 0.0, "longitude": 0.0}), block=True)
|
||||
self.mem_params.put("MapTargetVelocities",
|
||||
json.dumps([{"latitude": 0.0, "longitude": waypoint_lon_deg, "velocity": 24.0}]), block=True)
|
||||
|
||||
self.scc_m.update(True, False, 25.0, 0.0, 30.0)
|
||||
|
||||
assert self.scc_m.v_target == pytest.approx(24.0)
|
||||
|
||||
# TODO-SP: mock data from modelV2 to test other states
|
||||
|
||||
@@ -91,7 +91,7 @@ class SpeedLimitAssist:
|
||||
|
||||
self._plus_hold = 0.
|
||||
self._minus_hold = 0.
|
||||
self._last_carstate_ts = 0.
|
||||
self._release_toggle_prev = 0
|
||||
|
||||
# TODO-SP: SLA's own output_a_target for planner
|
||||
# Solution functions mapped to respective states
|
||||
@@ -146,16 +146,16 @@ class SpeedLimitAssist:
|
||||
set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
|
||||
self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
|
||||
|
||||
def update_car_state(self, CS: car.CarState) -> None:
|
||||
def update_buttons(self, release_toggle: int) -> None:
|
||||
released = self._release_toggle_prev ^ release_toggle
|
||||
self._release_toggle_prev = release_toggle
|
||||
if not released:
|
||||
return
|
||||
now = time.monotonic()
|
||||
self._last_carstate_ts = now
|
||||
|
||||
for b in CS.buttonEvents:
|
||||
if not b.pressed:
|
||||
if b.type in CRUISE_BUTTONS_PLUS:
|
||||
self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
elif b.type in CRUISE_BUTTONS_MINUS:
|
||||
self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
if any((released >> b) & 1 for b in CRUISE_BUTTONS_PLUS):
|
||||
self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
if any((released >> b) & 1 for b in CRUISE_BUTTONS_MINUS):
|
||||
self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
|
||||
def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
|
||||
now = time.monotonic()
|
||||
|
||||
+91
-1
@@ -5,11 +5,14 @@ This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import time
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.rivian.values import CAR as RIVIAN
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car.tesla.values import CAR as TESLA
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.common.constants import CV
|
||||
@@ -21,9 +24,13 @@ from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfac
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
|
||||
PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
|
||||
PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES, CRUISE_BUTTON_CONFIRM_HOLD
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
|
||||
|
||||
ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
|
||||
@@ -276,3 +283,86 @@ class TestSpeedLimitAssist:
|
||||
assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
|
||||
elif initial_state in ACTIVE_STATES:
|
||||
assert self.sla.state in ACTIVE_STATES
|
||||
|
||||
|
||||
class TestButtonStateTrackerSLAIntegration:
|
||||
|
||||
def setup_method(self, method):
|
||||
self.tracker = ButtonStateTracker()
|
||||
self.params = Params()
|
||||
self.params.put("IsReleaseSpBranch", True, block=True)
|
||||
self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
|
||||
self.params.put_bool("IsMetric", False, block=True)
|
||||
self.params.put("SpeedLimitOffsetType", 0, block=True)
|
||||
self.params.put("SpeedLimitValueOffset", 0, block=True)
|
||||
|
||||
CarInterface = interfaces[DEFAULT_CAR]
|
||||
CP = CarInterface.get_non_essential_params(DEFAULT_CAR)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, DEFAULT_CAR)
|
||||
self.sla = SpeedLimitAssist(CP, CP_SP)
|
||||
|
||||
def _make_cs(self, events=None) -> car.CarState:
|
||||
CS = car.CarState()
|
||||
CS.buttonEvents = events or []
|
||||
return CS
|
||||
|
||||
def _run_ctrl_frames(self, frames: list[car.CarState]) -> None:
|
||||
for cs in frames:
|
||||
self.tracker.update(cs)
|
||||
|
||||
def test_button_confirm_via_tracker(self) -> None:
|
||||
self._run_ctrl_frames([
|
||||
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]),
|
||||
self._make_cs(),
|
||||
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]),
|
||||
self._make_cs(),
|
||||
self._make_cs(),
|
||||
])
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
assert self.sla._get_button_release(req_plus=True, req_minus=False)
|
||||
|
||||
def test_rapid_press_release_between_polls(self) -> None:
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
|
||||
self._run_ctrl_frames([
|
||||
self._make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=True)]),
|
||||
self._make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=False)]),
|
||||
self._make_cs(),
|
||||
self._make_cs(),
|
||||
self._make_cs(),
|
||||
])
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
assert self.sla._get_button_release(req_plus=False, req_minus=True)
|
||||
|
||||
def test_multiple_releases_between_polls(self) -> None:
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
|
||||
self._run_ctrl_frames([
|
||||
self._make_cs([
|
||||
ButtonEvent(type=ButtonType.accelCruise, pressed=True),
|
||||
ButtonEvent(type=ButtonType.decelCruise, pressed=True),
|
||||
]),
|
||||
self._make_cs([
|
||||
ButtonEvent(type=ButtonType.accelCruise, pressed=False),
|
||||
ButtonEvent(type=ButtonType.decelCruise, pressed=False),
|
||||
]),
|
||||
])
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
assert self.sla._get_button_release(req_plus=True, req_minus=False)
|
||||
assert self.sla._get_button_release(req_plus=False, req_minus=True)
|
||||
|
||||
def test_no_false_positive_same_toggle(self) -> None:
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
assert not self.sla._get_button_release(req_plus=True, req_minus=False)
|
||||
assert not self.sla._get_button_release(req_plus=False, req_minus=True)
|
||||
|
||||
def test_button_confirm_expires(self) -> None:
|
||||
self._run_ctrl_frames([
|
||||
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]),
|
||||
self._make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]),
|
||||
])
|
||||
self.sla.update_buttons(self.tracker.release_toggle)
|
||||
time.sleep(CRUISE_BUTTON_CONFIRM_HOLD + 0.1)
|
||||
assert not self.sla._get_button_release(req_plus=True, req_minus=False)
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from opendbc.car import structs
|
||||
|
||||
|
||||
class ButtonStateTracker:
|
||||
def __init__(self) -> None:
|
||||
self.pressed: int = 0
|
||||
self.release_toggle: int = 0
|
||||
|
||||
def update(self, CS: structs.CarState) -> None:
|
||||
for b in CS.buttonEvents:
|
||||
bit = 1 << b.type.raw
|
||||
if b.pressed:
|
||||
self.pressed |= bit
|
||||
else:
|
||||
self.pressed &= ~bit
|
||||
self.release_toggle ^= bit
|
||||
|
||||
def publish(self, ss_sp) -> None:
|
||||
ss_sp.buttonsPressed = self.pressed
|
||||
ss_sp.buttonsReleaseToggle = self.release_toggle
|
||||
@@ -0,0 +1,67 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
|
||||
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class TestButtonStateTracker:
|
||||
def setup_method(self) -> None:
|
||||
self.tracker = ButtonStateTracker()
|
||||
|
||||
def make_cs(self, events: list) -> car.CarState:
|
||||
CS = car.CarState()
|
||||
CS.buttonEvents = events
|
||||
return CS
|
||||
|
||||
def test_initial_state(self) -> None:
|
||||
assert self.tracker.pressed == 0
|
||||
assert self.tracker.release_toggle == 0
|
||||
|
||||
def test_press_sets_bit(self) -> None:
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
|
||||
assert self.tracker.pressed == (1 << ButtonType.accelCruise)
|
||||
assert self.tracker.release_toggle == 0
|
||||
|
||||
def test_release_clears_and_toggles(self) -> None:
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
|
||||
assert self.tracker.pressed == 0
|
||||
assert self.tracker.release_toggle == (1 << ButtonType.accelCruise)
|
||||
|
||||
def test_multiple_buttons(self) -> None:
|
||||
self.tracker.update(self.make_cs([
|
||||
ButtonEvent(type=ButtonType.accelCruise, pressed=True),
|
||||
ButtonEvent(type=ButtonType.decelCruise, pressed=True),
|
||||
]))
|
||||
assert self.tracker.pressed == (1 << ButtonType.accelCruise) | (1 << ButtonType.decelCruise)
|
||||
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
|
||||
assert self.tracker.pressed == (1 << ButtonType.decelCruise)
|
||||
assert self.tracker.release_toggle == (1 << ButtonType.accelCruise)
|
||||
|
||||
def test_release_toggle_flips(self) -> None:
|
||||
for _ in range(2):
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=True)]))
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)]))
|
||||
assert self.tracker.release_toggle == 0
|
||||
|
||||
def test_publish(self) -> None:
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=True)]))
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.decelCruise, pressed=True)]))
|
||||
self.tracker.update(self.make_cs([ButtonEvent(type=ButtonType.accelCruise, pressed=False)]))
|
||||
|
||||
class MockSP:
|
||||
buttonsPressed = 0
|
||||
buttonsReleaseToggle = 0
|
||||
|
||||
sp = MockSP()
|
||||
self.tracker.publish(sp)
|
||||
assert sp.buttonsPressed == self.tracker.pressed
|
||||
assert sp.buttonsReleaseToggle == self.tracker.release_toggle
|
||||
@@ -2161,6 +2161,42 @@
|
||||
"type": "offroad_only"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "TeslaMadsScreenButton",
|
||||
"widget": "multiple_button",
|
||||
"title": "MADS Screen Activation",
|
||||
"description": "Use a multi-finger press on the infotainment screen to toggle MADS. This allows the use of full MADS functionality when enabled. Selecting a higher finger count may reduce accidental activations. Note: Setting this to Off will reset your MADS settings to default.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Off"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "3-Finger"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "4-Finger"
|
||||
},
|
||||
{
|
||||
"value": 3,
|
||||
"label": "5-Finger"
|
||||
}
|
||||
],
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "tesla_has_vehicle_bus",
|
||||
"equals": true
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "offroad_only"
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
},
|
||||
|
||||
@@ -56,6 +56,28 @@ sections:
|
||||
title: Cooperative Steering (Beta)
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- key: TeslaMadsScreenButton
|
||||
widget: multiple_button
|
||||
title: MADS Screen Activation
|
||||
description: 'Use a multi-finger press on the infotainment screen to toggle MADS.
|
||||
This allows the use of full MADS functionality when enabled. Selecting a higher
|
||||
finger count may reduce accidental activations. Note: Setting this to Off will
|
||||
reset your MADS settings to default.'
|
||||
options:
|
||||
- value: 0
|
||||
label: 'Off'
|
||||
- value: 1
|
||||
label: 3-Finger
|
||||
- value: 2
|
||||
label: 4-Finger
|
||||
- value: 3
|
||||
label: 5-Finger
|
||||
visibility:
|
||||
- type: capability
|
||||
field: tesla_has_vehicle_bus
|
||||
equals: true
|
||||
enablement:
|
||||
- $ref: '#/macros/offroad'
|
||||
- id: toyota
|
||||
title: Toyota / Lexus Settings
|
||||
description: ''
|
||||
|
||||
@@ -17,6 +17,26 @@ ONROAD_BRIGHTNESS_TIMER_VALUES = {0: 3, 1: 5, 2: 7, 3: 10, 4: 15, 5: 30, **{i: (
|
||||
VALID_TIMER_VALUES = set(ONROAD_BRIGHTNESS_TIMER_VALUES.values())
|
||||
|
||||
|
||||
def _resolve_brand(_params) -> str:
|
||||
bundle = _params.get("CarPlatformBundle")
|
||||
if isinstance(bundle, dict) and bundle.get("brand"):
|
||||
return str(bundle["brand"])
|
||||
|
||||
# Auto-fingerprinted cars have no bundle, fall back to the last known CarParams.
|
||||
CP_bytes = _params.get("CarParamsPersistent")
|
||||
if CP_bytes is None:
|
||||
return ""
|
||||
|
||||
# Never raises: callers rely on "" to mean "brand unknown, skip the migration".
|
||||
try:
|
||||
from openpilot.cereal import messaging # lazy: avoids heavy import at module level
|
||||
from opendbc.car.structs import car
|
||||
return str(messaging.log_from_bytes(CP_bytes, car.CarParams).brand)
|
||||
except Exception as e:
|
||||
cloudlog.exception(f"params_migration: failed to resolve brand from CarParamsPersistent: {e}")
|
||||
return ""
|
||||
|
||||
|
||||
def _migrate_car_platform_bundle(_params):
|
||||
bundle = _params.get("CarPlatformBundle")
|
||||
if bundle is None:
|
||||
@@ -47,6 +67,23 @@ def _migrate_car_platform_bundle(_params):
|
||||
cloudlog.info(f"params_migration: CarPlatformBundle migrated {old_platform!r} -> {new_platform!r}")
|
||||
|
||||
|
||||
def _migrate_tesla_mads_screen_button(_params):
|
||||
# TeslaMadsScreenButton defaults to Off for fresh installs, but the screen button was previously always
|
||||
# active on Teslas with a vehicle bus. Seed existing Tesla installs with 3-finger to preserve that.
|
||||
try:
|
||||
if _params.get("TeslaMadsScreenButton") is not None:
|
||||
return
|
||||
|
||||
if _resolve_brand(_params) != "tesla":
|
||||
return
|
||||
|
||||
from opendbc.sunnypilot.car.tesla.values import MadsScreenButtonType # lazy: avoids heavy import at module level
|
||||
_params.put("TeslaMadsScreenButton", MadsScreenButtonType.THREE_FINGER, block=True)
|
||||
cloudlog.info("params_migration: seeded TeslaMadsScreenButton with 3-finger to preserve existing behavior")
|
||||
except Exception as e:
|
||||
cloudlog.exception(f"Error migrating TeslaMadsScreenButton: {e}")
|
||||
|
||||
|
||||
def run_migration(_params):
|
||||
# migrate OnroadScreenOffBrightness
|
||||
if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
|
||||
@@ -80,3 +117,6 @@ def run_migration(_params):
|
||||
cloudlog.exception(f"Error migrating OnroadScreenOffTimer: {e}")
|
||||
|
||||
_migrate_car_platform_bundle(_params)
|
||||
|
||||
# seed TeslaMadsScreenButton for existing Tesla installs
|
||||
_migrate_tesla_mads_screen_button(_params)
|
||||
|
||||
@@ -1,30 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
SCRIPT_DIR=$(dirname "$0")
|
||||
OPENPILOT_DIR=$SCRIPT_DIR/../../
|
||||
|
||||
DOCKER_IMAGE=sunnypilot
|
||||
DOCKER_FILE=Dockerfile.openpilot
|
||||
DOCKER_REGISTRY=ghcr.io/sunnypilot
|
||||
COMMIT_SHA=$(git rev-parse HEAD)
|
||||
|
||||
if [ -n "$TARGET_ARCHITECTURE" ]; then
|
||||
PLATFORM="linux/$TARGET_ARCHITECTURE"
|
||||
TAG_SUFFIX="-$TARGET_ARCHITECTURE"
|
||||
else
|
||||
PLATFORM="linux/$(uname -m)"
|
||||
TAG_SUFFIX=""
|
||||
fi
|
||||
|
||||
LOCAL_TAG=$DOCKER_IMAGE$TAG_SUFFIX
|
||||
REMOTE_TAG=$DOCKER_REGISTRY/$LOCAL_TAG
|
||||
REMOTE_SHA_TAG=$DOCKER_REGISTRY/$LOCAL_TAG:$COMMIT_SHA
|
||||
|
||||
DOCKER_BUILDKIT=1 docker buildx build --provenance false --pull --platform $PLATFORM --load -t $DOCKER_IMAGE:latest -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR
|
||||
|
||||
if [ -n "$PUSH_IMAGE" ]; then
|
||||
docker push $REMOTE_TAG
|
||||
docker tag $REMOTE_TAG $REMOTE_SHA_TAG
|
||||
docker push $REMOTE_SHA_TAG
|
||||
fi
|
||||
+38
-109
@@ -5,8 +5,6 @@ This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import os
|
||||
import pickle
|
||||
import sys
|
||||
import hashlib
|
||||
import json
|
||||
@@ -14,46 +12,8 @@ import re
|
||||
from pathlib import Path
|
||||
from datetime import datetime, UTC
|
||||
|
||||
REQUIRED_OUTPUT_KEYS = frozenset({
|
||||
"plan",
|
||||
"lane_lines",
|
||||
"road_edges",
|
||||
"lead",
|
||||
"desire_state",
|
||||
"desire_pred",
|
||||
"meta",
|
||||
"lead_prob",
|
||||
"lane_lines_prob",
|
||||
"pose",
|
||||
"wide_from_device_euler",
|
||||
"road_transform",
|
||||
"hidden_state",
|
||||
})
|
||||
OPTIONAL_OUTPUT_KEYS = frozenset({
|
||||
"planplus",
|
||||
"sim_pose",
|
||||
"desired_curvature",
|
||||
})
|
||||
|
||||
|
||||
def validate_model_outputs(metadata_paths: list[Path]) -> None:
|
||||
combined_keys: set[str] = set()
|
||||
for path in metadata_paths:
|
||||
if path.stat().st_size == 0:
|
||||
print(f"skipping empty metadata: {path}")
|
||||
continue
|
||||
with open(path, "rb") as f:
|
||||
metadata = pickle.load(f)
|
||||
combined_keys.update(metadata.get("output_slices", {}).keys())
|
||||
missing = REQUIRED_OUTPUT_KEYS - combined_keys
|
||||
if missing:
|
||||
raise ValueError(f"Combined model metadata is missing required output keys: {sorted(missing)}")
|
||||
detected_optional = sorted(OPTIONAL_OUTPUT_KEYS & combined_keys)
|
||||
if detected_optional:
|
||||
print(f"Optional output keys detected: {detected_optional}")
|
||||
|
||||
|
||||
def create_short_name(full_name):
|
||||
def create_short_name(full_name: str) -> str:
|
||||
# Remove parentheses and extract alphanumeric words
|
||||
clean_name = re.sub(r'\([^)]*\)', '', full_name)
|
||||
words = [re.sub(r'[^a-zA-Z0-9]', '', word) for word in clean_name.split() if re.sub(r'[^a-zA-Z0-9]', '', word)]
|
||||
@@ -121,49 +81,41 @@ def _rename_pkl_with_chunks(old_pkl: Path, new_pkl: Path) -> Path:
|
||||
return old_pkl.rename(new_pkl)
|
||||
|
||||
|
||||
def generate_metadata(model_path: Path, output_dir: Path, short_name: str, driving_pkl: Path):
|
||||
base = model_path.stem
|
||||
metadata_file = output_dir / f"{base}_metadata.pkl"
|
||||
|
||||
if short_name:
|
||||
renamed_meta = output_dir / f"{base}_{short_name.lower()}_metadata.pkl"
|
||||
if metadata_file.exists() and not renamed_meta.exists():
|
||||
metadata_file = metadata_file.rename(renamed_meta)
|
||||
elif renamed_meta.exists():
|
||||
metadata_file = renamed_meta
|
||||
|
||||
if not metadata_file.exists():
|
||||
print(f"Warning: Missing metadata for {base} ({metadata_file}), skipping", file=sys.stderr)
|
||||
return
|
||||
|
||||
def generate_chunked_model(driving_pkl: Path) -> dict:
|
||||
tinygrad_hash = hashlib.sha256(_read_pkl_bytes(driving_pkl)).hexdigest()
|
||||
|
||||
with open(metadata_file, 'rb') as f:
|
||||
metadata_hash = hashlib.sha256(f.read()).hexdigest()
|
||||
chunks_config = []
|
||||
manifest_file = Path(f"{driving_pkl}.chunkmanifest")
|
||||
if manifest_file.exists():
|
||||
num_chunks = int(manifest_file.read_text().strip())
|
||||
for i in range(num_chunks):
|
||||
chunk_path = Path(f"{driving_pkl}.chunk{i + 1:02d}of{num_chunks:02d}")
|
||||
if chunk_path.exists():
|
||||
chunk_hash = hashlib.sha256(chunk_path.read_bytes()).hexdigest()
|
||||
chunks_config.append({
|
||||
"file_name": chunk_path.name,
|
||||
"sha256": chunk_hash
|
||||
})
|
||||
|
||||
model_type = "offPolicy" if "off_policy" in base else "onPolicy" if "on_policy" in base else base.split("_")[-1]
|
||||
|
||||
return {
|
||||
"type": model_type,
|
||||
"artifact": {
|
||||
"file_name": driving_pkl.name,
|
||||
"download_uri": {
|
||||
"url": "https://gitlab.com/sunnypilot/public/docs.sunnypilot.ai/-/raw/main/",
|
||||
"sha256": tinygrad_hash
|
||||
}
|
||||
},
|
||||
"metadata": {
|
||||
"file_name": metadata_file.name,
|
||||
"download_uri": {
|
||||
"url": "https://gitlab.com/sunnypilot/public/docs.sunnypilot.ai/-/raw/main/",
|
||||
"sha256": metadata_hash
|
||||
}
|
||||
artifact_data = {
|
||||
"file_name": driving_pkl.name,
|
||||
"download_uri": {
|
||||
"url": "https://gitlab.com/sunnypilot/public/docs.sunnypilot.ai/-/raw/main/",
|
||||
"sha256": tinygrad_hash
|
||||
}
|
||||
}
|
||||
|
||||
if chunks_config:
|
||||
artifact_data["chunks"] = chunks_config
|
||||
|
||||
def create_metadata_json(models: list, output_dir: Path, custom_name=None, short_name=None, is_20hz=False, upstream_branch="unknown"):
|
||||
metadata_json = {
|
||||
return {
|
||||
"type": "chunked",
|
||||
"artifact": artifact_data,
|
||||
}
|
||||
|
||||
|
||||
def create_metadata_json(models: list, output_dir: Path, custom_name=None, short_name=None, is_20hz=False, upstream_branch="unknown") -> None:
|
||||
bundle_json = {
|
||||
"short_name": short_name,
|
||||
"display_name": custom_name or upstream_branch,
|
||||
"is_20hz": is_20hz,
|
||||
@@ -179,40 +131,26 @@ def create_metadata_json(models: list, output_dir: Path, custom_name=None, short
|
||||
}
|
||||
|
||||
# Write metadata to output_dir
|
||||
metadata_json = {
|
||||
"bundles": [bundle_json]
|
||||
}
|
||||
|
||||
with open(output_dir / "metadata.json", "w") as f:
|
||||
json.dump(metadata_json, f, indent=2)
|
||||
|
||||
print(f"Generated metadata.json with {len(models)} models.")
|
||||
print("Generated metadata.json")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import argparse
|
||||
import glob
|
||||
|
||||
parser = argparse.ArgumentParser(description="Generate metadata for model files")
|
||||
parser.add_argument("--model-dir", default="./models", help="Directory containing ONNX model files")
|
||||
parser = argparse.ArgumentParser(description="Generate metadata JSON for the compiled JIT model")
|
||||
parser.add_argument("--model-dir", default="./models", help="Directory containing the model files")
|
||||
parser.add_argument("--output-dir", default="./output", help="Output directory for metadata")
|
||||
parser.add_argument("--custom-name", help="Custom display name for the model")
|
||||
parser.add_argument("--is-20hz", action="store_true", help="Whether this is a 20Hz model")
|
||||
parser.add_argument("--validate-only", action="store_true")
|
||||
parser.add_argument("--upstream-branch", default="unknown", help="Upstream branch name")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.validate_only:
|
||||
metadata_paths = glob.glob(os.path.join(args.model_dir, "*_metadata.pkl"))
|
||||
if not metadata_paths:
|
||||
print(f"No metadata files found in {args.model_dir}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
validate_model_outputs([Path(p) for p in metadata_paths])
|
||||
print(f"Validated {len(metadata_paths)} metadata files successfully.")
|
||||
sys.exit(0)
|
||||
|
||||
# Find all ONNX files in the given directory
|
||||
model_paths = glob.glob(os.path.join(args.model_dir, "*.onnx"))
|
||||
if not model_paths:
|
||||
print(f"No ONNX files found in {args.model_dir}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
|
||||
_output_dir = Path(args.output_dir)
|
||||
_output_dir.mkdir(exist_ok=True, parents=True)
|
||||
_short_name = create_short_name(args.custom_name) if args.custom_name else None
|
||||
@@ -229,14 +167,5 @@ if __name__ == "__main__":
|
||||
else:
|
||||
_driving_pkl = new_pkl
|
||||
|
||||
_models = []
|
||||
|
||||
for _model_path in model_paths:
|
||||
_model_metadata = generate_metadata(Path(_model_path), _output_dir, _short_name, _driving_pkl)
|
||||
if _model_metadata:
|
||||
_models.append(_model_metadata)
|
||||
|
||||
if _models:
|
||||
create_metadata_json(_models, _output_dir, args.custom_name, _short_name, args.is_20hz, args.upstream_branch)
|
||||
else:
|
||||
print("No models processed.", file=sys.stderr)
|
||||
_model_metadata = generate_chunked_model(_driving_pkl)
|
||||
create_metadata_json([_model_metadata], _output_dir, args.custom_name, _short_name, args.is_20hz, args.upstream_branch)
|
||||
|
||||
+1
-1
Submodule tinygrad_repo updated: ac1632ab96...2fecac4e4a
@@ -1,62 +0,0 @@
|
||||
# Hyundai/Kia/Genesis radar range matrix
|
||||
|
||||
This is a forum-ready summary of the routes analyzed for the sunnypilot radar
|
||||
tracks work. It describes observed protocol families, not guaranteed equipment
|
||||
for every model year, market, trim, or harness. A range can be present but empty
|
||||
when the radar is inactive. Raw bus number is also not a layout identifier:
|
||||
follow the sustained logical A-CAN source and reject brief forwarded copies.
|
||||
|
||||
Status terms:
|
||||
|
||||
- **Confirmed**: complete range, checksum/cadence, and tracked-object layout
|
||||
were validated across route data.
|
||||
- **Observed/optional**: verified on at least one qualifying route, but not
|
||||
every route or trim carrying the main front-radar family.
|
||||
- **Candidate**: source association or exact semantics remain incomplete.
|
||||
- **Not radar**: camera or fused-output traffic overlapping radar-like ranges.
|
||||
|
||||
## Platform matrix
|
||||
|
||||
| Platform or platform group | Front tracked-object range | Front layout | Additional radar traffic | Camera/fused traffic that is not another radar | Status and notes |
|
||||
| --- | --- | --- | --- | --- | --- |
|
||||
| Kia K7 2017; older Hyundai Ioniq; sampled Ioniq PHEV | `0x500–0x53F` | 64 slots, 8 B each, about 20 Hz | `0x4E0`, `0x4E1`, `0x4E3` status/path messages | Legacy `0x238–0x24F` may coexist on applicable cars | **Confirmed** 64-slot legacy/Delphi-ESR dialect. |
|
||||
| Kia Niro EV first generation; legacy Hyundai Palisade; Sonata; Sonata Hybrid | `0x500–0x51F` | 32 slots, 8 B each, about 20 Hz | `0x4E0–0x4E5` status/build/health/alignment family | Legacy `0x238–0x255` or related camera candidates may coexist by platform | **Confirmed** 32-slot legacy dialect. The upper `0x520–0x53F` bank is absent. |
|
||||
| Hyundai Kona EV 2022; Kia Ceed PHEV | `0x602–0x617` | Two packed tracks in implemented messages | None established | None established | **Observed/candidate** legacy front family; the relationship between `0x602–0x611` and `0x612–0x617` remains incomplete. |
|
||||
| Hyundai Elantra HEV 2024 | `0x210–0x21F` | Detailed dialect, 32 tracks in 16 × 32 B messages, 20 Hz | No paired corner family established | Platform-dependent camera traffic | **Confirmed** detailed `210` front radar. |
|
||||
| Hyundai Ioniq 5; Hyundai Palisade 2023; Kia EV6, older HDA2 samples | `0x210–0x21F` | Detailed dialect, 32 tracks | Right-front candidate: `0x240–0x24F` objects plus `0x270–0x277` compact records. Left-front candidate: `0x278–0x287` objects plus `0x288–0x28F` compact records. | Isolated `0x235` traffic is not a complete camera-object family | **Confirmed** front radar. Paired front corners are **observed/optional** only on qualifying routes; A=right and B=left are strongly inferred but still need physical occlusion confirmation. |
|
||||
| Hyundai Santa Cruz 2025; Tucson 4th generation; Tucson HEV 2025; Kia Sportage 5th generation | `0x210–0x21F` | Compact dialect, 32 tracks | No corner family established in checked routes | Platform-dependent camera traffic | **Confirmed** compact `210` front radar. Compact and detailed `210` share core kinematics but use different lifecycle fields. |
|
||||
| Hyundai Ioniq 6 | `0x3A5–0x3C4` | Common 32-track layout, 24 B each, 20 Hz | No independent corner range established | `0x235–0x248` camera objects and `0x360–0x366` camera lane/path may be present | **Confirmed** common `3A5` front radar. |
|
||||
| Hyundai Kona/Kona HEV/Kona EV 2nd generation; Santa Fe HEV 5th generation; Sonata 2024 | `0x3A5–0x3C4` | Common 32-track layout | No independent corner range established | Platform-dependent `0x235`, `0x360`, and CCNC `0x162` fused traffic | **Confirmed** common `3A5` front radar. CCNC/HDA generation does not imply rich radar extensions. |
|
||||
| Kia Carnival HEV 2026; EV6 2025; K4 2025; K5 2025; Niro EV/HEV 2nd generation | `0x3A5–0x3C4` | Common 32-track layout | No independent corner range established | Platform-dependent `0x235`, `0x360`, and CCNC `0x162` fused traffic | **Confirmed** common `3A5` front radar. Bus 0/1 varies by architecture; a bus-2 copy can be forwarding. |
|
||||
| Kia EV9; Hyundai Ioniq 9 | `0x3A5–0x3C4` | Rich 32-track layout with optional width, length, absolute speed, and orientation | `0x3D0–0x3D4` synchronized 32-position auxiliary scan array | `0x235–0x248` camera objects; `0x360–0x366` camera lane/path; CCNC `0x162` fused traffic | **Confirmed** rich `3A5`; `3D0` is **observed/optional** radar auxiliary traffic. Former `3D0` speed/azimuth interpretations were falsified, so it is counted but not projected as geometry. |
|
||||
| Kia Sportage HEV 2026 forum sample | Overlaps `0x3A5–0x3C4` addresses | Payload is incompatible with the common `3A5` layout | Unknown | Unknown | **Excluded** from the common decoder; address overlap alone is not layout compatibility. |
|
||||
|
||||
## Other observed ranges
|
||||
|
||||
| Range | Applicable examples | Classification |
|
||||
| --- | --- | --- |
|
||||
| `0x235–0x248`, 32 B at about 33 Hz | Newer CAN-FD platforms, including rich EV9/Ioniq 9 captures | **Not radar:** forward-camera object list with class, motion, ID, geometry, velocity, and acceleration. |
|
||||
| `0x360–0x366`, 32 B | Newer CAN-FD platforms carrying camera lane/path output | **Not radar:** forward-camera lane/path family. |
|
||||
| `0x180–0x184`, `0x1B6–0x1B9`, `0x2BB–0x2BE` | Sampled Kia Sorento/Sorento HEV and Genesis GV70 candidates | **Not radar:** forward-camera auxiliary lists according to downloaded Hyundai DBCs. |
|
||||
| `0x238–0x24F`, 8 B | Ioniq PHEV/HEV 2022, legacy Palisade, Niro PHEV candidates | **Not established as radar:** strong legacy forward-camera-perception candidate. |
|
||||
| `0x238–0x255`, 8 B, often with `0x25A–0x25E` | Custin, Elantra/Elantra HEV 2021, Sonata/Sonata Hybrid candidates | **Not established as radar:** extended legacy forward-camera-perception candidate. |
|
||||
| `CCNC_0x162` | CCNC platforms | **Not raw radar:** stock fused/cluster lead, side, and rear slots with unknown upstream fusion. |
|
||||
| Bus-9 `0x300/0x400/0x500/0x600` blocks | Historical EV6 prototype capture | **Historical candidate:** four-corner FL/FR/RL/RR prototype; not reconfirmed in the current production-route corpus. |
|
||||
|
||||
## Supported combinations observed in the corpus
|
||||
|
||||
| Combination | Front | Corners or radar auxiliary |
|
||||
| --- | --- | --- |
|
||||
| Legacy front, 32 slots | `0x500–0x51F` | `0x4E0–0x4E5` companion/status |
|
||||
| Legacy front, 64 slots | `0x500–0x53F` | `0x4E0`, `0x4E1`, `0x4E3` companion/status |
|
||||
| Legacy alternate front | `0x602–0x617` | None established |
|
||||
| CAN-FD detailed front only | `0x210–0x21F` | None established |
|
||||
| CAN-FD detailed front plus paired front corners | `0x210–0x21F` | A/right candidate `0x240/0x270` families plus B/left candidate `0x278/0x288` families |
|
||||
| CAN-FD compact front only | `0x210–0x21F` | None established |
|
||||
| CAN-FD common front | `0x3A5–0x3C4` | Camera/fused families may coexist but are not independent radar tracks |
|
||||
| CAN-FD rich front plus auxiliary scan | `0x3A5–0x3C4` | Optional `0x3D0–0x3D4` auxiliary radar array |
|
||||
|
||||
No checked route proves a combination containing both the paired
|
||||
`0x240/0x270–0x28F` corner family and `0x3D0–0x3D4`. No distinct production
|
||||
rear-radar tracked-object range has been confirmed. Rear objects in
|
||||
`CCNC_0x162` are fused outputs and do not prove a raw rear-radar range.
|
||||
@@ -1,228 +0,0 @@
|
||||
# Hyundai/Kia/Genesis radar source inventory
|
||||
|
||||
This document separates physical radar sources from camera and fused-object
|
||||
traffic. Signal-level evidence is in [SIGNALS.md](SIGNALS.md).
|
||||
The copy-ready vehicle/range summary is in
|
||||
[PLATFORM_MATRIX.md](PLATFORM_MATRIX.md).
|
||||
|
||||
## Confidence and safety rules
|
||||
|
||||
- **Confirmed layout** means that bit positions, scales, and behavior agree
|
||||
across multiple routes. It does not automatically prove the physical ECU.
|
||||
- **Strong role** means that topology, cadence, geometry, and related messages
|
||||
support the assignment, but a controlled sensor-occlusion test is still
|
||||
desirable.
|
||||
- **Candidate** means useful research data only. Candidate fields must not
|
||||
affect control or production filtering.
|
||||
- Identify a source by the complete address set, payload sizes, cadence,
|
||||
checksum behavior, and logical CAN path. An address or raw bus number alone
|
||||
is not enough.
|
||||
- A short copy on another bus is normally a forwarding echo. It is not a
|
||||
second sensor unless timestamps and payload differences prove independence.
|
||||
|
||||
## Current source families
|
||||
|
||||
| Source | Layout | Role | Current conclusion |
|
||||
| --- | --- | --- | --- |
|
||||
| `0x500–0x51F` or `0x500–0x53F` | 32 or 64 × 8 B at about 20 Hz | Front radar | Shared legacy core with a Delphi ESR extension on the 64-slot dialect. K7, older Ioniq, and Ioniq PHEV samples carry 64 slots; Niro EV, Palisade, Sonata, and Sonata Hybrid samples carry 32. Implemented as standardized `RADAR_500_53F` with an optional upper half, per-source cycle end, and runtime 32-to-64 upgrade. Corrected 0.1-degree geometry is shared; 64-slot-only ESR fields use explicit aliases for 0.05 m/s² acceleration, lateral rate, width, oncoming, grouping, bridge-object, and range mode, while 32-slot acceleration retains 0.02 m/s². |
|
||||
| `0x4E0–0x4E5` | 8 B at the `500` radar cadence | Front-radar companion/status | Strong source association across every sampled `500` route. Shared `4E0` is decoded as ESR vehicle speed, yaw, curvature, 16-bit scan index, timestamp, counter, and communication health. The 64-slot radar's `4E1` matches ESR Status2; 32-slot `4E1` retains only the shared counter plus unresolved static metadata. `4E3` path IDs are shared and route-validated. The 32-slot-only `4E2`, `4E4`, and `4E5` carry BCD build metadata, protocol-derived raw ADC channels, and factory/vertical alignment status. |
|
||||
| `0x602–0x617` | 8 B, two packed tracks in the implemented messages | Front radar | Implemented legacy front layout. The relationship between `602–611` and the alternate `612–617` definitions remains incomplete. |
|
||||
| `0x210–0x21F` | 16 × 32 B at 20 Hz, two tracks per message | Front radar | Confirmed 32-track front layout. Detailed and compact dialects share the same core. |
|
||||
| `0x3A5–0x3C4` | 32 × 24 B at 20 Hz, one track per message | Front radar | Confirmed front layout. EV9 and Ioniq 9 populate its optional object-size/orientation extension. |
|
||||
| `0x235–0x248` | 20 × 32 B at about 33 Hz | **Forward-camera objects** | Corrected classification. Its first 124 bits exactly match the downloaded `FR_CMR_Obj` schema and carry camera quality, class, motion, ID, geometry, velocity, and acceleration. It is not corner radar. |
|
||||
| `0x240–0x24F` + `0x270–0x277` | status + 15 × 24 B objects + 8 × 32 B scan blocks | Candidate right-front corner channel A | Strong two-sensor topology on older HDA2 Ioniq 5, Palisade 2023, and EV6 routes. A=right is route/video-inferred; tracked-object position and velocity are decoded. Compact scan semantics remain unresolved. |
|
||||
| `0x278–0x28F` | status + 15 × 24 B objects + 8 × 32 B scan blocks | Candidate left-front corner channel B | Symmetric partner to channel A. B=left is route/video-inferred. The compact low field is only a distance candidate; property/auxiliary bits are unresolved. |
|
||||
| `0x3D0–0x3D4` | 5 × 32 B, seven packed records per message | Front-radar auxiliary scan list | Optional companion to `3A5–3C4` on sampled EV9/Ioniq 9 routes. It has 32 usable record positions and shares the radar cycle counter, but synchronized testing falsified the former radial-speed and azimuth interpretations. The low 12 bits remain only a distance candidate. |
|
||||
| `0x360–0x366` | 7 × 32 B | **Forward-camera lane/path** | Camera lane/path geometry, not radar. `362` contains lane confidence; `360/361/363/364` are active geometry, while `365/366` are often default-filled optional slots. |
|
||||
| legacy `0x238–0x24F` or `0x238–0x255` | 24 or 30 × 8 B at about 32 Hz | **Candidate forward-camera perception** | All 13 checked legacy routes carry one of these exact variants. Several carry a simultaneous `500` front radar at a different cadence, arguing strongly against this being a second radar. The 30-message variant also carries synchronized `25A–25E`; exact fields remain unparsed. Do not confuse these 8-byte messages with the CAN-FD `235–248` camera layout or `240/270` corner layout. |
|
||||
| CAN-FD `0x180–0x184`, `0x1B6–0x1B9`, `0x2BB–0x2BE` | 32 B at about 30 Hz | **Forward-camera auxiliary lists** | Downloaded Hyundai DBCs identify the transmitters/messages as camera traffic, and route payloads have camera-list cadence and slot behavior. Not a radar family. |
|
||||
| `CCNC_0x162` | fixed lead/left/right/rear slots | Fused/cluster output | Stock ADAS/cluster output with unknown upstream fusion. Do not publish it as independent raw radar. |
|
||||
| bus 9 `0x300/0x400/0x500/0x600` eight-address blocks | old prototype, eight subtracks per message | Four-corner prototype | Prior EV6 reverse-engineering mapped four blocks to FL/FR/RL/RR, but status and relative speed were unfinished and the family has not been reconfirmed in the current corpus. |
|
||||
|
||||
The confirmed `3A5–3C4` layout was checked across 21 compatible forum routes
|
||||
and 817,852 checksum-valid frames. All 726 advertised segments from the 20
|
||||
unique `210–21F` routes were scanned: 16 routes had active targets, three had
|
||||
a complete but empty stream, and one non-HDA2 Telluride route had no 210
|
||||
range. The 16 active routes split evenly between detailed and compact
|
||||
dialects, and all 13,210,543 checked 210 frames passed the HKG checksum.
|
||||
|
||||
The forum candidates are now classified at the family level. The legacy
|
||||
`238–24F` and `238–255` variants are strong forward-camera candidates but
|
||||
remain unparsed. The `180/1B6/2BB` groups are CAN-FD camera auxiliary lists.
|
||||
The incomplete Ioniq PHEV `500–?` observation is a complete `500–53F`
|
||||
64-slot front radar. None must be conflated with the CAN-FD `235` camera or
|
||||
`240/270` corner layouts merely because their addresses overlap.
|
||||
|
||||
The `4E0/4E1` 2-bit counters matched in all 2,552 paired frames checked across
|
||||
seven routes. On the three 64-slot routes, `4E3` matched the same counter in
|
||||
all 1,133 paired frames. Every one of the 2,369 distinct `4E0` 16-bit
|
||||
`SCAN_INDEX` steps incremented by one. In a fresh six-route full-segment pass,
|
||||
decoded `4E0` vehicle speed correlated with `carState.vEgo` at
|
||||
0.9976–0.99996, with 0.02–0.19 m/s mean absolute error. Both checked 64-slot
|
||||
routes held `MAXIMUM_TRACKS_ACK=64`; all four 32-slot routes decoded that ESR
|
||||
field as 1, confirming a different `4E1` dialect. The four sampled 32-slot
|
||||
variants carried plausible BCD build timestamps in `4E2`; their remaining
|
||||
`4E1` metadata stayed platform-static. All 7,347 nonzero `4E3` path IDs
|
||||
referenced active one-based target slots. The 32-slot `4E4/4E5` payloads match
|
||||
Hyundai-relocated ESR Status5/6 layouts: raw ADC health channels and alignment
|
||||
status. Across 4,698 `4E5` frames, both factory-alignment fields remained
|
||||
within the documented 0–3 enum subset; unexercised flags and physical ADC or
|
||||
misalignment scales remain research-only.
|
||||
|
||||
Two other consecutive lookalikes are currently excluded from radar work:
|
||||
downloaded Hyundai DBCs identify `630–63D` as head-unit/amplifier traffic, and
|
||||
`5ED–5EF` occurs on routes without `500` tracks and does not follow the
|
||||
otherwise strong `4E0`/`500` source association.
|
||||
|
||||
### Outstanding forum platform queue
|
||||
|
||||
| Platform | Remaining candidate |
|
||||
| --- | --- |
|
||||
| Hyundai Ioniq PHEV / Ioniq HEV 2022 / Palisade / Kia Niro PHEV 2022 | legacy camera candidate `238–24F`, plus its synchronized `201/20A/266/26D` companion groups |
|
||||
| Hyundai Custin / Elantra 2021 / Elantra HEV 2021 / Sonata / Sonata Hybrid | extended legacy camera candidate `238–255`, plus `25A–25E` and the same companion groups |
|
||||
| Kia Sorento / Sorento HEV 4th gen / Genesis GV70 | confirm semantics within the classified CAN-FD camera `180/1B6/2BB` auxiliary groups |
|
||||
| Legacy `500` front-radar platforms | identify static 32-slot `4E1` metadata and calibrate/actively exercise the decoded `4E4/4E5` health and alignment fields |
|
||||
|
||||
The forum also listed Acura MDX and Honda Clarity examples. They remain out of
|
||||
this Hyundai/Kia/Genesis inventory because the Acura range was unrelated and
|
||||
the Honda routes did not expose tracks.
|
||||
|
||||
## Confirmed and possible vehicle combinations
|
||||
|
||||
These are distinct source combinations supported by the corpus. “Possible”
|
||||
describes protocol combinations, not a promise that every trim exposes them.
|
||||
|
||||
| Combination | Front tracked objects | Corner source | Camera/auxiliary source | Known examples or status |
|
||||
| --- | --- | --- | --- | --- |
|
||||
| Legacy front only A | `500–51F` or `500–53F` | none observed | `4E0–4E5` radar companion/status | 32- and 64-slot platform variants |
|
||||
| Legacy front only B | `602–617` | none observed | none established | Kona EV 2022, Ceed PHEV candidates |
|
||||
| CAN-FD front only A | `210–21F` | none observed | none established | Elantra HEV 2024 and compact-dialect Tucson/Santa Cruz/Sportage routes |
|
||||
| CAN-FD front + paired front corners | `210–21F` | inferred right A `240–24F` + `270–277`; inferred left B `278–28F` | none required | Older HDA2 Ioniq 5, Palisade 2023, and EV6 observations |
|
||||
| CAN-FD front only B | `3A5–3C4` | none observed | camera objects/lanes may be absent or not logged | Compatible HDA1 routes |
|
||||
| CAN-FD front + camera perception | `3A5–3C4` | none established | camera objects `235–248` and lanes `360–366` | Common newer HDA2 pattern; camera traffic is not another radar |
|
||||
| Rich front + camera perception + auxiliary radar scan | rich `3A5–3C4` | none established | `235–248`, `360–366`, plus radar auxiliary records `3D0–3D4` | EV9 and Ioniq 9 sampled routes |
|
||||
| Front + four-corner prototype | layout depends on the old EV6 capture | bus-9 FL/FR/RL/RR blocks | unknown | Historical prototype only; not current-production support |
|
||||
|
||||
No current evidence proves a combination containing both the paired
|
||||
`240/270` corner family and the newer `3D0` auxiliary scan family. No distinct
|
||||
rear-radar tracked-object range has been confirmed in the current routes.
|
||||
Rear objects shown by `CCNC_0x162` may be fused from other sensors and do not
|
||||
prove a rear radar source.
|
||||
|
||||
## Platform observations
|
||||
|
||||
| Platform group | Front | Additional observed traffic |
|
||||
| --- | --- | --- |
|
||||
| Ioniq 5 / Palisade 2023 / EV6 older HDA2 samples | detailed `210–21F` | paired `240/270–28F` front corners; A=right and B=left strongly inferred |
|
||||
| Elantra HEV 2024 | detailed `210–21F` | no corner family established |
|
||||
| Santa Cruz 2025 / Tucson family / Sportage 5th gen | compact `210–21F` | no corner family established in the checked samples |
|
||||
| Ioniq 6 / Kona 2 / Kona EV 2 / Santa Fe 5 / Sonata 2024 / Carnival HEV / EV6 2025 / K4 / K5 / Niro 2 | common `3A5–3C4` where compatible | platform-dependent camera `235` and `360` traffic; no decoded corner family established |
|
||||
| EV9 / Ioniq 9 | rich `3A5–3C4` | camera objects `235`, camera lanes `360`, optional front-radar auxiliary scan `3D0` |
|
||||
| Sportage HEV 2026 forum sample | overlapping but incompatible `3A5` bytes | excluded from the common-layout decoder |
|
||||
|
||||
HDA2 changes where traffic is exposed, but raw bus number is not a reliable
|
||||
layout discriminator. Follow sustained logical A-CAN and reject short
|
||||
forwarded copies.
|
||||
|
||||
## What remains to prove
|
||||
|
||||
1. Confirm the route/video-derived A=right and B=left assignment with a
|
||||
controlled sensor occlusion or authoritative service mapping.
|
||||
2. Finish both corner status messages and the remaining 24-byte object
|
||||
fields: the stable 2-bit unknown category, persistent ID, classification,
|
||||
dimensions, and sensor-health flags. The former acceleration candidate was
|
||||
falsified and returned to raw bits. Position and longitudinal/lateral
|
||||
velocity are resolved, and `0x278` byte 3 is the confirmed pre-truncation
|
||||
channel-A object count: exported A slots equal `min(count, 15)`.
|
||||
Preserve all remaining unknown bits until they correlate with the compact
|
||||
scan bins.
|
||||
3. Decode the 56 fixed-position `270–277` / `288–28F` scan candidates,
|
||||
including the low 13-bit distance candidate, seven-bit property, and
|
||||
nine-bit auxiliary fields. Direct same-cycle range association and a linear
|
||||
record-index-to-angle map both failed time-shift controls, so do not project
|
||||
these records as geometry until an independent mapping is found.
|
||||
4. Decode the `3D0` auxiliary scan array without restoring the falsified
|
||||
radial-speed or azimuth names. Determine whether the low 12 bits are
|
||||
distance, identify the role and physical angular ordering of the 32 record
|
||||
positions, and decode the four flag bits plus both unknown bytes. Two rich
|
||||
routes confirm exact counter synchronization and the sentinel/zero empty
|
||||
forms, but reject range-only, candidate-geometry, and direct slot-to-slot
|
||||
associations with `3A5`. Production RadarData must continue to exclude it.
|
||||
5. Decode the active polynomial/path fields in `360/361/363/364`. Treat
|
||||
`365/366` constant payloads as unavailable lane slots, not missing radar.
|
||||
6. Resolve `235–248` `UNKNOWN_1/2/3`, identify every checksum variant, and
|
||||
validate native camera classification/motion values against video. Confirm
|
||||
the camera-source assignment and forwarding path on each platform carrying
|
||||
the range.
|
||||
7. Search explicitly for the old four-corner topology and for a distinct rear
|
||||
tracked-object source. Record verified absence when the correct buses and
|
||||
maneuvers were covered.
|
||||
8. Resolve the static 32-slot `4E1` metadata, physically calibrate the
|
||||
protocol-derived `4E4` ADC channels, and capture alignment events to verify
|
||||
the inactive `4E5` flags and misalignment scales. Exercise the currently
|
||||
zero FCW path IDs and low `4E3` status flags on a 32-slot platform. Shared
|
||||
`4E0` and path IDs are resolved; do not transfer unsupported 64-slot
|
||||
Status2 fields onto the 32-slot payload.
|
||||
9. Decode the legacy camera candidates `238–24F` and `238–255`, including
|
||||
their `201/20A/25A/266/26D` companion groups. Determine whether `250–255`
|
||||
and `25A–25E` are extra objects, path/lane data, or metadata, and correlate
|
||||
classification and geometry against video. Keep these out of production
|
||||
RadarData unless source evidence overturns the current camera assignment.
|
||||
10. Complete the per-platform combination matrix across HDA1/HDA2 and
|
||||
CCNC/non-CCNC cars. Record the sustained logical CAN source, forwarded
|
||||
echoes, empty-but-present ranges, and verified absences rather than relying
|
||||
on raw bus numbers.
|
||||
11. Add representative replay fixtures and debugger/on-road UI tests for each
|
||||
confirmed source combination. Promote candidate names or fields only after
|
||||
multi-route and controlled-scene validation.
|
||||
|
||||
## Useful work that does not require video
|
||||
|
||||
1. Cluster every unknown field by platform, object lifecycle, motion class,
|
||||
address/slot, and empty/default state. Stable cross-platform enums can be
|
||||
separated from platform configuration bytes.
|
||||
2. Use consecutive-cycle derivatives and ego-motion compensation to test
|
||||
velocity, acceleration, yaw, and sensor-frame hypotheses. Always compare
|
||||
against opposite-channel and time-shifted controls.
|
||||
3. Search counter phase and bounded latency, not merely nearest timestamps,
|
||||
when testing associations between tracked objects, compact scan arrays,
|
||||
camera objects, and status messages.
|
||||
4. Build transition matrices for the corner unknown category and remaining
|
||||
flags. The first pass proved the 2-bit category is
|
||||
stable within a continuous object but is not lifecycle or validity: all four
|
||||
values occur on active targets, while empty slots use `LONG_DIST=204.7 m`.
|
||||
Continue searching the remaining fields for birth, coast, replacement, and
|
||||
deletion behavior without assuming the visual object class.
|
||||
5. Compare the same physical field across detailed `210`, compact `210`,
|
||||
`3A5`, corner objects, and legacy `500` tracks after normalizing coordinates
|
||||
and lifecycle. Shared scaling should survive route and platform changes.
|
||||
6. Mine downloaded and public DBC/source comments for transmitter identity and
|
||||
bit boundaries, but require route evidence before adopting physical names or
|
||||
scales.
|
||||
7. Measure source dropouts, checksum/counter discontinuities, and status-bit
|
||||
transitions together. This can identify communication health, blockage,
|
||||
alignment, and reset fields without video.
|
||||
8. Generate debugger capture instructions for later physical confirmation:
|
||||
cover one corner sensor at a time, use a single stationary target at a
|
||||
measured distance, and record straight approach/recede passes at known
|
||||
speed. These tests can settle side assignment and physical scan fields with
|
||||
much less ambiguity than unconstrained road scenes.
|
||||
|
||||
## Implementation policy
|
||||
|
||||
- Production RadarData contains the confirmed tracked-object layouts only.
|
||||
- `235–248` remains available through RadarData as a camera-object source for
|
||||
research, with explicit source labeling and its native object-ID validity.
|
||||
- The corner and `3D0` generators are research DBCs. Their unresolved fields
|
||||
are named unknown or candidate in comments and are not used for control.
|
||||
- `4E0–4E5` status fields are present in the `RADAR_500_53F` DBC for research,
|
||||
and inventoried by the desktop radar debugger, but are not required for
|
||||
source detection or production track parsing.
|
||||
- The on-road UI labels `235–248` as `CAM` and uses triangle markers.
|
||||
- The desktop radar debugger plots confirmed production tracks, the
|
||||
debugger-only `612–617` alternate objects, and the decoded `240/278` corner
|
||||
objects. Its `SIGNALS` table exposes the compact `270/288` records, `3D0`
|
||||
records, `4E0–4E5` status, `360–366` raw camera payloads, and all six
|
||||
`CCNC_0x162` fused slots. Compact scans and `3D0` remain table-only and are
|
||||
never projected as geometry. `0x162` is explicitly labeled fused and is not
|
||||
published as an independent radar source.
|
||||
@@ -1,450 +0,0 @@
|
||||
# Hyundai radar, camera-object, and auxiliary signal notes
|
||||
|
||||
This document tracks the evidence behind the 24-byte `RADAR_3A5_3C4` layout
|
||||
and its shared signal semantics with the 32-byte `RADAR_210_21F` layout.
|
||||
It also records the now-identified `0x235–0x248` camera objects and the
|
||||
research-only corner and raw-detection families. It deliberately separates a
|
||||
signal being active from its meaning being known.
|
||||
The cross-family and per-platform investigation plan lives in
|
||||
[RADAR_RESEARCH.md](RADAR_RESEARCH.md).
|
||||
The forum-ready platform/range summary lives in
|
||||
[PLATFORM_MATRIX.md](PLATFORM_MATRIX.md).
|
||||
|
||||
## Status terms
|
||||
|
||||
- **Confirmed core**: populated across compatible platforms and its meaning is
|
||||
supported by position, motion, or lifecycle behavior.
|
||||
- **Optional useful**: meaning is supported, but many platforms transmit a
|
||||
default value instead of the signal.
|
||||
- **Active unknown**: the field changes consistently, but its meaning is not
|
||||
established well enough to use.
|
||||
- **Redundant**: understood, but duplicates a better primary field.
|
||||
- **Reserved/dead**: remained zero across the tested raw frames.
|
||||
|
||||
An optional field that is dead on one platform is not globally dead.
|
||||
|
||||
## Evidence corpus
|
||||
|
||||
- The original layout was checked against 817,852 frames from 21 forum routes.
|
||||
- The 3A5 checksum was rechecked on all 817,852 frames and matched the HKG
|
||||
CAN-FD checksum with the CAN address as data ID, with zero failures.
|
||||
- All 726 advertised 210-route segments were scanned across 20 unique routes:
|
||||
16 routes had active targets, three had complete but empty streams, and one
|
||||
non-HDA2 Telluride route had no 210 stream. The 16 active routes split evenly
|
||||
between detailed and compact dialects, with no third active layout.
|
||||
- The 210 HKG checksum matched all 13,210,543 checked frames.
|
||||
- The documented route corpus was expanded to 107 route/segment entries:
|
||||
all 36 canonical Hyundai/Kia CAN-FD test routes plus the forum radar and
|
||||
consecutive-range route lists. A qlog pre-screen identified candidates
|
||||
carrying 24-byte 3A5 traffic, then full rlogs were used for layout decisions.
|
||||
- The combined full-rlog pass contains 24 compatible route segments and
|
||||
492,679 active (`STATE` 3/4) observations. Fourteen segments are HDA2 and ten
|
||||
are HDA1. The compatible set covers every architecture combination:
|
||||
HDA1/HDA2 and CCNC/non-CCNC.
|
||||
- Full geometry/kinematics data was populated only by EV9 and Ioniq 9. Both
|
||||
are HDA2 + CCNC, but HDA2 + CCNC Kona EV, EV6 2025, and K4 routes left those
|
||||
fields at their unavailable defaults. A second Ioniq 9 segment independently
|
||||
reproduced the rich extension.
|
||||
- The 36 canonical CAN-FD test routes contributed eight compatible 3A5
|
||||
segments and 150,489 active observations. This independently reproduced the
|
||||
layout on HDA1 and HDA2 cars.
|
||||
- An additional eight-segment local recording supplied 89,233 active track
|
||||
observations. Its replay fingerprint is unavailable, so it is used for
|
||||
layout/range evidence but not platform attribution.
|
||||
- The known reserved bits were rechecked across 320,454 additional K5 and
|
||||
local-recording frames; all remained zero.
|
||||
- The Sportage HEV 2026 forum route has an incompatible overlapping layout and
|
||||
is excluded from this matrix.
|
||||
- A downloaded Hyundai DBC archive supplied two related but non-bit-compatible
|
||||
object layouts. The BN7 front-radar DBC defines a persistent global object
|
||||
ID and `0/1/2/3 = unknown/stationary/moving/stopped`; the HMVS4 object DBC
|
||||
independently defines value 3 as stopped, a 7-bit quality level, and alive
|
||||
age/lifetime. Neither DBC directly contains the 210 or 3A5 radar layout.
|
||||
- The same archive's `FR_CMR_Obj` definition matches the first 124 bits of
|
||||
`0x235–0x248` exactly. Together with its 33 Hz cadence and co-location with
|
||||
`0x360–0x366`, this corrects the former corner-radar classification:
|
||||
`0x235–0x248` is a forward-camera object list.
|
||||
- The repository's Delphi `ESR.dbc` exactly matches the `0x500–0x53F` target
|
||||
packing and the 64-slot radar's `0x4E0/0x4E1/0x4E3` status packing. A fresh
|
||||
six-route full-segment check covered two 64-slot and four 32-slot variants.
|
||||
`0x4E0` radar speed correlated with `carState.vEgo` at 0.9976–0.99996 with
|
||||
0.02–0.19 m/s mean absolute error. Both 64-slot variants acknowledged 64
|
||||
tracks in `0x4E1`; the four 32-slot payloads decoded as 1, proving that
|
||||
their `0x4E1` uses a different dialect.
|
||||
|
||||
## 500-53F legacy front-radar layout
|
||||
|
||||
Every eight-byte message is one target slot. The lower 32 slots
|
||||
`0x500–0x51F` are required; some radar variants add an identically packed
|
||||
upper bank at `0x520–0x53F`. Inter-arrival timing on all six freshly checked
|
||||
full segments is about 50 ms, confirming the established 20 Hz cadence. A gap
|
||||
inside these logs makes frame count divided by total segment span misleading.
|
||||
|
||||
| Signal | Start bit | Size | Scale / offset | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `UNKNOWN_1` | 7 | 8 signed | raw | Preserve on 32-slot variants. Its low bits are active but do not behave like ESR target flags. |
|
||||
| `ONCOMING_ESR` | 0 | 1 | boolean | 64-slot only. Every flagged target in both full-segment checks had negative ground-frame speed. Clear does not distinguish stationary from same-direction moving. |
|
||||
| `GROUPING_CHANGED_ESR` | 1 | 1 | boolean | 64-slot ESR identity; exact tracker behavior remains to be correlated. |
|
||||
| `REL_LAT_SPEED_ESR` | 7 | 6 signed | 0.25 / 0 m/s | 64-slot ESR lateral target rate. It is not published for 32-slot variants. |
|
||||
| `AZIMUTH` | 12 | 10 signed | 0.1 / 0 deg | Corrected from the former 0.2-degree scale. Lateral projection now uses the angle directly rather than a compensating half-scale. |
|
||||
| `STATE` | 15 | 3 | enum | Track lifecycle; values 3 and 4 are the measured/coasted valid states used by the parser. |
|
||||
| `LONG_DIST` | 18 | 11 | 0.1 / 0 m | Target range. |
|
||||
| `REL_ACCEL` | 33 | 10 signed | 0.02 / 0 m/s² | 32-slot dialect scale retained from route behavior. |
|
||||
| `REL_ACCEL_ESR` | 33 | 10 signed | 0.05 / 0 m/s² | 64-slot ESR scale. Separate overlapping names prevent applying it to the 32-slot dialect. |
|
||||
| `WIDTH_ESR` | 37 | 4 | 0.5 / 0 m | 64-slot ESR target width. It was populated on both checked 64-slot routes and identically zero on all four 32-slot routes. |
|
||||
| `COUNTER` | 38 | 1 | 1 / 0 | Per-target rolling count. |
|
||||
| `BRIDGE_OBJECT_ESR` | 39 | 1 | boolean | 64-slot ESR identity; exact tracker behavior remains to be correlated. |
|
||||
| `REL_SPEED` | 53 | 14 signed | 0.01 / 0 m/s | Radial range rate. |
|
||||
| `MED_RANGE_MODE_ESR` | 55 | 2 | enum | 64-slot ESR medium-range operating-mode field. |
|
||||
|
||||
### 4E0-4E5 companion messages
|
||||
|
||||
`0x4E0` has the shared ESR status layout on both 32- and 64-slot variants:
|
||||
|
||||
| Signal | Start bit | Size | Scale / offset |
|
||||
| --- | ---: | ---: | --- |
|
||||
| `DSP_TIMESTAMP` | 5 | 7 | 2 / 0 ms |
|
||||
| `GROUP_COUNTER` | 6 | 2 | 1 / 0 |
|
||||
| `COMM_ERROR` | 14 | 1 | boolean |
|
||||
| `RADIUS_CURVATURE` | 13 | 14 signed | 1 / 0 m |
|
||||
| `SCAN_INDEX` | 31 | 16 | 1 / 0 |
|
||||
| `YAW_RATE` | 47 | 12 signed | 0.0625 / 0 deg/s |
|
||||
| `VEHICLE_SPEED` | 50 | 11 | 0.0625 / 0 m/s |
|
||||
|
||||
The 64-slot variant exactly matches ESR Status2 at `0x4E1`. On 32-slot
|
||||
variants only its low two-bit rolling counter is active; the other 62 bits
|
||||
are platform-static and do not acknowledge 32 tracks, so the ESR Status2
|
||||
names remain 64-slot-only. The `0x4E3` in-path path-ID bytes are shared across
|
||||
both variants, while its low status flags, counter, range mode, and alignment
|
||||
angle are confirmed only on the 64-slot variant.
|
||||
|
||||
| `0x4E1` 64-slot signal | Start bit | Size | Scale / offset |
|
||||
| --- | ---: | ---: | --- |
|
||||
| `GROUP_COUNTER` | 1 | 2 | 1 / 0 |
|
||||
| `MAXIMUM_TRACKS_ACK` | 7 | 6 | 1 / 1 |
|
||||
| `STEERING_ANGLE_ACK` | 10 | 11 | 1 / 0 deg |
|
||||
| `RAW_DATA_MODE`, `TRANSCEIVER_OPERATIONAL`, `INTERNAL_ERROR`, `RANGE_PERFORMANCE_ERROR`, `OVERHEAT_ERROR` | 11–15 | 1 each | boolean |
|
||||
| `TEMPERATURE` | 31 | 8 signed | 1 / 0 °C |
|
||||
| `GROUPING_MODE` | 33 | 2 | 1 / 0 |
|
||||
| `VEHICLE_SPEED_COMP_FACTOR` | 39 | 6 signed | 0.00195 / 1 |
|
||||
| `YAW_RATE_BIAS` | 47 | 8 signed | 0.125 / 0 deg/s |
|
||||
| `DSP_SOFTWARE_VERSION` | 55 | 16 | 1 / 0 |
|
||||
|
||||
| `0x4E3` signal | Start bit | Size | Scale / offset |
|
||||
| --- | ---: | ---: | --- |
|
||||
| `GROUP_COUNTER` | 1 | 2 | 1 / 0; 64-slot confirmed |
|
||||
| `MEDIUM_LONG_RANGE_MODE` | 3 | 2 | 1 / 0; 64-slot confirmed |
|
||||
| `PARTIAL_BLOCKAGE`, `SIDELOBE_BLOCKAGE`, `LONG_RANGE_GRATING_LOBE_DETECTED`, `TRUCK_TARGET_DETECTED` | 4–7 | 1 each; 64-slot confirmed |
|
||||
| `ACC_MOVING_PATH_ID` | 15 | 8 | one-based target slot; shared |
|
||||
| `CMBB_MOVING_PATH_ID`, `CMBB_STATIONARY_PATH_ID` | 23, 31 | 8 each | one-based target slots; shared |
|
||||
| `FCW_MOVING_PATH_ID`, `FCW_STATIONARY_PATH_ID` | 39, 47 | 8 each | protocol-defined; unexercised on sampled 32-slot routes |
|
||||
| `AUTO_ALIGN_ANGLE` | 55 | 8 signed | 0.0625 / 0 deg; 64-slot confirmed |
|
||||
| `ACC_STATIONARY_PATH_ID` | 63 | 8 | one-based target slot; shared |
|
||||
|
||||
All 7,347 nonzero `0x4E3` path-ID observations across the four 32-slot routes
|
||||
referenced an active `0x500–0x51F` target slot. IDs reached 32, proving
|
||||
one-based indexing. ACC and CMBB moving selectors commonly referenced the
|
||||
same slot, as did their stationary selectors.
|
||||
|
||||
The 32-slot variants additionally send `0x4E2`, `0x4E4`, and `0x4E5`:
|
||||
|
||||
- `0x4E2` consistently encodes plausible BCD build timestamps across all four
|
||||
sampled platforms.
|
||||
- `0x4E4` matches the byte order and address-shifted envelope of Delphi ESR
|
||||
Status5: switched-battery, ignition, two temperature, and four supply ADC
|
||||
channels. The identities are protocol-derived and the raw ADC values are
|
||||
intentionally uncalibrated; they must not drive a health decision.
|
||||
- `0x4E5` matches the similarly relocated ESR Status6 alignment payload. In
|
||||
4,698 checked frames its two factory-alignment fields used only documented
|
||||
enum values 0–3. The other flags and alignment-value bytes remained zero,
|
||||
so their identities are protocol-derived but not independently exercised.
|
||||
|
||||
Although generic ESR defines eight-byte input messages at `0x4F0/0x4F1`, no
|
||||
such messages were present on these source buses. The observed Hyundai
|
||||
`0x4F1` was four bytes on other/forwarded buses and is not assigned the ESR
|
||||
input schema.
|
||||
|
||||
## Signal matrix
|
||||
|
||||
| Signal | Status | Availability and evidence |
|
||||
| --- | --- | --- |
|
||||
| `CHECKSUM` | Confirmed core | 16-bit HKG CAN-FD checksum using the CAN address as data ID. It matched all 817,852 checked 3A5 frames. |
|
||||
| `COUNTER` | Confirmed core | Global transmit-cycle counter covering 0-255 and wrapping. |
|
||||
| `STATE_ALT` | Redundant | Exact compressed mirror in all 325,110 active samples: `STATE=3 -> 2`, `STATE=4 -> 3`. |
|
||||
| `MOTION_STATE` | Confirmed core | Ground-frame class: unknown, stationary, or moving. Correctly marked observed crossing traffic as moving. Related Hyundai radar data assigns the unused fourth 2-bit value to stopped, not oncoming; that bit remained zero throughout the tested 3A5 corpus. |
|
||||
| `TRACK_COUNTER` | Confirmed core | Modulo-4 per-track update counter. Separate from the transmit counter. |
|
||||
| `TRACK_QUALITY` | Confirmed core | Unsigned 7-bit track quality/existence score. Empty and tentative tracks are low, measured tracks are high, and coasted tracks decline with `COAST_AGE`. A related Hyundai object layout describes its 7-bit quality level as reliability, validity, or probability evidence; the radar's raw scale is not confirmed to be a percentage. |
|
||||
| `AGE` | Confirmed core | Per-track alive age/lifetime count, 0-255. |
|
||||
| `COAST_AGE` | Confirmed core | Zero for measured tracks and increments while a track is coasted/predicted. |
|
||||
| `STATE` | Confirmed core | Track lifecycle: empty, tentative, measured, coasted, or unresolved tentative. |
|
||||
| `RCS` | Confirmed core | Signed radar cross-section/return-strength value. At controlled distances its median consistently increases from small to passenger to large targets. The raw unit is not calibrated. |
|
||||
| `LONG_DIST` | Confirmed core | Longitudinal target distance in meters. |
|
||||
| `LAT_DIST` | Confirmed core | Lateral target position relative to the ego axis in meters. |
|
||||
| `REL_SPEED` | Confirmed core | Longitudinal target speed relative to ego. |
|
||||
| `NEW_SIGNAL_4` | Active unknown | Category 0-2. Nonzero in about 25% of moving samples, 10% of stationary samples, and 6% of unknown samples; it is related to target state but is not another motion class. |
|
||||
| `REL_LAT_SPEED` | Confirmed core | Lateral target speed relative to the ego axis. A live crossing target showed `+6.78 m/s` lateral versus `-0.51 m/s` forward. An EV9 route independently supplied hundreds of perpendicular-motion samples. |
|
||||
| `REL_ACCEL` | Confirmed core | Longitudinal target acceleration relative to ego. |
|
||||
| `NEW_SIGNAL_18` | Optional active unknown | A lifecycle/measurement-status attribute on rich layouts. In the public EV9/Ioniq 9 data, values 1/2 occur almost exclusively while `STATE=3` is measured and 0 while `STATE=4` is coasted; one anomalous coasted sample retained 2. The distinction between 1 and 2 is unknown. Most platforms always send 0, and a compatible long recording contains the rare value 3. |
|
||||
| `NEW_SIGNAL_5` | Optional active unknown | Sparse value observed from 0 to 32 on both HDA1 and HDA2 routes, including platforms without rich geometry. No stable correlation is known. |
|
||||
| `WIDTH` | Optional useful | Target width in 0.1 m units. Populated on EV9/Ioniq 9; passenger-car values cluster around 1.8-2.0 m. Zero means unavailable on other sampled platforms. Every address in 3A5-3C4 can carry it, so it does not identify a separate track bank. |
|
||||
| `LENGTH` | Optional useful | Target length in 0.1 m units. Passenger cars cluster around 4 m and long vehicles reach roughly 13-15.5 m. It is available on the same target samples as `WIDTH`. |
|
||||
| `ABS_SPEED` | Optional useful | Absolute target-speed magnitude. On Ioniq 9 it matches `hypot(vEgo + REL_SPEED, REL_LAT_SPEED)` with about 0.08-0.15 m/s median error. It was populated for 28,735/28,860 active Ioniq 9 samples and 23,252/27,339 EV9 samples, including many targets without dimensions. Zero is the platform default when unsupported. |
|
||||
| `ORIENTATION_ANGLE` | Optional useful | Target orientation relative to the ego axis. Ioniq 9 follows motion heading within roughly 2 degrees median; EV9 crossing traffic rotated from about -45 to -83 degrees. It is associated with dimension-bearing targets. Values near `+/-180` can represent real rearward orientation on rich tracks, while compatible non-rich platforms consistently use +180 as unavailable/default. |
|
||||
| `NEW_SIGNAL_13` | Optional active unknown | Shape/geometry metadata from 0-10. It was nonzero only on dimension-bearing targets in the EV9/Ioniq 9 routes. Confidence-like behavior is plausible but not established. |
|
||||
| `NEW_SIGNAL_12` | Optional active unknown | Strong geometry-age/maturity candidate. Two independent Ioniq 9 routes progressed `0 -> 1 -> 2 -> ... -> 10` and saturated at 10. It remains unnamed pending confirmation on another rich platform. |
|
||||
| `NEW_SIGNAL_14` | Optional active unknown | Shape/geometry category 0-3. It is zero when the associated geometry metadata is unavailable. |
|
||||
| `NEW_SIGNAL_15` | Optional active unknown | Shape/geometry category 0-2. It is zero when the associated geometry metadata is unavailable. |
|
||||
| `NEW_SIGNAL_16` | Optional active unknown | Shape/geometry category 0-3. It is zero when the associated geometry metadata is unavailable. |
|
||||
| `NEW_SIGNAL_17` | Optional active unknown | Shape/geometry category 0-2. It is zero when the associated geometry metadata is unavailable. The most common rich-layout `14/15/16/17` tuple is `2/2/2/1`. |
|
||||
|
||||
## Cross-family layout and naming findings
|
||||
|
||||
Both families expose 32 tracks at approximately 20 Hz per address:
|
||||
|
||||
| Layout | Envelope | Tracks per message | Shared core | Family-specific extension |
|
||||
| --- | --- | --- | --- | --- |
|
||||
| 210 detailed | `0x210-0x21F`, 32 bytes | 2 | State, motion, quality, age/coast age, RCS, range, relative velocity/acceleration | Detailed state/RCS/category are populated; compact object-ID bytes are normally zero |
|
||||
| 210 compact | `0x210-0x21F`, 32 bytes | 2 | Same bit positions and scales except for the documented lateral-speed sign convention | Uses `STATE_ALT`, per-target status and persistent 6-bit `OBJECT_ID`; detailed state/RCS/category remain zero |
|
||||
| 3A5 common | `0x3A5-0x3C4`, 24 bytes | 1 | Homologous first 16 bytes | Bytes 16-23 carry measurement status and optional dimensions, absolute speed, orientation, and geometry metadata |
|
||||
|
||||
The shared signal conclusions are:
|
||||
|
||||
- The old signed `NEW_SIGNAL_2` decoding was wrong. Raw values use 0-99 in the
|
||||
tested corpus, and the signed interpretation created an artificial wrap at
|
||||
63/64. The standardized name is `TRACK_QUALITY`, unsigned 7-bit.
|
||||
- Detailed 210 and 3A5 `TRACK_QUALITY` distributions are nearly identical.
|
||||
Compact 210 uses a higher calibration, commonly around 70 while measured,
|
||||
that falls toward the low 20s at long coast ages. The semantic name is
|
||||
shared, but no percentage unit is assigned.
|
||||
- The old `NEW_SIGNAL_8` is standardized as `RCS`. Detailed 210 and 3A5 have
|
||||
closely matching distributions, and size ordering is preserved within every
|
||||
tested distance band. 210 supplies eight signed bits; 3A5 supplies seven
|
||||
because the remaining bit extends `LONG_DIST` from 12 to 13 bits.
|
||||
- `NEW_SIGNAL_4` occupies the same physical two bits and has nearly identical
|
||||
0-2 distributions in detailed 210 and 3A5, but its exact category remains
|
||||
unknown.
|
||||
- Compact 210 `OBJECT_ID` remains strongly supported: it held through 99.9%
|
||||
of consecutive same-track updates and changed when a slot acquired a
|
||||
replacement target. A related Hyundai BN7 radar DBC independently defines
|
||||
`ObjectId` as a global ID that remains constant while an object is tracked.
|
||||
Sparse 3A5 `NEW_SIGNAL_5` does not behave like the same field and remains
|
||||
unknown.
|
||||
|
||||
## Oncoming classification
|
||||
|
||||
Oncoming candidates were cross-checked using fresh ego speed and
|
||||
`vEgo + REL_SPEED < -3 m/s`.
|
||||
|
||||
| Layout | Evidence | Conclusion |
|
||||
| --- | --- | --- |
|
||||
| 210 compact | Motion code 4 appeared on 4,870 active rows. Synchronized code-4 rows had -13.30 m/s median ground-frame longitudinal speed, with 3,516 strong oncoming candidates. Related Hyundai BN7 radar and HMVS4 object DBCs independently label motion value 3 as stopped. | `MOTION_STATE=3` is stopped and `MOTION_STATE=4` is the compact oncoming class. |
|
||||
| 210 detailed | Bit 29 never set across 317,965 active rows. Its 4,924 physics-based candidates remained in motion values 0-2. | No dedicated detailed-210 oncoming value. |
|
||||
| 3A5 common | Bit 29 remained zero in all 817,852 frames. Among 6,104 physics-based candidates across 14 routes, motion was 0 for 5,792, 1 for 312, and never 2. | No dedicated 3A5 oncoming value or flag. |
|
||||
|
||||
`NEW_SIGNAL_4` and `NEW_SIGNAL_18` also failed to isolate the 3A5 oncoming
|
||||
candidates. Do not widen 3A5 `MOTION_STATE` to three bits. If oncoming
|
||||
classification is needed for 3A5 or detailed 210, derive it from ego-relative
|
||||
kinematics with persistence, hysteresis, and lane plausibility.
|
||||
|
||||
## Platform and cluster conclusions
|
||||
|
||||
| Architecture | Compatible examples | Rich geometry |
|
||||
| --- | --- | --- |
|
||||
| HDA2 + CCNC | Kona EV 2, EV6 2025, K4, EV9, Ioniq 9 | EV9 and Ioniq 9 only |
|
||||
| HDA2 + non-CCNC | Ioniq 6, Niro EV 2, Carnival HEV | None observed |
|
||||
| HDA1 + CCNC | Kona 2/HEV, Sonata/HEV, K4, K5, Santa Fe 5 | None observed |
|
||||
| HDA1 + non-CCNC | Niro EV 2/HEV | None observed |
|
||||
|
||||
- HDA2 changes the normal 3A5 bus from bus 1 to bus 0, but does not imply
|
||||
rich extension support.
|
||||
- EV6 2025 carried a complete 3A5 range on buses 0 and 2. The bus-2 subset was
|
||||
a 99.6% exact byte-for-byte subset of bus 0, consistent with forwarding
|
||||
rather than a second radar. The runtime selects one active bus per range.
|
||||
- The optional bytes are attributes on the same 32 addresses, not extra
|
||||
objects. Dimension-bearing targets appeared throughout 3A5-3C4 rather than
|
||||
in a dedicated address subset.
|
||||
- Stock `CCNC_0x162` is a separate fused cluster-object output. Two HDA2 +
|
||||
CCNC Kona EV routes exercised lead, left/right, and both rear slots while
|
||||
every 3A5 width/length/absolute-speed/shape field remained unavailable.
|
||||
Rear-slot distance capped at 20 m and the stock lateral value was fixed at
|
||||
3.0 m. `LEAD_ALT` remained hidden in both sampled segments.
|
||||
- Therefore, the additional cluster tofus are real stock CCNC/ADAS outputs,
|
||||
but they cannot be reconstructed merely by reading the 3A5 rich extension.
|
||||
Rear tofus in particular need a source beyond the forward 3A5 radar.
|
||||
|
||||
## 235-248 forward-camera object layout
|
||||
|
||||
Each 32-byte message is one camera-object slot. `OBJECT_ID=0` is unused in the
|
||||
observed routes. The production parser normalizes the native camera motion
|
||||
enum to the shared `unknown/stationary/moving` display enum, but preserves the
|
||||
raw DBC value.
|
||||
|
||||
| Signal | Start bit | Size | Scale / offset | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `CHECKSUM` | 0 | 16 | 1 / 0 | Checksum field; platform variants exist, so the generator does not force the standard HKG algorithm. |
|
||||
| `COUNTER` | 16 | 8 | 1 / 0 | Transmit-cycle counter. |
|
||||
| `QUALITY` | 24 | 7 | 1 / 0 | Camera-object quality/reliability level. |
|
||||
| `AGE` | 32 | 8 | 1 / 0 | Object alive age. |
|
||||
| `MOTION_STATE` | 40 | 4 | 1 / 0 | Native camera motion class. |
|
||||
| `OBJECT_ID` | 44 | 7 | 1 / 0 | Persistent object ID; zero is unused. |
|
||||
| `WIDTH` | 52 | 7 | 0.05 / 0 m | Estimated width. |
|
||||
| `CLASSIFICATION` | 60 | 3 | 1 / 0 | Unknown, truck, car, motorcycle, bicycle, pedestrian, undecided. |
|
||||
| `LONG_DIST` | 64 | 13 | 0.05 / 0 m | Longitudinal relative position. |
|
||||
| `LAT_DIST` | 78 | 12 | 0.05 / -102.4 m | Lateral relative position. |
|
||||
| `REL_SPEED` | 91 | 12 | 0.05 / -100 m/s | Longitudinal relative velocity. |
|
||||
| `REL_LAT_SPEED` | 104 | 10 | 0.05 / -25 m/s | Lateral relative velocity. |
|
||||
| `REL_ACCEL` | 115 | 9 signed | 0.05 / 0 m/s² | Longitudinal relative acceleration. |
|
||||
| `UNKNOWN_1` | 125 | 12 | raw | Active extension; semantics unknown. |
|
||||
| `UNKNOWN_2` | 138 | 12 | raw | Active extension; semantics unknown. |
|
||||
| `UNKNOWN_3` | 151 | 10 | raw | Active extension; semantics unknown. |
|
||||
| `AZIMUTH` | 176 | 14 | 360/16384 / -180 deg | Route-validated high-resolution object azimuth. |
|
||||
|
||||
Native motion values are: 0 undefined, 1 standing, 2 parked, 3 stopped,
|
||||
4 unknown movable, 5 moving, 6 stopped oncoming, 7 unknown oncoming,
|
||||
8 moving oncoming, and 9 crossing bicycle. This is the only newly decoded
|
||||
family with explicit oncoming moving and stopped-oncoming values.
|
||||
|
||||
## 240/270-28F candidate corner-radar layout
|
||||
|
||||
The address sizes and symmetry form two complete sensor channels:
|
||||
|
||||
| Channel | Inferred physical side | Status | Object slots | Compact scan records |
|
||||
| --- | --- | --- | --- | --- |
|
||||
| A | right front | `240`, 16 B | `241–24F`, 15 × 24 B | `270–277`, 8 × 32 B, seven records each |
|
||||
| B | left front | `278`, 8 B | `279–287`, 15 × 24 B | `288–28F`, 8 × 32 B, seven records each |
|
||||
|
||||
Both channels run at about 20 Hz and validate with the HKG CAN-FD checksum in
|
||||
the sampled Ioniq 5, Palisade 2023, and EV6 data. All 288,021 checked frames
|
||||
from five complete segment samples passed. A synchronized Palisade
|
||||
parking-garage sweep strongly assigns A to the right-front sensor and B to
|
||||
the left-front sensor: A alone retained close objects at large negative
|
||||
lateral position, while B alone retained the corresponding positive-side
|
||||
objects. This is strong route/video evidence, but a controlled sensor
|
||||
occlusion remains the final physical confirmation.
|
||||
|
||||
The 24-byte tracked-object record is:
|
||||
|
||||
| Signal | Bit | Size | Scale / offset | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `UNKNOWN_CATEGORY` | 24 | 2 | raw | Stable within continuous same-slot tracks and changes mainly when a spatially different target replaces the slot. All values 0–3 occur on active objects, so this is not lifecycle or validity. |
|
||||
| `RCS` | 56 | 7 signed | 1 / 0 | Strong shared-core identification; raw unit uncalibrated. |
|
||||
| `LONG_DIST` | 63 | 13 | 0.05 / 0 m | Confirmed vehicle-frame longitudinal position. |
|
||||
| `LAT_DIST` | 76 | 11 signed | 0.05 / 0 m | Confirmed vehicle-frame lateral position. |
|
||||
| `UNKNOWN_BIT_87` | 87 | 1 | raw | Active unknown. |
|
||||
| `REL_SPEED` | 88 | 10 signed | 0.2 / 0 m/s | Confirmed longitudinal relative velocity. Four held-out Palisade samples exercised bit 97 independently of bit 96 at 351–357 m; consecutive distance changes matched approximately -52 m/s rather than the false positive speed produced by the former 9-bit decode. |
|
||||
| `REL_LAT_SPEED` | 98 | 10 signed | 0.05 / 0 m/s | Confirmed lateral relative velocity. |
|
||||
| `UNKNOWN_BITS_108_117` | 108 | 10 | raw | Former centered acceleration candidate. Its correlations with consecutive `REL_SPEED` derivatives were -0.445, -0.043, -0.009, -0.111, and -0.124 across five Ioniq 5, Palisade, and EV6 samples, so the physical name and scale were falsified. |
|
||||
|
||||
Empty object slots use the exact `LONG_DIST=204.7 m` (`0xFFE`) sentinel;
|
||||
`UNKNOWN_CATEGORY=0` must not be used as an empty test. This correction was
|
||||
validated on five Ioniq 5, Palisade 2023, and EV6 samples. Including
|
||||
category-zero objects produced longitudinal position-derivative correlations
|
||||
of 0.949, 0.603, 0.962, 0.948, and 0.967 across those samples.
|
||||
|
||||
The position decode was independently checked against simultaneous
|
||||
`210–21F` front-radar targets. The 116 unambiguous A matches and 86 B matches
|
||||
had longitudinal/lateral correlations of 0.999/0.991 and 0.999/0.993,
|
||||
respectively, with about 0.3 m mean absolute error. Across 14,939 consecutive
|
||||
same-slot updates, decoded longitudinal and lateral velocity correlated with
|
||||
position derivatives at 0.964 and 0.944. No lifecycle field has been
|
||||
identified, and the former acceleration candidate failed derivative checks.
|
||||
The remaining header, classification, dimension, ID, and health fields stay
|
||||
bit-preserved rather than borrowing names from the related front-radar core.
|
||||
|
||||
The first payload byte after the counter in `0x278` is
|
||||
`CHANNEL_A_OBJECT_COUNT`. Across 5,999 synchronized cycles on five Ioniq 5,
|
||||
Palisade, and EV6 samples, the number of non-sentinel `0x241–0x24F` slots equaled
|
||||
`min(CHANNEL_A_OBJECT_COUNT, 15)` exactly. It ranged from 0–22; values above
|
||||
15 prove it is the pre-truncation internal candidate count rather than merely
|
||||
the number of exported slots. The `0x240` status payload remains unresolved;
|
||||
notably, it was entirely zero after checksum/counter on one Palisade and one
|
||||
EV6 sample while being active on the other three routes.
|
||||
|
||||
The compact arrays are not alternate encodings of that object count. Depending
|
||||
on route and channel, 11–55 of their 56 bins were non-sentinel in a cycle,
|
||||
versus at most 15 exported tracked objects; count correlations ranged from
|
||||
weak to moderate and were never exact.
|
||||
|
||||
The compact 32-bit scan record is:
|
||||
|
||||
| Record field | Relative bit | Size | Scale | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `DISTANCE_CANDIDATE` | 0 | 13 | 0.05 m | Range-like distribution and 400 m endpoint; no synchronized object association, so physical distance is not confirmed. |
|
||||
| `RESERVED` | 13 | 3 | raw | Preserve. |
|
||||
| `PROPERTY` | 16 | 7 | raw | Active categorical unknown; sampled values were quantized. |
|
||||
| `AUX` | 23 | 9 | raw | Active unknown; velocity and split flag/state hypotheses were falsified. |
|
||||
|
||||
Record 0 starts at message bit 24, message bit 56 is a frame-status byte, and
|
||||
records 1–6 start at bits 64, 96, 128, 160, 192, and 224. Unused records
|
||||
commonly contain `0x010D1F40`, with AUX variants such as `0x018D1F40`; its
|
||||
low-field component is the 400 m endpoint. Contemporaneous range matching was no
|
||||
better than the opposite channel or a 500 ms-shifted control. Each of the 56
|
||||
record positions instead has a characteristic occupancy/range profile, making
|
||||
this a strong fixed scan/bin-array candidate rather than 56 freely assigned
|
||||
tracked detections. A direct linear record-index-to-angle search on Ioniq 5
|
||||
and Palisade data also performed no better than a 500 ms-shifted control.
|
||||
Decoding the low field, physical ordering, `PROPERTY`, and `AUX` remains open.
|
||||
|
||||
## 3D0-3D4 front-radar auxiliary scan records
|
||||
|
||||
This optional list accompanies `3A5–3C4` on sampled EV9 and Ioniq 9 routes.
|
||||
Five 32-byte messages contain seven record positions each; the final three
|
||||
positions of `3D4` are outside the 32-position list. The repeated
|
||||
`0xC8782EE0` value and, on EV9, zero are unused records. All 12,090 checked
|
||||
message frames passed the HKG CAN-FD checksum and carried the same cycle
|
||||
counter as `3A5–3C4`.
|
||||
|
||||
| Record field | Relative bit | Size | Scale / offset | Status |
|
||||
| --- | ---: | ---: | --- | --- |
|
||||
| `DISTANCE_CANDIDATE` | 0 | 12 | 0.05 / 0 m | Range-like distribution and endpoint, but no synchronized tracked-object match; not confirmed. |
|
||||
| `FLAGS_UNKNOWN` | 12 | 4 | raw | Active values 0–2 in the sampled routes; semantics unknown. |
|
||||
| `UNKNOWN_BYTE_16` | 16 | 8 | raw | Former radial-speed candidate was falsified. |
|
||||
| `UNKNOWN_BYTE_24` | 24 | 8 | raw | Former azimuth candidate was falsified. |
|
||||
|
||||
Packing positions match the corner scan records: record 0 at message bit 24,
|
||||
one alignment byte at bit 56, then records 1–6 at bits 64 through 224. However,
|
||||
the exact corner `13/3/7/9` field split is not transferred onto this
|
||||
`12/4/8/8` layout without evidence.
|
||||
|
||||
The stronger synchronized test falsified three tempting interpretations:
|
||||
|
||||
- Range-only, candidate polar-geometry, and direct record-index-to-track-index
|
||||
associations were no better than opposite or 500 ms-shifted controls.
|
||||
- `UNKNOWN_BYTE_16` had essentially zero correlation with `3A5` longitudinal
|
||||
or radial relative speed.
|
||||
- `UNKNOWN_BYTE_24` had essentially zero correlation with `3A5` target angle.
|
||||
|
||||
The 32 positions do show stable position-dependent occupancy and low-field
|
||||
profiles, so a fixed scan/bin or intermediate radar-array role remains
|
||||
plausible. They are not independent tracked objects, remain excluded from
|
||||
production RadarData, and are shown only as raw records in the debugger's
|
||||
`SIGNALS` table, never projected as geometry.
|
||||
|
||||
## 360-366 forward-camera lane/path family
|
||||
|
||||
These seven 32-byte messages belong to the forward camera. `362` already
|
||||
contains left/right lane-line confidence. `360`, `361`, `363`, and `364`
|
||||
carry changing lane/path polynomial-like geometry; their field boundaries are
|
||||
not yet decoded. `365` and `366` are commonly constant/default-filled,
|
||||
consistent with unavailable extra lane slots. Their presence or emptiness
|
||||
must not be interpreted as a radar hardware combination.
|
||||
|
||||
## Reserved/dead bits
|
||||
|
||||
Bits `26`, `29`, `39`, `55`, `117`, `136-137`, `143`, `171`, and
|
||||
`188-191` remain zero. No new signal location was found in the follow-up
|
||||
routes.
|
||||
|
||||
## Parser guidance
|
||||
|
||||
- Detect support per radar source instead of treating extension defaults as
|
||||
real measurements.
|
||||
- Treat zero dimensions as unavailable. Treat +180 as unavailable on non-rich
|
||||
platforms, but do not discard near-180 rich tracks without checking their
|
||||
other geometry and lifecycle fields.
|
||||
- Do not use any `NEW_SIGNAL_*` field for control or filtering until its
|
||||
meaning is independently validated.
|
||||
- `TRACK_QUALITY` and `RCS` are decoded for research and diagnostics, but
|
||||
neither has a calibrated physical/percentage scale suitable for control.
|
||||
- `REL_LAT_SPEED` is suitable for debugging cross traffic, but a production
|
||||
cross-traffic filter still needs lane-change validation and track history.
|
||||
@@ -1,343 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import concurrent.futures
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from collections import Counter, defaultdict
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from msgq.ipc_pyx import IpcError
|
||||
from opendbc.car.tests.routes import CarTestRoute, routes
|
||||
from openpilot.tools.radar.custom_routes import CUSTOM_ROUTES
|
||||
from openpilot.tools.radar.radar_helpers import (
|
||||
build_seen_address_map,
|
||||
get_radar_spec,
|
||||
is_exclusive_full_range_match,
|
||||
)
|
||||
|
||||
|
||||
REPLAY_PATH = os.path.join(os.path.dirname(__file__), "..", "replay", "replay")
|
||||
REPLAY_PLAYBACK_SPEED = "3.0"
|
||||
REPLAY_ALLOW_SERVICES = "can"
|
||||
DETECTION_TIMEOUT_SECONDS = 20.0
|
||||
DETECTION_MIN_HITS = 10
|
||||
SOCKET_WAIT_TIMEOUT_SECONDS = 10.0
|
||||
CAR_MODEL_WIDTH = 38
|
||||
RADAR_TYPE_WIDTH = 10
|
||||
|
||||
ANSI_RESET = "\033[0m"
|
||||
ANSI_BOLD = "\033[1m"
|
||||
ANSI_DIM = "\033[2m"
|
||||
ANSI_RED = "\033[31m"
|
||||
ANSI_GREEN = "\033[32m"
|
||||
ANSI_YELLOW = "\033[33m"
|
||||
ANSI_BLUE = "\033[34m"
|
||||
ANSI_MAGENTA = "\033[35m"
|
||||
ANSI_CYAN = "\033[36m"
|
||||
|
||||
RADAR_FAMILY_COLORS = {
|
||||
"RADAR_500_51F": ANSI_CYAN,
|
||||
"RADAR_210_21F": ANSI_YELLOW,
|
||||
"RADAR_3A5_3C4": ANSI_GREEN,
|
||||
"RADAR_602_611": ANSI_BLUE,
|
||||
}
|
||||
|
||||
|
||||
def build_arg_parser() -> argparse.ArgumentParser:
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Replay Hyundai/Kia/Genesis test routes and auto-detect radar type.",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
|
||||
)
|
||||
parser.add_argument("--routes", action="store_true",
|
||||
help="Use Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.")
|
||||
parser.add_argument("--custom-routes", action="store_true",
|
||||
help="Use routes from tools/radar/custom_routes.py.")
|
||||
parser.add_argument("--route", action="append", default=[],
|
||||
help="Specific route(s) to test. If omitted, use Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.")
|
||||
parser.add_argument("--data-dir", default=None,
|
||||
help="Optional local directory of route data to pass through to replay.")
|
||||
parser.add_argument("--timeout", type=float, default=DETECTION_TIMEOUT_SECONDS,
|
||||
help="Seconds to wait for a radar-family detection before giving up on a route.")
|
||||
parser.add_argument("--min-hits", type=int, default=DETECTION_MIN_HITS,
|
||||
help="Minimum CAN hits in a radar-family address range before considering it detected.")
|
||||
parser.add_argument("--playback", default=REPLAY_PLAYBACK_SPEED,
|
||||
help="Replay playback speed.")
|
||||
parser.add_argument("--limit", type=int, default=None,
|
||||
help="Only process the first N matching routes.")
|
||||
parser.add_argument("--jobs", type=int, default=1,
|
||||
help="Number of routes to process in parallel.")
|
||||
parser.add_argument("--prefix", default="auto-radar-detect",
|
||||
help="Base OPENPILOT_PREFIX to isolate replay sockets.")
|
||||
return parser
|
||||
|
||||
|
||||
def get_hkg_routes() -> list[CarTestRoute]:
|
||||
hkg_prefixes = ("HYUNDAI_", "KIA_", "GENESIS_")
|
||||
return [
|
||||
route for route in routes
|
||||
if route.car_model is not None and getattr(route.car_model, "name", str(route.car_model)).startswith(hkg_prefixes)
|
||||
]
|
||||
|
||||
|
||||
def get_selected_routes(args: argparse.Namespace) -> list[CarTestRoute]:
|
||||
selected_sources = int(bool(args.route)) + int(bool(args.routes)) + int(bool(args.custom_routes))
|
||||
if selected_sources > 1:
|
||||
raise ValueError("Use only one of --route, --routes, or --custom-routes.")
|
||||
|
||||
if args.route:
|
||||
route_map = {route.route: route for route in get_hkg_routes()}
|
||||
selected = []
|
||||
for route_name in args.route:
|
||||
selected.append(route_map.get(route_name, CarTestRoute(route_name, None)))
|
||||
return selected[:args.limit] if args.limit is not None else selected
|
||||
|
||||
if args.custom_routes:
|
||||
selected = [CarTestRoute(route.route, route.car_model) for route in CUSTOM_ROUTES]
|
||||
return selected[:args.limit] if args.limit is not None else selected
|
||||
|
||||
selected = get_hkg_routes()
|
||||
return selected[:args.limit] if args.limit is not None else selected
|
||||
|
||||
|
||||
def terminate_process(proc: subprocess.Popen) -> None:
|
||||
if proc.poll() is not None:
|
||||
return
|
||||
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except subprocess.TimeoutExpired:
|
||||
proc.kill()
|
||||
proc.wait(timeout=5)
|
||||
|
||||
|
||||
def start_replay(route: str, prefix: str, args: argparse.Namespace) -> subprocess.Popen:
|
||||
cmd = [
|
||||
REPLAY_PATH,
|
||||
"--no-vipc",
|
||||
"--no-loop",
|
||||
"--playback", args.playback,
|
||||
"--allow", REPLAY_ALLOW_SERVICES,
|
||||
"--prefix", prefix,
|
||||
]
|
||||
if args.data_dir:
|
||||
cmd.extend(["--data_dir", args.data_dir])
|
||||
cmd.append(route)
|
||||
|
||||
env = os.environ.copy()
|
||||
env["OPENPILOT_PREFIX"] = prefix
|
||||
return subprocess.Popen(
|
||||
cmd,
|
||||
cwd=os.path.dirname(REPLAY_PATH),
|
||||
env=env,
|
||||
stdout=subprocess.DEVNULL,
|
||||
stderr=subprocess.DEVNULL,
|
||||
preexec_fn=os.setsid if os.name != "nt" else None,
|
||||
)
|
||||
|
||||
|
||||
def stop_replay(proc: subprocess.Popen) -> None:
|
||||
if proc.poll() is not None:
|
||||
return
|
||||
|
||||
if os.name != "nt":
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGTERM)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
else:
|
||||
proc.terminate()
|
||||
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except subprocess.TimeoutExpired:
|
||||
if os.name != "nt":
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGKILL)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
else:
|
||||
proc.kill()
|
||||
proc.wait(timeout=5)
|
||||
|
||||
|
||||
def wait_for_can_socket(prefix: str, timeout: float) -> messaging.SubSocket:
|
||||
socket_path = os.path.join("/tmp", f"msgq_{prefix}", "can")
|
||||
started = time.monotonic()
|
||||
while True:
|
||||
if os.path.exists(socket_path):
|
||||
break
|
||||
if time.monotonic() - started > timeout:
|
||||
raise TimeoutError(f"Timed out waiting for CAN socket at {socket_path}")
|
||||
time.sleep(0.1)
|
||||
|
||||
while True:
|
||||
try:
|
||||
return messaging.sub_sock("can", conflate=False, timeout=100)
|
||||
except IpcError:
|
||||
if time.monotonic() - started > timeout:
|
||||
raise
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def detect_radar_family(route: CarTestRoute, route_idx: int, args: argparse.Namespace) -> dict:
|
||||
prefix = f"{args.prefix}-{os.getpid()}-{route_idx}"
|
||||
os.environ["OPENPILOT_PREFIX"] = prefix
|
||||
messaging.reset_context()
|
||||
proc = start_replay(route.route, prefix, args)
|
||||
logcan = wait_for_can_socket(prefix, min(args.timeout, SOCKET_WAIT_TIMEOUT_SECONDS))
|
||||
counts = Counter()
|
||||
buses = defaultdict(set)
|
||||
seen_addresses = build_seen_address_map()
|
||||
started = time.monotonic()
|
||||
detected = None
|
||||
|
||||
try:
|
||||
while True:
|
||||
if proc.poll() is not None and (time.monotonic() - started) > 1.0:
|
||||
break
|
||||
if time.monotonic() - started > args.timeout:
|
||||
break
|
||||
|
||||
msgs = messaging.drain_sock(logcan, wait_for_one=True)
|
||||
for msg in msgs:
|
||||
for can_msg in msg.can:
|
||||
radar_spec = get_radar_spec(can_msg.address)
|
||||
if radar_spec is not None:
|
||||
counts[radar_spec.name] += 1
|
||||
buses[radar_spec.name].add(can_msg.src)
|
||||
seen_addresses[radar_spec.name].add(can_msg.address)
|
||||
if counts[radar_spec.name] >= args.min_hits and is_exclusive_full_range_match(radar_spec, seen_addresses):
|
||||
detected = radar_spec.name
|
||||
if detected is not None:
|
||||
break
|
||||
if detected is not None:
|
||||
break
|
||||
if detected is not None:
|
||||
break
|
||||
finally:
|
||||
stop_replay(proc)
|
||||
|
||||
dominant_family = detected
|
||||
|
||||
return {
|
||||
"route": route.route,
|
||||
"car_model": str(route.car_model) if route.car_model is not None else "UNKNOWN",
|
||||
"detected": dominant_family or "NONE",
|
||||
"counts": dict(counts),
|
||||
"buses": {name: sorted(bus_set) for name, bus_set in buses.items()},
|
||||
"seen_addresses": {name: sorted(addresses) for name, addresses in seen_addresses.items()},
|
||||
}
|
||||
|
||||
|
||||
def print_results(results: list[dict]) -> None:
|
||||
print(f"{'Car Model':<{CAR_MODEL_WIDTH}} {'Detected':<{RADAR_TYPE_WIDTH}} Route")
|
||||
print("-" * (CAR_MODEL_WIDTH + RADAR_TYPE_WIDTH + 9 + 60))
|
||||
for result in results:
|
||||
detected_plain = "" if result["detected"] == "NONE" else result["detected"]
|
||||
model = format_car_model(result["car_model"])[:CAR_MODEL_WIDTH]
|
||||
detected = f"{detected_plain:<{RADAR_TYPE_WIDTH}}"
|
||||
if supports_color() and detected_plain:
|
||||
detected = f"{colorize_detected(detected_plain)}" + " " * max(0, RADAR_TYPE_WIDTH - len(detected_plain))
|
||||
print(f"{model:<{CAR_MODEL_WIDTH}} {detected} {result['route']}")
|
||||
|
||||
|
||||
def supports_color() -> bool:
|
||||
return sys.stdout.isatty() and os.getenv("TERM") not in (None, "dumb")
|
||||
|
||||
|
||||
def format_car_model(car_model: object) -> str:
|
||||
return str(car_model) if car_model is not None else "UNKNOWN"
|
||||
|
||||
|
||||
def colorize_detected(detected: str) -> str:
|
||||
if detected == "NONE":
|
||||
return ""
|
||||
|
||||
if not supports_color():
|
||||
return detected
|
||||
|
||||
color = RADAR_FAMILY_COLORS.get(detected, ANSI_MAGENTA)
|
||||
return f"{color}{detected}{ANSI_RESET}"
|
||||
|
||||
|
||||
def print_progress_line(index: int, total: int, route: CarTestRoute, detected: str) -> None:
|
||||
left = f"[{index}/{total}]"
|
||||
model = format_car_model(route.car_model)[:CAR_MODEL_WIDTH]
|
||||
detected_plain = "" if detected == "NONE" else detected
|
||||
route_text = route.route
|
||||
if supports_color():
|
||||
left = f"{ANSI_DIM}{left}{ANSI_RESET}"
|
||||
model = f"{ANSI_CYAN}{model:<{CAR_MODEL_WIDTH}}{ANSI_RESET}"
|
||||
detected_text = colorize_detected(detected_plain).ljust(len(colorize_detected(detected_plain)))
|
||||
route_text = f"{ANSI_DIM}{route_text}{ANSI_RESET}"
|
||||
else:
|
||||
model = f"{model:<{CAR_MODEL_WIDTH}}"
|
||||
detected_text = detected_plain
|
||||
|
||||
detected_text = f"{detected_plain:<{RADAR_TYPE_WIDTH}}" if not supports_color() else (
|
||||
f"{colorize_detected(detected_plain)}" + " " * max(0, RADAR_TYPE_WIDTH - len(detected_plain))
|
||||
)
|
||||
|
||||
print(f"{left} {model} {detected_text} {route_text}")
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = build_arg_parser().parse_args(sys.argv[1:])
|
||||
try:
|
||||
selected_routes = get_selected_routes(args)
|
||||
except ValueError as e:
|
||||
print(str(e), file=sys.stderr)
|
||||
return 2
|
||||
if not selected_routes:
|
||||
print("No matching routes found.", file=sys.stderr)
|
||||
return 1
|
||||
|
||||
results = []
|
||||
total = len(selected_routes)
|
||||
jobs = max(1, min(args.jobs, total))
|
||||
|
||||
header_model = "Car Model"
|
||||
header_detected = "Radar"
|
||||
if supports_color():
|
||||
header_model = f"{ANSI_BOLD}{header_model:<{CAR_MODEL_WIDTH}}{ANSI_RESET}"
|
||||
header_detected = f"{ANSI_BOLD}{header_detected:<{RADAR_TYPE_WIDTH}}{ANSI_RESET}"
|
||||
else:
|
||||
header_model = f"{header_model:<{CAR_MODEL_WIDTH}}"
|
||||
header_detected = f"{header_detected:<{RADAR_TYPE_WIDTH}}"
|
||||
print(f" {header_model} {header_detected} Route")
|
||||
|
||||
if jobs == 1:
|
||||
for idx, route in enumerate(selected_routes):
|
||||
result = detect_radar_family(route, idx, args)
|
||||
print_progress_line(idx + 1, total, route, result['detected'])
|
||||
results.append((idx, result))
|
||||
else:
|
||||
with concurrent.futures.ProcessPoolExecutor(max_workers=jobs) as executor:
|
||||
future_to_job = {
|
||||
executor.submit(detect_radar_family, route, idx, args): (idx, route)
|
||||
for idx, route in enumerate(selected_routes)
|
||||
}
|
||||
completed = 0
|
||||
for future in concurrent.futures.as_completed(future_to_job):
|
||||
idx, route = future_to_job[future]
|
||||
result = future.result()
|
||||
completed += 1
|
||||
print_progress_line(completed, total, route, result['detected'])
|
||||
results.append((idx, result))
|
||||
|
||||
results = sorted(
|
||||
(result for _, result in results),
|
||||
key=lambda result: (result["car_model"], result["route"]),
|
||||
)
|
||||
|
||||
print()
|
||||
print_results(results)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -1,68 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CustomRoute:
|
||||
car_model: str
|
||||
route: str
|
||||
|
||||
|
||||
CUSTOM_ROUTES = [
|
||||
CustomRoute("HYUNDAI_ELANTRA_HEV_2024", "07a48901db7b2503|0000012d--61aba9f832"),
|
||||
CustomRoute("HYUNDAI_IONIQ_5", "c5a5f79df9b6a084/0000005d--24c0a7a7ee"),
|
||||
CustomRoute("HYUNDAI_IONIQ_5", "90950642f47cf05b/00000681--6a904aaa98"),
|
||||
CustomRoute("HYUNDAI_IONIQ_5", "102161de13e822d1/0000000d--f74a52a624"),
|
||||
CustomRoute("HYUNDAI_PALISADE_2023", "17ef028fec0ccf81/00000010--dba8455015"),
|
||||
CustomRoute("HYUNDAI_PALISADE_2023", "6b8052042ba3ee05/00000199--3ed3fb5799"),
|
||||
CustomRoute("KIA_EV6", "455a0ab75ce5c1e0/000000d1--098f32028c"),
|
||||
CustomRoute("KIA_EV6", "6d2092783bf67457/0000008c--5caf525813"),
|
||||
CustomRoute("KIA_EV6", "4961cb0f7bdd77a2/00000105--b636ed2f65"),
|
||||
CustomRoute("KIA_K8_HEV_1ST_GEN", "78ad5150de133637|2023-09-13--16-15-57"),
|
||||
CustomRoute("GENESIS_GV60_EV_1ST_GEN", "b1e441e63f1c99dd|0000009d--14cde5aeaf"),
|
||||
CustomRoute("HYUNDAI_IONIQ_6", "faa815c21279fef7/00000187--ce36ae7c11"),
|
||||
CustomRoute("HYUNDAI_IONIQ_6", "faa815c21279fef7/00000191--e5c3ffb55b"),
|
||||
CustomRoute("HYUNDAI_IONIQ_6", "ce8a3764a8bb9141/000000f6--775693a6d6"),
|
||||
CustomRoute("HYUNDAI_IONIQ_6", "ce8a3764a8bb9141/00000100--ba8bd692f5"),
|
||||
CustomRoute("HYUNDAI_KONA_2ND_GEN", "32025f26789d8fab/00000022--a499e8ffa3"),
|
||||
CustomRoute("HYUNDAI_KONA_EV_2ND_GEN", "1618132d68afc876/00000021--bf0f957649"),
|
||||
CustomRoute("HYUNDAI_KONA_HEV_2ND_GEN", "97ca61196eb73e0d/00000052--4555329470"),
|
||||
CustomRoute("HYUNDAI_SANTA_FE_HEV_5TH_GEN", "d54302f1d5e7a7cc|00000656--5dae9f54a7"),
|
||||
CustomRoute("HYUNDAI_SONATA_2024", "4267ea8a353cdb36/00000262--8a427003c7"),
|
||||
CustomRoute("KIA_CARNIVAL_HEV_4TH_GEN_2026", "7b8cc7bb46000e53/0000000b--2abacaff78"),
|
||||
CustomRoute("KIA_EV6_2025", "48c27f77f9fd1a9b|00000199--193ee1ba20"),
|
||||
CustomRoute("KIA_EV9", "ccfd4a1af758ee73/00000091--7fa49719a5"),
|
||||
CustomRoute("KIA_K4_2025", "baf39eeaba1217ca/00000002--b36e3fa031"),
|
||||
CustomRoute("KIA_K4_2025", "ac30936e794b297a/00000021--dde25fcfda/24"),
|
||||
CustomRoute("KIA_K5_2025", "c4a804b067623789/0000007c--163f831540"),
|
||||
CustomRoute("KIA_NIRO_EV_2ND_GEN", "80b8e9a6ad0acec3/0000032d--8379197bd3"),
|
||||
CustomRoute("KIA_NIRO_EV_2ND_GEN", "80b8e9a6ad0acec3/0000032c--0055eeee96"),
|
||||
CustomRoute("KIA_NIRO_EV_2ND_GEN", "cdde60a79a049bf4/00000002--f448ca40b4"),
|
||||
CustomRoute("KIA_NIRO_HEV_2ND_GEN", "0d4257d1c6741384/00000067--42230e4b8d"),
|
||||
CustomRoute("HYUNDAI_KONA_EV_2022", "e174e1a0b92263ed/00000003--d0591c14ec"),
|
||||
CustomRoute("HYUNDAI_KONA_EV_2022", "e174e1a0b92263ed/00000001--18955a967e"),
|
||||
CustomRoute("KIA_CEED_PHEV", "26064e18b24ae44c/00000000--4eb95fc137"),
|
||||
CustomRoute("HYUNDAI_IONIQ", "ac27c9808ac91710/00000002--e0aab81e4d"),
|
||||
CustomRoute("KIA_K7_2017", "74fbff45aa20fe9e/00000010--6f173d5799"),
|
||||
CustomRoute("KIA_NIRO_EV", "b27d9eafeb61976e/00000233--03a9c0858c"),
|
||||
CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000157--ecaf77a801"),
|
||||
CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000158--f7e70e1435"),
|
||||
CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000154--3471fc4ab3"),
|
||||
CustomRoute("KIA_SORENTO_4TH_GEN", "bf42073ef09e0af2/00000017--3edc1f8259"),
|
||||
CustomRoute("KIA_SORENTO_HEV_4TH_GEN", "833262b0c9e4016a/0000004a--0227a058e3"),
|
||||
CustomRoute("HYUNDAI_PALISADE", "662bedbf8453b81e/00000124--7dc358f20c"),
|
||||
CustomRoute("HYUNDAI_SONATA", "8f52823c702cb300/00000177--177d1aa6ff"),
|
||||
CustomRoute("HYUNDAI_SONATA_HYBRID", "e3ae9e987a2a4e57/0000002b--30a6963e88"),
|
||||
CustomRoute("HYUNDAI_ELANTRA_HEV_2021", "7bc0de9da607c543/00000031--4cb0d8aca5"),
|
||||
CustomRoute("HYUNDAI_ELANTRA_HEV_2021", "7bc0de9da607c543/00000042--302b371266"),
|
||||
CustomRoute("HYUNDAI_IONIQ_HEV_2022", "4a975fc1d9e71801/00000005--50d176008a"),
|
||||
CustomRoute("HYUNDAI_IONIQ_9", "71e4e67d29034771/00000014--25fc6b216f"),
|
||||
CustomRoute("HYUNDAI_IONIQ_9", "71e4e67d29034771/0000000e--d3b19ad6f6/10"),
|
||||
CustomRoute("HYUNDAI_SANTA_CRUZ_2025", "6e7904b03a4aafc2/00000010--31034184b6"),
|
||||
CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "26da1db30eff4fcc/00000061--a60470e363"),
|
||||
CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "a6d25e95d936fdc4/000002ed--89d7944c52"),
|
||||
CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "6722665fbbf2a644/00000534--4ab7b733c5"),
|
||||
CustomRoute("HYUNDAI_TUCSON_HEV_2025", "5868ec006e2bb61e/00000021--7d10df95e8"),
|
||||
CustomRoute("KIA_SPORTAGE_5TH_GEN", "ce05e32158479f42/000003f4--9543087719"),
|
||||
CustomRoute("KIA_SPORTAGE_HEV_2026", "343d5e350abaedcf/00000020--1c864750b6"),
|
||||
]
|
||||
@@ -1,329 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from collections import Counter, defaultdict
|
||||
from collections.abc import Iterable, Iterator
|
||||
from pathlib import Path
|
||||
import re
|
||||
import sys
|
||||
from urllib.parse import unquote
|
||||
|
||||
|
||||
SUNNYPILOT_ROOT = Path(__file__).resolve().parents[4]
|
||||
if (SUNNYPILOT_ROOT / "tools/lib/logreader.py").exists():
|
||||
sys.path.insert(0, str(SUNNYPILOT_ROOT))
|
||||
|
||||
|
||||
DEFAULT_ROUTES = """
|
||||
faa815c21279fef7/00000187--ce36ae7c11
|
||||
faa815c21279fef7/00000191--e5c3ffb55b
|
||||
ce8a3764a8bb9141/000000f6--775693a6d6
|
||||
ce8a3764a8bb9141/00000100--ba8bd692f5
|
||||
32025f26789d8fab/00000022--a499e8ffa3
|
||||
1618132d68afc876/00000021--bf0f957649
|
||||
97ca61196eb73e0d/00000052--4555329470
|
||||
d54302f1d5e7a7cc/00000656--5dae9f54a7
|
||||
4267ea8a353cdb36/00000262--8a427003c7
|
||||
7b8cc7bb46000e53/0000000b--2abacaff78
|
||||
48c27f77f9fd1a9b/00000199--193ee1ba20
|
||||
ccfd4a1af758ee73/00000091--7fa49719a5
|
||||
baf39eeaba1217ca/00000002--b36e3fa031
|
||||
ac30936e794b297a/00000021--dde25fcfda
|
||||
c4a804b067623789/0000007c--163f831540
|
||||
80b8e9a6ad0acec3/0000032d--8379197bd3
|
||||
80b8e9a6ad0acec3/0000032c--0055eeee96
|
||||
cdde60a79a049bf4/00000002--f448ca40b4
|
||||
0d4257d1c6741384/00000067--42230e4b8d
|
||||
343d5e350abaedcf/00000020--1c864750b6
|
||||
71e4e67d29034771/00000014--25fc6b216f
|
||||
71e4e67d29034771/0000000e--d3b19ad6f6
|
||||
07a48901db7b2503/0000012d--61aba9f832
|
||||
c5a5f79df9b6a084/0000005d--24c0a7a7ee
|
||||
90950642f47cf05b/00000681--6a904aaa98
|
||||
102161de13e822d1/0000000d--f74a52a624
|
||||
17ef028fec0ccf81/00000010--dba8455015
|
||||
6b8052042ba3ee05/00000199--3ed3fb5799
|
||||
464ac801173f6dca/00000382--27ce9e4ade
|
||||
455a0ab75ce5c1e0/000000d1--098f32028c
|
||||
6d2092783bf67457/0000008c--5caf525813
|
||||
4961cb0f7bdd77a2/00000105--b636ed2f65
|
||||
f9522b6cd6e4e621/00000170--ac6979bdda
|
||||
ed2717e2c1912f87/00000009--d550f4d5b6
|
||||
78ad5150de133637/2023-09-13--16-15-57
|
||||
6e7904b03a4aafc2/00000010--31034184b6
|
||||
26da1db30eff4fcc/00000061--a60470e363
|
||||
a6d25e95d936fdc4/000002ed--89d7944c52
|
||||
6722665fbbf2a644/00000534--4ab7b733c5
|
||||
477e0b6c352e8bba/0000000e--5e3a5c981c
|
||||
5868ec006e2bb61e/00000021--7d10df95e8
|
||||
ce05e32158479f42/000003f4--9543087719
|
||||
ac27c9808ac91710/00000002--e0aab81e4d
|
||||
74fbff45aa20fe9e/00000010--6f173d5799
|
||||
b27d9eafeb61976e/00000233--03a9c0858c
|
||||
f6324c8fb8b6f68d/00000058--5330471811
|
||||
e174e1a0b92263ed/00000001--18955a967e
|
||||
26064e18b24ae44c/00000000--4eb95fc137
|
||||
4709fe68b0cc9c0a/00000125--ed2a02e7a8
|
||||
4709fe68b0cc9c0a/0000012b--f29c054b05
|
||||
ce7eada09318e376/0000000e--0cc3d1c6cc
|
||||
662bedbf8453b81e/00000124--7dc358f20c
|
||||
4a975fc1d9e71801/00000005--50d176008a
|
||||
3e4f79d5bf692518/00000001--f53d01f8e3
|
||||
3e4f79d5bf692518/00000002--b18171f9a9
|
||||
810a7ca7cb31ed86/00000001--b6f9cdd134
|
||||
224d1e774b6acc2e/0000001d--0ae7d5fa70
|
||||
8f52823c702cb300/00000177--177d1aa6ff
|
||||
e3ae9e987a2a4e57/0000002b--30a6963e88
|
||||
7bc0de9da607c543/00000031--4cb0d8aca5
|
||||
7bc0de9da607c543/00000042--302b371266
|
||||
bf42073ef09e0af2/00000017--3edc1f8259
|
||||
833262b0c9e4016a/0000004a--0227a058e3
|
||||
3cce51ba4cc49950/0000003c--51ebc96f4c
|
||||
3cce51ba4cc49950/0000003e--d78c337cc5
|
||||
ad9840558640c31d/0000001a--d6cd4871c2
|
||||
a468064f19bc0267/00000001--6a0c7d2514
|
||||
a468064f19bc0267/00000012--1feb5188ec
|
||||
a468064f19bc0267/00000015--27b5b89e1b
|
||||
a468064f19bc0267/00000016--31baf70a9f
|
||||
a468064f19bc0267/00000017--c63c64192b
|
||||
518f8ad60a03f394/00000016--e0300496c8
|
||||
"""
|
||||
|
||||
|
||||
def import_logreader():
|
||||
for mod_name in ("openpilot.tools.lib.logreader", "tools.lib.logreader"):
|
||||
try:
|
||||
mod = __import__(mod_name, fromlist=["LogReader"])
|
||||
return mod.LogReader, mod.ReadMode
|
||||
except ImportError:
|
||||
pass
|
||||
|
||||
raise SystemExit(
|
||||
"Could not import LogReader. Run this from an openpilot/comma tooling environment, "
|
||||
+ "or install the package that provides openpilot.tools.lib.logreader."
|
||||
)
|
||||
|
||||
|
||||
def normalize_route(route: str) -> str:
|
||||
return unquote(route.strip()).replace("|", "/")
|
||||
|
||||
|
||||
def add_segment(route: str, segment: int | None) -> str:
|
||||
if segment is None:
|
||||
return route
|
||||
|
||||
parts = route.split("/")
|
||||
if len(parts) == 2:
|
||||
return f"{route}/{segment}"
|
||||
return route
|
||||
|
||||
|
||||
def parse_routes(text: str, segment: int | None) -> list[str]:
|
||||
oneboxes = re.findall(r"onebox=([^) \t\n\r]+)", text)
|
||||
routes = oneboxes if oneboxes else [line for line in text.splitlines() if line.strip() and not line.lstrip().startswith("#")]
|
||||
|
||||
deduped = dict.fromkeys(add_segment(normalize_route(route), segment) for route in routes)
|
||||
return list(deduped)
|
||||
|
||||
|
||||
def event_time_s(event, start_time_ns: int) -> float:
|
||||
return (event.logMonoTime - start_time_ns) / 1e9
|
||||
|
||||
|
||||
def can_messages(event) -> Iterable:
|
||||
which = event.which()
|
||||
if which == "can":
|
||||
return event.can
|
||||
if which == "sendcan":
|
||||
return event.sendcan
|
||||
return ()
|
||||
|
||||
|
||||
def similar_count(count: int, reference: float, tolerance: float) -> bool:
|
||||
if reference == 0:
|
||||
return count == 0
|
||||
return abs(count - reference) / reference <= tolerance
|
||||
|
||||
|
||||
def consecutive_ranges(address_counts: Counter[int], min_len: int, count_tolerance: float) -> Iterator[tuple[int, int]]:
|
||||
sorted_addresses = sorted(address_counts)
|
||||
if not sorted_addresses:
|
||||
return
|
||||
|
||||
start = prev = sorted_addresses[0]
|
||||
run_counts = [address_counts[start]]
|
||||
for addr in sorted_addresses[1:]:
|
||||
reference_count = sum(run_counts) / len(run_counts)
|
||||
if addr == prev + 1 and similar_count(address_counts[addr], reference_count, count_tolerance):
|
||||
run_counts.append(address_counts[addr])
|
||||
prev = addr
|
||||
continue
|
||||
|
||||
if prev - start + 1 >= min_len:
|
||||
yield start, prev
|
||||
start = prev = addr
|
||||
run_counts = [address_counts[addr]]
|
||||
|
||||
if prev - start + 1 >= min_len:
|
||||
yield start, prev
|
||||
|
||||
|
||||
def parse_buses(buses: str) -> set[int]:
|
||||
return {int(bus.strip(), 0) for bus in buses.split(",") if bus.strip()}
|
||||
|
||||
|
||||
def scan_route(LogReader, ReadMode, route: str, skip_s: float, duration_s: float | None,
|
||||
include_sendcan: bool, buses: set[int]) -> tuple[dict[int, Counter[int]], float]:
|
||||
counts_by_bus: dict[int, Counter[int]] = defaultdict(Counter)
|
||||
start_time_ns = None
|
||||
end_s = None if duration_s is None else skip_s + duration_s
|
||||
last_scanned_t = skip_s
|
||||
|
||||
for event in LogReader(route, default_mode=ReadMode.RLOG):
|
||||
if start_time_ns is None:
|
||||
start_time_ns = event.logMonoTime
|
||||
|
||||
t = event_time_s(event, start_time_ns)
|
||||
if t < skip_s:
|
||||
continue
|
||||
if end_s is not None and t > end_s:
|
||||
break
|
||||
last_scanned_t = t
|
||||
|
||||
if event.which() == "sendcan" and not include_sendcan:
|
||||
continue
|
||||
|
||||
for msg in can_messages(event):
|
||||
if msg.src not in buses:
|
||||
continue
|
||||
counts_by_bus[msg.src][msg.address] += 1
|
||||
|
||||
return counts_by_bus, max(last_scanned_t - skip_s, 0.0)
|
||||
|
||||
|
||||
def range_freq(counts: Counter[int], start: int, end: int, duration_s: float) -> float:
|
||||
if duration_s <= 0:
|
||||
return 0.0
|
||||
|
||||
freqs = [counts[addr] / duration_s for addr in range(start, end + 1)]
|
||||
return sum(freqs) / len(freqs)
|
||||
|
||||
|
||||
def fmt_addr(addr: int) -> str:
|
||||
return f"0x{addr:X}"
|
||||
|
||||
|
||||
def sorted_range_counts(counts: Counter[tuple[int | None, int, int]]) -> list[tuple[tuple[int | None, int, int], int]]:
|
||||
return sorted(counts.items(), key=lambda item: (-((item[0][2] - item[0][1]) + 1), -item[1], -1 if item[0][0] is None else item[0][0], item[0][1]))
|
||||
|
||||
|
||||
def print_range(bus: int | None, start: int, end: int, count: int, avg_hz: float, example: str | None = None, bus_label: str | None = None) -> None:
|
||||
bus_label = bus_label or ("all buses" if bus is None else f"bus {bus}")
|
||||
example_text = "" if example is None else f" e.g. {example}"
|
||||
print(f"{count:>4}x {bus_label:>8} {fmt_addr(start)}-{fmt_addr(end)} len={end - start + 1:<3} freq={avg_hz:>6.1f} Hz{example_text}", flush=True)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="Find consecutive CAN address ranges observed after a route warm-up period.")
|
||||
parser.add_argument("input", nargs="?", help="Optional route, onebox URL, or file containing onebox URLs/routes. Defaults to the pasted side-quest routes.")
|
||||
parser.add_argument("--segment", type=int, default=1, help="Segment to append to route identifiers that do not already include one. Defaults to 1.")
|
||||
parser.add_argument("--all-segments", action="store_true", help="Do not append a segment; scan all segments selected by each identifier.")
|
||||
parser.add_argument("--skip-s", type=float, default=60.0, help="Seconds to skip from the start of each loaded route/segment.")
|
||||
parser.add_argument("--duration-s", type=float, default=None, help="Optional scan duration after --skip-s. Defaults to the rest of the log.")
|
||||
parser.add_argument("--min-len", type=int, default=4, help="Minimum consecutive address range length.")
|
||||
parser.add_argument("--count-tolerance", type=float, default=0.2, help="Maximum relative message-count difference within a range. Defaults to 0.2.")
|
||||
parser.add_argument("--buses", default="0,1,2", help="Comma-separated CAN buses to scan. Defaults to 0,1,2.")
|
||||
parser.add_argument("--include-sendcan", action="store_true", help="Include sendcan events in addition to received can events.")
|
||||
parser.add_argument("--merge-buses", "--no-bus", action="store_true", help="Merge addresses from all CAN buses before finding ranges.")
|
||||
parser.add_argument("--split-summary-buses", action="store_true", help="Keep aggregate summary counts separate per bus.")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.input:
|
||||
input_path = Path(args.input)
|
||||
text = input_path.read_text() if input_path.exists() else args.input
|
||||
else:
|
||||
text = DEFAULT_ROUTES
|
||||
routes = parse_routes(text, None if args.all_segments else args.segment)
|
||||
LogReader, ReadMode = import_logreader()
|
||||
buses = parse_buses(args.buses)
|
||||
|
||||
counts: Counter[tuple[int | None, int, int]] = Counter()
|
||||
freq_sums: Counter[tuple[int | None, int, int]] = Counter()
|
||||
examples: dict[tuple[int | None, int, int], str] = {}
|
||||
failed: list[tuple[str, str]] = []
|
||||
|
||||
for idx, route in enumerate(routes, 1):
|
||||
print(f"[{idx:>3}/{len(routes)}] scanning {route}", file=sys.stderr, flush=True)
|
||||
try:
|
||||
counts_by_bus, scanned_duration_s = scan_route(LogReader, ReadMode, route, args.skip_s, args.duration_s, args.include_sendcan, buses)
|
||||
except Exception as e:
|
||||
failed.append((route, f"{type(e).__name__}: {e}"))
|
||||
continue
|
||||
|
||||
if args.merge_buses:
|
||||
merged = Counter()
|
||||
for address_counts in counts_by_bus.values():
|
||||
merged.update(address_counts)
|
||||
range_sources = {None: merged}
|
||||
else:
|
||||
range_sources = counts_by_bus
|
||||
|
||||
route_ranges = set()
|
||||
route_freqs = {}
|
||||
for bus, address_counts in range_sources.items():
|
||||
for start, end in consecutive_ranges(address_counts, args.min_len, args.count_tolerance):
|
||||
route_ranges.add((bus, start, end))
|
||||
route_freqs[(bus, start, end)] = range_freq(address_counts, start, end, scanned_duration_s)
|
||||
|
||||
per_route_counts: Counter[tuple[int | None, int, int]] = Counter(route_ranges)
|
||||
print(f"\n{route}", flush=True)
|
||||
for (bus, start, end), route_count in sorted_range_counts(per_route_counts):
|
||||
print_range(bus, start, end, route_count, route_freqs[(bus, start, end)])
|
||||
|
||||
for range_key in route_ranges:
|
||||
counts[range_key] += 1
|
||||
freq_sums[range_key] += route_freqs[range_key]
|
||||
examples.setdefault(range_key, route)
|
||||
|
||||
print(f"\nScanned routes: {len(routes) - len(failed)}/{len(routes)}", flush=True)
|
||||
print(f"Minimum range length: {args.min_len}", flush=True)
|
||||
print(f"Count tolerance: {args.count_tolerance:g}", flush=True)
|
||||
print(f"Skipped first: {args.skip_s:g}s", flush=True)
|
||||
print(f"Buses: {','.join(str(bus) for bus in sorted(buses))}", flush=True)
|
||||
if args.duration_s is not None:
|
||||
print(f"Scan duration: {args.duration_s:g}s", flush=True)
|
||||
print(flush=True)
|
||||
|
||||
if args.split_summary_buses:
|
||||
for (bus, start, end), count in sorted_range_counts(counts):
|
||||
avg_hz = freq_sums[(bus, start, end)] / count
|
||||
print_range(bus, start, end, count, avg_hz, examples[(bus, start, end)])
|
||||
else:
|
||||
combined_counts: Counter[tuple[int, int]] = Counter()
|
||||
combined_freq_sums: Counter[tuple[int, int]] = Counter()
|
||||
combined_examples: dict[tuple[int, int], str] = {}
|
||||
combined_buses: dict[tuple[int, int], set[int]] = defaultdict(set)
|
||||
|
||||
for (bus, start, end), count in counts.items():
|
||||
range_key = (start, end)
|
||||
combined_counts[range_key] += count
|
||||
combined_freq_sums[range_key] += freq_sums[(bus, start, end)]
|
||||
combined_examples.setdefault(range_key, examples[(bus, start, end)])
|
||||
if bus is not None:
|
||||
combined_buses[range_key].add(bus)
|
||||
|
||||
sorted_combined = sorted(combined_counts.items(), key=lambda item: (-((item[0][1] - item[0][0]) + 1), -item[1], item[0][0]))
|
||||
for (start, end), count in sorted_combined:
|
||||
avg_hz = combined_freq_sums[(start, end)] / count
|
||||
buses_label = "buses " + ",".join(str(bus) for bus in sorted(combined_buses[(start, end)]))
|
||||
print_range(None, start, end, count, avg_hz, combined_examples[(start, end)], bus_label=buses_label)
|
||||
|
||||
if failed:
|
||||
print("\nFailed routes:", file=sys.stderr)
|
||||
for route, error in failed:
|
||||
print(f" {route}: {error}", file=sys.stderr)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,232 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import os
|
||||
import tempfile
|
||||
from dataclasses import dataclass
|
||||
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.dbc.generator.hyundai.hyundai_radar_3a5_3c4 import RADAR_3A5_3C4_MESSAGE_COMMENT, RADAR_3A5_3C4_SIGNAL_COMMENTS
|
||||
|
||||
|
||||
RADAR_500_51F_DBC_TEMPLATE = """
|
||||
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 8 RADAR
|
||||
SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
|
||||
SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
|
||||
SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
|
||||
SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
|
||||
SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
|
||||
SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
|
||||
"""
|
||||
|
||||
RADAR_3A5_3C4_DBC_TEMPLATE = """
|
||||
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 24 RADAR
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ STATE_ALT : 25|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ MOTION_STATE : 28|2@0+ (1,0) [0|2] "" XXX
|
||||
SG_ TRACK_COUNTER : 31|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ TRACK_QUALITY : 38|7@0+ (1,0) [0|127] "" XXX
|
||||
SG_ AGE : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ COAST_AGE : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ STATE : 54|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ RCS : 62|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ LONG_DIST : 63|13@1+ (0.05,0) [0|409.55] "m" XXX
|
||||
SG_ LAT_DIST : 76|12@1- (0.05,0) [-102.4|102.35] "m" XXX
|
||||
SG_ REL_SPEED : 88|14@1- (0.01,0) [-81.92|81.91] "m/s" XXX
|
||||
SG_ NEW_SIGNAL_4 : 103|2@0+ (1,0) [0|2] "" XXX
|
||||
SG_ REL_LAT_SPEED : 104|13@1- (0.01,0) [-40.96|40.95] "m/s" XXX
|
||||
SG_ REL_ACCEL : 118|10@1- (0.02,0) [-10.24|10.22] "m/s^2" XXX
|
||||
SG_ NEW_SIGNAL_18 : 129|2@0+ (1,0) [0|2] "" XXX
|
||||
SG_ NEW_SIGNAL_5 : 135|6@0+ (1,0) [0|32] "" XXX
|
||||
SG_ WIDTH : 138|5@1+ (0.1,0) [0|3.1] "m" XXX
|
||||
SG_ LENGTH : 151|8@0+ (0.1,0) [0|25.5] "m" XXX
|
||||
SG_ ABS_SPEED : 152|10@1+ (0.1,0) [0|102.3] "m/s" XXX
|
||||
SG_ ORIENTATION_ANGLE : 162|9@1- (1,0) [-180|180] "deg" XXX
|
||||
SG_ NEW_SIGNAL_13 : 175|4@0+ (1,0) [0|10] "" XXX
|
||||
SG_ NEW_SIGNAL_12 : 179|4@0+ (1,0) [0|10] "" XXX
|
||||
SG_ NEW_SIGNAL_14 : 181|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_15 : 183|2@0+ (1,0) [0|2] "" XXX
|
||||
SG_ NEW_SIGNAL_16 : 185|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_17 : 187|2@0+ (1,0) [0|2] "" XXX
|
||||
|
||||
VAL_ {addr_dec} STATE_ALT 0 "EMPTY" 1 "TENTATIVE" 2 "MEASURED" 3 "COASTED" ;
|
||||
VAL_ {addr_dec} MOTION_STATE 0 "UNKNOWN" 1 "STATIONARY" 2 "MOVING" ;
|
||||
VAL_ {addr_dec} STATE 0 "EMPTY" 1 "TENTATIVE_1" 2 "TENTATIVE_2" 3 "MEASURED" 4 "COASTED" 7 "UNRESOLVED_TENTATIVE" ;
|
||||
"""
|
||||
|
||||
RADAR_3A5_3C4_DBC_TEMPLATE += "\n".join((
|
||||
'CM_ BO_ {addr_dec} "' + RADAR_3A5_3C4_MESSAGE_COMMENT + '";',
|
||||
*(f'CM_ SG_ {{addr_dec}} {signal} "{comment}";' for signal, comment in RADAR_3A5_3C4_SIGNAL_COMMENTS),
|
||||
)) + "\n"
|
||||
|
||||
RADAR_210_21F_DBC_TEMPLATE = """
|
||||
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 32 RADAR
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ 1_AGE : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ 1_STATE_ALT : 51|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ 1_STATE : 55|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ 1_NEW_SIGNAL_3 : 63|8@0- (1,0) [0|255] "" XXX
|
||||
SG_ 1_LONG_DIST : 64|12@1+ (0.05,0) [0|4095] "" XXX
|
||||
SG_ 1_LAT_DIST : 76|12@1- (0.05,0) [0|4095] "" XXX
|
||||
SG_ 1_REL_SPEED : 88|14@1- (0.01,0) [0|16383] "" XXX
|
||||
SG_ 1_NEW_SIGNAL_1 : 102|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ 1_LAT_ACCEL : 104|13@1- (1,0) [0|8191] "" XXX
|
||||
SG_ 1_REL_ACCEL : 118|10@1- (1,0) [0|1023] "" XXX
|
||||
SG_ 2_AGE : 175|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ 2_STATE_ALT : 179|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ 2_STATE : 183|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ 2_NEW_SIGNAL_3 : 191|8@0- (1,0) [0|255] "" XXX
|
||||
SG_ 2_LONG_DIST : 192|12@1+ (0.05,0) [0|4095] "" XXX
|
||||
SG_ 2_LAT_DIST : 204|12@1- (0.05,0) [0|4095] "" XXX
|
||||
SG_ 2_REL_SPEED : 216|14@1- (0.01,0) [0|65535] "" XXX
|
||||
SG_ 2_NEW_SIGNAL_1 : 230|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ 2_LAT_ACCEL : 232|13@1- (1,0) [0|8191] "" XXX
|
||||
SG_ 2_REL_ACCEL : 246|10@1- (1,0) [0|1023] "" XXX
|
||||
"""
|
||||
|
||||
RADAR_602_611_DBC_TEMPLATE = """
|
||||
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 8 RADAR
|
||||
SG_ 1_DISTANCE : 0|10@1+ (0.25,0) [0|255.75] "" XXX
|
||||
SG_ 1_LATERAL : 10|11@1+ (0.03,-30.705) [-30.705|30.705] "" XXX
|
||||
SG_ 1_SPEED : 21|10@1+ (0.25,-128) [-128|127.75] "" XXX
|
||||
SG_ 2_DISTANCE : 31|10@1+ (0.25,0) [0|255.75] "" XXX
|
||||
SG_ 2_LATERAL : 41|11@1+ (0.03,-30.705) [-30.705|30.705] "" XXX
|
||||
SG_ 2_SPEED : 52|8@1- (0.25,0) [-32|31.75] "" XXX
|
||||
SG_ UNKNOWN_2 : 60|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ COUNTER : 62|2@1+ (1,0) [0|3] "" XXX
|
||||
"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RadarSpec:
|
||||
name: str
|
||||
start_addr: int
|
||||
msg_count: int
|
||||
dbc_template: str
|
||||
track_prefixes: tuple[str, ...]
|
||||
frequency: int
|
||||
|
||||
@property
|
||||
def end_addr(self) -> int:
|
||||
return self.start_addr + self.msg_count - 1
|
||||
|
||||
def contains(self, address: int) -> bool:
|
||||
return self.start_addr <= address <= self.end_addr
|
||||
|
||||
|
||||
RADAR_SPECS = (
|
||||
RadarSpec("RADAR_500_51F", 0x500, 32, RADAR_500_51F_DBC_TEMPLATE, ("",), frequency=20),
|
||||
RadarSpec("RADAR_210_21F", 0x210, 16, RADAR_210_21F_DBC_TEMPLATE, ("1_", "2_"), frequency=20),
|
||||
RadarSpec("RADAR_3A5_3C4", 0x3A5, 32, RADAR_3A5_3C4_DBC_TEMPLATE, ("",), frequency=20),
|
||||
RadarSpec("RADAR_602_611", 0x602, 16, RADAR_602_611_DBC_TEMPLATE, ("1_", "2_"), frequency=10),
|
||||
)
|
||||
|
||||
|
||||
def build_seen_address_map() -> dict[str, set[int]]:
|
||||
return {radar_spec.name: set() for radar_spec in RADAR_SPECS}
|
||||
|
||||
|
||||
def get_radar_spec(address: int) -> RadarSpec | None:
|
||||
for radar_spec in RADAR_SPECS:
|
||||
if radar_spec.contains(address):
|
||||
return radar_spec
|
||||
return None
|
||||
|
||||
|
||||
def is_exclusive_full_range_match(radar_spec: RadarSpec, seen_addresses: dict[str, set[int]]) -> bool:
|
||||
expected_addresses = set(range(radar_spec.start_addr, radar_spec.end_addr + 1))
|
||||
if seen_addresses[radar_spec.name] != expected_addresses:
|
||||
return False
|
||||
|
||||
for other_spec in RADAR_SPECS:
|
||||
if other_spec.name == radar_spec.name:
|
||||
continue
|
||||
|
||||
other_expected_addresses = set(range(other_spec.start_addr, other_spec.end_addr + 1))
|
||||
if seen_addresses[other_spec.name] == other_expected_addresses:
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
|
||||
def get_radar_dbc_path(radar_spec: RadarSpec) -> str:
|
||||
dbc_path = os.path.join(tempfile.gettempdir(), f"{radar_spec.name.lower()}_radar_ui.dbc")
|
||||
dbc_content = "\n".join(
|
||||
radar_spec.dbc_template.format(addr_dec=addr, addr_hex=f"{addr:x}")
|
||||
for addr in range(radar_spec.start_addr, radar_spec.end_addr + 1)
|
||||
)
|
||||
if not os.path.exists(dbc_path) or open(dbc_path).read() != dbc_content:
|
||||
with open(dbc_path, "w") as f:
|
||||
f.write(dbc_content)
|
||||
return dbc_path
|
||||
|
||||
|
||||
def get_radar_can_parser(radar_spec: RadarSpec, bus: int) -> CANParser:
|
||||
messages = [(f"RADAR_TRACK_{addr:x}", radar_spec.frequency) for addr in range(radar_spec.start_addr, radar_spec.end_addr + 1)]
|
||||
return CANParser(get_radar_dbc_path(radar_spec), messages, bus)
|
||||
|
||||
|
||||
def get_track_storage_key(radar_spec: RadarSpec, bus: int, addr: int, track_prefix: str) -> tuple[str, int, int]:
|
||||
if radar_spec.name in ("RADAR_500_51F", "RADAR_3A5_3C4"):
|
||||
return (radar_spec.name, bus, addr)
|
||||
|
||||
track_index = int(track_prefix[0]) - 1
|
||||
return (radar_spec.name, bus, addr * 2 + track_index)
|
||||
|
||||
|
||||
def get_track_ts_nanos(parser: CANParser, msg_name: str, radar_spec: RadarSpec, track_prefix: str) -> int:
|
||||
if radar_spec.name == "RADAR_602_611":
|
||||
return parser.ts_nanos[msg_name][f"{track_prefix}DISTANCE"]
|
||||
if radar_spec.name == "RADAR_210_21F":
|
||||
return parser.ts_nanos[msg_name][f"{track_prefix}LONG_DIST"]
|
||||
return parser.ts_nanos[msg_name]["LONG_DIST"]
|
||||
|
||||
|
||||
def decode_radar_track(radar_spec: RadarSpec, track_msg, track_prefix: str) -> tuple[float, float, float, float]:
|
||||
if radar_spec.name == "RADAR_602_611":
|
||||
return (
|
||||
track_msg[f"{track_prefix}DISTANCE"],
|
||||
track_msg[f"{track_prefix}LATERAL"],
|
||||
track_msg[f"{track_prefix}SPEED"],
|
||||
float("nan"),
|
||||
)
|
||||
|
||||
if radar_spec.name == "RADAR_210_21F":
|
||||
return (
|
||||
track_msg[f"{track_prefix}LONG_DIST"],
|
||||
track_msg[f"{track_prefix}LAT_DIST"],
|
||||
track_msg[f"{track_prefix}REL_SPEED"],
|
||||
float("nan"),
|
||||
)
|
||||
|
||||
if radar_spec.name == "RADAR_500_51F":
|
||||
azimuth = math.radians(track_msg["AZIMUTH"])
|
||||
long_dist = track_msg["LONG_DIST"]
|
||||
return (
|
||||
math.cos(azimuth) * long_dist,
|
||||
0.5 * -math.sin(azimuth) * long_dist,
|
||||
track_msg["REL_SPEED"],
|
||||
track_msg["REL_ACCEL"],
|
||||
)
|
||||
|
||||
return (
|
||||
track_msg["LONG_DIST"],
|
||||
track_msg["LAT_DIST"],
|
||||
track_msg["REL_SPEED"],
|
||||
track_msg["REL_ACCEL"],
|
||||
)
|
||||
|
||||
|
||||
def is_radar_track_valid(radar_spec: RadarSpec, track_msg, track_prefix: str) -> bool:
|
||||
if radar_spec.name == "RADAR_602_611":
|
||||
return 0 < track_msg[f"{track_prefix}DISTANCE"] < 255.75
|
||||
|
||||
if radar_spec.name == "RADAR_210_21F":
|
||||
return track_msg[f"{track_prefix}STATE"] in (3, 4)
|
||||
|
||||
return track_msg["STATE"] in (3, 4)
|
||||
@@ -1,912 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import os
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from opendbc.car.tests.routes import routes
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.tools.radar.custom_routes import CUSTOM_ROUTES
|
||||
from openpilot.tools.replay.lib.ui_helpers import (
|
||||
UP,
|
||||
BLACK,
|
||||
GREEN,
|
||||
Calibration,
|
||||
get_blank_lid_overlay,
|
||||
init_plots,
|
||||
plot_lead,
|
||||
plot_model,
|
||||
to_topdown_pt,
|
||||
)
|
||||
from openpilot.tools.radar.radar_helpers import (
|
||||
RADAR_SPECS,
|
||||
build_seen_address_map,
|
||||
decode_radar_track,
|
||||
get_radar_can_parser,
|
||||
get_radar_spec,
|
||||
get_track_storage_key,
|
||||
get_track_ts_nanos,
|
||||
is_exclusive_full_range_match,
|
||||
is_radar_track_valid,
|
||||
)
|
||||
from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType
|
||||
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
|
||||
ANGLE_SCALE = 5.0
|
||||
RADAR_TRACK_TIMEOUT_FRAMES = 10
|
||||
RADAR_FORMAT_SWITCH_MISS_FRAMES = 30
|
||||
RADAR_TRACK_RADIUS = 4
|
||||
CAMERA_RADAR_Y_OFFSET = 25
|
||||
RADAR_HEATMAP_DECAY = 0.975
|
||||
RADAR_HEATMAP_ALPHA = 0.65
|
||||
CAMERA_RADAR_HEATMAP_ALPHA = 0.45
|
||||
REPLAY_PATH = os.path.join(os.path.dirname(__file__), "..", "replay", "replay")
|
||||
REPLAY_SOCKET_WAIT_TIMEOUT_SECONDS = 10.0
|
||||
REPLAY_SPEEDS = (0.2, 0.5, 1.0, 2.0, 4.0, 8.0)
|
||||
CAMERA_DRAW_WIDTH = 640
|
||||
CAMERA_DRAW_HEIGHT = 480
|
||||
TOP_DOWN_DRAW_WIDTH = 384
|
||||
PLOT_DRAW_WIDTH = 480
|
||||
PLOT_DRAW_HEIGHT = 480
|
||||
RADAR_HEATMAP_MODES = ("OFF", "TOP", "CAMERA", "BOTH")
|
||||
CYAN = (90, 235, 255)
|
||||
AMBER = (255, 210, 90)
|
||||
SOFT_WHITE = (235, 235, 235)
|
||||
SLATE = (140, 180, 210)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RadarTrackPoint:
|
||||
trackId: int
|
||||
measured: bool = True
|
||||
dRel: float = 0.0
|
||||
yRel: float = 0.0
|
||||
vRel: float = 0.0
|
||||
aRel: float = 0.0
|
||||
yvRel: float = float("nan")
|
||||
|
||||
|
||||
def draw_radar_points(tracks, lid_overlay):
|
||||
for track in tracks:
|
||||
px, py = to_topdown_pt(track.dRel, -track.yRel)
|
||||
if px != -1:
|
||||
cv2.circle(lid_overlay, (py, px), RADAR_TRACK_RADIUS, 255, thickness=-1, lineType=cv2.LINE_AA)
|
||||
|
||||
|
||||
def update_radar_heatmap(tracks, radar_heatmap):
|
||||
radar_heatmap *= RADAR_HEATMAP_DECAY
|
||||
for track in tracks:
|
||||
px, py = to_topdown_pt(track.dRel, -track.yRel)
|
||||
if px != -1:
|
||||
cv2.circle(radar_heatmap, (py, px), RADAR_TRACK_RADIUS + 2, 255.0, thickness=-1, lineType=cv2.LINE_AA)
|
||||
|
||||
|
||||
def update_radar_camera_heatmap(tracks, radar_heatmap, calibration, shape):
|
||||
radar_heatmap *= RADAR_HEATMAP_DECAY
|
||||
if calibration is None:
|
||||
return
|
||||
|
||||
height, width = shape[:2]
|
||||
for track in tracks:
|
||||
if track.dRel <= 0.0:
|
||||
continue
|
||||
|
||||
pt = calibration.car_space_to_bb(
|
||||
np.asarray([track.dRel - RADAR_TO_CAMERA]),
|
||||
np.asarray([-track.yRel]),
|
||||
np.asarray([1.0]),
|
||||
)
|
||||
x, y = np.round(pt[0]).astype(int)
|
||||
y += CAMERA_RADAR_Y_OFFSET
|
||||
if 0 <= x < width and 0 <= y < height:
|
||||
cv2.circle(radar_heatmap, (x, y), RADAR_TRACK_RADIUS + 3, 255.0, thickness=-1, lineType=cv2.LINE_AA)
|
||||
|
||||
|
||||
def overlay_heatmap(img, radar_heatmap, alpha_scale):
|
||||
radar_heat_uint8 = np.ascontiguousarray(np.clip(radar_heatmap, 0, 255).astype(np.uint8))
|
||||
radar_heat_mask = radar_heat_uint8 > 0
|
||||
if not np.any(radar_heat_mask):
|
||||
return
|
||||
|
||||
heat_rgb = cv2.cvtColor(cv2.applyColorMap(radar_heat_uint8, cv2.COLORMAP_TURBO), cv2.COLOR_BGR2RGB).astype(np.float32)
|
||||
img_rgb = img.astype(np.float32)
|
||||
alpha = ((radar_heat_uint8.astype(np.float32) / 255.0) * alpha_scale)[..., None]
|
||||
img[:] = np.where(
|
||||
radar_heat_mask[..., None],
|
||||
np.clip(img_rgb * (1.0 - alpha) + heat_rgb * alpha, 0, 255).astype(np.uint8),
|
||||
img,
|
||||
)
|
||||
|
||||
|
||||
def draw_radar_points_camera(tracks, img, calibration):
|
||||
if calibration is None:
|
||||
return
|
||||
|
||||
for track in tracks:
|
||||
if track.dRel <= 0.0:
|
||||
continue
|
||||
|
||||
# Match the road-space projection convention used by other UI overlays.
|
||||
pt = calibration.car_space_to_bb(
|
||||
np.asarray([track.dRel - RADAR_TO_CAMERA]),
|
||||
np.asarray([-track.yRel]),
|
||||
np.asarray([1.0]),
|
||||
)
|
||||
x, y = np.round(pt[0]).astype(int)
|
||||
y += CAMERA_RADAR_Y_OFFSET
|
||||
if 0 <= x < img.shape[1] and 0 <= y < img.shape[0]:
|
||||
cv2.circle(img, (x, y), RADAR_TRACK_RADIUS, (255, 255, 255), thickness=-1, lineType=cv2.LINE_AA)
|
||||
|
||||
|
||||
def draw_loading_overlay(font, lines, camera_texture, top_down_texture, hor_mode, panel_x, panel_y):
|
||||
rl.draw_texture_pro(
|
||||
camera_texture,
|
||||
rl.Rectangle(0, 0, camera_texture.width, camera_texture.height),
|
||||
rl.Rectangle(0, 0, CAMERA_DRAW_WIDTH, CAMERA_DRAW_HEIGHT),
|
||||
rl.Vector2(0, 0),
|
||||
0.0,
|
||||
rl.WHITE,
|
||||
)
|
||||
rl.draw_texture(top_down_texture, CAMERA_DRAW_WIDTH, 0, rl.WHITE) # noqa: TID251
|
||||
|
||||
if hor_mode:
|
||||
panel_width = 620
|
||||
panel_height = 300
|
||||
else:
|
||||
panel_width = 700
|
||||
panel_height = 320
|
||||
|
||||
rl.draw_rectangle(panel_x - 20, panel_y - 30, panel_width, panel_height, rl.Color(0, 0, 0, 140))
|
||||
rl.draw_rectangle_lines(panel_x - 20, panel_y - 30, panel_width, panel_height, rl.Color(255, 255, 255, 90))
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
if line:
|
||||
rl.draw_text_ex(font, line, rl.Vector2(panel_x, panel_y + i * 40), 28 if i == 0 else 20, 0, rl.WHITE)
|
||||
|
||||
|
||||
def start_replay(route: str, prefix: str, playback: str, data_dir: str | None, start_seconds: int) -> subprocess.Popen:
|
||||
cmd = [
|
||||
REPLAY_PATH,
|
||||
"--playback", playback,
|
||||
"--prefix", prefix,
|
||||
]
|
||||
if start_seconds > 0:
|
||||
cmd.extend(["--start", str(start_seconds)])
|
||||
if data_dir:
|
||||
cmd.extend(["--data_dir", data_dir])
|
||||
cmd.append(route)
|
||||
|
||||
env = os.environ.copy()
|
||||
env["OPENPILOT_PREFIX"] = prefix
|
||||
return subprocess.Popen(
|
||||
cmd,
|
||||
cwd=os.path.dirname(REPLAY_PATH),
|
||||
env=env,
|
||||
stdout=subprocess.DEVNULL,
|
||||
stderr=subprocess.DEVNULL,
|
||||
preexec_fn=os.setsid if os.name != "nt" else None,
|
||||
)
|
||||
|
||||
|
||||
def stop_replay(proc: subprocess.Popen | None) -> None:
|
||||
if proc is not None and proc.poll() is None:
|
||||
if os.name != "nt":
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGTERM)
|
||||
except ProcessLookupError:
|
||||
pass
|
||||
else:
|
||||
proc.terminate()
|
||||
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except subprocess.TimeoutExpired:
|
||||
if os.name != "nt":
|
||||
try:
|
||||
os.killpg(proc.pid, signal.SIGKILL)
|
||||
except ProcessLookupError:
|
||||
pass
|
||||
else:
|
||||
proc.kill()
|
||||
proc.wait(timeout=5)
|
||||
|
||||
# `replay` can leave helper processes like `replayd` behind, so do one
|
||||
# targeted cleanup pass on shutdown as well.
|
||||
for pattern in ("/tools/replay/replay", "replayd"):
|
||||
try:
|
||||
subprocess.run(
|
||||
["pkill", "-f", pattern],
|
||||
stdout=subprocess.DEVNULL,
|
||||
stderr=subprocess.DEVNULL,
|
||||
check=False,
|
||||
)
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
|
||||
|
||||
def wait_for_can_socket(prefix: str, timeout: float) -> None:
|
||||
socket_path = os.path.join("/tmp", f"msgq_{prefix}", "can")
|
||||
started = time.monotonic()
|
||||
while not os.path.exists(socket_path):
|
||||
if time.monotonic() - started > timeout:
|
||||
raise TimeoutError(f"Timed out waiting for CAN socket at {socket_path}")
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def get_hkg_routes() -> list[tuple[str, str]]:
|
||||
hkg_prefixes = ("HYUNDAI_", "KIA_", "GENESIS_")
|
||||
return [
|
||||
(route.route, getattr(route.car_model, "name", str(route.car_model)))
|
||||
for route in routes
|
||||
if route.car_model is not None and getattr(route.car_model, "name", str(route.car_model)).startswith(hkg_prefixes)
|
||||
]
|
||||
|
||||
|
||||
def get_custom_routes() -> list[tuple[str, str]]:
|
||||
return [(route.route, route.car_model) for route in CUSTOM_ROUTES]
|
||||
|
||||
|
||||
def make_submaster(addr):
|
||||
return messaging.SubMaster(
|
||||
[
|
||||
'carState',
|
||||
'longitudinalPlan',
|
||||
'carControl',
|
||||
'radarState',
|
||||
'liveCalibration',
|
||||
'controlsState',
|
||||
'selfdriveState',
|
||||
'liveTracks',
|
||||
'modelV2',
|
||||
'liveParameters',
|
||||
'roadCameraState',
|
||||
],
|
||||
addr=addr,
|
||||
)
|
||||
|
||||
|
||||
def reset_radar_state():
|
||||
return (
|
||||
0,
|
||||
None,
|
||||
0,
|
||||
{radar_spec.name: 0 for radar_spec in RADAR_SPECS},
|
||||
build_seen_address_map(),
|
||||
{},
|
||||
0,
|
||||
{},
|
||||
{},
|
||||
{},
|
||||
)
|
||||
|
||||
|
||||
def ui_thread(addr, route_entries=None, playback="1.0", data_dir=None, prefix="ui-replay", start_route_idx=0):
|
||||
cv2.setNumThreads(1)
|
||||
|
||||
# Get monitor info before creating window
|
||||
rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT)
|
||||
rl.init_window(1, 1, "")
|
||||
max_height = rl.get_monitor_height(0)
|
||||
rl.close_window()
|
||||
|
||||
hor_mode = os.getenv("HORIZONTAL") is not None
|
||||
hor_mode = True if max_height < 960 + 300 else hor_mode
|
||||
|
||||
if hor_mode:
|
||||
size = (CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH + PLOT_DRAW_WIDTH, 960)
|
||||
write_x = 5
|
||||
write_y = 480
|
||||
else:
|
||||
size = (CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH, 960 + 300)
|
||||
write_x = CAMERA_DRAW_WIDTH + 5
|
||||
write_y = 970
|
||||
|
||||
rl.set_trace_log_level(rl.TraceLogLevel.LOG_ERROR)
|
||||
rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT)
|
||||
rl.init_window(size[0], size[1], "openpilot debug UI")
|
||||
rl.set_target_fps(60)
|
||||
|
||||
# Load font
|
||||
font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf")
|
||||
font = rl.load_font_ex(font_path, 32, None, 0)
|
||||
|
||||
# Create textures for camera and top-down view
|
||||
camera_image = rl.gen_image_color(640, 480, rl.BLACK)
|
||||
camera_texture = rl.load_texture_from_image(camera_image)
|
||||
rl.unload_image(camera_image)
|
||||
|
||||
# lid_overlay array is (lidar_x, lidar_y) = (384, 960)
|
||||
# pygame treats first axis as width, so texture is 384 wide x 960 tall
|
||||
# For raylib, we need to transpose to get (height, width) = (960, 384) for the RGBA array
|
||||
top_down_image = rl.gen_image_color(UP.lidar_x, UP.lidar_y, rl.BLACK)
|
||||
top_down_texture = rl.load_texture_from_image(top_down_image)
|
||||
rl.unload_image(top_down_image)
|
||||
|
||||
current_route_idx = start_route_idx
|
||||
current_route_name = None
|
||||
current_route_model = None
|
||||
replay_proc = None
|
||||
current_start_seconds = 0
|
||||
paused = False
|
||||
state_checks_enabled = False
|
||||
radar_heatmap_mode_idx = 0
|
||||
current_playback = min(REPLAY_SPEEDS, key=lambda speed: abs(speed - float(playback)))
|
||||
last_replay_started_at = time.monotonic()
|
||||
playback_ready = False
|
||||
loading_status = "Initializing UI"
|
||||
|
||||
def current_offset_seconds() -> int:
|
||||
if paused or not playback_ready or replay_proc is None or replay_proc.poll() is not None:
|
||||
return current_start_seconds
|
||||
return max(0, int(current_start_seconds + (time.monotonic() - last_replay_started_at) * current_playback))
|
||||
|
||||
def connect_streams():
|
||||
nonlocal replay_proc, current_route_name, current_route_model, current_route_idx, current_start_seconds, loading_status, paused, \
|
||||
last_replay_started_at, current_playback, playback_ready
|
||||
|
||||
if route_entries:
|
||||
current_route_name, current_route_model = route_entries[current_route_idx]
|
||||
loading_status = f"Starting replay for route {current_route_idx + 1}/{len(route_entries)} at {current_start_seconds}s ({current_playback:.1f}x)"
|
||||
stop_replay(replay_proc)
|
||||
os.environ["OPENPILOT_PREFIX"] = prefix
|
||||
messaging.reset_context()
|
||||
replay_proc = start_replay(current_route_name, prefix, f"{current_playback:.1f}", data_dir, current_start_seconds)
|
||||
paused = False
|
||||
playback_ready = False
|
||||
loading_status = "Waiting for CAN socket"
|
||||
wait_for_can_socket(prefix, REPLAY_SOCKET_WAIT_TIMEOUT_SECONDS)
|
||||
|
||||
loading_status = "Connecting to messaging streams"
|
||||
sm_local = make_submaster(addr)
|
||||
logcan_local = messaging.sub_sock("can", addr=addr, conflate=False, timeout=100)
|
||||
loading_status = "Waiting for road camera frames"
|
||||
return sm_local, logcan_local
|
||||
|
||||
sm, logcan = connect_streams()
|
||||
|
||||
img = np.zeros((480, 640, 3), dtype='uint8')
|
||||
imgff = None
|
||||
num_px = 0
|
||||
calibration = None
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
|
||||
lid_overlay_blank = get_blank_lid_overlay(UP)
|
||||
radar_heatmap = np.zeros_like(lid_overlay_blank, dtype=np.float32)
|
||||
camera_radar_heatmap = np.zeros(img.shape[:2], dtype=np.float32)
|
||||
|
||||
# plots
|
||||
name_to_arr_idx = {
|
||||
"gas": 0,
|
||||
"computer_gas": 1,
|
||||
"user_brake": 2,
|
||||
"computer_brake": 3,
|
||||
"v_ego": 4,
|
||||
"v_pid": 5,
|
||||
"angle_steers_des": 6,
|
||||
"angle_steers": 7,
|
||||
"angle_steers_k": 8,
|
||||
"steer_torque": 9,
|
||||
"v_override": 10,
|
||||
"v_cruise": 11,
|
||||
"a_ego": 12,
|
||||
"a_target": 13,
|
||||
}
|
||||
|
||||
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
|
||||
|
||||
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
|
||||
plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0.0, 75.0), (-3.0, 2.0)]
|
||||
plot_names = [
|
||||
["gas", "computer_gas", "user_brake", "computer_brake"],
|
||||
["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"],
|
||||
["v_ego", "v_override", "v_pid", "v_cruise"],
|
||||
["a_ego", "a_target"],
|
||||
]
|
||||
plot_colors = [["b", "b", "g", "r", "y"], ["b", "g", "y", "r"], ["b", "g", "r", "y"], ["b", "r"]]
|
||||
plot_styles = [["-", "-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-"]]
|
||||
|
||||
draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles)
|
||||
|
||||
# Palette for converting lid_overlay grayscale indices to RGBA colors
|
||||
palette = np.zeros((256, 4), dtype=np.uint8)
|
||||
palette[:, 3] = 255 # alpha
|
||||
palette[1] = [255, 0, 0, 255] # RED
|
||||
palette[2] = [0, 255, 0, 255] # GREEN
|
||||
palette[3] = [0, 0, 255, 255] # BLUE
|
||||
palette[4] = [255, 255, 0, 255] # YELLOW
|
||||
palette[110] = [110, 110, 110, 255] # car_color (gray)
|
||||
palette[255] = [255, 255, 255, 255] # WHITE
|
||||
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
while not rl.window_should_close():
|
||||
# ***** frame *****
|
||||
if not vipc_client.is_connected():
|
||||
vipc_client.connect(False)
|
||||
|
||||
rl.begin_drawing()
|
||||
rl.clear_background(rl.Color(64, 64, 64, 255))
|
||||
|
||||
if rl.is_key_released(rl.KeyboardKey.KEY_Q):
|
||||
rl.end_drawing()
|
||||
stop_replay(replay_proc)
|
||||
replay_proc = None
|
||||
break
|
||||
|
||||
shift_down = rl.is_key_down(rl.KeyboardKey.KEY_LEFT_SHIFT) or rl.is_key_down(rl.KeyboardKey.KEY_RIGHT_SHIFT)
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_SPACE):
|
||||
if paused:
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
else:
|
||||
current_start_seconds = current_offset_seconds()
|
||||
paused = True
|
||||
loading_status = "Paused"
|
||||
stop_replay(replay_proc)
|
||||
replay_proc = None
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_RIGHT):
|
||||
current_route_idx = (current_route_idx + 1) % len(route_entries)
|
||||
current_start_seconds = 0
|
||||
paused = False
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_LEFT):
|
||||
current_route_idx = (current_route_idx - 1) % len(route_entries)
|
||||
current_start_seconds = 0
|
||||
paused = False
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_M):
|
||||
current_start_seconds = max(0, current_offset_seconds() + (-60 if shift_down else 60))
|
||||
paused = False
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_S):
|
||||
current_start_seconds = max(0, current_offset_seconds() + (-10 if shift_down else 10))
|
||||
paused = False
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and (rl.is_key_released(rl.KeyboardKey.KEY_EQUAL) or rl.is_key_released(rl.KeyboardKey.KEY_KP_ADD)):
|
||||
for speed in REPLAY_SPEEDS:
|
||||
if speed > current_playback:
|
||||
current_playback = speed
|
||||
break
|
||||
if not paused:
|
||||
current_start_seconds = current_offset_seconds()
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if route_entries and (rl.is_key_released(rl.KeyboardKey.KEY_MINUS) or rl.is_key_released(rl.KeyboardKey.KEY_KP_SUBTRACT)):
|
||||
for speed in reversed(REPLAY_SPEEDS):
|
||||
if speed < current_playback:
|
||||
current_playback = speed
|
||||
break
|
||||
if not paused:
|
||||
current_start_seconds = current_offset_seconds()
|
||||
(can_range_msg_count,
|
||||
active_radar_format_name,
|
||||
active_radar_format_miss_count,
|
||||
radar_format_total_counts,
|
||||
radar_format_seen_addresses,
|
||||
radar_track_ids,
|
||||
next_radar_track_id,
|
||||
radar_tracks,
|
||||
radar_track_last_seen,
|
||||
radar_parsers) = reset_radar_state()
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
sm, logcan = connect_streams()
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
|
||||
if rl.is_key_released(rl.KeyboardKey.KEY_C):
|
||||
state_checks_enabled = not state_checks_enabled
|
||||
|
||||
if rl.is_key_released(rl.KeyboardKey.KEY_H):
|
||||
radar_heatmap_mode_idx = (radar_heatmap_mode_idx + 1) % len(RADAR_HEATMAP_MODES)
|
||||
if RADAR_HEATMAP_MODES[radar_heatmap_mode_idx] == "OFF":
|
||||
radar_heatmap.fill(0)
|
||||
camera_radar_heatmap.fill(0)
|
||||
|
||||
yuv_img_raw = vipc_client.recv()
|
||||
if yuv_img_raw is None or not yuv_img_raw.data.any():
|
||||
if replay_proc is not None and replay_proc.poll() is not None:
|
||||
loading_status = f"Replay exited with code {replay_proc.poll()}"
|
||||
|
||||
loading_lines = [
|
||||
f"{loading_status}{' (paused)' if paused else ''}",
|
||||
f"Route {current_route_idx + 1}/{len(route_entries)}" if route_entries else "Connected to external replay",
|
||||
f"Platform: {current_route_model}" if current_route_model is not None else "",
|
||||
f"Route: {current_route_name}" if current_route_name is not None else "",
|
||||
f"Offset: {current_offset_seconds()}s" if route_entries else "",
|
||||
f"Playback: {current_playback:.1f}x" if route_entries else "",
|
||||
f"Radar state checks: {'ON' if state_checks_enabled else 'OFF'}",
|
||||
f"Radar heatmap: {RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]}",
|
||||
"Keys: SPACE play/pause, RIGHT next, LEFT prev, M +/-60s, S +/-10s, +/- speed, C checks, H heatmap, Q quit" \
|
||||
if route_entries else "Keys: C checks, H heatmap, Q quit",
|
||||
]
|
||||
draw_loading_overlay(font, loading_lines, camera_texture, top_down_texture, hor_mode, 80, 160)
|
||||
rl.end_drawing()
|
||||
continue
|
||||
|
||||
if not playback_ready:
|
||||
playback_ready = True
|
||||
last_replay_started_at = time.monotonic()
|
||||
loading_status = "Streaming"
|
||||
|
||||
lid_overlay = lid_overlay_blank.copy()
|
||||
top_down = top_down_texture, lid_overlay
|
||||
|
||||
sm.update(0)
|
||||
|
||||
camera = DEVICE_CAMERAS[("tici", str(sm['roadCameraState'].sensor))]
|
||||
|
||||
# Use received buffer dimensions (full HEVC can have stride != buffer_len/rows due to VENUS padding)
|
||||
h, w, stride = yuv_img_raw.height, yuv_img_raw.width, yuv_img_raw.stride
|
||||
nv12_size = h * 3 // 2 * stride
|
||||
imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8, count=nv12_size).reshape((h * 3 // 2, stride))
|
||||
num_px = w * h
|
||||
rgb = cv2.cvtColor(imgff[: h * 3 // 2, : w], cv2.COLOR_YUV2RGB_NV12)
|
||||
|
||||
qcam = "QCAM" in os.environ
|
||||
bb_scale = 0.825 if qcam else 0.8
|
||||
calib_scale = camera.fcam.width / 640.0
|
||||
zoom_matrix = np.asarray([[bb_scale, 0.0, 0.0], [0.0, bb_scale, 0.0], [0.0, 0.0, 1.0]])
|
||||
cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
|
||||
|
||||
intrinsic_matrix = camera.fcam.intrinsics
|
||||
|
||||
w = sm['controlsState'].lateralControlState.which()
|
||||
if w == 'lqrStateDEPRECATED':
|
||||
angle_steers_k = sm['controlsState'].lateralControlState.lqrStateDEPRECATED.steeringAngleDeg
|
||||
elif w == 'indiState':
|
||||
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg
|
||||
else:
|
||||
angle_steers_k = np.inf
|
||||
|
||||
plot_arr[:-1] = plot_arr[1:]
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
|
||||
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gasDEPRECATED
|
||||
# TODO gas is deprecated
|
||||
plot_arr[-1, name_to_arr_idx['computer_gas']] = np.clip(sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
|
||||
plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.torque * ANGLE_SCALE
|
||||
# TODO brake is deprecated
|
||||
plot_arr[-1, name_to_arr_idx['computer_brake']] = np.clip(-sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo
|
||||
plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
|
||||
plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
|
||||
|
||||
if len(sm['longitudinalPlan'].accels):
|
||||
plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].accels[0]
|
||||
|
||||
if sm.recv_frame['modelV2']:
|
||||
plot_model(sm['modelV2'], img, calibration, top_down)
|
||||
|
||||
if sm.recv_frame['radarState']:
|
||||
plot_lead(sm['radarState'], top_down)
|
||||
|
||||
if sm.updated['liveCalibration'] and num_px:
|
||||
rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
|
||||
calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
|
||||
|
||||
can_packets = messaging.drain_sock(logcan)
|
||||
if can_packets:
|
||||
can_strings = [
|
||||
(can_packet.logMonoTime, [(msg.address, msg.dat, msg.src) for msg in can_packet.can])
|
||||
for can_packet in can_packets
|
||||
]
|
||||
detected_format_counts = {radar_spec.name: 0 for radar_spec in RADAR_SPECS}
|
||||
|
||||
for can_packet in can_packets:
|
||||
for msg in can_packet.can:
|
||||
radar_spec = get_radar_spec(msg.address)
|
||||
if radar_spec is not None:
|
||||
can_range_msg_count += 1
|
||||
detected_format_counts[radar_spec.name] += 1
|
||||
radar_format_total_counts[radar_spec.name] += 1
|
||||
radar_format_seen_addresses[radar_spec.name].add(msg.address)
|
||||
if radar_spec.name not in radar_parsers:
|
||||
radar_parsers[radar_spec.name] = {}
|
||||
if msg.src not in radar_parsers[radar_spec.name]:
|
||||
radar_parsers[radar_spec.name][msg.src] = get_radar_can_parser(radar_spec, msg.src)
|
||||
|
||||
matching_formats = [
|
||||
radar_spec.name
|
||||
for radar_spec in RADAR_SPECS
|
||||
if is_exclusive_full_range_match(radar_spec, radar_format_seen_addresses)
|
||||
]
|
||||
if len(matching_formats) == 1:
|
||||
if active_radar_format_name == matching_formats[0]:
|
||||
active_radar_format_miss_count = 0
|
||||
elif active_radar_format_name is None:
|
||||
active_radar_format_name = matching_formats[0]
|
||||
active_radar_format_miss_count = 0
|
||||
else:
|
||||
active_radar_format_miss_count += 1
|
||||
if active_radar_format_miss_count >= RADAR_FORMAT_SWITCH_MISS_FRAMES:
|
||||
active_radar_format_name = matching_formats[0]
|
||||
active_radar_format_miss_count = 0
|
||||
elif len(matching_formats) == 0 and active_radar_format_name is not None:
|
||||
active_radar_format_miss_count += 1
|
||||
if active_radar_format_miss_count >= RADAR_FORMAT_SWITCH_MISS_FRAMES:
|
||||
active_radar_format_name = None
|
||||
active_radar_format_miss_count = 0
|
||||
|
||||
active_radar_spec = next((spec for spec in RADAR_SPECS if spec.name == active_radar_format_name), None)
|
||||
if active_radar_spec is not None:
|
||||
for bus, parser in radar_parsers.get(active_radar_spec.name, {}).items():
|
||||
updated_addrs = parser.update(can_strings)
|
||||
relevant_updated_addrs = {
|
||||
track_addr for track_addr in updated_addrs
|
||||
if active_radar_spec.start_addr <= track_addr <= active_radar_spec.end_addr
|
||||
}
|
||||
if not relevant_updated_addrs:
|
||||
continue
|
||||
|
||||
for track_addr in relevant_updated_addrs:
|
||||
msg_name = f"RADAR_TRACK_{track_addr:x}"
|
||||
track_msg = parser.vl[msg_name]
|
||||
for track_prefix in active_radar_spec.track_prefixes:
|
||||
track_key = get_track_storage_key(active_radar_spec, bus, track_addr, track_prefix)
|
||||
ts_nanos = get_track_ts_nanos(parser, msg_name, active_radar_spec, track_prefix)
|
||||
if ts_nanos == 0:
|
||||
continue
|
||||
|
||||
if state_checks_enabled and not is_radar_track_valid(active_radar_spec, track_msg, track_prefix):
|
||||
radar_tracks.pop(track_key, None)
|
||||
radar_track_last_seen.pop(track_key, None)
|
||||
continue
|
||||
|
||||
d_rel, y_rel, v_rel, a_rel = decode_radar_track(active_radar_spec, track_msg, track_prefix)
|
||||
|
||||
if track_key not in radar_track_ids:
|
||||
radar_track_ids[track_key] = next_radar_track_id
|
||||
next_radar_track_id += 1
|
||||
|
||||
radar_tracks[track_key] = RadarTrackPoint(
|
||||
trackId=radar_track_ids[track_key],
|
||||
dRel=d_rel,
|
||||
yRel=y_rel,
|
||||
vRel=v_rel,
|
||||
aRel=a_rel,
|
||||
)
|
||||
radar_track_last_seen[track_key] = sm.frame
|
||||
|
||||
stale_tracks = [
|
||||
track_key for track_key, last_seen in radar_track_last_seen.items()
|
||||
if (sm.frame - last_seen) > RADAR_TRACK_TIMEOUT_FRAMES
|
||||
]
|
||||
for track_key in stale_tracks:
|
||||
radar_track_last_seen.pop(track_key, None)
|
||||
radar_tracks.pop(track_key, None)
|
||||
|
||||
active_radar_tracks = [
|
||||
track for track_key, track in radar_tracks.items()
|
||||
if active_radar_format_name is not None and track_key[0] == active_radar_format_name
|
||||
]
|
||||
active_radar_buses = sorted({
|
||||
track_key[1] for track_key in radar_tracks
|
||||
if active_radar_format_name is not None and track_key[0] == active_radar_format_name
|
||||
})
|
||||
if len(active_radar_tracks) == 0:
|
||||
active_radar_tracks = sm['liveTracks'].points
|
||||
active_radar_buses = []
|
||||
|
||||
radar_heatmap_mode = RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]
|
||||
if radar_heatmap_mode in ("TOP", "BOTH"):
|
||||
update_radar_heatmap(active_radar_tracks, radar_heatmap)
|
||||
if radar_heatmap_mode in ("CAMERA", "BOTH"):
|
||||
update_radar_camera_heatmap(active_radar_tracks, camera_radar_heatmap, calibration, img.shape)
|
||||
overlay_heatmap(img, camera_radar_heatmap, CAMERA_RADAR_HEATMAP_ALPHA)
|
||||
|
||||
# draw decoded radar tracks when present, otherwise fall back to liveTracks
|
||||
draw_radar_points(active_radar_tracks, top_down[1])
|
||||
draw_radar_points_camera(active_radar_tracks, img, calibration)
|
||||
|
||||
# *** blits ***
|
||||
# Update camera texture from numpy array
|
||||
img_rgba = cv2.cvtColor(img, cv2.COLOR_RGB2RGBA)
|
||||
rl.update_texture(camera_texture, rl.ffi.cast("void *", img_rgba.ctypes.data))
|
||||
rl.draw_texture_pro(
|
||||
camera_texture,
|
||||
rl.Rectangle(0, 0, camera_texture.width, camera_texture.height),
|
||||
rl.Rectangle(0, 0, CAMERA_DRAW_WIDTH, CAMERA_DRAW_HEIGHT),
|
||||
rl.Vector2(0, 0),
|
||||
0.0,
|
||||
rl.WHITE,
|
||||
)
|
||||
|
||||
# display alerts
|
||||
rl.draw_text_ex(font, sm['selfdriveState'].alertText1, rl.Vector2(180, 150), 30, 0, rl.RED)
|
||||
rl.draw_text_ex(font, sm['selfdriveState'].alertText2, rl.Vector2(180, 190), 20, 0, rl.RED)
|
||||
|
||||
# draw plots (texture is reused internally)
|
||||
plot_texture = draw_plots(plot_arr)
|
||||
if hor_mode:
|
||||
rl.draw_texture_pro(
|
||||
plot_texture,
|
||||
rl.Rectangle(0, 0, plot_texture.width, plot_texture.height),
|
||||
rl.Rectangle(CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH, 0, PLOT_DRAW_WIDTH, PLOT_DRAW_HEIGHT),
|
||||
rl.Vector2(0, 0),
|
||||
0.0,
|
||||
rl.WHITE,
|
||||
)
|
||||
else:
|
||||
rl.draw_texture(plot_texture, 0, 300, rl.WHITE) # noqa: TID251
|
||||
|
||||
# Convert lid_overlay to RGBA and update top_down texture
|
||||
# lid_overlay is (384, 960), need to transpose to (960, 384) for row-major RGBA buffer
|
||||
lid_rgba = palette[lid_overlay.T]
|
||||
if radar_heatmap_mode in ("TOP", "BOTH"):
|
||||
overlay_heatmap(lid_rgba[..., :3], radar_heatmap.T, RADAR_HEATMAP_ALPHA)
|
||||
rl.update_texture(top_down_texture, rl.ffi.cast("void *", np.ascontiguousarray(lid_rgba).ctypes.data))
|
||||
rl.draw_texture(top_down_texture, CAMERA_DRAW_WIDTH, 0, rl.WHITE) # noqa: TID251
|
||||
|
||||
SPACING = 20
|
||||
lines = [
|
||||
("ENABLED", GREEN if sm['selfdriveState'].enabled else BLACK),
|
||||
("SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", CYAN),
|
||||
("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), CYAN),
|
||||
("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), CYAN),
|
||||
(f"RADAR FORMAT: {active_radar_format_name or 'NONE'}", AMBER),
|
||||
(f"RADAR CAN MSGS: {can_range_msg_count}", AMBER),
|
||||
(f"RADAR TRACKS: {len(active_radar_tracks)}"
|
||||
+ (f" (BUS {','.join(str(bus) for bus in active_radar_buses)})" if active_radar_buses else ""),
|
||||
AMBER),
|
||||
(f"RADAR STATE CHECKS: {'ON' if state_checks_enabled else 'OFF'}", AMBER),
|
||||
(f"RADAR HEATMAP: {RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]}", AMBER),
|
||||
(f"ROUTE: {current_route_name}" if current_route_name is not None else "", SLATE),
|
||||
(f"PLATFORM: {current_route_model}" if current_route_model is not None else "", SLATE),
|
||||
(f"OFFSET: {current_offset_seconds()}s" if route_entries else "", SOFT_WHITE),
|
||||
(f"PLAYBACK: {current_playback:.1f}x" if route_entries else "", SOFT_WHITE),
|
||||
(f"STATUS: {'PAUSED' if paused else 'PLAYING'}" if route_entries else "", SOFT_WHITE),
|
||||
("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", SOFT_WHITE),
|
||||
("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 2)) + " deg", SOFT_WHITE),
|
||||
("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100.0, 2)) + " %", SOFT_WHITE),
|
||||
("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), SOFT_WHITE),
|
||||
]
|
||||
|
||||
hud_height = len(lines) * SPACING + 18
|
||||
rl.draw_rectangle(write_x - 10, write_y - 12, 560, hud_height, rl.Color(8, 12, 18, 155))
|
||||
rl.draw_rectangle(write_x - 10, write_y - 12, 4, hud_height, rl.Color(90, 235, 255, 255))
|
||||
rl.draw_rectangle_lines(write_x - 10, write_y - 12, 560, hud_height, rl.Color(90, 235, 255, 80))
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
if line is not None:
|
||||
color = rl.Color(line[1][0], line[1][1], line[1][2], 255)
|
||||
rl.draw_text_ex(font, line[0], rl.Vector2(write_x, write_y + i * SPACING), 20, 0, color)
|
||||
|
||||
rl.end_drawing()
|
||||
|
||||
rl.unload_texture(camera_texture)
|
||||
rl.unload_texture(top_down_texture)
|
||||
rl.unload_font(font)
|
||||
rl.close_window()
|
||||
stop_replay(replay_proc)
|
||||
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(description="Show replay data in a UI.", formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("ip_address", nargs="?", default="127.0.0.1", help="The ip address on which to receive zmq messages.")
|
||||
parser.add_argument("--route", default=None, help="Route to replay locally before opening the UI.")
|
||||
parser.add_argument("--routes", action="store_true", help="Cycle Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.")
|
||||
parser.add_argument("--custom-routes", nargs="?", type=int, const=1, default=None,
|
||||
help="Cycle routes from tools/radar/custom_routes.py, optionally starting from a 1-based route index.")
|
||||
parser.add_argument("--data-dir", default=None, help="Optional local route data directory to pass to replay.")
|
||||
parser.add_argument("--playback", default="1.0", help="Replay playback speed when using --route.")
|
||||
parser.add_argument("--prefix", default="ui-replay", help="OPENPILOT_PREFIX to use when launching replay from the UI.")
|
||||
parser.add_argument("--frame-address", default=None, help="The frame address (fully qualified ZMQ endpoint for frames) on which to receive zmq messages.")
|
||||
return parser
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
|
||||
selected_sources = int(args.route is not None) + int(args.routes) + int(args.custom_routes is not None)
|
||||
if selected_sources > 1:
|
||||
raise SystemExit("Use only one of --route, --routes, or --custom-routes.")
|
||||
|
||||
route_entries = None
|
||||
start_route_idx = 0
|
||||
if args.route is not None:
|
||||
route_entries = [(args.route, "MANUAL_ROUTE")]
|
||||
elif args.routes:
|
||||
route_entries = get_hkg_routes()
|
||||
elif args.custom_routes is not None:
|
||||
route_entries = get_custom_routes()
|
||||
start_route_idx = max(0, min(len(route_entries) - 1, args.custom_routes - 1))
|
||||
|
||||
if route_entries:
|
||||
os.environ["OPENPILOT_PREFIX"] = args.prefix
|
||||
messaging.reset_context()
|
||||
elif args.ip_address != "127.0.0.1":
|
||||
os.environ["ZMQ"] = "1"
|
||||
messaging.reset_context()
|
||||
|
||||
ui_thread(args.ip_address, route_entries=route_entries, playback=args.playback, data_dir=args.data_dir, prefix=args.prefix, start_route_idx=start_route_idx)
|
||||
-3192
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user