mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-21 05:43:49 +08:00
Compare commits
15 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 48b241fb6c | |||
| 11dc83736f | |||
| 2412ae892b | |||
| 75011ef0f0 | |||
| 9489b2f481 | |||
| c270c7835e | |||
| c6e564cdcd | |||
| b55c4be825 | |||
| dbba01d933 | |||
| 06604f601f | |||
| 8108740b60 | |||
| 64f2381c2e | |||
| 61af0a0512 | |||
| 066c0e2957 | |||
| 26b8501e7b |
@@ -42,7 +42,7 @@ jobs:
|
||||
WARP_PKL="openpilot/sunnypilot/modeld_v2/models/driving_warp_${res}_tinygrad.pkl"
|
||||
echo "Compiling $WARP_PKL on QCOM"
|
||||
taskset -c 7 env DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1 python3 openpilot/sunnypilot/modeld_v2/compile_warp.py \
|
||||
--frame ${FRAME_PARAM} --warp-to ${MODEL_SIZE} --output "${WARP_PKL}"
|
||||
--frame ${FRAME_PARAM} --warp-to ${MODEL_SIZE} --split-frames --output "${WARP_PKL}"
|
||||
|
||||
BIG_WARP_PKL="openpilot/sunnypilot/modeld_v2/models/big_driving_warp_${res}_tinygrad.pkl"
|
||||
echo "Compiling $BIG_WARP_PKL on AMD"
|
||||
|
||||
+7
-1
@@ -4,7 +4,7 @@
|
||||
|
||||
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
|
||||
|
||||
# 351 Supported Cars
|
||||
# 357 Supported Cars
|
||||
|
||||
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br> |Video|Setup Video|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
|
||||
@@ -139,6 +139,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Ioniq Plug-in Hybrid 2020-22">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|6 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona 2020">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai O connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona 2022-23">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona (without HDA II) 2024-25|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona (without HDA II) 2024-25">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Electric 2018-21">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona Electric 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai O connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Electric 2022-23">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona Electric (with HDA II, Korea only) 2023|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Electric (with HDA II, Korea only) 2023">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=U2fOCmcQ8hw" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
@@ -152,19 +153,23 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Hyundai|Santa Fe Plug-in Hybrid 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Santa Fe Plug-in Hybrid 2022-23">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata 2018-19">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Sonata 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata 2020-23">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=ix63r9kE3Fw" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Hyundai|Sonata (without HDA II) 2024-26|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata (without HDA II) 2024-26">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Sonata Hybrid 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata Hybrid 2020-23">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Sonata Hybrid (without HDA II) 2024-26|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Sonata Hybrid (without HDA II) 2024-26">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Staria 2023|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Staria 2023">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson 2021">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson 2022|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson 2022">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson 2023-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson 2023-24">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Diesel 2019">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson Hybrid 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Hybrid 2022-24">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson Hybrid (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Hybrid (without HDA II) 2025-26">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson Plug-in Hybrid 2024|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Plug-in Hybrid 2024">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Veloster 2019-20">Buy Here</a></sub></details>|||
|
||||
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Jeep Grand Cherokee 2016-18">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=eLR9o2JkuRk" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Jeep Grand Cherokee 2019-21">Buy Here</a></sub></details>|||
|
||||
|Kia|Carnival 2022-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Carnival 2022-24">Buy Here</a></sub></details>|||
|
||||
|Kia|Carnival (China only) 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Carnival (China only) 2023">Buy Here</a></sub></details>|||
|
||||
|Kia|Carnival Hybrid (with HDA II) 2025-26|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai Q connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Carnival Hybrid (with HDA II) 2025-26">Buy Here</a></sub></details>|||
|
||||
|Kia|Ceed 2019-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Ceed 2019-21">Buy Here</a></sub></details>|||
|
||||
|Kia|Ceed Plug-in Hybrid 2021|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Ceed Plug-in Hybrid 2021">Buy Here</a></sub></details>|||
|
||||
|Kia|EV6 (Southeast Asia only) 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai P connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia EV6 (Southeast Asia only) 2022-24">Buy Here</a></sub></details>|||
|
||||
@@ -190,6 +195,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Kia|Niro Plug-in Hybrid 2020|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai D connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro Plug-in Hybrid 2020">Buy Here</a></sub></details>|||
|
||||
|Kia|Niro Plug-in Hybrid 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai D connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro Plug-in Hybrid 2021">Buy Here</a></sub></details>|||
|
||||
|Kia|Niro Plug-in Hybrid 2022|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro Plug-in Hybrid 2022">Buy Here</a></sub></details>|||
|
||||
|Kia|Niro Plus EV 2024|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro Plus EV 2024">Buy Here</a></sub></details>|||
|
||||
|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Optima 2017">Buy Here</a></sub></details>|||
|
||||
|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Optima 2019-20">Buy Here</a></sub></details>|||
|
||||
|Kia|Optima Hybrid 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Optima Hybrid 2019">Buy Here</a></sub></details>|||
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: 48611ae9d6...c0e74738a8
@@ -3,7 +3,7 @@ from collections.abc import Callable
|
||||
import base64
|
||||
import ctypes
|
||||
import gc
|
||||
from functools import cached_property
|
||||
from functools import cached_property, partial
|
||||
import os
|
||||
os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
|
||||
os.environ.setdefault('AM_POWER_LIMIT', '100')
|
||||
@@ -43,6 +43,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
|
||||
from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob, wait_for_chestnut
|
||||
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
|
||||
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
|
||||
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
@@ -54,7 +55,8 @@ BIG_MODEL_TIMEOUT = 60
|
||||
|
||||
|
||||
def get_action_from_model(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:
|
||||
lat_action_t: float, long_action_t: float, v_ego: float,
|
||||
turn_shaper: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
|
||||
if 'action' not in model_output:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
|
||||
@@ -69,6 +71,8 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
|
||||
else:
|
||||
desired_accel = model_output['action'][0,1]
|
||||
desired_curvature = model_output['action'][0,0] / (max(1.0, v_ego))**2
|
||||
if turn_shaper is not None:
|
||||
desired_curvature = turn_shaper(desired_curvature)
|
||||
stop = should_stop(v_ego, desired_accel)
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
|
||||
if v_ego > MIN_LAT_CONTROL_SPEED:
|
||||
@@ -340,6 +344,7 @@ def main(demo=False):
|
||||
|
||||
DH = DesireHelper()
|
||||
RELC = RoadEdgeLaneChangeController()
|
||||
turn_planner = TurnPlanner(CP.wheelbase)
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -448,7 +453,9 @@ def main(demo=False):
|
||||
|
||||
if not sm['carControl'].latActive:
|
||||
prev_action.desiredCurvature = sm['controlsState'].curvature
|
||||
action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
|
||||
action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
|
||||
partial(turn_planner.update, model_output, v_ego, sm['controlsState'].curvature,
|
||||
sm['carControl'].latActive, is_rhd))
|
||||
prev_action = action
|
||||
fill_model_msg(modelv2_send, model_output, action,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
|
||||
@@ -249,7 +249,11 @@ class ModelsLayout(Widget):
|
||||
return {"name": name, "downloading": True, "progress": progress, "status_text": tr("verifying")}
|
||||
if ds.downloading in statuses:
|
||||
speed = max((p.speed for p in progresses), default=0.0)
|
||||
status_text = f"{speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "--.- MB/s"
|
||||
speed_text = f"{speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "--.- MB/s"
|
||||
if any(p.resuming for p in progresses):
|
||||
status_text = tr("resuming...") + f" {speed_text}"
|
||||
else:
|
||||
status_text = speed_text
|
||||
return {"name": name, "downloading": True, "progress": progress, "status_text": status_text}
|
||||
if statuses <= {ds.downloaded, ds.cached}:
|
||||
return {"name": name, "text_color": ON_COLOR, "icon": "icons/checkmark.png"}
|
||||
|
||||
@@ -267,8 +267,8 @@ class ModelsLayoutMici(NavScroller):
|
||||
self.current_model_info.info_text.set_text(info_text)
|
||||
|
||||
if manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.failed:
|
||||
self.current_model_info.info_header.set_text(tr("error") + self._download_progress)
|
||||
self.current_model_info.info_text.set_text(tr("download failed"))
|
||||
self.current_model_info.info_header.set_text(tr("error"))
|
||||
self.current_model_info.info_text.set_text(f"{manager.selectedBundle.internalName.lower()} | {tr('download failed')}")
|
||||
|
||||
elif manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.downloading:
|
||||
self.cancel_download_btn.set_visible(ui_state.params.get("ModelManager_DownloadRef") is not None)
|
||||
@@ -277,6 +277,7 @@ class ModelsLayoutMici(NavScroller):
|
||||
count = 0
|
||||
verifying = False
|
||||
canceling = False
|
||||
resuming = False
|
||||
speed = 0.0
|
||||
for model in manager.selectedBundle.models:
|
||||
count += 1
|
||||
@@ -286,12 +287,13 @@ class ModelsLayoutMici(NavScroller):
|
||||
progress += p.progress
|
||||
verifying = verifying or p.status == custom.ModelManagerSP.DownloadStatus.verifying
|
||||
canceling = canceling or p.canceling
|
||||
resuming = resuming or p.resuming
|
||||
speed = max(speed, p.speed) # only one file downloads at a time, so the non-zero speed is the live one
|
||||
elif p.status in (custom.ModelManagerSP.DownloadStatus.downloaded,
|
||||
custom.ModelManagerSP.DownloadStatus.cached):
|
||||
progress += 100.0
|
||||
|
||||
header = tr("canceling...") if canceling else tr("verifying") if verifying else tr("downloading")
|
||||
header = tr("canceling...") if canceling else tr("verifying") if verifying else tr("resuming...") if resuming else tr("downloading")
|
||||
self.current_model_info.current_model_header.set_text(header)
|
||||
self.cancel_download_btn.set_text(tr("cancel verification") if verifying else tr("cancel download"))
|
||||
self.current_model_info.current_model_header._shimmer = True
|
||||
|
||||
@@ -17,16 +17,25 @@ def main():
|
||||
parser.add_argument("--frame", required=True, help="width,height,stride,y_height,uv_height,buffer_size")
|
||||
parser.add_argument("--warp-to", required=True, help="model input WxH")
|
||||
parser.add_argument("--output", required=True)
|
||||
parser.add_argument("--split-frames", action="store_true", help="one input per camera (frame, big_frame), bound to the camera buffers by pointer")
|
||||
args = parser.parse_args()
|
||||
|
||||
from examples.openpilot.compile_warp import make_frame_prepare, parse_frame, parse_size
|
||||
from tinygrad import Device, Tensor, TinyJit
|
||||
frame = parse_frame(args.frame)
|
||||
warp = make_frame_prepare(frame, *parse_size(args.warp_to))
|
||||
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[i], M_inv[i]) for i in range(2))), prune=True)
|
||||
rng = np.random.default_rng(42)
|
||||
inputs = {"input_frame": Tensor(rng.integers(0, 256, (2, frame.size), dtype=np.uint8), device=Device.DEFAULT).realize(),
|
||||
"M_inv": Tensor((rng.standard_normal((2, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()}
|
||||
frames = rng.integers(0, 256, (2, frame.size), dtype=np.uint8)
|
||||
m_inv = Tensor((rng.standard_normal((2, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()
|
||||
if args.split_frames:
|
||||
run = TinyJit(lambda frame, big_frame, M_inv: Tensor.stack(warp(frame, M_inv[0]), warp(big_frame, M_inv[1])), prune=True)
|
||||
inputs = {"frame": Tensor(frames[0], device=Device.DEFAULT).realize(), "big_frame": Tensor(frames[1], device=Device.DEFAULT).realize(), "M_inv": m_inv}
|
||||
specs = {"frame": ((frame.size,), "|u1", Device.DEFAULT), "big_frame": ((frame.size,), "|u1", Device.DEFAULT),
|
||||
"M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
|
||||
else:
|
||||
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[i], M_inv[i]) for i in range(2))), prune=True)
|
||||
inputs = {"input_frame": Tensor(frames, device=Device.DEFAULT).realize(), "M_inv": m_inv}
|
||||
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
|
||||
with record_kernel_linear() as recorded:
|
||||
run(**inputs)
|
||||
run(**inputs)
|
||||
@@ -34,7 +43,6 @@ def main():
|
||||
assert len(recorded) == 1, f"expected one captured kernel list, got {len(recorded)}"
|
||||
part = serialize_jit(run, recorded[0], check)
|
||||
PlainJit(*part)
|
||||
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
|
||||
write_bundle(args.output, {"run": part}, {"input_specs": specs, "metadata": {}})
|
||||
print(f"Saved warp to {args.output} ({len(part[0]['calls'])} kernels)")
|
||||
|
||||
|
||||
@@ -12,6 +12,8 @@ from functools import partial
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.sunnypilot.modeld_v2.helpers import _dtype, read_parts
|
||||
|
||||
SPLIT_FRAMES = ("big_frame", "frame")
|
||||
|
||||
|
||||
def pack_layout(shapes: dict[str, tuple], frame_size: int) -> tuple[dict[str, int], int]:
|
||||
offsets, offset = {}, 0
|
||||
@@ -39,29 +41,30 @@ def frame_part(warp_path: str, parts: dict, extras: dict) -> dict:
|
||||
(model, model_blob), (warp, warp_blob) = parts["run"], read_parts(warp_path)[0]["run"]
|
||||
states = sorted(name for name in extras["input_specs"] if f"next_{name}" in extras["metadata"]["output_shapes"])
|
||||
host = {name: shape for name, (shape, _, _) in extras["input_specs"].items() if name not in states and name != "new_img"}
|
||||
frame_size = next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2
|
||||
offsets, packed = pack_layout(host, frame_size)
|
||||
frame_specs = {s["name"]: s for s in warp["inputs"] if s["name"] in SPLIT_FRAMES}
|
||||
offsets, packed = pack_layout(host, 0 if frame_specs else next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2)
|
||||
shift, upload, u8 = len(model_blob), len(model["buffers"]) + len(warp["buffers"]), repr(_dtype("dtypes.uint8"))
|
||||
|
||||
def view(name: str, nbytes: int, dtype: str) -> dict:
|
||||
return {"buffer": upload, "offset": offsets[name], "nbytes": nbytes, "dtype": dtype}
|
||||
outputs = sorted(s["slot"] for s in model["inputs"] if s["name"] is None)
|
||||
names = sorted(["host", *states])
|
||||
names = sorted(["host", *states, *frame_specs])
|
||||
slots = {s["slot"]: names.index(s["name"]) for s in model["inputs"] if s["name"] in states} | \
|
||||
{slot: len(names) + i for i, slot in enumerate(outputs)}
|
||||
returned = warp["returns"]["tensors"][0]
|
||||
model_inputs = {s["slot"]: {"input": slots[s["slot"]]} if s["slot"] in slots else
|
||||
{k: v for k, v in returned.items() if k != "shape"} | {"buffer": returned["buffer"] + len(model["buffers"])} if s["name"] == "new_img" else
|
||||
view(s["name"], s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in model["inputs"]}
|
||||
warp_inputs = {s["slot"]: view("frames" if s["name"] == "input_frame" else "tfm", s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"])
|
||||
for s in warp["inputs"]}
|
||||
warp_inputs = {s["slot"]: {"input": names.index(s["name"])} if s["name"] in frame_specs else
|
||||
view("frames" if s["name"] == "input_frame" else "tfm", s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in warp["inputs"]}
|
||||
|
||||
def remap(calls: list, inputs: dict, buffers: int, lib: int) -> list:
|
||||
return [call | ({"lib": [call["lib"][0] + lib, call["lib"][1] + lib]} if "lib" in call else {}) |
|
||||
{"args": [dict(inputs[a["input"]]) if "input" in a else a | {"buffer": a["buffer"] + buffers} for a in call["args"]]} for call in calls]
|
||||
header = {"input_names": names, "returns": {"kind": "NoneType", "tensors": []}, "check": None,
|
||||
"inputs": [{"slot": names.index("host"), "name": "host", "dtype": u8, "numel": packed, "device": "NPY"}] +
|
||||
[s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots],
|
||||
[s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots] +
|
||||
[s | {"slot": names.index(name)} for name, s in frame_specs.items()],
|
||||
"buffers": model["buffers"] + [b | ({"init": [b["init"][0] + shift, b["init"][1] + shift]} if "init" in b else {}) for b in warp["buffers"]] +
|
||||
[{"nbytes": packed, "device": model["inputs"][0]["device"], "dtype": u8}],
|
||||
"calls": [{"kind": "copy", "args": [{"buffer": upload, "offset": 0, "nbytes": packed, "dtype": u8}, {"input": names.index("host")}]}] +
|
||||
|
||||
@@ -270,15 +270,32 @@ def _program_uop(call: dict, lib: bytes):
|
||||
UOp(Ops.SOURCE, arg=""), UOp(Ops.BINARY, arg=lib)), arg=info)
|
||||
|
||||
|
||||
def _input_tensors(kwargs: dict) -> list: # in _prepare_jit_inputs' order
|
||||
from tinygrad import Tensor
|
||||
tensors = [t for _, t in sorted(kwargs.items()) if t.__class__ is Tensor]
|
||||
for value in kwargs.values():
|
||||
tensors += [t for t in (value.values() if isinstance(value, dict) else value if isinstance(value, (tuple, list)) else ())
|
||||
if t.__class__ is Tensor and not any(t is y for y in tensors)]
|
||||
return tensors
|
||||
|
||||
|
||||
class _Jit:
|
||||
def __init__(self, captured, input_names: list, buffers: list):
|
||||
self.captured, self.input_names, self.buffers = captured, input_names, buffers
|
||||
|
||||
def __call__(self, *args, **kwargs):
|
||||
import tinygrad.engine.jit as jit
|
||||
from tinygrad.uop.ops import Ops
|
||||
# same names, no variables: the binding is the tensors' own uops, without _prepare_jit_inputs' graph rewrites
|
||||
if not args and self.__dict__.get("_names") == sorted(kwargs):
|
||||
tensors = _input_tensors(kwargs)
|
||||
if all(t.uop.is_realized for t in tensors):
|
||||
return self.captured([t.uop.src[0] if t.uop.op is Ops.UNSHARD else t.uop for t in tensors], {})
|
||||
input_uops, var_vals, names, _ = jit._prepare_jit_inputs(args, kwargs)
|
||||
if names != self.input_names:
|
||||
raise ValueError(f"expected inputs {self.input_names}, got {names}")
|
||||
if not args and not var_vals:
|
||||
self._names = sorted(kwargs)
|
||||
return self.captured(input_uops, var_vals)
|
||||
|
||||
|
||||
@@ -311,7 +328,7 @@ class PlainJit(_Jit):
|
||||
calls.append(_program_uop(call, bytes(blob[slice(*call["lib"])])).call(*arguments))
|
||||
else:
|
||||
calls.append(arguments[0].store_call(arguments[1]))
|
||||
# stand-ins for JIT inputs: copy staging to GPU is decided from the input buffers, the real ones bind per call
|
||||
# allocate gpu buffers
|
||||
stand_ins = {slot: UOp.from_buffer(Buffer(_device(spec["device"], device_map), spec["numel"] * _dtype(spec["dtype"]).itemsize).ensure_allocated(),
|
||||
_dtype(spec["dtype"])) for slot, spec in inputs.items()}
|
||||
input_uops = tuple(stand_ins.get(slot, next(iter(stand_ins.values()), None)) for slot in range(max(stand_ins, default=-1) + 1))
|
||||
|
||||
@@ -11,7 +11,7 @@ import pickle
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.sunnypilot.modeld_v2.frame_fusion import pack_layout
|
||||
from openpilot.sunnypilot.modeld_v2.frame_fusion import pack_layout, SPLIT_FRAMES
|
||||
from openpilot.sunnypilot.modeld_v2.helpers import load_model
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import (POLICY_INPUTS, derive_frame_skip,
|
||||
make_split_input_queues, make_supercombo_input_queues)
|
||||
@@ -169,16 +169,18 @@ class NativeTinygradAdapter(BaseModelAdapter):
|
||||
self.warp_frame_size = self.frame_copy_size
|
||||
self._init_common()
|
||||
self.run_model = self.jits['run']
|
||||
self.split = [name for name in SPLIT_FRAMES if name in self.run_model.input_names]
|
||||
self.frame_inputs: dict[str, Tensor] = {}
|
||||
|
||||
self.input_queues = {name: Tensor(np.zeros(shape, dtype=dtype), device=self.model_device).realize()
|
||||
for name, (shape, dtype) in self.input_shapes.items() if name in self.state_pairs}
|
||||
shapes = {name: shape for name, (shape, _) in self.input_shapes.items() if name not in self.state_pairs and name != 'new_img'}
|
||||
offsets, packed = pack_layout(shapes, self.warp_frame_size)
|
||||
offsets, packed = pack_layout(shapes, 0 if self.split else self.warp_frame_size)
|
||||
self.packed_input = np.zeros(packed, dtype=np.uint8)
|
||||
self.host = Tensor(self.packed_input, device='NPY')
|
||||
self.numpy_inputs = {name: np.ndarray(shape, dtype=np.float32, buffer=self.packed_input, offset=offsets[name])
|
||||
for name, shape in ({'tfm': (2, 3, 3)} | shapes).items()}
|
||||
self.frames = self.packed_input[offsets['frames']:].reshape(2, self.warp_frame_size)
|
||||
self.frames = self.packed_input[offsets['frames']:].reshape(2, -1)
|
||||
|
||||
self.outputs = {name: Tensor(np.zeros(shape, dtype=dtype), device=device).realize() for name, (shape, dtype, device) in self.jits['output_specs'].items()}
|
||||
for name, next_name in self.state_pairs.items():
|
||||
@@ -187,17 +189,29 @@ class NativeTinygradAdapter(BaseModelAdapter):
|
||||
|
||||
def copy_frames(self, bufs):
|
||||
for i, key in enumerate(self._vision_input_names):
|
||||
if key in bufs:
|
||||
if key not in bufs:
|
||||
continue
|
||||
if not self.split:
|
||||
data = bufs[key].data if hasattr(bufs[key], 'data') else bufs[key]
|
||||
np.copyto(self.frames[i], np.frombuffer(data, dtype=np.uint8, count=self.warp_frame_size))
|
||||
elif (name := ('frame', 'big_frame')[i]) in self.split:
|
||||
if isinstance(bufs[key], np.ndarray): # cached memory: the GPU would read stale lines in place
|
||||
self.frame_inputs[name] = Tensor(np.ascontiguousarray(bufs[key][:self.warp_frame_size]), device=self.model_device).realize()
|
||||
continue
|
||||
ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data # visionipc buffers are coherent
|
||||
if (key, ptr) not in self._blob_cache:
|
||||
self._blob_cache[(key, ptr)] = Tensor.from_blob(ptr, (self.warp_frame_size,), dtype='uint8', device=self.model_device)
|
||||
self.frame_inputs[name] = self._blob_cache[(key, ptr)]
|
||||
|
||||
def reset_warmup_buffers(self) -> None:
|
||||
self.packed_input[:] = 0
|
||||
self.frame_inputs.clear()
|
||||
self._blob_cache.clear()
|
||||
for key in self.state_pairs:
|
||||
self.input_queues[key].assign(0).realize()
|
||||
|
||||
def run(self):
|
||||
self.run_model(host=self.host, **self.input_queues, output_buffers=self.outputs)
|
||||
self.run_model(host=self.host, **self.input_queues, **self.frame_inputs, output_buffers=self.outputs)
|
||||
return self.outputs['outputs']
|
||||
|
||||
|
||||
|
||||
@@ -41,6 +41,7 @@ from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
|
||||
from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
|
||||
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
|
||||
from openpilot.sunnypilot.modeld_v2.blendful import Blendful
|
||||
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
|
||||
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
|
||||
@@ -231,7 +232,8 @@ 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,
|
||||
blendful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
|
||||
blendful: Callable[[float], float] | None = None,
|
||||
turn_shaper: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
|
||||
if 'action' not in model_output:
|
||||
plan = model_output['plan'][0]
|
||||
desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
|
||||
@@ -245,6 +247,8 @@ class ModelState(ModelStateBase):
|
||||
desired_curvature = model_output['action'][0, 0] / (max(2.5, v_ego))**2
|
||||
if blendful is not None:
|
||||
desired_curvature += blendful(desired_curvature)
|
||||
if turn_shaper is not None:
|
||||
desired_curvature = turn_shaper(desired_curvature)
|
||||
|
||||
stop = v_ego < 0.3 and desired_accel < 0.1
|
||||
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
|
||||
@@ -373,6 +377,7 @@ def main(demo=False):
|
||||
RELC = RoadEdgeLaneChangeController()
|
||||
blendful = Blendful()
|
||||
blendful_enabled = False
|
||||
turn_planner = TurnPlanner(CP.wheelbase)
|
||||
|
||||
while True:
|
||||
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
|
||||
@@ -492,7 +497,9 @@ def main(demo=False):
|
||||
prev_action.desiredCurvature = sm['controlsState'].curvature
|
||||
blendful_active = desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off
|
||||
action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
|
||||
partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled))
|
||||
partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled),
|
||||
partial(turn_planner.update, model_output, v_ego, sm['controlsState'].curvature,
|
||||
sm['carControl'].latActive, is_rhd))
|
||||
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,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:4b22fd6bcb3a3e8a642e6866449dd9c30c12e694ff0741cad9e34e4b00d8c5fa
|
||||
size 3698799
|
||||
oid sha256:d776c72125156b5dd3eb59768409401e28c8a52a3a619cd3b9f02c10a156fda6
|
||||
size 3699072
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:9a17fd6acece4fab23784ef0f3f8bba28a049e9599e5e9e5ee7b36e4b1d24a6f
|
||||
size 7924847
|
||||
oid sha256:a3f13ed12e087817a86afd48b2496c66c4f61a1925065b676b2945e81fabaacd
|
||||
size 7924864
|
||||
|
||||
@@ -144,6 +144,21 @@ class TestPlainArtifact(OpenpilotTestCase):
|
||||
for name in ("outputs", "next_state_feat_q"):
|
||||
assert np.array_equal(inputs["output_buffers"][name].numpy(), expected["output_buffers"][name].numpy()), name
|
||||
|
||||
split_path = os.path.join(self.tmp.name, "warp_split_tinygrad.pkl")
|
||||
with mock.patch.object(sys, "argv", ["compile_warp", "--frame", FRAME, "--warp-to", f"{MODEL_W}x{MODEL_H}", "--output", split_path, "--split-frames"]):
|
||||
compile_warp.main()
|
||||
split = load_model(path, cache=False, extend=functools.partial(frame_part, split_path))
|
||||
assert split["run"].input_names == ["big_frame", "frame", "host", "state_feat_q"]
|
||||
offsets, packed = pack_layout(HOST, 0)
|
||||
host = np.zeros(packed, np.uint8)
|
||||
for name, shape in ({"tfm": (2, 3, 3)} | HOST).items():
|
||||
np.ndarray(shape, np.float32, host, offsets[name])[...] = tfm.numpy() if name == "tfm" else inputs[name].numpy()
|
||||
out = {"outputs": Tensor.zeros(OUT).contiguous().realize(), "next_state_feat_q": Tensor.zeros(STATE).contiguous().realize()}
|
||||
split["run"](host=Tensor(host, device="NPY"), frame=Tensor(frames.numpy()[0]).realize(), big_frame=Tensor(frames.numpy()[1]).realize(),
|
||||
state_feat_q=expected["state_feat_q"], output_buffers=out)
|
||||
for name in ("outputs", "next_state_feat_q"):
|
||||
assert np.array_equal(out[name].numpy(), expected["output_buffers"][name].numpy()), name
|
||||
|
||||
def test_tinygrad_api_the_loader_reads(self):
|
||||
import tinygrad.engine.jit as jit
|
||||
from tinygrad.device import Buffer, TinyELF
|
||||
|
||||
@@ -0,0 +1,239 @@
|
||||
"""
|
||||
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 math
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner, compute_plan_peak_curv
|
||||
|
||||
|
||||
def make_plan(curv: float, speed: float = 4.0) -> dict[str, np.ndarray]:
|
||||
"""Build a synthetic model_output dict with a constant-curvature arc plan."""
|
||||
t = np.array(ModelConstants.T_IDXS, dtype=np.float32)
|
||||
s = speed * t
|
||||
plan = np.zeros((1, len(t), ModelConstants.PLAN_WIDTH), dtype=np.float32)
|
||||
if abs(curv) < 1e-6:
|
||||
plan[0, :, Plan.POSITION][:, 0] = s
|
||||
plan[0, :, Plan.POSITION][:, 1] = 0.0
|
||||
plan[0, :, Plan.T_FROM_CURRENT_EULER][:, 2] = 0.0
|
||||
else:
|
||||
r = 1.0 / curv
|
||||
theta = s * curv
|
||||
plan[0, :, Plan.POSITION][:, 0] = r * np.sin(theta)
|
||||
plan[0, :, Plan.POSITION][:, 1] = r * (1.0 - np.cos(theta))
|
||||
plan[0, :, Plan.T_FROM_CURRENT_EULER][:, 2] = theta
|
||||
return {'plan': plan}
|
||||
|
||||
|
||||
class TestTurnPlanner(OpenpilotTestCase):
|
||||
def test_plan_peak_curv_extraction(self):
|
||||
assert abs(compute_plan_peak_curv(make_plan(0.0, 5.0))) < 1e-5
|
||||
pk = compute_plan_peak_curv(make_plan(0.12, 4.0))
|
||||
assert abs(pk - 0.12) < 0.01
|
||||
pk_l = compute_plan_peak_curv(make_plan(-0.15, 4.0))
|
||||
assert abs(pk_l + 0.15) < 0.01
|
||||
assert compute_plan_peak_curv({}) == 0.0
|
||||
|
||||
def test_untouched_at_highway_speed(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
for i in range(60):
|
||||
k = 0.01 * math.sin(i * 0.1)
|
||||
out = tp.update(make_plan(k, 25.0), v_ego=25.0, actual_curv=k, active=True, is_rhd=False, model_curv=k)
|
||||
assert out == k
|
||||
|
||||
def test_untouched_on_gentle_low_speed_curve(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
for i in range(80):
|
||||
k = 0.025 * min(1.0, i / 20.0)
|
||||
out = tp.update(make_plan(k, 6.0), v_ego=6.0, actual_curv=k, active=True, is_rhd=False, model_curv=k)
|
||||
assert out == k
|
||||
|
||||
def test_sign_preservation_and_magnitude_bound_under_jitter(self):
|
||||
"""Output must never oppose model_curv or exceed APEX_PATH_BOOST_CAP * |model_curv|."""
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Reproduces a5 seg31 Turn 1: brief negative jitter then sharp positive right turn
|
||||
ks = [-0.015] * 15 + [0.016, 0.05, 0.11, 0.14, 0.17, 0.18, 0.14, 0.08, 0.02, -0.01]
|
||||
plan_right = make_plan(0.15, 3.0)
|
||||
for k in ks:
|
||||
out = tp.update(plan_right, v_ego=3.0, actual_curv=k * 0.7, active=True, is_rhd=False, model_curv=k)
|
||||
assert abs(out) <= abs(k) * 1.35 + 1e-9
|
||||
if k > 0:
|
||||
assert out >= 0.0
|
||||
elif k < 0:
|
||||
assert out <= 0.0
|
||||
|
||||
def test_zero_lag_on_turn_exit_unwinding(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
plan_tight = make_plan(0.14, 4.0)
|
||||
# Ramp up entry and hold through apex so buffer is at peak curvature
|
||||
for i in range(25):
|
||||
k = 0.14 * (i / 25.0)
|
||||
tp.update(plan_tight, v_ego=4.0, actual_curv=k * 0.8, active=True, is_rhd=False, model_curv=k)
|
||||
for _ in range(15):
|
||||
tp.update(plan_tight, v_ego=4.0, actual_curv=0.14, active=True, is_rhd=False, model_curv=0.14)
|
||||
# Now unwind monotonically: output must equal model_curv with zero lag
|
||||
for i in range(1, 20):
|
||||
k = max(0.0, 0.14 * (1.0 - i / 15.0))
|
||||
out = tp.update(make_plan(k, 4.0), v_ego=4.0, actual_curv=k + 0.01, active=True, is_rhd=False, model_curv=k)
|
||||
assert abs(out - k) < 1e-3
|
||||
|
||||
def test_standstill_launch_and_mid_turn_pause(self):
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Stopped straight at stop bar, then launches into a sharp right turn
|
||||
for _ in range(10):
|
||||
tp.update(make_plan(0.12, 0.0), v_ego=0.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.002)
|
||||
# First few steps of launch should hold back early curvature ramp
|
||||
outs = []
|
||||
for i in range(1, 20):
|
||||
k = 0.006 * i
|
||||
outs.append(tp.update(make_plan(0.12, 2.5), v_ego=2.5, actual_curv=0.004 * i, active=True, is_rhd=False, model_curv=k))
|
||||
assert outs[10] < 0.006 * 11 - 0.01
|
||||
|
||||
# Pause mid-turn (actual_curv = 0.05) should preserve turn_psi and not dip below 0.05 on resume
|
||||
tp.turn_psi = math.radians(30.0)
|
||||
tp.update(make_plan(0.08, 0.0), v_ego=0.0, actual_curv=0.05, active=True, is_rhd=False, model_curv=0.05)
|
||||
assert tp.turn_psi >= math.radians(29.0)
|
||||
resumed = tp.update(make_plan(0.08, 2.0), v_ego=2.0, actual_curv=0.05, active=True, is_rhd=False, model_curv=0.06)
|
||||
assert resumed >= 0.05 - 1e-6
|
||||
|
||||
def test_wheelbase_and_rhd_scaling(self):
|
||||
tp_short = TurnPlanner(wheelbase=2.4)
|
||||
tp_long = TurnPlanner(wheelbase=3.4)
|
||||
plan_r = make_plan(0.14, 4.2)
|
||||
for _ in range(25):
|
||||
tp_short.update(plan_r, 4.2, 0.0, True, False, 0.02)
|
||||
tp_long.update(plan_r, 4.2, 0.0, True, False, 0.02)
|
||||
assert tp_long.shift_dist > tp_short.shift_dist * 1.3
|
||||
|
||||
# RHD gives larger shift on left turns (k < 0) than right turns (k > 0)
|
||||
tp_rhd_l = TurnPlanner(wheelbase=2.89)
|
||||
tp_rhd_r = TurnPlanner(wheelbase=2.89)
|
||||
for _ in range(25):
|
||||
tp_rhd_l.update(make_plan(-0.14, 3.5), 3.5, 0.0, True, True, -0.02)
|
||||
tp_rhd_r.update(make_plan(0.14, 3.5), 3.5, 0.0, True, True, 0.02)
|
||||
assert tp_rhd_l.shift_dist > tp_rhd_r.shift_dist * 1.2
|
||||
|
||||
def test_closed_loop_90deg_turn_clears_curb_without_oncoming_overshoot(self):
|
||||
"""Closed-loop Tesla Model Y 90-deg right turn around R=2.5 m curb."""
|
||||
L, W_half, x_cam, lane_w, curb_r = 2.89, 0.96, 1.50, 3.60, 2.5
|
||||
y_curb = lane_w / 2.0
|
||||
x_dest = curb_r + lane_w / 2.0
|
||||
x_yellow = x_dest + lane_w / 2.0
|
||||
|
||||
pts = [(x, 0.0) for x in np.linspace(-20, x_dest - 2.0, 200)]
|
||||
pts += [(x_dest - 2.0 + 2.0 * math.cos(th), 2.0 + 2.0 * math.sin(th)) for th in np.linspace(-math.pi / 2, 0, 50)]
|
||||
pts += [(x_dest, y) for y in np.linspace(2.0, 25.0, 200)]
|
||||
ref = np.array(pts)
|
||||
|
||||
def run(use_planner: bool) -> tuple[float, float]:
|
||||
X, Y, psi, k_act = -15.0, 0.0, 0.0, 0.0
|
||||
v = 3.5
|
||||
tp = TurnPlanner(wheelbase=L, dt=DT_MDL) if use_planner else None
|
||||
min_clr, max_ovr = 99.0, -99.0
|
||||
for _ in range(400):
|
||||
xc, yc = X + x_cam * math.cos(psi), Y + x_cam * math.sin(psi)
|
||||
i_near = int(np.argmin(np.hypot(ref[:, 0] - xc, ref[:, 1] - yc)))
|
||||
i_tgt = i_near
|
||||
while i_tgt < len(ref) - 1 and np.hypot(ref[i_tgt, 0] - xc, ref[i_tgt, 1] - yc) < 6.5:
|
||||
i_tgt += 1
|
||||
dx, dy = ref[i_tgt, 0] - xc, ref[i_tgt, 1] - yc
|
||||
lx = dx * math.cos(psi) + dy * math.sin(psi)
|
||||
ly = -dx * math.sin(psi) + dy * math.cos(psi)
|
||||
k_mod = float(np.clip(2.0 * ly / max(lx**2 + ly**2, 1.0), -0.19, 0.19))
|
||||
plan = make_plan(0.14 if (max(0.0, x_dest - xc) < 15.0 and psi < math.radians(75)) else 0.0, v)
|
||||
|
||||
k_cmd = tp.update(plan, v, k_act, True, False, k_mod) if tp is not None else k_mod
|
||||
k_act += (float(np.clip(k_cmd, -0.19, 0.19)) - k_act) * (DT_MDL / (0.22 + DT_MDL))
|
||||
|
||||
ds = v * DT_MDL
|
||||
psi += k_act * ds
|
||||
X += ds * math.cos(psi)
|
||||
Y += ds * math.sin(psi)
|
||||
|
||||
x_ir, y_ir = X - W_half * math.sin(psi), Y + W_half * math.cos(psi)
|
||||
x_of, y_of = X + L * math.cos(psi) + W_half * math.sin(psi), Y + L * math.sin(psi) - W_half * math.cos(psi)
|
||||
if x_ir <= 0.0:
|
||||
clr = y_curb - y_ir
|
||||
elif y_ir >= y_curb + curb_r:
|
||||
clr = x_ir - curb_r
|
||||
else:
|
||||
clr = math.hypot(x_ir, y_ir - (y_curb + curb_r)) - curb_r
|
||||
min_clr = min(min_clr, clr)
|
||||
if y_of > 2.0:
|
||||
max_ovr = max(max_ovr, x_of - x_yellow)
|
||||
if Y > 15.0:
|
||||
break
|
||||
return min_clr, max_ovr
|
||||
|
||||
base_clr, _ = run(False)
|
||||
plan_clr, plan_ovr = run(True)
|
||||
assert base_clr < -0.5 # baseline curbs the rear wheel badly
|
||||
assert plan_clr > 0.20 # TurnPlanner clears the curb by > 20 cm
|
||||
assert plan_ovr < 0.0 # stays inside the oncoming yellow line
|
||||
|
||||
def test_rolling_speed_taper(self):
|
||||
"""Shift is strong at low speed, light at rolling speed, 0 at 19+ mph, and time-capped at crawl."""
|
||||
tp_crawl = TurnPlanner(wheelbase=2.89)
|
||||
tp_low = TurnPlanner(wheelbase=2.89)
|
||||
tp_roll = TurnPlanner(wheelbase=2.89)
|
||||
tp_fast = TurnPlanner(wheelbase=2.89)
|
||||
for _ in range(30):
|
||||
tp_crawl.update(make_plan(0.14, 1.8), v_ego=1.8, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
tp_low.update(make_plan(0.14, 3.5), v_ego=3.5, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
tp_roll.update(make_plan(0.14, 6.0), v_ego=6.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
tp_fast.update(make_plan(0.14, 9.0), v_ego=9.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
|
||||
assert tp_crawl.shift_dist <= 1.8 * 0.48 + 0.05 # time-capped so crawl starts don't stall
|
||||
assert tp_low.shift_dist > 1.3
|
||||
assert 0.3 < tp_roll.shift_dist < 0.7
|
||||
assert tp_fast.shift_dist == 0.0
|
||||
|
||||
def test_apex_1s_path_boost_and_exit_unwind(self):
|
||||
"""Once past entry, blends with 1s path to boost under-steering apex and unwind hanging exit."""
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Drive past entry (psi > 30 deg)
|
||||
for _ in range(35):
|
||||
tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10)
|
||||
# Apex (psi ~ 48 deg): action under-steers at 0.10 while 1s path says 0.14 -> boosted cleanly above 0.10
|
||||
apex_out = tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10)
|
||||
assert apex_out > 0.115
|
||||
# Advance past 60 deg into late-turn exit
|
||||
for _ in range(20):
|
||||
tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.14)
|
||||
# Exit hang: action still stuck at 0.20 while 1s path already unwound to 0.10 -> pulls down cleanly
|
||||
exit_out = tp.update(make_plan(0.10, 4.0), v_ego=4.0, actual_curv=0.15, active=True, is_rhd=False, model_curv=0.20)
|
||||
assert exit_out < 0.15
|
||||
|
||||
def test_road_edge_curb_guard_holds_until_opening(self):
|
||||
"""Confident straight curb beside car prolongs turn-in until curb flares open or disappears."""
|
||||
x = np.array(ModelConstants.X_IDXS, dtype=np.float32)
|
||||
|
||||
def with_curb(plan_dict, e0=2.4, flare_5m=0.1, std=0.4):
|
||||
edges = np.zeros((1, 2, len(x), 2), dtype=np.float32)
|
||||
edges[0, 0, :, 0] = -5.0
|
||||
edges[0, 1, :, 0] = e0 + (flare_5m / 25.0) * (x ** 2)
|
||||
stds = np.full((1, 2, len(x), 2), std, dtype=np.float32)
|
||||
return {**plan_dict, 'road_edges': edges, 'road_edges_stds': stds}
|
||||
|
||||
tp = TurnPlanner(wheelbase=2.89)
|
||||
# Rolling 15 mph (6.7 m/s) right turn with solid straight curb right beside car -> curb guard holds it down
|
||||
mo_curb = with_curb(make_plan(0.03, 6.7), e0=2.4, flare_5m=0.1, std=0.4)
|
||||
for _ in range(5):
|
||||
out_guarded = tp.update(mo_curb, v_ego=6.7, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.035)
|
||||
assert out_guarded < 0.016
|
||||
|
||||
# Curb flares open (intersection/turn lane opens) -> releases immediately!
|
||||
mo_open = with_curb(make_plan(0.06, 6.7), e0=2.4, flare_5m=1.2, std=1.1)
|
||||
out_open = tp.update(mo_open, v_ego=6.7, actual_curv=0.01, active=True, is_rhd=False, model_curv=0.06)
|
||||
assert out_open > 0.04
|
||||
|
||||
# Normal curved road (curb itself curves at k=0.03 => flare_5m = 0.5 * 0.03 * 25 = 0.375 m) -> untouched
|
||||
tp2 = TurnPlanner(wheelbase=2.89)
|
||||
mo_curve = with_curb(make_plan(0.03, 6.7), e0=2.4, flare_5m=0.375, std=0.4)
|
||||
out_curve = tp2.update(mo_curve, v_ego=6.7, actual_curv=0.03, active=True, is_rhd=False, model_curv=0.03)
|
||||
assert abs(out_curve - 0.03) < 1e-6
|
||||
@@ -0,0 +1,272 @@
|
||||
"""
|
||||
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 math
|
||||
from collections import deque
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
|
||||
|
||||
# Two-stage rear-axle protected turn planner:
|
||||
# Stage 1 (Entry): wheelbase-scaled spatial delay on curvature buildup so the
|
||||
# inside rear tire clears the corner curb instead of off-tracking over it.
|
||||
# Capped in time (MAX_SHIFT_TIME) so crawl-speed starts never feel stalled.
|
||||
# Stage 2 (Apex & Exit): once past initial corner entry, smoothly blends
|
||||
# action curvature with the model's own 1-second path curvature so tight
|
||||
# apexes don't under-steer/go wide and exits unwind cleanly.
|
||||
SHIFT_WHEELBASE_RATIO = 0.58 # fraction of wheelbase to delay turn-in (~1.68 m for 2.89 m WB)
|
||||
WHEELBASE_CLIP = (2.0, 4.0) # m
|
||||
OPPOSITE_SIDE_WEIGHT = 0.70 # lighter shift for non-curb-side turns
|
||||
MAX_SHIFT_TIME = 0.48 # s, caps entry delay at crawl speeds (2-5 mph)
|
||||
|
||||
TURN_SPEED_TAPER = (7.5, 10.5) # m/s (~17-23.5 mph): overall active window
|
||||
SHIFT_SPEED_BP = (3.5, 6.0, 8.5) # m/s (~7.8, 13.4, 19.0 mph)
|
||||
SHIFT_SPEED_V = (1.0, 0.35, 0.0) # full from stop, light on rolling turns, 0 above 19 mph
|
||||
CURV_GATE = (0.04, 0.09) # 1/m (R=25 m -> R=11 m): ignores gentle curves
|
||||
|
||||
ENTRY_PHASE_RELEASE_DEG = (8.0, 22.0) # deg: release entry shift crisply once rotating
|
||||
APEX_BLEND_PHASE_DEG = (10.0, 25.0) # deg: ramp in 1s-path blend through apex & exit
|
||||
APEX_PATH_BLEND_MAX = 0.65 # max weight on 1s path curvature during apex/exit
|
||||
APEX_PATH_BOOST_CAP = 1.35 # max ratio vs |model_curv| for safety
|
||||
PATH_AVG_HORIZON_S = 1.0 # s, horizon for average path curvature
|
||||
|
||||
SHIFT_BUILD_TAU = 0.15 # s
|
||||
SHIFT_RELEASE_TAU = 0.07 # s
|
||||
APEX_BLEND_TAU = 0.12 # s
|
||||
BUFFER_MAX_DIST = 4.5 # m
|
||||
MIN_MOVE_SPEED = 0.3 # m/s
|
||||
MID_TURN_CURV_THRESH = 0.02 # 1/m
|
||||
PLAN_S_RANGE = (1.0, 18.0) # m
|
||||
PLAN_MIN_SEG_DS = 0.8 # m
|
||||
|
||||
# Road-edge curb guard: prevents premature turn-in (e.g. immediately on blinker)
|
||||
# while a confident, unbroken curb still runs straight beside the vehicle.
|
||||
CURB_STD_GATE = (0.55, 0.95) # m: confident curb below 0.55, gone/opening above 0.95
|
||||
CURB_CLOSE_GATE = (2.5, 3.1) # m: curb close to lane below 2.5, open turn bay above 3.1
|
||||
CURB_OPENING_GATE = (0.35, 0.90) # m: (edge_5m - edge_0m) flare indicating intersection opening
|
||||
CURB_FOLLOW_MARGIN_K = 0.005 # 1/m: small lane-keeping allowance above curb's own curvature
|
||||
CURB_CHECK_X = 5.0 # m: lookahead along road edge for curb wall check
|
||||
CURB_GUARD_PHASE_DEG = (10.0, 22.0) # deg: only guards turn entry before car has rotated in
|
||||
|
||||
T_IDXS = np.array(ModelConstants.T_IDXS, dtype=np.float32)
|
||||
X_IDXS = np.array(ModelConstants.X_IDXS, dtype=np.float32)
|
||||
|
||||
|
||||
def compute_plan_metrics(model_output: dict[str, np.ndarray]) -> tuple[float, float]:
|
||||
"""Return (peak_upcoming_curv, avg_1s_path_curv) from model_output['plan']."""
|
||||
plan_arr = model_output.get('plan')
|
||||
if plan_arr is None or plan_arr.ndim < 3 or plan_arr.shape[1] < 3:
|
||||
return 0.0, 0.0
|
||||
|
||||
plan = plan_arr[0]
|
||||
pos = plan[:, Plan.POSITION]
|
||||
yaws = plan[:, Plan.T_FROM_CURRENT_EULER][:, 2]
|
||||
ds = np.hypot(np.diff(pos[:, 0]), np.diff(pos[:, 1]))
|
||||
s = np.concatenate(([0.0], np.cumsum(ds)))
|
||||
|
||||
peak_k = 0.0
|
||||
n = len(yaws)
|
||||
for i in range(n - 2):
|
||||
if s[i] < PLAN_S_RANGE[0]:
|
||||
continue
|
||||
if s[i] > PLAN_S_RANGE[1]:
|
||||
break
|
||||
seg_ds = s[i + 2] - s[i]
|
||||
if seg_ds < PLAN_MIN_SEG_DS:
|
||||
continue
|
||||
k = float((yaws[i + 2] - yaws[i]) / seg_ds)
|
||||
if abs(k) > abs(peak_k):
|
||||
peak_k = k
|
||||
peak_k = float(np.clip(peak_k, -0.25, 0.25))
|
||||
|
||||
if len(yaws) == len(T_IDXS):
|
||||
s_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, s))
|
||||
yaw_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, yaws))
|
||||
path_k_1s = float(np.clip(yaw_1s / s_1s, -0.25, 0.25)) if s_1s >= PLAN_MIN_SEG_DS else 0.0
|
||||
else:
|
||||
path_k_1s = 0.0
|
||||
|
||||
return peak_k, path_k_1s
|
||||
|
||||
|
||||
def compute_plan_peak_curv(model_output: dict[str, np.ndarray]) -> float:
|
||||
return compute_plan_metrics(model_output)[0]
|
||||
|
||||
|
||||
def compute_curb_guard(model_output: dict[str, np.ndarray], turn_sign: float) -> tuple[float, float]:
|
||||
"""
|
||||
Check if a confident, unbroken road edge (curb) still runs straight beside the car
|
||||
on the turning side (turn_sign > 0 = right edge, turn_sign < 0 = left edge).
|
||||
Returns (curb_weight in [0, 1], max_safe_curv_abs).
|
||||
"""
|
||||
edges = model_output.get('road_edges')
|
||||
stds = model_output.get('road_edges_stds')
|
||||
if edges is None or stds is None or edges.ndim < 4 or edges.shape[1] < 2:
|
||||
return 0.0, 1.0
|
||||
|
||||
idx = 1 if turn_sign > 0.0 else 0
|
||||
edge_y = edges[0, idx, :, 0]
|
||||
if len(edge_y) != len(X_IDXS):
|
||||
return 0.0, 1.0
|
||||
edge_std = float(stds[0, idx, 0, 0])
|
||||
|
||||
std_w = float(np.interp(edge_std, CURB_STD_GATE, [1.0, 0.0]))
|
||||
if std_w <= 0.0:
|
||||
return 0.0, 1.0
|
||||
|
||||
e0 = float(edge_y[0]) * turn_sign
|
||||
e5 = float(np.interp(CURB_CHECK_X, X_IDXS, edge_y)) * turn_sign
|
||||
if e0 <= 0.0:
|
||||
return 0.0, 1.0
|
||||
|
||||
close_w = float(np.interp(e0, CURB_CLOSE_GATE, [1.0, 0.0]))
|
||||
flare = e5 - e0
|
||||
unopened_w = float(np.interp(flare, CURB_OPENING_GATE, [1.0, 0.0]))
|
||||
curb_w = std_w * close_w * unopened_w
|
||||
if curb_w <= 0.0:
|
||||
return 0.0, 1.0
|
||||
|
||||
# Allow following the curb's own curvature (so normal curved roads are untouched) plus small margin.
|
||||
k_curb = max(0.0, 2.0 * flare / (CURB_CHECK_X * CURB_CHECK_X))
|
||||
return curb_w, k_curb + CURB_FOLLOW_MARGIN_K
|
||||
|
||||
|
||||
class TurnPlanner:
|
||||
"""
|
||||
Two-stage rear-axle protected turn planner for tight low-speed turns.
|
||||
|
||||
Stage 1 (Entry): delays curvature buildup by a wheelbase-proportional distance
|
||||
(time-capped at MAX_SHIFT_TIME) so the rear axle clears the inside curb.
|
||||
Stage 2 (Apex & Exit): blends action curvature with the model's 1-second path
|
||||
curvature once past entry, preventing wide apexes and exit hang-ups.
|
||||
"""
|
||||
|
||||
def __init__(self, wheelbase: float = 2.7, dt: float = DT_MDL):
|
||||
self.wheelbase = float(np.clip(wheelbase, *WHEELBASE_CLIP))
|
||||
self.dt = dt
|
||||
self.s = 0.0
|
||||
self.buf_s: deque[float] = deque()
|
||||
self.buf_k: deque[float] = deque()
|
||||
self.turn_sign = 0.0
|
||||
self.turn_psi = 0.0
|
||||
self.shift_dist = 0.0
|
||||
self.apex_blend = 0.0
|
||||
self.plan_peak_curv = 0.0
|
||||
self.path_curv_1s = 0.0
|
||||
|
||||
def reset(self) -> None:
|
||||
self.s = 0.0
|
||||
self.buf_s.clear()
|
||||
self.buf_k.clear()
|
||||
self.turn_sign = 0.0
|
||||
self.turn_psi = 0.0
|
||||
self.shift_dist = 0.0
|
||||
self.apex_blend = 0.0
|
||||
self.plan_peak_curv = 0.0
|
||||
self.path_curv_1s = 0.0
|
||||
|
||||
def update(self, model_output: dict[str, np.ndarray], v_ego: float, actual_curv: float,
|
||||
active: bool, is_rhd: bool, model_curv: float, enabled: bool = True) -> float:
|
||||
if not enabled or not active:
|
||||
self.reset()
|
||||
return model_curv
|
||||
|
||||
if v_ego < MIN_MOVE_SPEED:
|
||||
# Stopped straight (e.g., at a stop bar): reset turn phase so launch gets full protection.
|
||||
# Stopped mid-turn (e.g., yielding to a pedestrian): preserve turn_psi so resume doesn't re-delay.
|
||||
if abs(actual_curv) < MID_TURN_CURV_THRESH:
|
||||
self.turn_psi = 0.0
|
||||
self.turn_sign = 0.0
|
||||
self.shift_dist = 0.0
|
||||
self.apex_blend = 0.0
|
||||
self.buf_s.clear()
|
||||
self.buf_k.clear()
|
||||
self.buf_s.append(self.s)
|
||||
self.buf_k.append(float(actual_curv))
|
||||
return model_curv
|
||||
|
||||
ds = v_ego * self.dt
|
||||
self.s += ds
|
||||
self.buf_s.append(self.s)
|
||||
self.buf_k.append(float(model_curv))
|
||||
while len(self.buf_s) > 2 and (self.s - self.buf_s[0]) > BUFFER_MAX_DIST:
|
||||
self.buf_s.popleft()
|
||||
self.buf_k.popleft()
|
||||
|
||||
turn_speed_w = float(np.interp(v_ego, TURN_SPEED_TAPER, [1.0, 0.0]))
|
||||
if turn_speed_w <= 0.0 and self.shift_dist < 1e-3 and self.apex_blend < 1e-3:
|
||||
self.turn_psi = 0.0
|
||||
self.turn_sign = 0.0
|
||||
self.plan_peak_curv = 0.0
|
||||
self.path_curv_1s = 0.0
|
||||
return model_curv
|
||||
|
||||
plan_peak, path_k_1s = compute_plan_metrics(model_output)
|
||||
self.plan_peak_curv = plan_peak
|
||||
self.path_curv_1s = path_k_1s
|
||||
|
||||
tightness = max(abs(model_curv), abs(plan_peak))
|
||||
curv_w = float(np.interp(tightness, CURV_GATE, [0.0, 1.0]))
|
||||
|
||||
ref_k = model_curv if abs(model_curv) > 0.015 else plan_peak
|
||||
if abs(ref_k) > 0.035 and turn_speed_w > 0.05:
|
||||
new_sign = 1.0 if ref_k > 0.0 else -1.0
|
||||
if new_sign != self.turn_sign:
|
||||
self.turn_sign = new_sign
|
||||
self.turn_psi = 0.0
|
||||
self.turn_psi = max(0.0, self.turn_psi + actual_curv * self.turn_sign * ds)
|
||||
else:
|
||||
self.turn_psi = max(0.0, self.turn_psi - 0.5 * self.dt)
|
||||
if self.turn_psi < 0.02:
|
||||
self.turn_sign = 0.0
|
||||
|
||||
psi_deg = math.degrees(self.turn_psi)
|
||||
|
||||
# Stage 1: Entry rear-axle spatial shift (time-capped so crawl-speed starts stay responsive)
|
||||
shift_speed_w = float(np.interp(v_ego, SHIFT_SPEED_BP, SHIFT_SPEED_V))
|
||||
entry_phase_w = float(np.interp(psi_deg, ENTRY_PHASE_RELEASE_DEG, [1.0, 0.0]))
|
||||
is_curb_side = (ref_k < 0.0) if is_rhd else (ref_k > 0.0)
|
||||
side_w = 1.0 if is_curb_side else OPPOSITE_SIDE_WEIGHT
|
||||
|
||||
raw_shift = SHIFT_WHEELBASE_RATIO * self.wheelbase * shift_speed_w * curv_w * entry_phase_w * side_w
|
||||
target_shift = min(raw_shift, v_ego * MAX_SHIFT_TIME)
|
||||
tau = SHIFT_RELEASE_TAU if target_shift < self.shift_dist else SHIFT_BUILD_TAU
|
||||
self.shift_dist += (target_shift - self.shift_dist) * (self.dt / (tau + self.dt))
|
||||
|
||||
out_k = model_curv
|
||||
if self.shift_dist >= 0.05 and len(self.buf_s) >= 2:
|
||||
k_delayed = float(np.interp(self.s - self.shift_dist, self.buf_s, self.buf_k))
|
||||
# Same sign as model_curv (or 0), never exceeds |model_curv|, zero lag on unwind.
|
||||
if model_curv >= 0.0:
|
||||
out_k = float(np.clip(k_delayed, 0.0, model_curv))
|
||||
else:
|
||||
out_k = float(np.clip(k_delayed, model_curv, 0.0))
|
||||
|
||||
# Stage 1b: Road-edge curb guard during turn entry (prolongs straight travel while
|
||||
# a confident, unbroken curb is still beside the car before the turn lane/intersection opens).
|
||||
curb_phase_w = float(np.interp(psi_deg, CURB_GUARD_PHASE_DEG, [1.0, 0.0]))
|
||||
if turn_speed_w > 0.05 and curb_phase_w > 0.01 and abs(out_k) > CURB_FOLLOW_MARGIN_K:
|
||||
curb_w, k_cap = compute_curb_guard(model_output, 1.0 if out_k > 0.0 else -1.0)
|
||||
guard_w = curb_w * turn_speed_w * curb_phase_w
|
||||
if guard_w > 0.01 and abs(out_k) > k_cap:
|
||||
capped = math.copysign(k_cap, out_k)
|
||||
out_k = (1.0 - guard_w) * out_k + guard_w * capped
|
||||
|
||||
# Stage 2: Apex & Exit 1s-path blend (prevents wide apexes and exit hang-ups)
|
||||
apex_phase_w = float(np.interp(psi_deg, APEX_BLEND_PHASE_DEG, [0.0, 1.0]))
|
||||
target_blend = APEX_PATH_BLEND_MAX * turn_speed_w * curv_w * apex_phase_w
|
||||
self.apex_blend += (target_blend - self.apex_blend) * (self.dt / (APEX_BLEND_TAU + self.dt))
|
||||
|
||||
if self.apex_blend > 0.01 and out_k * path_k_1s > 0.0:
|
||||
# Through the apex, boost when 1s path asks for tighter curvature than action (prevents wide turns).
|
||||
# Only pull curvature down toward 1s path late in the turn (psi > 60 deg) to prevent exit hang-ups.
|
||||
if abs(path_k_1s) > abs(out_k) or psi_deg > 60.0:
|
||||
max_boosted = abs(model_curv) * APEX_PATH_BOOST_CAP
|
||||
target_k = float(np.clip(path_k_1s, -max_boosted, max_boosted))
|
||||
out_k = (1.0 - self.apex_blend) * out_k + self.apex_blend * target_k
|
||||
|
||||
return out_k
|
||||
@@ -6,6 +6,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import threading
|
||||
@@ -28,8 +29,11 @@ DOWNLOAD_TIMEOUT = (30, 30)
|
||||
# HuggingFace throttles each connection to ~1-2 MB/s but not the connection count; 12 saturates the device link
|
||||
MAX_CONCURRENT_PIECES = 12
|
||||
PIECE_SIZE = 8 * 1024 * 1024
|
||||
PIECE_ATTEMPTS = 3
|
||||
PIECE_ATTEMPTS = 3 # per piece, before the transfer as a whole is retried
|
||||
PIECE_BACKOFF = 1.0 # seconds, growing linearly: waits 1 s then 2 s between a piece's three attempts
|
||||
DOWNLOAD_ATTEMPTS = 4 # whole-transfer attempts, each resuming from the sidecar
|
||||
RETRY_BACKOFF = 2.0 # seconds, doubling: waits 2, 4 then 8 s between the four transfer attempts
|
||||
RETRY_POLL = 0.25 # how often a backoff checks for a cancel
|
||||
TRANSIENT_HTTP_STATUS = frozenset({408, 429, 500, 502, 503, 504})
|
||||
REPORT_INTERVAL = 0.5 # seconds between progress publications
|
||||
# byte ranges count encoded bytes but iter_content yields decoded ones, so ask for an unencoded body
|
||||
@@ -49,26 +53,69 @@ def _is_transient(e: BaseException) -> bool:
|
||||
|
||||
|
||||
class _Progress:
|
||||
def __init__(self):
|
||||
self._bytes = 0
|
||||
def __init__(self, done: set[int], done_bytes: int):
|
||||
self._bytes = done_bytes
|
||||
self._done = set(done)
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def add_bytes(self, n: int) -> None:
|
||||
with self._lock:
|
||||
self._bytes += n
|
||||
|
||||
def snapshot(self) -> int:
|
||||
def mark_done(self, index: int) -> None:
|
||||
with self._lock:
|
||||
return self._bytes
|
||||
self._done.add(index)
|
||||
|
||||
def snapshot(self) -> tuple[int, set[int]]:
|
||||
with self._lock:
|
||||
return self._bytes, set(self._done)
|
||||
|
||||
|
||||
def _piece_ranges(total: int, piece_size: int) -> list[tuple[int, int]]:
|
||||
return [(start, min(start + piece_size, total)) for start in range(0, total, piece_size)]
|
||||
|
||||
|
||||
def _prepare_target(path: str, total: int) -> None:
|
||||
def _sidecar_path(path: str) -> str:
|
||||
return f"{path}.download"
|
||||
|
||||
|
||||
def _download_in_progress(path: str) -> bool:
|
||||
return os.path.isfile(_sidecar_path(path))
|
||||
|
||||
|
||||
def _save_resume_state(path: str, layout: dict, done: set[int]) -> None:
|
||||
tmp = _sidecar_path(path) + ".tmp"
|
||||
with open(tmp, "w") as f:
|
||||
json.dump({**layout, "done": sorted(done)}, f)
|
||||
f.flush()
|
||||
os.fsync(f.fileno())
|
||||
os.replace(tmp, _sidecar_path(path)) # a power loss leaves the old sidecar or the new one, never a torn file
|
||||
dir_fd = os.open(os.path.dirname(path) or ".", os.O_RDONLY)
|
||||
try:
|
||||
os.fsync(dir_fd)
|
||||
finally:
|
||||
os.close(dir_fd)
|
||||
|
||||
|
||||
def _remove_download_state(path: str) -> None:
|
||||
for stale in (_sidecar_path(path), _sidecar_path(path) + ".tmp"):
|
||||
if os.path.isfile(stale):
|
||||
os.remove(stale)
|
||||
|
||||
|
||||
def _prepare_target(path: str, layout: dict) -> set[int]:
|
||||
total = layout["total"]
|
||||
try:
|
||||
with open(_sidecar_path(path)) as f:
|
||||
state = json.load(f)
|
||||
if isinstance(state, dict) and all(state.get(k) == v for k, v in layout.items()) and os.path.getsize(path) == total:
|
||||
return {i for i in state.get("done", []) if isinstance(i, int)}
|
||||
except (OSError, ValueError, TypeError): # missing, unreadable or malformed sidecar: start over
|
||||
pass
|
||||
_save_resume_state(path, layout, set())
|
||||
with open(path, "wb") as f:
|
||||
f.truncate(total)
|
||||
return set()
|
||||
|
||||
|
||||
def _content_range_total(response: requests.Response) -> int | None:
|
||||
@@ -110,6 +157,9 @@ def _fetch_piece(url: str, path: str, index: int, start: int, end: int | None, p
|
||||
f.truncate() # unknown length: the body defines the file size
|
||||
if end is not None and written != end - start:
|
||||
raise requests.exceptions.ConnectionError(f"short read: {written} of {end - start} bytes")
|
||||
f.flush()
|
||||
os.fsync(f.fileno()) # before it is recorded, so a power loss never leaves the sidecar naming unsynced bytes
|
||||
progress.mark_done(index)
|
||||
return
|
||||
except requests.RequestException as e:
|
||||
progress.add_bytes(-written) # the piece restarts from scratch
|
||||
@@ -120,6 +170,11 @@ def _fetch_piece(url: str, path: str, index: int, start: int, end: int | None, p
|
||||
raise DownloadCancelled("Download cancelled") from None
|
||||
|
||||
|
||||
def _remove_file(path: str) -> None:
|
||||
if os.path.isfile(path):
|
||||
os.remove(path)
|
||||
|
||||
|
||||
class ModelManagerSP:
|
||||
"""Manages model downloads and status reporting"""
|
||||
|
||||
@@ -186,11 +241,12 @@ class ModelManagerSP:
|
||||
else:
|
||||
eta = self._calculate_eta(artifact.fileName, progress)
|
||||
self._set_progress(artifact, custom.ModelManagerSP.DownloadStatus.downloading, progress, eta, speed)
|
||||
artifact.downloadProgress.resuming = False # shown only for the first report, when the bar jumps to the resumed point
|
||||
|
||||
async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, total: int,
|
||||
started: tuple[int, float]) -> None:
|
||||
async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, path: str, layout: dict,
|
||||
started: tuple[int, set[int], float]) -> None:
|
||||
pending = set(tasks)
|
||||
last_bytes, last_time = started
|
||||
last_bytes, saved, last_time = started
|
||||
speed = 0.0
|
||||
while pending:
|
||||
done, pending = await asyncio.wait(pending, timeout=REPORT_INTERVAL)
|
||||
@@ -199,14 +255,17 @@ class ModelManagerSP:
|
||||
if self._download_interrupted():
|
||||
raise DownloadCancelled("Download cancelled")
|
||||
now = time.monotonic()
|
||||
done_bytes = progress.snapshot()
|
||||
done_bytes, done_pieces = progress.snapshot()
|
||||
if done_pieces != saved:
|
||||
_save_resume_state(path, layout, done_pieces)
|
||||
saved = done_pieces
|
||||
if (dt := now - last_time) > 0:
|
||||
instant = max(0.0, (done_bytes - last_bytes) / dt) # bytes/s this tick; clamped since retried pieces can lower the byte count
|
||||
speed = instant if speed == 0 else SPEED_SMOOTHING * speed + (1 - SPEED_SMOOTHING) * instant
|
||||
if speed < SPEED_FLOOR: # a stall decays toward 0 without reaching it, and a near-0 speed overflows the eta
|
||||
speed = 0.0
|
||||
last_time, last_bytes = now, done_bytes
|
||||
self._publish_progress(artifact, done_bytes, total, speed)
|
||||
self._publish_progress(artifact, done_bytes, layout["total"], speed)
|
||||
|
||||
async def _download_file(self, url: str, path: str, artifact) -> None:
|
||||
self._download_start_times[artifact.fileName] = time.monotonic()
|
||||
@@ -214,16 +273,18 @@ class ModelManagerSP:
|
||||
|
||||
total, ranged = await loop.run_in_executor(None, _probe_size, url)
|
||||
pieces: list[tuple[int, int | None]] = list(_piece_ranges(total, PIECE_SIZE)) if ranged else [(0, None)]
|
||||
await loop.run_in_executor(None, _prepare_target, path, total)
|
||||
progress = _Progress()
|
||||
layout = {"total": total, "piece_size": PIECE_SIZE, "ranged": ranged, "sha256": artifact.downloadUri.sha256}
|
||||
done = await loop.run_in_executor(None, _prepare_target, path, layout)
|
||||
resumed_bytes = sum(end - start for i, (start, end) in enumerate(pieces) if i in done and end is not None)
|
||||
progress = _Progress(done, resumed_bytes) # the bar and eta start from the resumed pieces
|
||||
cancel = threading.Event()
|
||||
started = progress.snapshot(), time.monotonic() # before any worker can land bytes
|
||||
started = *progress.snapshot(), time.monotonic() # before any worker can land bytes
|
||||
|
||||
with ThreadPoolExecutor(max_workers=MAX_CONCURRENT_PIECES) as pool:
|
||||
tasks = [loop.run_in_executor(pool, _fetch_piece, url, path, i, start, end, progress, cancel, self._block_size)
|
||||
for i, (start, end) in enumerate(pieces)]
|
||||
for i, (start, end) in enumerate(pieces) if i not in done]
|
||||
try:
|
||||
await self._report_until_done(tasks, artifact, progress, total, started)
|
||||
await self._report_until_done(tasks, artifact, progress, path, layout, started)
|
||||
except BaseException as e:
|
||||
cancel.set()
|
||||
if isinstance(e, DownloadCancelled):
|
||||
@@ -232,9 +293,32 @@ class ModelManagerSP:
|
||||
for task in tasks:
|
||||
task.cancel() # drops queued pieces; running ones see the event at their next block
|
||||
await asyncio.gather(*tasks, return_exceptions=True)
|
||||
await loop.run_in_executor(None, pool.shutdown) # let running pieces stop so none lands after the save
|
||||
_save_resume_state(path, layout, progress.snapshot()[1])
|
||||
raise
|
||||
|
||||
_remove_download_state(path)
|
||||
artifact.downloadProgress.resuming = False
|
||||
del self._download_start_times[artifact.fileName]
|
||||
|
||||
async def _download_with_retries(self, url: str, path: str, artifact) -> None:
|
||||
for attempt in range(DOWNLOAD_ATTEMPTS):
|
||||
try:
|
||||
await self._download_file(url, path, artifact)
|
||||
return
|
||||
except Exception as e:
|
||||
if not _is_transient(e) or attempt == DOWNLOAD_ATTEMPTS - 1:
|
||||
raise
|
||||
delay = RETRY_BACKOFF * 2 ** attempt
|
||||
cloudlog.warning(f"Retrying {artifact.fileName} in {delay:g}s after {type(e).__name__}: {e}")
|
||||
progress = artifact.downloadProgress
|
||||
self._set_progress(artifact, progress.status, progress.progress, progress.eta, 0.0) # stalled: no speed
|
||||
deadline = time.monotonic() + delay
|
||||
while (remaining := deadline - time.monotonic()) > 0:
|
||||
if self._download_interrupted():
|
||||
raise DownloadCancelled("Download cancelled") from e
|
||||
await asyncio.sleep(min(RETRY_POLL, remaining))
|
||||
|
||||
async def _process_artifact(self, artifact, destination_path: str) -> None:
|
||||
if not artifact.downloadUri.uri:
|
||||
return None
|
||||
@@ -246,37 +330,39 @@ class ModelManagerSP:
|
||||
filename = artifact.fileName
|
||||
full_path = os.path.join(destination_path, filename)
|
||||
status = custom.ModelManagerSP.DownloadStatus
|
||||
artifact.downloadProgress.resuming = False
|
||||
artifact.downloadProgress.canceling = False
|
||||
|
||||
try:
|
||||
self._set_progress(artifact, status.verifying, 0)
|
||||
if await verify_file(full_path, expected_hash):
|
||||
self._set_progress(artifact, status.cached, 100)
|
||||
return
|
||||
if _download_in_progress(full_path):
|
||||
cloudlog.info(f"Resuming interrupted download of {filename}")
|
||||
artifact.downloadProgress.resuming = True
|
||||
else:
|
||||
self._set_progress(artifact, status.verifying, 0)
|
||||
if await verify_file(full_path, expected_hash):
|
||||
self._set_progress(artifact, status.cached, 100)
|
||||
return
|
||||
|
||||
self._set_progress(artifact, status.downloading, 0)
|
||||
await self._download_file(url, full_path, artifact)
|
||||
await self._download_with_retries(url, full_path, artifact)
|
||||
self._set_progress(artifact, status.verifying, 99)
|
||||
if not await verify_file(full_path, expected_hash):
|
||||
_remove_file(full_path)
|
||||
_remove_download_state(full_path)
|
||||
raise ValueError(f"Hash validation failed for {filename}")
|
||||
|
||||
self._set_progress(artifact, status.downloaded, 100)
|
||||
|
||||
except DownloadCancelled:
|
||||
except DownloadCancelled: # not a failure: the request handler clears the row
|
||||
self._download_start_times.pop(artifact.fileName, None)
|
||||
artifact.downloadProgress.status = status.failed
|
||||
artifact.downloadProgress.eta = 0
|
||||
artifact.downloadProgress.speed = 0
|
||||
self._sync_artifact_progress(artifact)
|
||||
if self.selected_bundle:
|
||||
self.selected_bundle.status = status.failed
|
||||
self._report_status()
|
||||
raise
|
||||
|
||||
except Exception as e:
|
||||
cloudlog.error(f"Error downloading {filename}: {str(e)}")
|
||||
if os.path.isfile(full_path): # noqa: ASYNC240
|
||||
os.remove(full_path)
|
||||
artifact.downloadProgress.status = status.failed
|
||||
artifact.downloadProgress.eta = 0
|
||||
artifact.downloadProgress.speed = 0
|
||||
@@ -333,6 +419,8 @@ class ModelManagerSP:
|
||||
self.params.put(ACTIVE_BUNDLE_KEYS[source], model_bundle.to_dict(), block=True)
|
||||
self.active_bundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
|
||||
|
||||
except DownloadCancelled:
|
||||
raise # not a failure
|
||||
except Exception:
|
||||
if self.selected_bundle is not None:
|
||||
self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.failed
|
||||
@@ -360,11 +448,14 @@ class ModelManagerSP:
|
||||
self._download_ref = ref_to_download
|
||||
try:
|
||||
self.download(model_to_download, Paths.model_root(), source)
|
||||
except DownloadCancelled:
|
||||
self.selected_bundle = None
|
||||
except Exception as e:
|
||||
cloudlog.exception(e)
|
||||
cloudlog.exception(e) # a failure stays on the row until the next request
|
||||
else:
|
||||
self.selected_bundle = None
|
||||
finally:
|
||||
self._release_download_ref()
|
||||
self.selected_bundle = None
|
||||
|
||||
def main_thread(self) -> None:
|
||||
"""Main thread for model management"""
|
||||
|
||||
@@ -11,11 +11,13 @@ import contextlib
|
||||
import dataclasses
|
||||
import hashlib
|
||||
import http.server
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import random
|
||||
import re
|
||||
import requests
|
||||
import stat
|
||||
import tempfile
|
||||
import threading
|
||||
import time
|
||||
@@ -55,6 +57,8 @@ class ServerState:
|
||||
fail_ranges: dict[tuple[int, int], int] = dataclasses.field(default_factory=dict) # range -> HTTP status, every time
|
||||
fail_times: dict[tuple[int, int], int] = dataclasses.field(default_factory=dict) # range -> how many more requests 503
|
||||
short_once: set[tuple[int, int]] = dataclasses.field(default_factory=set) # ranges whose first answer is cut short
|
||||
fail_after_others: bool = False # hold each failure until every other piece has been sent
|
||||
sent: set[tuple[int, int]] = dataclasses.field(default_factory=set) # ranges sent in full
|
||||
stall_ranges: set[tuple[int, int]] = dataclasses.field(default_factory=set)
|
||||
stall_event: threading.Event | None = None
|
||||
stall_released_by_event: bool | None = None
|
||||
@@ -88,6 +92,10 @@ class DownloadHandler(http.server.BaseHTTPRequestHandler):
|
||||
state.fail_times[rng] -= 1
|
||||
status = 503
|
||||
if status is not None:
|
||||
others = {piece_range(i) for i in range(NUM_PIECES)} - {rng}
|
||||
deadline = time.monotonic() + 5
|
||||
while state.fail_after_others and not others <= state.sent and time.monotonic() < deadline:
|
||||
time.sleep(0.01)
|
||||
self.send_response(status)
|
||||
self.end_headers()
|
||||
return
|
||||
@@ -115,6 +123,8 @@ class DownloadHandler(http.server.BaseHTTPRequestHandler):
|
||||
self.wfile.write(payload[100:])
|
||||
else:
|
||||
self.wfile.write(payload)
|
||||
if rng is not None:
|
||||
state.sent.add(rng)
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
@@ -133,7 +143,10 @@ def threaded_server(handler):
|
||||
class InlineExecutor(concurrent.futures.Executor):
|
||||
def submit(self, fn, /, *args, **kwargs):
|
||||
future: concurrent.futures.Future = concurrent.futures.Future()
|
||||
future.set_result(fn(*args, **kwargs)) # the piece lands before the reporter starts
|
||||
try:
|
||||
future.set_result(fn(*args, **kwargs)) # the piece lands before the reporter starts
|
||||
except BaseException as e:
|
||||
future.set_exception(e)
|
||||
return future
|
||||
|
||||
|
||||
@@ -147,11 +160,12 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
self.addCleanup(self._tmp.cleanup)
|
||||
self.dest = self._tmp.name
|
||||
|
||||
for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01), ('SPEED_FLOOR', 100)):
|
||||
for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01), ('SPEED_FLOOR', 100), ('RETRY_BACKOFF', 0.01)):
|
||||
self.enterContext(mock.patch.object(manager_module, name, value))
|
||||
|
||||
self.reported: list[float] = []
|
||||
self.reported_statuses: list[int] = []
|
||||
self.reported_resuming: list[bool] = []
|
||||
|
||||
self.manager = ModelManagerSP.__new__(ModelManagerSP)
|
||||
self.params = self.manager.params = mock.MagicMock()
|
||||
@@ -172,6 +186,7 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
self.reported.append(float(artifact.downloadProgress.progress))
|
||||
status = artifact.downloadProgress.status
|
||||
self.reported_statuses.append(getattr(status, 'raw', status)) # .raw: _DynamicEnum equals its int but does not hash like it
|
||||
self.reported_resuming.append(bool(artifact.downloadProgress.resuming))
|
||||
|
||||
def make_artifact(self):
|
||||
bundle = custom.ModelManagerSP.ModelBundle.new_message()
|
||||
@@ -188,6 +203,24 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
def path(self) -> str:
|
||||
return os.path.join(self.dest, FILE_NAME)
|
||||
|
||||
@property
|
||||
def sidecar(self) -> str:
|
||||
return manager_module._sidecar_path(self.path)
|
||||
|
||||
def resume_state(self) -> dict:
|
||||
with open(self.sidecar) as f:
|
||||
return json.load(f)
|
||||
|
||||
def write_resume_state(self, done: list[int], total: int = len(WHOLE_BODY), file_size: int | None = None,
|
||||
file_sha256: str = sha256(WHOLE_BODY)) -> None:
|
||||
with open(self.path, 'wb') as f:
|
||||
f.truncate(len(WHOLE_BODY) if file_size is None else file_size)
|
||||
for i in done:
|
||||
f.seek(i * PIECE_SIZE)
|
||||
f.write(WHOLE_BODY[i * PIECE_SIZE:(i + 1) * PIECE_SIZE])
|
||||
with open(self.sidecar, 'w') as f:
|
||||
json.dump({"total": total, "piece_size": PIECE_SIZE, "ranged": True, "sha256": file_sha256, "done": done}, f)
|
||||
|
||||
def piece_requests(self) -> list[tuple[int, int]]:
|
||||
return [r for r in self.server.request_ranges if r is not None and r != (0, 0)]
|
||||
|
||||
@@ -209,6 +242,17 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
def download_bundle(self, source: str):
|
||||
asyncio.run(self.manager._download_bundle(self._bundle, self.dest, source))
|
||||
|
||||
def request_download(self) -> dict:
|
||||
self.make_artifact()
|
||||
self._bundle.ref = "ref"
|
||||
self._bundle.minimumSelectorVersion = helpers.REQUIRED_JSON_VERSION
|
||||
self.manager.source_models = {"qcom": [self._bundle], "chestnut": []}
|
||||
self.manager.params, store = self._make_params_with_store()
|
||||
store["ModelManager_DownloadRef"] = "ref"
|
||||
with mock.patch.object(manager_module.Paths, 'model_root', return_value=self.dest):
|
||||
self.manager._process_download_requests()
|
||||
return store
|
||||
|
||||
def read_path(self) -> bytes:
|
||||
with open(self.path, 'rb') as f:
|
||||
return f.read()
|
||||
@@ -225,6 +269,7 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
|
||||
params.get.side_effect = get
|
||||
params.put.side_effect = put
|
||||
params.remove.side_effect = lambda key: put(key, None)
|
||||
return params, store
|
||||
|
||||
|
||||
@@ -238,6 +283,7 @@ class TestManagerDownload(ManagerDownloadTestBase):
|
||||
assert max(self.reported) <= 99.0, "progress holds below 100 until the hash check"
|
||||
assert artifact.fileName not in self.manager._download_start_times
|
||||
assert artifact.downloadProgress.speed > 0, "a running download shows its speed"
|
||||
assert not os.path.exists(self.sidecar), "the resume state goes once every piece has landed"
|
||||
|
||||
def test_speed_counts_bytes_landed_before_the_first_report(self):
|
||||
self.enterContext(mock.patch.object(manager_module, 'ThreadPoolExecutor', lambda max_workers: InlineExecutor()))
|
||||
@@ -296,9 +342,42 @@ class TestManagerDownload(ManagerDownloadTestBase):
|
||||
|
||||
def test_permanent_failure_is_not_retried(self):
|
||||
self.server.fail_ranges = {piece_range(1): 404}
|
||||
self.enterContext(mock.patch.object(manager_module, 'ThreadPoolExecutor', lambda max_workers: InlineExecutor()))
|
||||
with self.assertRaises(requests.exceptions.HTTPError):
|
||||
self.download_file()
|
||||
self.process_artifact()
|
||||
assert self.piece_requests().count(piece_range(1)) == 1
|
||||
assert os.path.isfile(self.path)
|
||||
assert self.resume_state()["done"] == [i for i in range(NUM_PIECES) if i != 1]
|
||||
|
||||
def test_failed_transfer_resumes_on_retry(self):
|
||||
self.server.fail_times = {piece_range(1): manager_module.PIECE_ATTEMPTS} # the first transfer gives up on piece 1
|
||||
self.server.fail_after_others = True # by then every other piece is saved
|
||||
self.process_artifact()
|
||||
assert self.read_path() == WHOLE_BODY
|
||||
assert self.piece_requests().count(piece_range(1)) == manager_module.PIECE_ATTEMPTS + 1, "the retry fetched piece 1"
|
||||
others = [i for i in range(NUM_PIECES) if i != 1]
|
||||
assert all(self.piece_requests().count(piece_range(i)) == 1 for i in others), "the retry skipped every saved piece"
|
||||
|
||||
def test_lasting_transient_failure_gives_up(self):
|
||||
self.server.fail_ranges = {piece_range(1): 503}
|
||||
with self.assertRaises(requests.exceptions.HTTPError):
|
||||
self.process_artifact()
|
||||
assert self.piece_requests().count(piece_range(1)) == manager_module.PIECE_ATTEMPTS * manager_module.DOWNLOAD_ATTEMPTS
|
||||
|
||||
def test_cancel_ends_retry_backoff(self):
|
||||
self.enterContext(mock.patch.object(manager_module, 'RETRY_BACKOFF', 30.0))
|
||||
self.manager._download_ref = b"ref"
|
||||
|
||||
def fail_then_cancel(*args):
|
||||
self.params.get.return_value = None # the user cancels while the manager waits to retry
|
||||
raise requests.exceptions.ConnectionError("dropped")
|
||||
transfer = self.enterContext(mock.patch.object(self.manager, '_download_file', side_effect=fail_then_cancel))
|
||||
started = time.monotonic()
|
||||
with self.assertRaises(DownloadCancelled):
|
||||
self.process_artifact()
|
||||
assert time.monotonic() - started < 5, "the wait must end at the cancel, not after 30 s"
|
||||
assert transfer.call_count == 1, "no second transfer after a cancel"
|
||||
assert self.artifact.downloadProgress.status != custom.ModelManagerSP.DownloadStatus.failed
|
||||
|
||||
def test_removing_download_ref_cancels(self):
|
||||
stall_event = self.stall(3)
|
||||
@@ -316,6 +395,51 @@ class TestManagerDownload(ManagerDownloadTestBase):
|
||||
assert self.server.stall_released_by_event is True, "the cancel must have caught the piece mid-transfer"
|
||||
end = piece_range(3)[1] + 1
|
||||
assert self.read_path()[end - 100:end] == bytes(100), "the cancelled piece must stop before its last bytes land"
|
||||
done = self.resume_state()["done"]
|
||||
assert done and 3 not in done, "a cancel records the finished pieces, and only those, for resume"
|
||||
|
||||
def test_resume_skips_complete_pieces(self):
|
||||
self.write_resume_state(done=[0, 4])
|
||||
self.download_file()
|
||||
assert piece_range(0) not in self.piece_requests() and piece_range(4) not in self.piece_requests()
|
||||
assert self.read_path() == WHOLE_BODY
|
||||
assert min(self.reported) >= 2 * PIECE_SIZE / len(WHOLE_BODY) * 100 - 1, "progress must not restart below the resumed share"
|
||||
|
||||
def test_unusable_resume_state_starts_over(self):
|
||||
def malformed(content: bytes):
|
||||
self.write_resume_state(done=[0])
|
||||
with open(self.sidecar, 'wb') as f:
|
||||
f.write(content)
|
||||
|
||||
layout = {"total": len(WHOLE_BODY), "piece_size": PIECE_SIZE, "ranged": True, "sha256": sha256(WHOLE_BODY)}
|
||||
cases = {
|
||||
"other size": lambda: self.write_resume_state(done=[0], total=len(WHOLE_BODY) + 1),
|
||||
"other bytes": lambda: self.write_resume_state(done=[0], file_sha256="0" * 64),
|
||||
"truncated file": lambda: self.write_resume_state(done=[0], file_size=PIECE_SIZE),
|
||||
"not an object": lambda: malformed(b'[]'),
|
||||
"cut-off json": lambda: malformed(b'{"total": '),
|
||||
"done not a list": lambda: malformed(json.dumps({**layout, "done": 3}).encode()),
|
||||
"done not ints": lambda: malformed(json.dumps({**layout, "done": ["0", None]}).encode()),
|
||||
}
|
||||
for name, write_state in cases.items():
|
||||
with self.subTest(name):
|
||||
self.server.request_ranges = []
|
||||
write_state()
|
||||
self.download_file()
|
||||
assert sorted(self.piece_requests()) == [piece_range(i) for i in range(NUM_PIECES)], "every piece is fetched again"
|
||||
assert self.read_path() == WHOLE_BODY
|
||||
|
||||
def test_pieces_and_resume_state_are_synced_to_disk(self):
|
||||
real_fsync, synced = os.fsync, []
|
||||
|
||||
def record_sync(fd):
|
||||
st = os.fstat(fd)
|
||||
synced.append("dir" if stat.S_ISDIR(st.st_mode) else "model" if os.path.exists(self.path) and st.st_ino == os.stat(self.path).st_ino else "sidecar")
|
||||
real_fsync(fd)
|
||||
with mock.patch.object(manager_module.os, 'fsync', side_effect=record_sync):
|
||||
self.download_file()
|
||||
assert synced.count("model") == NUM_PIECES, f"expected one fsync per piece, got {synced.count('model')}"
|
||||
assert "sidecar" in synced and "dir" in synced, "sidecar file and directory must be fsync'd"
|
||||
|
||||
def test_replaced_download_ref_queues_instead_of_canceling(self):
|
||||
self.params.get.return_value = b"other-ref"
|
||||
@@ -345,9 +469,26 @@ class TestProcessArtifact(ManagerDownloadTestBase):
|
||||
self.server.corrupt = True
|
||||
with self.assertRaises(ValueError):
|
||||
self.process_artifact()
|
||||
assert not os.path.isfile(self.path)
|
||||
assert not os.path.isfile(self.path) and not os.path.exists(self.sidecar), "a corrupt file must not be resumed from"
|
||||
assert self.artifact.downloadProgress.status == custom.ModelManagerSP.DownloadStatus.failed
|
||||
|
||||
def test_interrupted_download_resumes_without_reverifying(self):
|
||||
self.write_resume_state(done=[0, 1, 2])
|
||||
artifact = self.process_artifact()
|
||||
ds = custom.ModelManagerSP.DownloadStatus
|
||||
assert self.reported_statuses[0] == ds.downloading, "an in-progress download is not verified first"
|
||||
assert piece_range(0) not in self.piece_requests()
|
||||
assert artifact.downloadProgress.status == ds.downloaded
|
||||
assert any(self.reported_resuming), "a resumed download is flagged resuming until its first progress report"
|
||||
assert not artifact.downloadProgress.resuming
|
||||
|
||||
def test_resume_with_every_piece_done_clears_resuming(self):
|
||||
self.write_resume_state(done=list(range(NUM_PIECES)))
|
||||
artifact = self.process_artifact()
|
||||
assert self.piece_requests() == []
|
||||
assert artifact.downloadProgress.status == custom.ModelManagerSP.DownloadStatus.downloaded
|
||||
assert not artifact.downloadProgress.resuming
|
||||
|
||||
def test_probe_failure_moves_past_verifying(self):
|
||||
self.server.fail_ranges = {(0, 0): 404} # the size probe
|
||||
with self.assertRaises(requests.exceptions.HTTPError):
|
||||
@@ -374,6 +515,23 @@ class TestProcessArtifact(ManagerDownloadTestBase):
|
||||
self.download_bundle("qcom")
|
||||
assert ACTIVE_BUNDLE_KEYS["qcom"] not in store
|
||||
assert os.path.isfile(self.path), "cancel must not delete a cached model"
|
||||
assert self.manager.selected_bundle.status != custom.ModelManagerSP.DownloadStatus.failed, "a cancel is not a failure"
|
||||
|
||||
def test_failed_download_stays_on_the_row(self):
|
||||
self.server.fail_ranges = {piece_range(1): 404}
|
||||
store = self.request_download()
|
||||
selected = self.manager.selected_bundle
|
||||
assert selected is not None and selected.status == custom.ModelManagerSP.DownloadStatus.failed, "a failure stays on the row"
|
||||
assert store["ModelManager_DownloadRef"] is None, "the failed request is released"
|
||||
|
||||
def test_finished_download_clears_the_row(self):
|
||||
self.request_download()
|
||||
assert self.manager.selected_bundle is None
|
||||
|
||||
def test_cancelled_download_clears_the_row(self):
|
||||
self.pm.send.side_effect = lambda *args: self.manager.params.remove("ModelManager_DownloadRef") # cancel at the first report
|
||||
self.request_download()
|
||||
assert self.manager.selected_bundle is None
|
||||
|
||||
def test_download_ref_is_released_only_if_unchanged(self):
|
||||
for current, removed in ((b"new-ref", False), (b"ref", True)):
|
||||
|
||||
@@ -19,10 +19,9 @@ from openpilot.tools.lib.route import Route
|
||||
REPO_ID = os.environ.get("SUNNYPILOT_CAR_SEGMENTS_REPO_ID", "sunnypilot/sunnypilotCarSegments")
|
||||
|
||||
|
||||
def upload_route(route_name: str, dry_run: bool = False) -> None:
|
||||
route = Route(route_name)
|
||||
log_paths = route.log_paths()
|
||||
valid_segments = [(i, url) for i, url in enumerate(log_paths) if url is not None]
|
||||
def upload_route(route_name: str, dry_run: bool = False, local_dir: str | None = None) -> None:
|
||||
log_paths = Route(route_name, data_dir=local_dir).log_paths()
|
||||
valid_segments = [(i, path) for i, path in enumerate(log_paths) if path is not None]
|
||||
|
||||
print(f"Route: {route_name}")
|
||||
print(f"Segments: {len(valid_segments)}/{len(log_paths)}")
|
||||
@@ -34,14 +33,17 @@ def upload_route(route_name: str, dry_run: bool = False) -> None:
|
||||
api = HfApi()
|
||||
|
||||
with tempfile.TemporaryDirectory() as tmpdir:
|
||||
for seg_idx, url in tqdm(valid_segments, desc="Uploading"):
|
||||
filename = url.split("?")[0].rsplit("/", 1)[-1]
|
||||
local_path = os.path.join(tmpdir, f"{seg_idx}_{filename}")
|
||||
resp = requests.get(url, stream=True)
|
||||
resp.raise_for_status()
|
||||
with open(local_path, "wb") as f:
|
||||
for chunk in resp.iter_content(chunk_size=8192):
|
||||
f.write(chunk)
|
||||
for seg_idx, src in tqdm(valid_segments, desc="Uploading"):
|
||||
filename = src.split("?")[0].rsplit("/", 1)[-1]
|
||||
if local_dir is not None:
|
||||
local_path = src
|
||||
else:
|
||||
local_path = os.path.join(tmpdir, f"{seg_idx}_{filename}")
|
||||
resp = requests.get(src, stream=True)
|
||||
resp.raise_for_status()
|
||||
with open(local_path, "wb") as f:
|
||||
for chunk in resp.iter_content(chunk_size=8192):
|
||||
f.write(chunk)
|
||||
|
||||
repo_path = f"segments/{route_name.replace('|', '/')}/{seg_idx}/{filename}"
|
||||
|
||||
@@ -63,6 +65,7 @@ if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Upload route rlogs to sunnypilot HuggingFace dataset")
|
||||
parser.add_argument("route", help="Route ID (e.g. 5beb9b58bd12b691/0000010a--a51155e496)")
|
||||
parser.add_argument("--dry-run", action="store_true", help="Download and show sizes without uploading")
|
||||
parser.add_argument("--local", metavar="DIR", help="Upload rlogs from DIR (<dongle id>|<log id>--<segment>/) instead of the comma API")
|
||||
args = parser.parse_args()
|
||||
|
||||
upload_route(args.route, dry_run=args.dry_run)
|
||||
upload_route(args.route, dry_run=args.dry_run, local_dir=args.local)
|
||||
|
||||
Reference in New Issue
Block a user