diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index cdafbbfede..beb426c669 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -91,7 +91,6 @@ jobs: build_mac: name: build macOS - if: false # temp disable since homebrew install is getting stuck runs-on: ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-macos-8x14' || 'macos-latest' }} steps: - uses: actions/checkout@v4 @@ -109,8 +108,8 @@ jobs: - name: Install dependencies run: ./tools/mac_setup.sh env: - # package install has DeprecationWarnings - PYTHONWARNINGS: default + PYTHONWARNINGS: default # package install has DeprecationWarnings + HOMEBREW_DISPLAY_INSTALL_TIMES: 1 - run: git lfs pull - name: Getting scons cache uses: ./.github/workflows/auto-cache diff --git a/RELEASES.md b/RELEASES.md index d89ee24628..f5cf630607 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,11 +1,14 @@ Version 0.10.1 (2025-09-08) ======================== -* New driving model +* New driving model #36087 * World Model: removed global localization inputs * World Model: 2x the number of parameters * World Model: trained on 4x the number of segments -* Record driving feedback using LKAS button -* Honda City 2023 support thanks to drFritz! + * Driving Vision Model: trained on 4x the number of segments +* Honda City 2023 support thanks to vanillagorillaa and drFritz! +* Honda N-Box 2018 support thanks to miettal! +* Honda Odyssey 2021-25 support thanks to csouers and MVL! +* Honda Passport 2026 support thanks to vanillagorillaa and MVL! Version 0.10.0 (2025-08-05) ======================== diff --git a/SConstruct b/SConstruct index 5b13bd635a..d8a390f5df 100644 --- a/SConstruct +++ b/SConstruct @@ -116,6 +116,7 @@ else: f"#third_party/acados/{arch}/lib", f"{brew_prefix}/lib", f"{brew_prefix}/opt/openssl@3.0/lib", + f"{brew_prefix}/opt/llvm/lib/c++", "/System/Library/Frameworks/OpenGL.framework/Libraries", ] diff --git a/common/api.py b/common/api.py index ac231400a4..005655b21d 100644 --- a/common/api.py +++ b/common/api.py @@ -22,7 +22,7 @@ class Api: def request(self, method, endpoint, timeout=None, access_token=None, **params): return api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params) - def get_token(self, expiry_hours=1): + def get_token(self, payload_extra=None, expiry_hours=1): now = datetime.now(UTC).replace(tzinfo=None) payload = { 'identity': self.dongle_id, @@ -30,6 +30,8 @@ class Api: 'iat': now, 'exp': now + timedelta(hours=expiry_hours) } + if payload_extra is not None: + payload.update(payload_extra) token = jwt.encode(payload, self.private_key, algorithm='RS256') if isinstance(token, bytes): token = token.decode('utf8') diff --git a/docs/CARS.md b/docs/CARS.md index 7269f25737..22cb0d46c8 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -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. -# 321 Supported Cars +# 324 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Hardware Needed
 |Video|Setup Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -83,7 +83,7 @@ A supported vehicle is one that just works when you install a comma device. All |Honda|Civic Hatchback 2017-18|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Civic Hatchback 2019-21|All|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Civic Hatchback 2022-24|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| -|Honda|Civic Hatchback Hybrid 2025|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Honda|Civic Hatchback Hybrid 2025-26|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Civic Hatchback Hybrid (Europe only) 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Civic Hybrid 2025|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|CR-V 2015-16|Touring Trim|openpilot|26 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| @@ -98,8 +98,11 @@ A supported vehicle is one that just works when you install a comma device. All |Honda|HR-V 2023-25|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch B connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Honda|N-Box 2018|All|openpilot available[1](#footnotes)|0 mph|11 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Odyssey 2018-20|Honda Sensing|openpilot|26 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Honda|Odyssey 2021-25|All|openpilot available[1](#footnotes)|0 mph|43 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Passport 2019-25|All|openpilot|26 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Honda|Passport 2026|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch C connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Pilot 2016-22|Honda Sensing|openpilot|26 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Pilot 2023-25|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Honda Bosch C connector
- 1 angled mount (8 degrees)
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Honda|Ridgeline 2017-25|Honda Sensing|openpilot|26 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| @@ -244,10 +247,10 @@ A supported vehicle is one that just works when you install a comma device. All |Škoda|Octavia Scout 2017-19[15](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,16](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 USB-C coupler
- 1 VW J533 connector
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Škoda|Scala 2020-23[15](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,16](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 USB-C coupler
- 1 VW J533 connector
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
[17](#footnotes)||| |Škoda|Superb 2015-22[15](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,16](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 USB-C coupler
- 1 VW J533 connector
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| -|Tesla[11](#footnotes)|Model 3 (with HW3) 2019-23[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla A connector
- 1 USB-C coupler
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| -|Tesla[11](#footnotes)|Model 3 (with HW4) 2024-25[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla B connector
- 1 USB-C coupler
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| -|Tesla[11](#footnotes)|Model Y (with HW3) 2020-23[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla A connector
- 1 USB-C coupler
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| -|Tesla[11](#footnotes)|Model Y (with HW4) 2024[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla B connector
- 1 USB-C coupler
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Tesla[11](#footnotes)|Model 3 (with HW3) 2019-23[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla A connector
- 1 USB-C coupler
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Tesla[11](#footnotes)|Model 3 (with HW4) 2024-25[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla B connector
- 1 USB-C coupler
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Tesla[11](#footnotes)|Model Y (with HW3) 2020-23[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla A connector
- 1 USB-C coupler
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| +|Tesla[11](#footnotes)|Model Y (with HW4) 2024-25[10](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Tesla B connector
- 1 USB-C coupler
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable (9.5 ft)
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| |Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v3
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
||| diff --git a/msgq_repo b/msgq_repo index 5483a02de3..89096d90d2 160000 --- a/msgq_repo +++ b/msgq_repo @@ -1 +1 @@ -Subproject commit 5483a02de303d40cb2632d59f3f3a54dabfb5965 +Subproject commit 89096d90d2f0f71be63a4af0152fe3b2aa55cf9d diff --git a/opendbc_repo b/opendbc_repo index 7afc25d8d4..c70bd060c6 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 7afc25d8d4096bb31e25c0b7ae0b961ea05f5394 +Subproject commit c70bd060c6a410c1083186a1e4165e43a4eda0df diff --git a/panda b/panda index 819fa5854e..a2064b86f3 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 819fa5854e2e75da7f982f7d06be69c61793d6e1 +Subproject commit a2064b86f3c9908883033a953503f150cedacbc7 diff --git a/pyproject.toml b/pyproject.toml index 88c4d06739..489876f78c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -23,7 +23,7 @@ dependencies = [ # core "cffi", "scons", - "pycapnp", + "pycapnp==2.1.0", "Cython", "setuptools", "numpy >=2.0", @@ -176,7 +176,7 @@ quiet-level = 3 # if you've got a short variable name that's getting flagged, add it here ignore-words-list = "bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup,bumb,nd,sie,preints,whit,indexIn,ws,uint,grey,deque,stdio,amin,BA,LITE,atEnd,UIs,errorString,arange,FocusIn,od,tim,relA,hist,copyable,jupyter,thead,TGE,abl,lite" builtin = "clear,rare,informal,code,names,en-GB_to_en-US" -skip = "./third_party/*, ./tinygrad/*, ./tinygrad_repo/*, ./msgq/*, ./panda/*, ./opendbc/*, ./opendbc_repo/*, ./rednose/*, ./rednose_repo/*, ./teleoprtc/*, ./teleoprtc_repo/*, *.ts, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*, docs/assets/*" +skip = "./third_party/*, ./tinygrad/*, ./tinygrad_repo/*, ./msgq/*, ./panda/*, ./opendbc/*, ./opendbc_repo/*, ./rednose/*, ./rednose_repo/*, ./teleoprtc/*, ./teleoprtc_repo/*, *.ts, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*, docs/assets/*, tools/plotjuggler/layouts/*" [tool.mypy] python_version = "3.11" diff --git a/selfdrive/assets/fonts/NotoColorEmoji-Regular.ttf b/selfdrive/assets/fonts/NotoColorEmoji-Regular.ttf new file mode 100644 index 0000000000..2579d30f65 --- /dev/null +++ b/selfdrive/assets/fonts/NotoColorEmoji-Regular.ttf @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:69f216a4ec672bb910d652678301ffe3094c44e5d03276e794ef793d936a1f1d +size 25096376 diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 71b5291dfb..3f9b846e82 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import os import numpy as np from collections import deque, defaultdict @@ -242,6 +243,8 @@ class TorqueEstimator(ParameterEstimator): def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) + DEBUG = bool(int(os.getenv("DEBUG", "0"))) + pm = messaging.PubMaster(['liveTorqueParameters']) sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose', 'liveDelay'], poll='livePose') @@ -258,7 +261,7 @@ def main(demo=False): # 4Hz driven by livePose if sm.frame % 5 == 0: - pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) + pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=DEBUG)) # Cache points every 60 seconds while onroad if sm.frame % 240 == 0: diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 188c79d12d..cc94f455bc 100644 --- a/selfdrive/modeld/models/driving_policy.onnx +++ b/selfdrive/modeld/models/driving_policy.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:74e79e50df530cea1f0033dbc4e37cf767e1128332660fedcfe67daab65b3be3 -size 12343535 +oid sha256:5b0ce3cc48eee07b1a59e47c25552528761547a98dd0c4fac65c42013fc955c5 +size 23927774 diff --git a/selfdrive/modeld/models/driving_vision.onnx b/selfdrive/modeld/models/driving_vision.onnx index 4b4fa05df8..0411ddb744 100644 --- a/selfdrive/modeld/models/driving_vision.onnx +++ b/selfdrive/modeld/models/driving_vision.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:befac016a247b7ad5dc5b55d339d127774ed7bd2b848f1583f72aa4caee37781 -size 46271991 +oid sha256:cf6376aa9a090f0da26c280ef69eabf9bbdd51d1faac9ed392919c3db69be916 +size 46271942 diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 8289df5491..2931eb4acd 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -66,7 +66,7 @@ Panda *connect(std::string serial="", uint32_t index=0) { } //panda->enable_deepsleep(); - for (int i = 0; i < PANDA_BUS_CNT; i++) { + for (int i = 0; i < PANDA_CAN_CNT; i++) { panda->set_can_fd_auto(i, true); } diff --git a/selfdrive/pandad/tests/test_pandad.py b/selfdrive/pandad/tests/test_pandad.py index 6a7359fd85..88d3939a6a 100644 --- a/selfdrive/pandad/tests/test_pandad.py +++ b/selfdrive/pandad/tests/test_pandad.py @@ -5,8 +5,7 @@ import time import cereal.messaging as messaging from cereal import log from openpilot.common.gpio import gpio_set, gpio_init -from panda import Panda, PandaDFU, PandaProtocolMismatch -from openpilot.common.retry import retry +from panda import Panda, PandaDFU from openpilot.system.manager.process_config import managed_processes from openpilot.system.hardware import HARDWARE from openpilot.system.hardware.tici.pins import GPIO @@ -50,8 +49,7 @@ class TestPandad: assert not Panda.wait_for_dfu(None, 3) assert not Panda.wait_for_panda(None, 3) - @retry(attempts=3) - def _flash_bootstub_and_test(self, fn, expect_mismatch=False): + def _flash_bootstub(self, fn): self._go_to_dfu() pd = PandaDFU(None) if fn is None: @@ -61,16 +59,6 @@ class TestPandad: pd.reset() HARDWARE.reset_internal_panda() - assert Panda.wait_for_panda(None, 10) - if expect_mismatch: - with pytest.raises(PandaProtocolMismatch): - Panda() - else: - with Panda() as p: - assert p.bootstub - - self._run_test(45) - def test_in_dfu(self): HARDWARE.recover_internal_panda() self._run_test(60) @@ -106,13 +94,14 @@ class TestPandad: print("startup times", ts, sum(ts) / len(ts)) assert 0.1 < (sum(ts)/len(ts)) < 0.7 - def test_protocol_version_check(self): - # flash old fw - fn = os.path.join(HERE, "bootstub.panda_h7_spiv0.bin") - self._flash_bootstub_and_test(fn, expect_mismatch=True) + def test_old_spi_protocol(self): + # flash firmware with old SPI protocol + self._flash_bootstub(os.path.join(HERE, "bootstub.panda_h7_spiv0.bin")) + self._run_test(45) def test_release_to_devel_bootstub(self): - self._flash_bootstub_and_test(None) + self._flash_bootstub(None) + self._run_test(45) def test_recover_from_bad_bootstub(self): self._go_to_dfu() diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index d4a00115d4..c865cc94a6 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -240,7 +240,7 @@ def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging. def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: return Alert( - f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", + f"Steer Assist Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", "", AlertStatus.userPrompt, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4) @@ -489,7 +489,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { EventName.steerTempUnavailableSilent: { ET.WARNING: Alert( - "Steering Temporarily Unavailable", + "Steering Assist Temporarily Unavailable", "", AlertStatus.userPrompt, AlertSize.small, Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8), @@ -735,7 +735,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { }, EventName.steerTempUnavailable: { - ET.SOFT_DISABLE: soft_disable_alert("Steering Temporarily Unavailable"), + ET.SOFT_DISABLE: soft_disable_alert("Steering Assist Temporarily Unavailable"), ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"), }, diff --git a/selfdrive/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py index dbc1919221..f012e8b06b 100644 --- a/selfdrive/ui/onroad/augmented_road_view.py +++ b/selfdrive/ui/onroad/augmented_road_view.py @@ -102,10 +102,13 @@ class AugmentedRoadView(CameraView): # Handle click events if no HUD interaction occurred if not self._hud_renderer.handle_mouse_event(): - if self._click_callback and rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT): + if self._click_callback is not None and rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT): if rl.check_collision_point_rec(rl.get_mouse_position(), self._content_rect): self._click_callback() + def _handle_mouse_release(self, _): + pass + def _draw_border(self, rect: rl.Rectangle): border_color = BORDER_COLORS.get(ui_state.status, BORDER_COLORS[UIStatus.DISENGAGED]) rl.draw_rectangle_lines_ex(rect, UI_BORDER_SIZE, border_color) diff --git a/selfdrive/ui/widgets/pairing_dialog.py b/selfdrive/ui/widgets/pairing_dialog.py index 63298914ea..79d05eaf42 100644 --- a/selfdrive/ui/widgets/pairing_dialog.py +++ b/selfdrive/ui/widgets/pairing_dialog.py @@ -24,11 +24,11 @@ class PairingDialog: def _get_pairing_url(self) -> str: try: dongle_id = self.params.get("DongleId") or "" - token = Api(dongle_id).get_token() + token = Api(dongle_id).get_token({'pair': True}) except Exception as e: cloudlog.warning(f"Failed to get pairing token: {e}") token = "" - return f"https://connect.comma.ai/setup?token={token}" + return f"https://connect.comma.ai/?pair={token}" def _generate_qr_code(self) -> None: try: diff --git a/system/hardware/base.py b/system/hardware/base.py index ce97bf294d..17d0ec1614 100644 --- a/system/hardware/base.py +++ b/system/hardware/base.py @@ -232,3 +232,12 @@ class HardwareBase(ABC): def get_modem_data_usage(self): return -1, -1 + + def get_voltage(self) -> float: + return 0. + + def get_current(self) -> float: + return 0. + + def set_ir_power(self, percent: int): + pass diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 0f50acdc38..36e65ad91c 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -116,6 +116,26 @@ class Tici(HardwareBase): def get_serial(self): return self.get_cmdline()['androidboot.serialno'] + def get_voltage(self): + with open("/sys/class/hwmon/hwmon1/in1_input") as f: + return int(f.read()) + + def get_current(self): + with open("/sys/class/hwmon/hwmon1/curr1_input") as f: + return int(f.read()) + + def set_ir_power(self, percent: int): + if self.get_device_type() in ("tici", "tizi"): + return + + value = int((percent / 100) * 300) + with open("/sys/class/leds/led:switch_2/brightness", "w") as f: + f.write("0\n") + with open("/sys/class/leds/led:torch_2/brightness", "w") as f: + f.write(f"{value}\n") + with open("/sys/class/leds/led:switch_2/brightness", "w") as f: + f.write(f"{value}\n") + def get_network_type(self): try: primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py index 46b45460b5..4fbde81673 100644 --- a/system/hardware/tici/tests/test_power_draw.py +++ b/system/hardware/tici/tests/test_power_draw.py @@ -31,9 +31,9 @@ class Proc: PROCS = [ - Proc(['camerad'], 1.75, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']), + Proc(['camerad'], 1.65, atol=0.4, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']), Proc(['modeld'], 1.24, atol=0.2, msgs=['modelV2']), - Proc(['dmonitoringmodeld'], 0.7, msgs=['driverStateV2']), + Proc(['dmonitoringmodeld'], 0.65, atol=0.35, msgs=['driverStateV2']), Proc(['encoderd'], 0.23, msgs=[]), ] diff --git a/system/ubloxd/glonass_fix.patch b/system/ubloxd/glonass_fix.patch deleted file mode 100644 index 7eb973a348..0000000000 --- a/system/ubloxd/glonass_fix.patch +++ /dev/null @@ -1,13 +0,0 @@ -diff --git a/system/ubloxd/generated/glonass.cpp b/system/ubloxd/generated/glonass.cpp -index 5b17bc327..b5c6aa610 100644 ---- a/system/ubloxd/generated/glonass.cpp -+++ b/system/ubloxd/generated/glonass.cpp -@@ -17,7 +17,7 @@ glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass - void glonass_t::_read() { - m_idle_chip = m__io->read_bits_int_be(1); - m_string_number = m__io->read_bits_int_be(4); -- m__io->align_to_byte(); -+ //m__io->align_to_byte(); - switch (string_number()) { - case 4: { - m_data = new string_4_t(m__io, this, m__root); diff --git a/system/ubloxd/tests/ubloxd.py b/system/ubloxd/tests/ubloxd.py deleted file mode 100755 index c17387114f..0000000000 --- a/system/ubloxd/tests/ubloxd.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/usr/bin/env python3 -# type: ignore - -from openpilot.selfdrive.locationd.test import ublox -import struct - -baudrate = 460800 -rate = 100 # send new data every 100ms - - -def configure_ublox(dev): - # configure ports and solution parameters and rate - dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB - dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC - - payload = struct.pack(' 0.0){ vec4 color = useGradient == 1 ? getGradientColor(pixel) : fillColor; finalColor = vec4(color.rgb, color.a * alpha); diff --git a/system/ui/widgets/__init__.py b/system/ui/widgets/__init__.py index cca2cec7ad..6372b1813d 100644 --- a/system/ui/widgets/__init__.py +++ b/system/ui/widgets/__init__.py @@ -15,12 +15,13 @@ class Widget(abc.ABC): def __init__(self): self._rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0) self._parent_rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0) - self._is_pressed = [False] * MAX_TOUCH_SLOTS + self.__is_pressed = [False] * MAX_TOUCH_SLOTS # if current mouse/touch down started within the widget's rectangle - self._tracking_is_pressed = [False] * MAX_TOUCH_SLOTS + self.__tracking_is_pressed = [False] * MAX_TOUCH_SLOTS self._enabled: bool | Callable[[], bool] = True self._is_visible: bool | Callable[[], bool] = True self._touch_valid_callback: Callable[[], bool] | None = None + self._click_callback: Callable[[], None] | None = None self._multi_touch = False @property @@ -40,7 +41,7 @@ class Widget(abc.ABC): @property def is_pressed(self) -> bool: - return any(self._is_pressed) + return any(self.__is_pressed) @property def enabled(self) -> bool: @@ -56,6 +57,10 @@ class Widget(abc.ABC): def set_visible(self, visible: bool | Callable[[], bool]) -> None: self._is_visible = visible + def set_click_callback(self, click_callback: Callable[[], None] | None) -> None: + """Set a callback to be called when the widget is clicked.""" + self._click_callback = click_callback + def set_touch_valid_callback(self, touch_callback: Callable[[], bool]) -> None: """Set a callback to determine if the widget can be clicked.""" self._touch_valid_callback = touch_callback @@ -91,28 +96,28 @@ class Widget(abc.ABC): # Allows touch to leave the rect and come back in focus if mouse did not release if mouse_event.left_pressed and self._touch_valid(): if rl.check_collision_point_rec(mouse_event.pos, self._rect): - self._is_pressed[mouse_event.slot] = True - self._tracking_is_pressed[mouse_event.slot] = True + self.__is_pressed[mouse_event.slot] = True + self.__tracking_is_pressed[mouse_event.slot] = True # Callback such as scroll panel signifies user is scrolling elif not self._touch_valid(): - self._is_pressed[mouse_event.slot] = False - self._tracking_is_pressed[mouse_event.slot] = False + self.__is_pressed[mouse_event.slot] = False + self.__tracking_is_pressed[mouse_event.slot] = False elif mouse_event.left_released: - if self._is_pressed[mouse_event.slot] and rl.check_collision_point_rec(mouse_event.pos, self._rect): + if self.__is_pressed[mouse_event.slot] and rl.check_collision_point_rec(mouse_event.pos, self._rect): self._handle_mouse_release(mouse_event.pos) - self._is_pressed[mouse_event.slot] = False - self._tracking_is_pressed[mouse_event.slot] = False + self.__is_pressed[mouse_event.slot] = False + self.__tracking_is_pressed[mouse_event.slot] = False # Mouse/touch is still within our rect elif rl.check_collision_point_rec(mouse_event.pos, self._rect): - if self._tracking_is_pressed[mouse_event.slot]: - self._is_pressed[mouse_event.slot] = True + if self.__tracking_is_pressed[mouse_event.slot]: + self.__is_pressed[mouse_event.slot] = True # Mouse/touch left our rect but may come back into focus later elif not rl.check_collision_point_rec(mouse_event.pos, self._rect): - self._is_pressed[mouse_event.slot] = False + self.__is_pressed[mouse_event.slot] = False return ret @@ -128,6 +133,8 @@ class Widget(abc.ABC): def _handle_mouse_release(self, mouse_pos: MousePos) -> bool: """Optionally handle mouse release events.""" + if self._click_callback: + self._click_callback() return False def show_event(self): diff --git a/system/ui/widgets/button.py b/system/ui/widgets/button.py index a5dab19789..eb38f20597 100644 --- a/system/ui/widgets/button.py +++ b/system/ui/widgets/button.py @@ -165,7 +165,7 @@ def gui_button( class Button(Widget): def __init__(self, text: str, - click_callback: Callable[[], None] = None, + click_callback: Callable[[], None] | None = None, font_size: int = DEFAULT_BUTTON_FONT_SIZE, font_weight: FontWeight = FontWeight.MEDIUM, button_style: ButtonStyle = ButtonStyle.NORMAL, @@ -190,10 +190,6 @@ class Button(Widget): def set_text(self, text): self._label.set_text(text) - def _handle_mouse_release(self, mouse_pos: MousePos): - if self._click_callback and self.enabled: - self._click_callback() - def _update_state(self): if self.enabled: self._label.set_text_color(BUTTON_TEXT_COLOR[self._button_style]) @@ -215,7 +211,7 @@ class ButtonRadio(Button): def __init__(self, text: str, icon, - click_callback: Callable[[], None] = None, + click_callback: Callable[[], None] | None = None, font_size: int = DEFAULT_BUTTON_FONT_SIZE, text_alignment: TextAlignment = TextAlignment.LEFT, border_radius: int = 10, @@ -230,9 +226,8 @@ class ButtonRadio(Button): self.selected = False def _handle_mouse_release(self, mouse_pos: MousePos): + super()._handle_mouse_release(mouse_pos) self.selected = not self.selected - if self._click_callback: - self._click_callback() def _update_state(self): if self.selected: diff --git a/system/updated/updated.py b/system/updated/updated.py index a80a663ec9..f9fad5f6f5 100755 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -113,6 +113,7 @@ def setup_git_options(cwd: str) -> None: ("protocol.version", "2"), ("gc.auto", "0"), ("gc.autoDetach", "false"), + ("remote.origin.fetch", "+refs/heads/*:refs/remotes/origin/*"), ] for option, value in git_cfg: run(["git", "config", option, value], cwd) @@ -389,6 +390,7 @@ class Updater: cloudlog.info("git reset in progress") cmds = [ ["git", "checkout", "--force", "--no-recurse-submodules", "-B", branch, "FETCH_HEAD"], + ["git", "branch", "--set-upstream-to", f"origin/{branch}"], ["git", "reset", "--hard"], ["git", "clean", "-xdff"], ["git", "submodule", "sync"], diff --git a/system/version.py b/system/version.py index 5e4fcf5c33..e32c3d6033 100755 --- a/system/version.py +++ b/system/version.py @@ -37,9 +37,7 @@ def is_prebuilt(path: str = BASEDIR) -> bool: @cache def is_dirty(cwd: str = BASEDIR) -> bool: - origin = get_origin() - branch = get_branch() - if not origin or not branch: + if not get_origin() or not get_short_branch(): return True dirty = False @@ -52,6 +50,9 @@ def is_dirty(cwd: str = BASEDIR) -> bool: except subprocess.CalledProcessError: pass + branch = get_branch() + if not branch: + return True dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"], cwd=cwd)) != 0 except subprocess.CalledProcessError: cloudlog.exception("git subprocess failed while checking dirty") diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 8361befb53..d65fc5b760 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -122,7 +122,7 @@ void MainWindow::createActions() { auto undo_act = UndoStack::instance()->createUndoAction(this, tr("&Undo")); undo_act->setShortcuts(QKeySequence::Undo); edit_menu->addAction(undo_act); - auto redo_act = UndoStack::instance()->createRedoAction(this, tr("&Rndo")); + auto redo_act = UndoStack::instance()->createRedoAction(this, tr("&Redo")); redo_act->setShortcuts(QKeySequence::Redo); edit_menu->addAction(redo_act); edit_menu->addSeparator(); diff --git a/tools/install_ubuntu_dependencies.sh b/tools/install_ubuntu_dependencies.sh index f33569704a..97e06bcc6b 100755 --- a/tools/install_ubuntu_dependencies.sh +++ b/tools/install_ubuntu_dependencies.sh @@ -20,18 +20,25 @@ fi # Install common packages function install_ubuntu_common_requirements() { $SUDO apt-get update + + # normal stuff, mostly for the bare docker image $SUDO apt-get install -y --no-install-recommends \ ca-certificates \ clang \ build-essential \ - gcc-arm-none-eabi \ - liblzma-dev \ - capnproto \ - libcapnp-dev \ curl \ + libssl-dev \ libcurl4-openssl-dev \ + locales \ git \ git-lfs \ + xvfb + + # TODO: vendor the rest of these in third_party/ + $SUDO apt-get install -y --no-install-recommends \ + gcc-arm-none-eabi \ + capnproto \ + libcapnp-dev \ ffmpeg \ libavformat-dev \ libavcodec-dev \ @@ -41,20 +48,16 @@ function install_ubuntu_common_requirements() { libbz2-dev \ libeigen3-dev \ libffi-dev \ - libglew-dev \ libgles2-mesa-dev \ libglfw3-dev \ libglib2.0-0 \ libjpeg-dev \ libqt5charts5-dev \ libncurses5-dev \ - libssl-dev \ libusb-1.0-0-dev \ libzmq3-dev \ libzstd-dev \ libsqlite3-dev \ - libsystemd-dev \ - locales \ opencl-headers \ ocl-icd-libopencl1 \ ocl-icd-opencl-dev \ @@ -63,8 +66,7 @@ function install_ubuntu_common_requirements() { libqt5svg5-dev \ libqt5serialbus5-dev \ libqt5x11extras5-dev \ - libqt5opengl5-dev \ - xvfb + libqt5opengl5-dev } # Install Ubuntu 24.04 LTS packages @@ -74,8 +76,6 @@ function install_ubuntu_lts_latest_requirements() { $SUDO apt-get install -y --no-install-recommends \ g++-12 \ qtbase5-dev \ - qtchooser \ - qt5-qmake \ qtbase5-dev-tools \ python3-dev \ python3-venv diff --git a/tools/jotpluggler/README.md b/tools/jotpluggler/README.md new file mode 100644 index 0000000000..c5b43dbd3a --- /dev/null +++ b/tools/jotpluggler/README.md @@ -0,0 +1,67 @@ +# JotPluggler + +JotPluggler is a tool to quickly visualize openpilot logs. + +## Usage + +``` +$ ./jotpluggler/pluggle.py -h +usage: pluggle.py [-h] [--demo] [--layout LAYOUT] [route] + +A tool for visualizing openpilot logs. + +positional arguments: + route Optional route name to load on startup. + +options: + -h, --help show this help message and exit + --demo Use the demo route instead of providing one + --layout LAYOUT Path to YAML layout file to load on startup +``` + +Example using route name: + +`./pluggle.py "a2a0ccea32023010/2023-07-27--13-01-19"` + +Examples using segment: + +`./pluggle.py "a2a0ccea32023010/2023-07-27--13-01-19/1"` + +`./pluggle.py "a2a0ccea32023010/2023-07-27--13-01-19/1/q" # use qlogs` + +Example using segment range: + +`./pluggle.py "a2a0ccea32023010/2023-07-27--13-01-19/0:1"` + +## Demo + +For a quick demo, run this command: + +`./pluggle.py --demo --layout=layouts/torque-controller.yaml` + + +## Basic Usage/Features: +- The text box to load a route is a the top left of the page, accepts standard openpilot format routes (e.g. `a2a0ccea32023010/2023-07-27--13-01-19/0:1`, `https://connect.comma.ai/a2a0ccea32023010/2023-07-27--13-01-19/`) +- The Play/Pause button is at the bottom of the screen, you can drag the bottom slider to seek. The timeline in timeseries plots are synced with the slider. +- The Timeseries List sidebar has several dropdowns, the fields each show the field name and value, synced with the timeline (will show N/A until the time of the first message in that field is reached). +- There is a search bar for the timeseries list, you can search for structs or fields, or both by separating with a "/" +- You can drag and drop any numeric/boolean field from the timeseries list into a timeseries panel. +- You can create more panels with the split buttons (buttons with two rectangles, either horizontal or vertical). You can resize the panels by dragging the grip in between any panel. +- You can load and save layouts with the corresponding buttons. Layouts will save all tabs, panels, titles, timeseries, etc. + +## Layouts + +If you create a layout that's useful for others, consider upstreaming it. + +## Plot Interaction Controls + +- **Left click and drag within the plot area** to pan X + - Left click and drag on an axis to pan an individual axis (disabled for Y-axis) +- **Scroll in the plot area** to zoom in X axes, Y-axis is autofit + - Scroll on an axis to zoom an individual axis +- **Right click and drag** to select data and zoom into the selected data + - Left click while box selecting to cancel the selection +- **Double left click** to fit all visible data + - Double left click on an axis to fit the individual axis (disabled for Y-axis, always autofit) +- **Double right click** to open the plot context menu +- **Click legend label icons** to show/hide plot items diff --git a/tools/jotpluggler/assets/pause.png b/tools/jotpluggler/assets/pause.png new file mode 100644 index 0000000000..8040099831 --- /dev/null +++ b/tools/jotpluggler/assets/pause.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3ea96d8193eb9067a5efdc5d88a3099730ecafa40efcd09d7402bb3efd723603 +size 2305 diff --git a/tools/jotpluggler/assets/play.png b/tools/jotpluggler/assets/play.png new file mode 100644 index 0000000000..b1556cf0ab --- /dev/null +++ b/tools/jotpluggler/assets/play.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:53097ac5403b725ff1841dfa186ea770b4bb3714205824bde36ec3c2a0fb5dba +size 2758 diff --git a/tools/jotpluggler/assets/plus.png b/tools/jotpluggler/assets/plus.png new file mode 100644 index 0000000000..6f8388b24d --- /dev/null +++ b/tools/jotpluggler/assets/plus.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:248b71eafd1b42b0861da92114da3d625221cd88121fff01e0514bf3d79ff3b1 +size 1364 diff --git a/tools/jotpluggler/assets/split_h.png b/tools/jotpluggler/assets/split_h.png new file mode 100644 index 0000000000..4fd88806e1 --- /dev/null +++ b/tools/jotpluggler/assets/split_h.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:54dd035ff898d881509fa686c402a61af8ef5fb408b92414722da01f773b0d33 +size 2900 diff --git a/tools/jotpluggler/assets/split_v.png b/tools/jotpluggler/assets/split_v.png new file mode 100644 index 0000000000..752e62a4ae --- /dev/null +++ b/tools/jotpluggler/assets/split_v.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:adbd4e5df1f58694dca9dde46d1d95b4e7471684e42e6bca9f41ea5d346e67c5 +size 3669 diff --git a/tools/jotpluggler/assets/x.png b/tools/jotpluggler/assets/x.png new file mode 100644 index 0000000000..3b2eabd447 --- /dev/null +++ b/tools/jotpluggler/assets/x.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a6d9c90cb0dd906e0b15e1f7f3fd9f0dfad3c3b0b34eeed7a7882768dc5f3961 +size 2053 diff --git a/tools/jotpluggler/data.py b/tools/jotpluggler/data.py index 41c305718e..cf27857d1f 100644 --- a/tools/jotpluggler/data.py +++ b/tools/jotpluggler/data.py @@ -3,8 +3,9 @@ import threading import multiprocessing import bisect from collections import defaultdict -import tqdm +from tqdm import tqdm from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.test.process_replay.migration import migrate_all from openpilot.tools.lib.logreader import _LogFileReader, LogReader @@ -70,9 +71,6 @@ def extract_field_types(schema, prefix, field_types_dict): def _convert_to_optimal_dtype(values_list, capnp_type): - if not values_list: - return np.array([]) - dtype_mapping = { 'bool': np.bool_, 'int8': np.int8, 'int16': np.int16, 'int32': np.int32, 'int64': np.int64, 'uint8': np.uint8, 'uint16': np.uint16, 'uint32': np.uint32, 'uint64': np.uint64, @@ -80,8 +78,8 @@ def _convert_to_optimal_dtype(values_list, capnp_type): 'enum': object, 'anyPointer': object, } - target_dtype = dtype_mapping.get(capnp_type) - return np.array(values_list, dtype=target_dtype) if target_dtype else np.array(values_list) + target_dtype = dtype_mapping.get(capnp_type, object) + return np.array(values_list, dtype=target_dtype) def _match_field_type(field_path, field_types): @@ -94,6 +92,21 @@ def _match_field_type(field_path, field_types): return field_types.get(template_path) +def _get_field_times_values(segment, field_name): + if field_name not in segment: + return None, None + + field_data = segment[field_name] + segment_times = segment['t'] + + if field_data['sparse']: + if len(field_data['t_index']) == 0: + return None, None + return segment_times[field_data['t_index']], field_data['values'] + else: + return segment_times, field_data['values'] + + def msgs_to_time_series(msgs): """Extract scalar fields and return (time_series_data, start_time, end_time).""" collected_data = defaultdict(lambda: {'timestamps': [], 'columns': defaultdict(list), 'sparse_fields': set()}) @@ -110,16 +123,22 @@ def msgs_to_time_series(msgs): max_time = timestamp sub_msg = getattr(msg, typ) - if not hasattr(sub_msg, 'to_dict') or typ in ('qcomGnss', 'ubloxGnss'): + if not hasattr(sub_msg, 'to_dict'): continue if hasattr(sub_msg, 'schema') and typ not in extracted_schemas: extract_field_types(sub_msg.schema, typ, field_types) extracted_schemas.add(typ) - msg_dict = sub_msg.to_dict(verbose=True) + try: + msg_dict = sub_msg.to_dict(verbose=True) + except Exception as e: + cloudlog.warning(f"Failed to convert sub_msg.to_dict() for message of type: {typ}: {e}") + continue + flat_dict = flatten_dict(msg_dict) flat_dict['_valid'] = msg.valid + field_types[f"{typ}/_valid"] = 'bool' type_data = collected_data[typ] columns, sparse_fields = type_data['columns'], type_data['sparse_fields'] @@ -152,11 +171,26 @@ def msgs_to_time_series(msgs): values = [None] * (len(data['timestamps']) - len(values)) + values sparse_fields.add(field_name) - if field_name in sparse_fields: - typ_result[field_name] = np.array(values, dtype=object) - else: - capnp_type = _match_field_type(f"{typ}/{field_name}", field_types) - typ_result[field_name] = _convert_to_optimal_dtype(values, capnp_type) + capnp_type = _match_field_type(f"{typ}/{field_name}", field_types) + + if field_name in sparse_fields: # extract non-None values and their indices + non_none_indices = [] + non_none_values = [] + for i, value in enumerate(values): + if value is not None: + non_none_indices.append(i) + non_none_values.append(value) + + if non_none_values: # check if indices > uint16 max, currently would require a 1000+ Hz signal since indices are within segments + assert max(non_none_indices) <= 65535, f"Sparse field {typ}/{field_name} has timestamp indices exceeding uint16 max. Max: {max(non_none_indices)}" + + typ_result[field_name] = { + 'values': _convert_to_optimal_dtype(non_none_values, capnp_type), + 'sparse': True, + 't_index': np.array(non_none_indices, dtype=np.uint16), + } + else: # dense representation + typ_result[field_name] = {'values': _convert_to_optimal_dtype(values, capnp_type), 'sparse': False} final_result[typ] = typ_result @@ -166,7 +200,8 @@ def msgs_to_time_series(msgs): def _process_segment(segment_identifier: str): try: lr = _LogFileReader(segment_identifier, sort_by_time=True) - return msgs_to_time_series(lr) + migrated_msgs = migrate_all(lr) + return msgs_to_time_series(migrated_msgs) except Exception as e: cloudlog.warning(f"Warning: Failed to process segment {segment_identifier}: {e}") return {}, 0.0, 0.0 @@ -195,22 +230,31 @@ class DataManager: times, values = [], [] for segment in self._segments: - if msg_type in segment and field in segment[msg_type]: - times.append(segment[msg_type]['t']) - values.append(segment[msg_type][field]) + if msg_type in segment: + field_times, field_values = _get_field_times_values(segment[msg_type], field) + if field_times is not None: + times.append(field_times) + values.append(field_values) if not times: - return [], [] + return np.array([]), np.array([]) combined_times = np.concatenate(times) - self._start_time - if len(values) > 1 and any(arr.dtype != values[0].dtype for arr in values): - values = [arr.astype(object) for arr in values] - return combined_times, np.concatenate(values) + if len(values) > 1: + first_dtype = values[0].dtype + if all(arr.dtype == first_dtype for arr in values): # check if all arrays have compatible dtypes + combined_values = np.concatenate(values) + else: + combined_values = np.concatenate([arr.astype(object) for arr in values]) + else: + combined_values = values[0] if values else np.array([]) + + return combined_times, combined_values def get_value_at(self, path: str, time: float): with self._lock: - MAX_LOOKBACK = 5.0 # seconds + MAX_LOOKBACK = 5.0 # seconds absolute_time = self._start_time + time message_type, field = path.split('/', 1) current_index = bisect.bisect_right(self._segment_starts, absolute_time) - 1 @@ -218,14 +262,14 @@ class DataManager: if not 0 <= index < len(self._segments): continue segment = self._segments[index].get(message_type) - if not segment or field not in segment: + if not segment: continue - times = segment['t'] - if len(times) == 0 or (index != current_index and absolute_time - times[-1] > MAX_LOOKBACK): + times, values = _get_field_times_values(segment, field) + if times is None or len(times) == 0 or (index != current_index and absolute_time - times[-1] > MAX_LOOKBACK): continue position = np.searchsorted(times, absolute_time, 'right') - 1 if position >= 0 and absolute_time - times[position] <= MAX_LOOKBACK: - return segment[field][position] + return values[position] return None def get_all_paths(self): @@ -237,10 +281,9 @@ class DataManager: return self._duration def is_plottable(self, path: str): - data = self.get_timeseries(path) - if data is None: + _, values = self.get_timeseries(path) + if len(values) == 0: return False - _, values = data return np.issubdtype(values.dtype, np.number) or np.issubdtype(values.dtype, np.bool_) def add_observer(self, callback): @@ -271,8 +314,14 @@ class DataManager: cloudlog.warning(f"Warning: No log segments found for route: {route}") return + total_segments = len(lr.logreader_identifiers) + with self._lock: + observers = self._observers.copy() + for callback in observers: + callback({'metadata_loaded': True, 'total_segments': total_segments}) + num_processes = max(1, multiprocessing.cpu_count() // 2) - with multiprocessing.Pool(processes=num_processes) as pool, tqdm.tqdm(total=len(lr.logreader_identifiers), desc="Processing Segments") as pbar: + with multiprocessing.Pool(processes=num_processes) as pool, tqdm(total=len(lr.logreader_identifiers), desc="Processing Segments") as pbar: for segment_result, start_time, end_time in pool.imap(_process_segment, lr.logreader_identifiers): pbar.update(1) if segment_result: @@ -292,9 +341,9 @@ class DataManager: self._duration = end_time - self._start_time for msg_type, data in segment_data.items(): - for field in data.keys(): - if field != 't': - self._paths.add(f"{msg_type}/{field}") + for field_name in data.keys(): + if field_name != 't': + self._paths.add(f"{msg_type}/{field_name}") observers = self._observers.copy() diff --git a/tools/jotpluggler/datatree.py b/tools/jotpluggler/datatree.py index 7bd026cf98..4f3219dc1b 100644 --- a/tools/jotpluggler/datatree.py +++ b/tools/jotpluggler/datatree.py @@ -2,7 +2,6 @@ import os import re import threading import numpy as np -from collections import deque import dearpygui.dearpygui as dpg @@ -12,8 +11,9 @@ class DataTreeNode: self.full_path = full_path self.parent = parent self.children: dict[str, DataTreeNode] = {} + self.filtered_children: dict[str, DataTreeNode] = {} + self.created_children: dict[str, DataTreeNode] = {} self.is_leaf = False - self.child_count = 0 self.is_plottable: bool | None = None self.ui_created = False self.children_ui_created = False @@ -28,154 +28,211 @@ class DataTree: self.playback_manager = playback_manager self.current_search = "" self.data_tree = DataTreeNode(name="root") - self._build_queue: deque[tuple[DataTreeNode, str | None, str | int]] = deque() - self._all_paths_cache: set[str] = set() - self._item_handlers: set[str] = set() - self._avg_char_width = None + self._build_queue: dict[str, tuple[DataTreeNode, DataTreeNode, str | int]] = {} # full_path -> (node, parent, before_tag) + self._current_created_paths: set[str] = set() + self._current_filtered_paths: set[str] = set() + self._path_to_node: dict[str, DataTreeNode] = {} # full_path -> node + self._expanded_tags: set[str] = set() + self._item_handlers: dict[str, str] = {} # ui_tag -> handler_tag + self._char_width = None self._queued_search = None self._new_data = False self._ui_lock = threading.RLock() + self._handlers_to_delete = [] self.data_manager.add_observer(self._on_data_loaded) def create_ui(self, parent_tag: str): with dpg.child_window(parent=parent_tag, border=False, width=-1, height=-1): - dpg.add_text("Available Data") + dpg.add_text("Timeseries List") dpg.add_separator() dpg.add_input_text(tag="search_input", width=-1, hint="Search fields...", callback=self.search_data) dpg.add_separator() - with dpg.group(tag="data_tree_container"): - pass + with dpg.child_window(border=False, width=-1, height=-1): + with dpg.group(tag="data_tree_container"): + pass def _on_data_loaded(self, data: dict): with self._ui_lock: - if data.get('segment_added'): + if data.get('segment_added') or data.get('reset'): self._new_data = True - elif data.get('reset'): - self._all_paths_cache = set() - self._new_data = True - - - def _populate_tree(self): - self._clear_ui() - self.data_tree = self._add_paths_to_tree(self._all_paths_cache, incremental=False) - if self.data_tree: - self._request_children_build(self.data_tree) - - def _add_paths_to_tree(self, paths, incremental=False): - search_term = self.current_search.strip().lower() - filtered_paths = [path for path in paths if self._should_show_path(path, search_term)] - target_tree = self.data_tree if incremental else DataTreeNode(name="root") - - if not filtered_paths: - return target_tree - - parent_nodes_to_recheck = set() - for path in sorted(filtered_paths): - parts = path.split('/') - current_node = target_tree - current_path_prefix = "" - - for i, part in enumerate(parts): - current_path_prefix = f"{current_path_prefix}/{part}" if current_path_prefix else part - if i < len(parts) - 1: - parent_nodes_to_recheck.add(current_node) # for incremental changes from new data - if part not in current_node.children: - current_node.children[part] = DataTreeNode(name=part, full_path=current_path_prefix, parent=current_node) - current_node = current_node.children[part] - - if not current_node.is_leaf: - current_node.is_leaf = True - - self._calculate_child_counts(target_tree) - if incremental: - for p_node in parent_nodes_to_recheck: - p_node.children_ui_created = False - self._request_children_build(p_node) - return target_tree def update_frame(self, font): + if self._handlers_to_delete: # we need to do everything in main thread, frame callbacks are flaky + dpg.render_dearpygui_frame() # wait a frame to ensure queued callbacks are done + with self._ui_lock: + for handler in self._handlers_to_delete: + dpg.delete_item(handler) + self._handlers_to_delete.clear() + with self._ui_lock: - if self._avg_char_width is None and dpg.is_dearpygui_running(): - self._avg_char_width = self.calculate_avg_char_width(font) + if self._char_width is None: + if size := dpg.get_text_size(" ", font=font): + self._char_width = size[0] / 2 # we scale font 2x and downscale to fix hidpi bug if self._new_data: - current_paths = set(self.data_manager.get_all_paths()) - new_paths = current_paths - self._all_paths_cache - all_paths_empty = not self._all_paths_cache - self._all_paths_cache = current_paths - if all_paths_empty: - self._populate_tree() - elif new_paths: - self._add_paths_to_tree(new_paths, incremental=True) + self._process_path_change() self._new_data = False return if self._queued_search is not None: self.current_search = self._queued_search - self._all_paths_cache = set(self.data_manager.get_all_paths()) - self._populate_tree() + self._process_path_change() self._queued_search = None return nodes_processed = 0 while self._build_queue and nodes_processed < self.MAX_NODES_PER_FRAME: - child_node, parent_tag, before_tag = self._build_queue.popleft() + child_node, parent, before_tag = self._build_queue.pop(next(iter(self._build_queue))) + parent_tag = "data_tree_container" if parent.name == "root" else parent.ui_tag if not child_node.ui_created: if child_node.is_leaf: self._create_leaf_ui(child_node, parent_tag, before_tag) else: self._create_tree_node_ui(child_node, parent_tag, before_tag) + parent.created_children[child_node.name] = parent.children[child_node.name] + self._current_created_paths.add(child_node.full_path) nodes_processed += 1 - def search_data(self): - self._queued_search = dpg.get_value("search_input") + def _process_path_change(self): + self._build_queue.clear() + search_term = self.current_search.strip().lower() + all_paths = set(self.data_manager.get_all_paths()) + new_filtered_leafs = {path for path in all_paths if self._should_show_path(path, search_term)} + new_filtered_paths = set(new_filtered_leafs) + for path in new_filtered_leafs: + parts = path.split('/') + for i in range(1, len(parts)): + prefix = '/'.join(parts[:i]) + new_filtered_paths.add(prefix) + created_paths_to_remove = self._current_created_paths - new_filtered_paths + filtered_paths_to_remove = self._current_filtered_paths - new_filtered_leafs + + if created_paths_to_remove or filtered_paths_to_remove: + self._remove_paths_from_tree(created_paths_to_remove, filtered_paths_to_remove) + self._apply_expansion_to_tree(self.data_tree, search_term) + + paths_to_add = new_filtered_leafs - self._current_created_paths + if paths_to_add: + self._add_paths_to_tree(paths_to_add) + self._apply_expansion_to_tree(self.data_tree, search_term) + self._current_filtered_paths = new_filtered_paths + + def _remove_paths_from_tree(self, created_paths_to_remove, filtered_paths_to_remove): + for path in sorted(created_paths_to_remove, reverse=True): + current_node = self._path_to_node[path] + + if len(current_node.created_children) == 0: + self._current_created_paths.remove(current_node.full_path) + if item_handler_tag := self._item_handlers.get(current_node.ui_tag): + dpg.configure_item(item_handler_tag, show=False) + self._handlers_to_delete.append(item_handler_tag) + del self._item_handlers[current_node.ui_tag] + dpg.delete_item(current_node.ui_tag) + current_node.ui_created = False + current_node.ui_tag = None + current_node.children_ui_created = False + del current_node.parent.created_children[current_node.name] + del current_node.parent.filtered_children[current_node.name] + + for path in filtered_paths_to_remove: + parts = path.split('/') + current_node = self._path_to_node[path] - def _clear_ui(self): - for handler_tag in self._item_handlers: - dpg.configure_item(handler_tag, show=False) - dpg.set_frame_callback(dpg.get_frame_count() + 1, callback=self._delete_handlers, user_data=list(self._item_handlers)) - self._item_handlers.clear() + part_array_index = -1 + while len(current_node.filtered_children) == 0 and part_array_index >= -len(parts): + current_node = current_node.parent + if parts[part_array_index] in current_node.filtered_children: + del current_node.filtered_children[parts[part_array_index]] + part_array_index -= 1 - if dpg.does_item_exist("data_tree_container"): - dpg.delete_item("data_tree_container", children_only=True) + def _add_paths_to_tree(self, paths): + parent_nodes_to_recheck = set() + for path in sorted(paths): + parts = path.split('/') + current_node = self.data_tree + current_path_prefix = "" - self._build_queue.clear() + for i, part in enumerate(parts): + current_path_prefix = f"{current_path_prefix}/{part}" if current_path_prefix else part + if i < len(parts): + parent_nodes_to_recheck.add(current_node) # for incremental changes from new data + if part not in current_node.children: + current_node.children[part] = DataTreeNode(name=part, full_path=current_path_prefix, parent=current_node) + self._path_to_node[current_path_prefix] = current_node.children[part] + current_node.filtered_children[part] = current_node.children[part] + current_node = current_node.children[part] - def _delete_handlers(self, sender, app_data, user_data): - for handler in user_data: - dpg.delete_item(handler) + if not current_node.is_leaf: + current_node.is_leaf = True + + for p_node in parent_nodes_to_recheck: + p_node.children_ui_created = False + self._request_children_build(p_node) + + def _get_node_label_and_expand(self, node: DataTreeNode, search_term: str): + label = f"{node.name} ({len(node.filtered_children)} fields)" + expand = len(search_term) > 0 and any(search_term in path for path in self._get_descendant_paths(node)) + if expand and node.parent and len(node.parent.filtered_children) > 100 and len(node.filtered_children) > 2: + label += " (+)" # symbol for large lists which aren't fully expanded for performance (only affects procLog rn) + expand = False + return label, expand + + def _apply_expansion_to_tree(self, node: DataTreeNode, search_term: str): + if node.ui_created and not node.is_leaf and node.ui_tag and dpg.does_item_exist(node.ui_tag): + label, expand = self._get_node_label_and_expand(node, search_term) + if expand: + self._expanded_tags.add(node.ui_tag) + dpg.set_value(node.ui_tag, expand) + elif node.ui_tag in self._expanded_tags: # not expanded and was expanded + self._expanded_tags.remove(node.ui_tag) + dpg.set_value(node.ui_tag, expand) + dpg.delete_item(node.ui_tag, children_only=True) # delete children (not visible since collapsed) + self._reset_ui_state_recursive(node) + node.children_ui_created = False + dpg.set_item_label(node.ui_tag, label) + for child in node.created_children.values(): + self._apply_expansion_to_tree(child, search_term) + + def _reset_ui_state_recursive(self, node: DataTreeNode): + for child in node.created_children.values(): + if child.ui_tag is not None: + if item_handler_tag := self._item_handlers.get(child.ui_tag): + self._handlers_to_delete.append(item_handler_tag) + dpg.configure_item(item_handler_tag, show=False) + del self._item_handlers[child.ui_tag] + self._reset_ui_state_recursive(child) + child.ui_created = False + child.ui_tag = None + child.children_ui_created = False + self._current_created_paths.remove(child.full_path) + node.created_children.clear() - def _calculate_child_counts(self, node: DataTreeNode): - if node.is_leaf: - node.child_count = 0 - else: - node.child_count = len(node.children) - for child in node.children.values(): - self._calculate_child_counts(child) + def search_data(self): + with self._ui_lock: + self._queued_search = dpg.get_value("search_input") def _create_tree_node_ui(self, node: DataTreeNode, parent_tag: str, before: str | int): - tag = f"tree_{node.full_path}" - node.ui_tag = tag - label = f"{node.name} ({node.child_count} fields)" + node.ui_tag = f"tree_{node.full_path}" search_term = self.current_search.strip().lower() - expand = bool(search_term) and len(search_term) > 1 and any(search_term in path for path in self._get_descendant_paths(node)) - if expand and node.parent and node.parent.child_count > 100 and node.child_count > 2: # don't fully autoexpand large lists (only affects procLog rn) - label += " (+)" - expand = False + label, expand = self._get_node_label_and_expand(node, search_term) + if expand: + self._expanded_tags.add(node.ui_tag) + elif node.ui_tag in self._expanded_tags: + self._expanded_tags.remove(node.ui_tag) with dpg.tree_node( - label=label, parent=parent_tag, tag=tag, default_open=expand, open_on_arrow=True, open_on_double_click=True, before=before, delay_search=True + label=label, parent=parent_tag, tag=node.ui_tag, default_open=expand, open_on_arrow=True, open_on_double_click=True, before=before, delay_search=True ): with dpg.item_handler_registry() as handler_tag: dpg.add_item_toggled_open_handler(callback=lambda s, a, u: self._request_children_build(node)) dpg.add_item_visible_handler(callback=lambda s, a, u: self._request_children_build(node)) - dpg.bind_item_handler_registry(tag, handler_tag) - self._item_handlers.add(handler_tag) - + dpg.bind_item_handler_registry(node.ui_tag, handler_tag) + self._item_handlers[node.ui_tag] = handler_tag node.ui_created = True def _create_leaf_ui(self, node: DataTreeNode, parent_tag: str, before: str | int): - with dpg.group(parent=parent_tag, tag=f"leaf_{node.full_path}", before=before, delay_search=True) as draggable_group: + node.ui_tag = f"leaf_{node.full_path}" + with dpg.group(parent=parent_tag, tag=node.ui_tag, before=before, delay_search=True): with dpg.table(header_row=False, policy=dpg.mvTable_SizingStretchProp, delay_search=True): dpg.add_table_column(init_width_or_weight=0.5) dpg.add_table_column(init_width_or_weight=0.5) @@ -186,25 +243,25 @@ class DataTree: if node.is_plottable is None: node.is_plottable = self.data_manager.is_plottable(node.full_path) if node.is_plottable: - with dpg.drag_payload(parent=draggable_group, drag_data=node.full_path, payload_type="TIMESERIES_PAYLOAD"): + with dpg.drag_payload(parent=node.ui_tag, drag_data=node.full_path, payload_type="TIMESERIES_PAYLOAD"): dpg.add_text(f"Plot: {node.full_path}") with dpg.item_handler_registry() as handler_tag: dpg.add_item_visible_handler(callback=self._on_item_visible, user_data=node.full_path) - dpg.bind_item_handler_registry(draggable_group, handler_tag) - self._item_handlers.add(handler_tag) - + dpg.bind_item_handler_registry(node.ui_tag, handler_tag) + self._item_handlers[node.ui_tag] = handler_tag node.ui_created = True - node.ui_tag = f"value_{node.full_path}" def _on_item_visible(self, sender, app_data, user_data): with self._ui_lock: path = user_data value_tag = f"value_{path}" - value_column_width = dpg.get_item_rect_size("sidebar_window")[0] // 2 + if not dpg.does_item_exist(value_tag): + return + value_column_width = dpg.get_item_rect_size(f"leaf_{path}")[0] // 2 value = self.data_manager.get_value_at(path, self.playback_manager.current_time_s) if value is not None: - formatted_value = self.format_and_truncate(value, value_column_width, self._avg_char_width) + formatted_value = self.format_and_truncate(value, value_column_width, self._char_width) dpg.set_value(value_tag, formatted_value) else: dpg.set_value(value_tag, "N/A") @@ -212,8 +269,7 @@ class DataTree: def _request_children_build(self, node: DataTreeNode): with self._ui_lock: if not node.children_ui_created and (node.name == "root" or (node.ui_tag is not None and dpg.get_value(node.ui_tag))): # check root or node expanded - parent_tag = "data_tree_container" if node.name == "root" else node.ui_tag - sorted_children = sorted(node.children.values(), key=self._natural_sort_key) + sorted_children = sorted(node.filtered_children.values(), key=self._natural_sort_key) next_existing: list[int | str] = [0] * len(sorted_children) current_before_tag: int | str = 0 @@ -228,7 +284,7 @@ class DataTree: for i, child_node in enumerate(sorted_children): if not child_node.ui_created: before_tag = next_existing[i] - self._build_queue.append((child_node, parent_tag, before_tag)) + self._build_queue[child_node.full_path] = (child_node, node, before_tag) node.children_ui_created = True def _should_show_path(self, path: str, search_term: str) -> bool: @@ -242,7 +298,7 @@ class DataTree: return (node_type_key, parts) def _get_descendant_paths(self, node: DataTreeNode): - for child_name, child_node in node.children.items(): + for child_name, child_node in node.filtered_children.items(): child_name_lower = child_name.lower() if child_node.is_leaf: yield child_name_lower @@ -251,16 +307,9 @@ class DataTree: yield f"{child_name_lower}/{path}" @staticmethod - def calculate_avg_char_width(font): - sample_text = "abcdefghijklmnopqrstuvwxyz0123456789" - if size := dpg.get_text_size(sample_text, font=font): - return size[0] / len(sample_text) - return None - - @staticmethod - def format_and_truncate(value, available_width: float, avg_char_width: float) -> str: + def format_and_truncate(value, available_width: float, char_width: float) -> str: s = f"{value:.5f}" if np.issubdtype(type(value), np.floating) else str(value) - max_chars = int(available_width / avg_char_width) - 3 + max_chars = int(available_width / char_width) if len(s) > max_chars: - return s[: max(0, max_chars)] + "..." + return s[: max(0, max_chars - 3)] + "..." return s diff --git a/tools/jotpluggler/layout.py b/tools/jotpluggler/layout.py index 0c40116e66..13fbee54e2 100644 --- a/tools/jotpluggler/layout.py +++ b/tools/jotpluggler/layout.py @@ -5,15 +5,156 @@ from openpilot.tools.jotpluggler.views import TimeSeriesPanel GRIP_SIZE = 4 MIN_PANE_SIZE = 60 +class LayoutManager: + def __init__(self, data_manager, playback_manager, worker_manager, scale: float = 1.0): + self.data_manager = data_manager + self.playback_manager = playback_manager + self.worker_manager = worker_manager + self.scale = scale + self.container_tag = "plot_layout_container" + self.tab_bar_tag = "tab_bar_container" + self.tab_content_tag = "tab_content_area" + + self.active_tab = 0 + initial_panel_layout = PanelLayoutManager(data_manager, playback_manager, worker_manager, scale) + self.tabs: dict = {0: {"name": "Tab 1", "panel_layout": initial_panel_layout}} + self._next_tab_id = self.active_tab + 1 + + def to_dict(self) -> dict: + return { + "tabs": { + str(tab_id): { + "name": tab_data["name"], + "panel_layout": tab_data["panel_layout"].to_dict() + } + for tab_id, tab_data in self.tabs.items() + } + } + + def clear_and_load_from_dict(self, data: dict): + tab_ids_to_close = list(self.tabs.keys()) + for tab_id in tab_ids_to_close: + self.close_tab(tab_id, force=True) + + for tab_id_str, tab_data in data["tabs"].items(): + tab_id = int(tab_id_str) + panel_layout = PanelLayoutManager.load_from_dict( + tab_data["panel_layout"], self.data_manager, self.playback_manager, + self.worker_manager, self.scale + ) + self.tabs[tab_id] = { + "name": tab_data["name"], + "panel_layout": panel_layout + } + + self.active_tab = min(self.tabs.keys()) if self.tabs else 0 + self._next_tab_id = max(self.tabs.keys()) + 1 if self.tabs else 1 + + def create_ui(self, parent_tag: str): + if dpg.does_item_exist(self.container_tag): + dpg.delete_item(self.container_tag) + + with dpg.child_window(tag=self.container_tag, parent=parent_tag, border=False, width=-1, height=-1, no_scrollbar=True, no_scroll_with_mouse=True): + self._create_tab_bar() + self._create_tab_content() + dpg.bind_item_theme(self.tab_bar_tag, "tab_bar_theme") + + def _create_tab_bar(self): + text_size = int(13 * self.scale) + with dpg.child_window(tag=self.tab_bar_tag, parent=self.container_tag, height=(text_size + 8), border=False, horizontal_scrollbar=True): + with dpg.group(horizontal=True, tag="tab_bar_group"): + for tab_id, tab_data in self.tabs.items(): + self._create_tab_ui(tab_id, tab_data["name"]) + dpg.add_image_button(texture_tag="plus_texture", callback=self.add_tab, width=text_size, height=text_size, tag="add_tab_button") + dpg.bind_item_theme("add_tab_button", "inactive_tab_theme") + + def _create_tab_ui(self, tab_id: int, tab_name: str): + text_size = int(13 * self.scale) + tab_width = int(140 * self.scale) + with dpg.child_window(width=tab_width, height=-1, border=False, no_scrollbar=True, tag=f"tab_window_{tab_id}", parent="tab_bar_group"): + with dpg.group(horizontal=True, tag=f"tab_group_{tab_id}"): + dpg.add_input_text( + default_value=tab_name, width=tab_width - text_size - 16, callback=lambda s, v, u: self.rename_tab(u, v), user_data=tab_id, tag=f"tab_input_{tab_id}" + ) + dpg.add_image_button( + texture_tag="x_texture", callback=lambda s, a, u: self.close_tab(u), user_data=tab_id, width=text_size, height=text_size, tag=f"tab_close_{tab_id}" + ) + with dpg.item_handler_registry(tag=f"tab_handler_{tab_id}"): + dpg.add_item_clicked_handler(callback=lambda s, a, u: self.switch_tab(u), user_data=tab_id) + dpg.bind_item_handler_registry(f"tab_group_{tab_id}", f"tab_handler_{tab_id}") + + theme_tag = "active_tab_theme" if tab_id == self.active_tab else "inactive_tab_theme" + dpg.bind_item_theme(f"tab_window_{tab_id}", theme_tag) + + def _create_tab_content(self): + with dpg.child_window(tag=self.tab_content_tag, parent=self.container_tag, border=False, width=-1, height=-1, no_scrollbar=True, no_scroll_with_mouse=True): + if self.active_tab in self.tabs: + active_panel_layout = self.tabs[self.active_tab]["panel_layout"] + active_panel_layout.create_ui() + + def add_tab(self): + new_panel_layout = PanelLayoutManager(self.data_manager, self.playback_manager, self.worker_manager, self.scale) + new_tab = {"name": f"Tab {self._next_tab_id + 1}", "panel_layout": new_panel_layout} + self.tabs[self._next_tab_id] = new_tab + self._create_tab_ui(self._next_tab_id, new_tab["name"]) + dpg.move_item("add_tab_button", parent="tab_bar_group") # move plus button to end + self.switch_tab(self._next_tab_id) + self._next_tab_id += 1 + + def close_tab(self, tab_id: int, force = False): + if len(self.tabs) <= 1 and not force: + return # don't allow closing the last tab + + tab_to_close = self.tabs[tab_id] + tab_to_close["panel_layout"].destroy_ui() + for suffix in ["window", "group", "input", "close", "handler"]: + tag = f"tab_{suffix}_{tab_id}" + if dpg.does_item_exist(tag): + dpg.delete_item(tag) + del self.tabs[tab_id] + + if self.active_tab == tab_id and self.tabs: # switch to another tab if we closed the active one + self.active_tab = next(iter(self.tabs.keys())) + self._switch_tab_content() + dpg.bind_item_theme(f"tab_window_{self.active_tab}", "active_tab_theme") + + def switch_tab(self, tab_id: int): + if tab_id == self.active_tab or tab_id not in self.tabs: + return + + current_panel_layout = self.tabs[self.active_tab]["panel_layout"] + current_panel_layout.destroy_ui() + dpg.bind_item_theme(f"tab_window_{self.active_tab}", "inactive_tab_theme") # deactivate old tab + self.active_tab = tab_id + dpg.bind_item_theme(f"tab_window_{tab_id}", "active_tab_theme") # activate new tab + self._switch_tab_content() + + def _switch_tab_content(self): + dpg.delete_item(self.tab_content_tag, children_only=True) + active_panel_layout = self.tabs[self.active_tab]["panel_layout"] + active_panel_layout.create_ui() + active_panel_layout.update_all_panels() + + def rename_tab(self, tab_id: int, new_name: str): + if tab_id in self.tabs: + self.tabs[tab_id]["name"] = new_name + + def update_all_panels(self): + self.tabs[self.active_tab]["panel_layout"].update_all_panels() -class PlotLayoutManager: + def on_viewport_resize(self): + self.tabs[self.active_tab]["panel_layout"].on_viewport_resize() + +class PanelLayoutManager: def __init__(self, data_manager: DataManager, playback_manager, worker_manager, scale: float = 1.0): self.data_manager = data_manager self.playback_manager = playback_manager self.worker_manager = worker_manager self.scale = scale - self.container_tag = "plot_layout_container" self.active_panels: list = [] + self.parent_tag = "tab_content_area" + self._queue_resize = False + self._created_handler_tags: set[str] = set() self.grip_size = int(GRIP_SIZE * self.scale) self.min_pane_size = int(MIN_PANE_SIZE * self.scale) @@ -21,33 +162,94 @@ class PlotLayoutManager: initial_panel = TimeSeriesPanel(data_manager, playback_manager, worker_manager) self.layout: dict = {"type": "panel", "panel": initial_panel} - def create_ui(self, parent_tag: str): - if dpg.does_item_exist(self.container_tag): - dpg.delete_item(self.container_tag) + def to_dict(self) -> dict: + return self._layout_to_dict(self.layout) - with dpg.child_window(tag=self.container_tag, parent=parent_tag, border=False, width=-1, height=-1, no_scrollbar=True): - container_width, container_height = dpg.get_item_rect_size(self.container_tag) - self._create_ui_recursive(self.layout, self.container_tag, [], container_width, container_height) + def _layout_to_dict(self, layout: dict) -> dict: + if layout["type"] == "panel": + return { + "type": "panel", + "panel": layout["panel"].to_dict() + } + else: # split + return { + "type": "split", + "orientation": layout["orientation"], + "proportions": layout["proportions"], + "children": [self._layout_to_dict(child) for child in layout["children"]] + } + + @classmethod + def load_from_dict(cls, data: dict, data_manager, playback_manager, worker_manager, scale: float = 1.0): + manager = cls(data_manager, playback_manager, worker_manager, scale) + manager.layout = manager._dict_to_layout(data) + return manager + + def _dict_to_layout(self, data: dict) -> dict: + if data["type"] == "panel": + panel_data = data["panel"] + if panel_data["type"] == "timeseries": + panel = TimeSeriesPanel.load_from_dict( + panel_data, self.data_manager, self.playback_manager, self.worker_manager + ) + return {"type": "panel", "panel": panel} + else: + # Handle future panel types here or make a general mapping + raise ValueError(f"Unknown panel type: {panel_data['type']}") + else: # split + return { + "type": "split", + "orientation": data["orientation"], + "proportions": data["proportions"], + "children": [self._dict_to_layout(child) for child in data["children"]] + } + + def create_ui(self): + self.active_panels.clear() + if dpg.does_item_exist(self.parent_tag): + dpg.delete_item(self.parent_tag, children_only=True) + self._cleanup_all_handlers() + + container_width, container_height = dpg.get_item_rect_size(self.parent_tag) + if container_width == 0 and container_height == 0: + self._queue_resize = True + self._create_ui_recursive(self.layout, self.parent_tag, [], container_width, container_height) + + def destroy_ui(self): + self._cleanup_ui_recursive(self.layout, []) + self._cleanup_all_handlers() + self.active_panels.clear() + + def _cleanup_all_handlers(self): + for handler_tag in list(self._created_handler_tags): + if dpg.does_item_exist(handler_tag): + dpg.delete_item(handler_tag) + self._created_handler_tags.clear() def _create_ui_recursive(self, layout: dict, parent_tag: str, path: list[int], width: int, height: int): if layout["type"] == "panel": - self._create_panel_ui(layout, parent_tag, path) + self._create_panel_ui(layout, parent_tag, path, width, height) else: self._create_split_ui(layout, parent_tag, path, width, height) - def _create_panel_ui(self, layout: dict, parent_tag: str, path: list[int]): + def _create_panel_ui(self, layout: dict, parent_tag: str, path: list[int], width: int, height: int): panel_tag = self._path_to_tag(path, "panel") panel = layout["panel"] self.active_panels.append(panel) + text_size = int(13 * self.scale) + bar_height = (text_size + 24) if width < int(329 * self.scale + 64) else (text_size + 8) # adjust height to allow for scrollbar - with dpg.child_window(tag=panel_tag, parent=parent_tag, border=True, width=-1, height=-1, no_scrollbar=True): + with dpg.child_window(parent=parent_tag, border=False, width=-1, height=-1, no_scrollbar=True): with dpg.group(horizontal=True): - dpg.add_input_text(default_value=panel.title, width=int(100 * self.scale), callback=lambda s, v: setattr(panel, "title", v)) - dpg.add_combo(items=["Time Series"], default_value="Time Series", width=int(100 * self.scale)) - dpg.add_button(label="Clear", callback=lambda: self.clear_panel(panel), width=int(40 * self.scale)) - dpg.add_button(label="Delete", callback=lambda: self.delete_panel(path), width=int(40 * self.scale)) - dpg.add_button(label="Split H", callback=lambda: self.split_panel(path, 0), width=int(40 * self.scale)) - dpg.add_button(label="Split V", callback=lambda: self.split_panel(path, 1), width=int(40 * self.scale)) + with dpg.child_window(tag=panel_tag, width=-(text_size + 16), height=bar_height, horizontal_scrollbar=True, no_scroll_with_mouse=True, border=False): + with dpg.group(horizontal=True): + # if you change the widths make sure to change the sum of widths (currently 329 * scale) + dpg.add_input_text(default_value=panel.title, width=int(150 * self.scale), callback=lambda s, v: setattr(panel, "title", v)) + dpg.add_combo(items=["Time Series"], default_value="Time Series", width=int(100 * self.scale)) + dpg.add_button(label="Clear", callback=lambda: self.clear_panel(panel), width=int(40 * self.scale)) + dpg.add_image_button(texture_tag="split_h_texture", callback=lambda: self.split_panel(path, 0), width=text_size, height=text_size) + dpg.add_image_button(texture_tag="split_v_texture", callback=lambda: self.split_panel(path, 1), width=text_size, height=text_size) + dpg.add_image_button(texture_tag="x_texture", callback=lambda: self.delete_panel(path), width=text_size, height=text_size) dpg.add_separator() @@ -63,7 +265,7 @@ class PlotLayoutManager: for i, child_layout in enumerate(layout["children"]): child_path = path + [i] container_tag = self._path_to_tag(child_path, "container") - pane_width, pane_height = [(pane_sizes[i], -1), (-1, pane_sizes[i])][orientation] # fill 2nd dim up to the border + pane_width, pane_height = [(pane_sizes[i], -1), (-1, pane_sizes[i])][orientation] # fill 2nd dim up to the border with dpg.child_window(tag=container_tag, width=pane_width, height=pane_height, border=False, no_scrollbar=True): child_width, child_height = [(pane_sizes[i], height), (width, pane_sizes[i])][orientation] self._create_ui_recursive(child_layout, container_tag, child_path, child_width, child_height) @@ -133,7 +335,7 @@ class PlotLayoutManager: if path: container_tag = self._path_to_tag(path, "container") else: # Root update - container_tag = self.container_tag + container_tag = self.parent_tag self._cleanup_ui_recursive(layout, path) dpg.delete_item(container_tag, children_only=True) @@ -151,11 +353,16 @@ class PlotLayoutManager: handler_tag = f"{self._path_to_tag(path, f'grip_{i}')}_handler" if dpg.does_item_exist(handler_tag): dpg.delete_item(handler_tag) + self._created_handler_tags.discard(handler_tag) for i, child in enumerate(layout["children"]): self._cleanup_ui_recursive(child, path + [i]) def update_all_panels(self): + if self._queue_resize: + if (size := dpg.get_item_rect_size(self.parent_tag)) != [0, 0]: + self._queue_resize = False + self._resize_splits_recursive(self.layout, [], *size) for panel in self.active_panels: panel.update() @@ -177,11 +384,17 @@ class PlotLayoutManager: dpg.configure_item(container_tag, **{size_properties[orientation]: pane_sizes[i]}) child_width, child_height = [(pane_sizes[i], available_sizes[1]), (available_sizes[0], pane_sizes[i])][orientation] self._resize_splits_recursive(child_layout, child_path, child_width, child_height) + else: # leaf node/panel - adjust bar height to allow for scrollbar + panel_tag = self._path_to_tag(path, "panel") + if width is not None and width < int(329 * self.scale + 64): # scaled widths of the elements in top bar + fixed 8 padding on left and right of each item + dpg.configure_item(panel_tag, height=(int(13 * self.scale) + 24)) + else: + dpg.configure_item(panel_tag, height=(int(13 * self.scale) + 8)) def _get_split_geometry(self, layout: dict, available_size: tuple[int, int]) -> tuple[int, int, list[int]]: orientation = layout["orientation"] num_grips = len(layout["children"]) - 1 - usable_size = max(self.min_pane_size, available_size[orientation] - (num_grips * self.grip_size)) + usable_size = max(self.min_pane_size, available_size[orientation] - (num_grips * (self.grip_size + 8 * (2 - orientation)))) # approximate, scaling is weird pane_sizes = [max(self.min_pane_size, int(usable_size * prop)) for prop in layout["proportions"]] return orientation, usable_size, pane_sizes @@ -207,16 +420,18 @@ class PlotLayoutManager: def _create_grip(self, parent_tag: str, path: list[int], grip_index: int, orientation: int): grip_tag = self._path_to_tag(path, f"grip_{grip_index}") + handler_tag = f"{grip_tag}_handler" width, height = [(self.grip_size, -1), (-1, self.grip_size)][orientation] with dpg.child_window(tag=grip_tag, parent=parent_tag, width=width, height=height, no_scrollbar=True, border=False): button_tag = dpg.add_button(label="", width=-1, height=-1) - with dpg.item_handler_registry(tag=f"{grip_tag}_handler"): + with dpg.item_handler_registry(tag=handler_tag): user_data = (path, grip_index, orientation) dpg.add_item_active_handler(callback=self._on_grip_drag, user_data=user_data) dpg.add_item_deactivated_handler(callback=self._on_grip_end, user_data=user_data) - dpg.bind_item_handler_registry(button_tag, f"{grip_tag}_handler") + dpg.bind_item_handler_registry(button_tag, handler_tag) + self._created_handler_tags.add(handler_tag) def _on_grip_drag(self, sender, app_data, user_data): path, grip_index, orientation = user_data diff --git a/tools/jotpluggler/layouts/torque-controller.yaml b/tools/jotpluggler/layouts/torque-controller.yaml new file mode 100644 index 0000000000..5503be9e64 --- /dev/null +++ b/tools/jotpluggler/layouts/torque-controller.yaml @@ -0,0 +1,128 @@ +tabs: + '0': + name: Lateral Plan Conformance + panel_layout: + type: split + orientation: 1 + proportions: + - 0.3333333333333333 + - 0.3333333333333333 + - 0.3333333333333333 + children: + - type: panel + panel: + type: timeseries + title: desired vs actual + series_paths: + - controlsState/lateralControlState/torqueState/desiredLateralAccel + - controlsState/lateralControlState/torqueState/actualLateralAccel + - type: panel + panel: + type: timeseries + title: ff vs output + series_paths: + - controlsState/lateralControlState/torqueState/f + - carState/steeringPressed + - carControl/actuators/torque + - type: panel + panel: + type: timeseries + title: vehicle speed + series_paths: + - carState/vEgo + '1': + name: Actuator Performance + panel_layout: + type: split + orientation: 1 + proportions: + - 0.3333333333333333 + - 0.3333333333333333 + - 0.3333333333333333 + children: + - type: panel + panel: + type: timeseries + title: calc vs learned latAccelFactor + series_paths: + - liveTorqueParameters/latAccelFactorFiltered + - liveTorqueParameters/latAccelFactorRaw + - carParams/lateralTuning/torque/latAccelFactor + - type: panel + panel: + type: timeseries + title: learned latAccelOffset + series_paths: + - liveTorqueParameters/latAccelOffsetRaw + - liveTorqueParameters/latAccelOffsetFiltered + - type: panel + panel: + type: timeseries + title: calc vs learned friction + series_paths: + - liveTorqueParameters/frictionCoefficientFiltered + - liveTorqueParameters/frictionCoefficientRaw + - carParams/lateralTuning/torque/friction + '2': + name: Vehicle Dynamics + panel_layout: + type: split + orientation: 1 + proportions: + - 0.3333333333333333 + - 0.3333333333333333 + - 0.3333333333333333 + children: + - type: panel + panel: + type: timeseries + title: initial vs learned steerRatio + series_paths: + - carParams/steerRatio + - liveParameters/steerRatio + - type: panel + panel: + type: timeseries + title: initial vs learned tireStiffnessFactor + series_paths: + - carParams/tireStiffnessFactor + - liveParameters/stiffnessFactor + - type: panel + panel: + type: timeseries + title: live steering angle offsets + series_paths: + - liveParameters/angleOffsetDeg + - liveParameters/angleOffsetAverageDeg + '3': + name: Controller PIF Terms + panel_layout: + type: split + orientation: 1 + proportions: + - 0.3333333333333333 + - 0.3333333333333333 + - 0.3333333333333333 + children: + - type: panel + panel: + type: timeseries + title: ff vs output + series_paths: + - carControl/actuators/torque + - controlsState/lateralControlState/torqueState/f + - carState/steeringPressed + - type: panel + panel: + type: timeseries + title: PIF terms + series_paths: + - controlsState/lateralControlState/torqueState/f + - controlsState/lateralControlState/torqueState/p + - controlsState/lateralControlState/torqueState/i + - type: panel + panel: + type: timeseries + title: road roll angle + series_paths: + - liveParameters/roll diff --git a/tools/jotpluggler/pluggle.py b/tools/jotpluggler/pluggle.py index 9868b998ed..879b677514 100755 --- a/tools/jotpluggler/pluggle.py +++ b/tools/jotpluggler/pluggle.py @@ -7,10 +7,12 @@ import dearpygui.dearpygui as dpg import multiprocessing import uuid import signal +import yaml # type: ignore +from openpilot.common.swaglog import cloudlog from openpilot.common.basedir import BASEDIR from openpilot.tools.jotpluggler.data import DataManager from openpilot.tools.jotpluggler.datatree import DataTree -from openpilot.tools.jotpluggler.layout import PlotLayoutManager +from openpilot.tools.jotpluggler.layout import LayoutManager DEMO_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19" @@ -64,6 +66,11 @@ class PlaybackManager: self.is_playing = False self.current_time_s = 0.0 self.duration_s = 0.0 + self.num_segments = 0 + + self.x_axis_bounds = (0.0, 0.0) # (min_time, max_time) + self.x_axis_observers = [] # callbacks for x-axis changes + self._updating_x_axis = False def set_route_duration(self, duration: float): self.duration_s = duration @@ -73,9 +80,10 @@ class PlaybackManager: if not self.is_playing and self.current_time_s >= self.duration_s: self.seek(0.0) self.is_playing = not self.is_playing + texture_tag = "pause_texture" if self.is_playing else "play_texture" + dpg.configure_item("play_pause_button", texture_tag=texture_tag) def seek(self, time_s: float): - self.is_playing = False self.current_time_s = max(0.0, min(time_s, self.duration_s)) def update_time(self, delta_t: float): @@ -83,8 +91,36 @@ class PlaybackManager: self.current_time_s = min(self.current_time_s + delta_t, self.duration_s) if self.current_time_s >= self.duration_s: self.is_playing = False + dpg.configure_item("play_pause_button", texture_tag="play_texture") return self.current_time_s + def set_x_axis_bounds(self, min_time: float, max_time: float, source_panel=None): + if self._updating_x_axis: + return + + new_bounds = (min_time, max_time) + if new_bounds == self.x_axis_bounds: + return + + self.x_axis_bounds = new_bounds + self._updating_x_axis = True # prevent recursive updates + + try: + for callback in self.x_axis_observers: + try: + callback(min_time, max_time, source_panel) + except Exception as e: + print(f"Error in x-axis sync callback: {e}") + finally: + self._updating_x_axis = False + + def add_x_axis_observer(self, callback): + if callback not in self.x_axis_observers: + self.x_axis_observers.append(callback) + + def remove_x_axis_observer(self, callback): + if callback in self.x_axis_observers: + self.x_axis_observers.remove(callback) class MainController: def __init__(self, scale: float = 1.0): @@ -94,34 +130,49 @@ class MainController: self.worker_manager = WorkerManager() self._create_global_themes() self.data_tree = DataTree(self.data_manager, self.playback_manager) - self.plot_layout_manager = PlotLayoutManager(self.data_manager, self.playback_manager, self.worker_manager, scale=self.scale) + self.layout_manager = LayoutManager(self.data_manager, self.playback_manager, self.worker_manager, scale=self.scale) self.data_manager.add_observer(self.on_data_loaded) + self._total_segments = 0 def _create_global_themes(self): - with dpg.theme(tag="global_line_theme"): + with dpg.theme(tag="line_theme"): with dpg.theme_component(dpg.mvLineSeries): scaled_thickness = max(1.0, self.scale) dpg.add_theme_style(dpg.mvPlotStyleVar_LineWeight, scaled_thickness, category=dpg.mvThemeCat_Plots) - with dpg.theme(tag="global_timeline_theme"): + with dpg.theme(tag="timeline_theme"): with dpg.theme_component(dpg.mvInfLineSeries): scaled_thickness = max(1.0, self.scale) dpg.add_theme_style(dpg.mvPlotStyleVar_LineWeight, scaled_thickness, category=dpg.mvThemeCat_Plots) dpg.add_theme_color(dpg.mvPlotCol_Line, (255, 0, 0, 128), category=dpg.mvThemeCat_Plots) + for tag, color in (("active_tab_theme", (37, 37, 38, 255)), ("inactive_tab_theme", (70, 70, 75, 255))): + with dpg.theme(tag=tag): + for cmp, target in ((dpg.mvChildWindow, dpg.mvThemeCol_ChildBg), (dpg.mvInputText, dpg.mvThemeCol_FrameBg), (dpg.mvImageButton, dpg.mvThemeCol_Button)): + with dpg.theme_component(cmp): + dpg.add_theme_color(target, color) + + with dpg.theme(tag="tab_bar_theme"): + with dpg.theme_component(dpg.mvChildWindow): + dpg.add_theme_color(dpg.mvThemeCol_ChildBg, (51, 51, 55, 255)) def on_data_loaded(self, data: dict): duration = data.get('duration', 0.0) self.playback_manager.set_route_duration(duration) - if data.get('reset'): + if data.get('metadata_loaded'): + self.playback_manager.num_segments = data.get('total_segments', 0) + self._total_segments = data.get('total_segments', 0) + dpg.set_value("load_status", f"Loading... 0/{self._total_segments} segments processed") + elif data.get('reset'): self.playback_manager.current_time_s = 0.0 self.playback_manager.duration_s = 0.0 self.playback_manager.is_playing = False + self._total_segments = 0 dpg.set_value("load_status", "Loading...") dpg.set_value("timeline_slider", 0.0) dpg.configure_item("timeline_slider", max_value=0.0) - dpg.configure_item("play_pause_button", label="Play") + dpg.configure_item("play_pause_button", texture_tag="play_texture") dpg.configure_item("load_button", enabled=True) elif data.get('loading_complete'): num_paths = len(self.data_manager.get_all_paths()) @@ -129,34 +180,99 @@ class MainController: dpg.configure_item("load_button", enabled=True) elif data.get('segment_added'): segment_count = data.get('segment_count', 0) - dpg.set_value("load_status", f"Loading... {segment_count} segments processed") + dpg.set_value("load_status", f"Loading... {segment_count}/{self._total_segments} segments processed") dpg.configure_item("timeline_slider", max_value=duration) + def save_layout_to_yaml(self, filepath: str): + layout_dict = self.layout_manager.to_dict() + with open(filepath, 'w') as f: + yaml.dump(layout_dict, f, default_flow_style=False, sort_keys=False) + + def load_layout_from_yaml(self, filepath: str): + with open(filepath) as f: + layout_dict = yaml.safe_load(f) + self.layout_manager.clear_and_load_from_dict(layout_dict) + self.layout_manager.create_ui("main_plot_area") + + def save_layout_dialog(self): + if dpg.does_item_exist("save_layout_dialog"): + dpg.delete_item("save_layout_dialog") + with dpg.file_dialog( + callback=self._save_layout_callback, tag="save_layout_dialog", width=int(700 * self.scale), height=int(400 * self.scale), + default_filename="layout", default_path=os.path.join(os.path.dirname(os.path.realpath(__file__)), "layouts") + ): + dpg.add_file_extension(".yaml") + + def load_layout_dialog(self): + if dpg.does_item_exist("load_layout_dialog"): + dpg.delete_item("load_layout_dialog") + with dpg.file_dialog( + callback=self._load_layout_callback, tag="load_layout_dialog", width=int(700 * self.scale), height=int(400 * self.scale), + default_path=os.path.join(os.path.dirname(os.path.realpath(__file__)), "layouts") + ): + dpg.add_file_extension(".yaml") + + def _save_layout_callback(self, sender, app_data): + filepath = app_data['file_path_name'] + try: + self.save_layout_to_yaml(filepath) + dpg.set_value("load_status", f"Layout saved to {os.path.basename(filepath)}") + except Exception: + dpg.set_value("load_status", "Error saving layout") + cloudlog.exception(f"Error saving layout to {filepath}") + dpg.delete_item("save_layout_dialog") + + def _load_layout_callback(self, sender, app_data): + filepath = app_data['file_path_name'] + try: + self.load_layout_from_yaml(filepath) + dpg.set_value("load_status", f"Layout loaded from {os.path.basename(filepath)}") + except Exception: + dpg.set_value("load_status", "Error loading layout") + cloudlog.exception(f"Error loading layout from {filepath}:") + dpg.delete_item("load_layout_dialog") + def setup_ui(self): + with dpg.texture_registry(): + script_dir = os.path.dirname(os.path.realpath(__file__)) + for image in ["play", "pause", "x", "split_h", "split_v", "plus"]: + texture = dpg.load_image(os.path.join(script_dir, "assets", f"{image}.png")) + dpg.add_static_texture(width=texture[0], height=texture[1], default_value=texture[3], tag=f"{image}_texture") + with dpg.window(tag="Primary Window"): with dpg.group(horizontal=True): # Left panel - Data tree - with dpg.child_window(label="Sidebar", width=300 * self.scale, tag="sidebar_window", border=True, resizable_x=True): + with dpg.child_window(label="Sidebar", width=int(300 * self.scale), tag="sidebar_window", border=True, resizable_x=True): with dpg.group(horizontal=True): - dpg.add_input_text(tag="route_input", width=-75 * self.scale, hint="Enter route name...") + dpg.add_input_text(tag="route_input", width=int(-75 * self.scale), hint="Enter route name...") dpg.add_button(label="Load", callback=self.load_route, tag="load_button", width=-1) dpg.add_text("Ready to load route", tag="load_status") dpg.add_separator() + + with dpg.table(header_row=False, policy=dpg.mvTable_SizingStretchProp): + dpg.add_table_column(init_width_or_weight=0.5) + dpg.add_table_column(init_width_or_weight=0.5) + with dpg.table_row(): + dpg.add_button(label="Save Layout", callback=self.save_layout_dialog, width=-1) + dpg.add_button(label="Load Layout", callback=self.load_layout_dialog, width=-1) + dpg.add_separator() + self.data_tree.create_ui("sidebar_window") # Right panel - Plots and timeline with dpg.group(tag="right_panel"): - with dpg.child_window(label="Plot Window", border=True, height=-(30 + 13 * self.scale), tag="main_plot_area"): - self.plot_layout_manager.create_ui("main_plot_area") + with dpg.child_window(label="Plot Window", border=True, height=int(-(32 + 13 * self.scale)), tag="main_plot_area"): + self.layout_manager.create_ui("main_plot_area") with dpg.child_window(label="Timeline", border=True): - with dpg.table(header_row=False, borders_innerH=False, borders_innerV=False, borders_outerH=False, borders_outerV=False): - dpg.add_table_column(width_fixed=True, init_width_or_weight=int(50 * self.scale)) # Play button + with dpg.table(header_row=False): + btn_size = int(13 * self.scale) + dpg.add_table_column(width_fixed=True, init_width_or_weight=(btn_size + 8)) # Play button dpg.add_table_column(width_stretch=True) # Timeline slider dpg.add_table_column(width_fixed=True, init_width_or_weight=int(50 * self.scale)) # FPS counter with dpg.table_row(): - dpg.add_button(label="Play", tag="play_pause_button", callback=self.toggle_play_pause, width=int(50 * self.scale)) + dpg.add_image_button(texture_tag="play_texture", tag="play_pause_button", callback=self.toggle_play_pause, width=btn_size, height=btn_size) dpg.add_slider_float(tag="timeline_slider", default_value=0.0, label="", width=-1, callback=self.timeline_drag) dpg.add_text("", tag="fps_counter") with dpg.item_handler_registry(tag="plot_resize_handler"): @@ -166,7 +282,7 @@ class MainController: dpg.set_primary_window("Primary Window", True) def on_plot_resize(self, sender, app_data, user_data): - self.plot_layout_manager.on_viewport_resize() + self.layout_manager.on_viewport_resize() def load_route(self): route_name = dpg.get_value("route_input").strip() @@ -177,12 +293,9 @@ class MainController: def toggle_play_pause(self, sender): self.playback_manager.toggle_play_pause() - label = "Pause" if self.playback_manager.is_playing else "Play" - dpg.configure_item(sender, label=label) def timeline_drag(self, sender, app_data): self.playback_manager.seek(app_data) - dpg.configure_item("play_pause_button", label="Play") def update_frame(self, font): self.data_tree.update_frame(font) @@ -191,7 +304,7 @@ class MainController: if not dpg.is_item_active("timeline_slider"): dpg.set_value("timeline_slider", new_time) - self.plot_layout_manager.update_all_panels() + self.layout_manager.update_all_panels() dpg.set_value("fps_counter", f"{dpg.get_frame_rate():.1f} FPS") @@ -199,7 +312,7 @@ class MainController: self.worker_manager.shutdown() -def main(route_to_load=None): +def main(route_to_load=None, layout_to_load=None): dpg.create_context() # TODO: find better way of calculating display scaling @@ -210,8 +323,9 @@ def main(route_to_load=None): scale = 1 with dpg.font_registry(): - default_font = dpg.add_font(os.path.join(BASEDIR, "selfdrive/assets/fonts/Inter-Regular.ttf"), int(13 * scale)) + default_font = dpg.add_font(os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf"), int(13 * scale * 2)) # 2x then scale for hidpi dpg.bind_font(default_font) + dpg.set_global_font_scale(0.5) viewport_width, viewport_height = int(1200 * scale), int(800 * scale) mouse_x, mouse_y = pyautogui.position() # TODO: find better way of creating the window where the user is (default dpg behavior annoying on multiple displays) @@ -223,6 +337,14 @@ def main(route_to_load=None): controller = MainController(scale=scale) controller.setup_ui() + if layout_to_load: + try: + controller.load_layout_from_yaml(layout_to_load) + print(f"Loaded layout from {layout_to_load}") + except Exception as e: + print(f"Failed to load layout from {layout_to_load}: {e}") + cloudlog.exception(f"Error loading layout from {layout_to_load}") + if route_to_load: dpg.set_value("route_input", route_to_load) controller.load_route() @@ -241,7 +363,8 @@ def main(route_to_load=None): if __name__ == "__main__": parser = argparse.ArgumentParser(description="A tool for visualizing openpilot logs.") parser.add_argument("--demo", action="store_true", help="Use the demo route instead of providing one") + parser.add_argument("--layout", type=str, help="Path to YAML layout file to load on startup") parser.add_argument("route", nargs='?', default=None, help="Optional route name to load on startup.") args = parser.parse_args() route = DEMO_ROUTE if args.demo else args.route - main(route_to_load=route) + main(route_to_load=route, layout_to_load=args.layout) diff --git a/tools/jotpluggler/views.py b/tools/jotpluggler/views.py index bcadd4f387..1c4d9a8f3c 100644 --- a/tools/jotpluggler/views.py +++ b/tools/jotpluggler/views.py @@ -33,6 +33,15 @@ class ViewPanel(ABC): def update(self): pass + @abstractmethod + def to_dict(self) -> dict: + pass + + @classmethod + @abstractmethod + def load_from_dict(cls, data: dict, data_manager, playback_manager, worker_manager): + pass + class TimeSeriesPanel(ViewPanel): def __init__(self, data_manager, playback_manager, worker_manager, panel_id: str | None = None): @@ -46,23 +55,42 @@ class TimeSeriesPanel(ViewPanel): self.y_axis_tag = f"{self.plot_tag}_y_axis" self.timeline_indicator_tag = f"{self.plot_tag}_timeline" self._ui_created = False - self._series_data: dict[str, tuple[list, list]] = {} + self._series_data: dict[str, tuple[np.ndarray, np.ndarray]] = {} self._last_plot_duration = 0 self._update_lock = threading.RLock() - self.results_deque: deque[tuple[str, list, list]] = deque() + self._results_deque: deque[tuple[str, list, list]] = deque() self._new_data = False + self._last_x_limits = (0.0, 0.0) + self._queued_x_sync: tuple | None = None + self._queued_reallow_x_zoom = False + self._total_segments = self.playback_manager.num_segments + + def to_dict(self) -> dict: + return { + "type": "timeseries", + "title": self.title, + "series_paths": list(self._series_data.keys()) + } + + @classmethod + def load_from_dict(cls, data: dict, data_manager, playback_manager, worker_manager): + panel = cls(data_manager, playback_manager, worker_manager) + panel.title = data.get("title", "Time Series Plot") + panel._series_data = {path: (np.array([]), np.array([])) for path in data.get("series_paths", [])} + return panel def create_ui(self, parent_tag: str): self.data_manager.add_observer(self.on_data_loaded) + self.playback_manager.add_x_axis_observer(self._on_x_axis_sync) with dpg.plot(height=-1, width=-1, tag=self.plot_tag, parent=parent_tag, drop_callback=self._on_series_drop, payload_type="TIMESERIES_PAYLOAD"): dpg.add_plot_legend() dpg.add_plot_axis(dpg.mvXAxis, no_label=True, tag=self.x_axis_tag) dpg.add_plot_axis(dpg.mvYAxis, no_label=True, tag=self.y_axis_tag) timeline_series_tag = dpg.add_inf_line_series(x=[0], label="Timeline", parent=self.y_axis_tag, tag=self.timeline_indicator_tag) - dpg.bind_item_theme(timeline_series_tag, "global_timeline_theme") + dpg.bind_item_theme(timeline_series_tag, "timeline_theme") - for series_path in list(self._series_data.keys()): - self.add_series(series_path) + self._new_data = True + self._queued_x_sync = self.playback_manager.x_axis_bounds self._ui_created = True def update(self): @@ -70,17 +98,48 @@ class TimeSeriesPanel(ViewPanel): if not self._ui_created: return + if self._queued_x_sync: + min_time, max_time = self._queued_x_sync + self._queued_x_sync = None + dpg.set_axis_limits(self.x_axis_tag, min_time, max_time) + self._last_x_limits = (min_time, max_time) + self._fit_y_axis(min_time, max_time) + self._queued_reallow_x_zoom = True # must wait a frame before allowing user changes so that axis limits take effect + return + + if self._queued_reallow_x_zoom: + self._queued_reallow_x_zoom = False + if tuple(dpg.get_axis_limits(self.x_axis_tag)) == self._last_x_limits: + dpg.set_axis_limits_auto(self.x_axis_tag) + else: + self._queued_x_sync = self._last_x_limits # retry, likely too early + return + if self._new_data: # handle new data in main thread self._new_data = False + if self._total_segments > 0: + dpg.set_axis_limits_constraints(self.x_axis_tag, -10, self._total_segments * 60 + 10) + self._fit_y_axis(*dpg.get_axis_limits(self.x_axis_tag)) for series_path in list(self._series_data.keys()): self.add_series(series_path, update=True) - while self.results_deque: # handle downsampled results in main thread - results = self.results_deque.popleft() + current_limits = dpg.get_axis_limits(self.x_axis_tag) + # downsample if plot zoom changed significantly + plot_duration = current_limits[1] - current_limits[0] + if plot_duration > self._last_plot_duration * 2 or plot_duration < self._last_plot_duration * 0.5: + self._downsample_all_series(plot_duration) + # sync x-axis if changed by user + if self._last_x_limits != current_limits: + self.playback_manager.set_x_axis_bounds(current_limits[0], current_limits[1], source_panel=self) + self._last_x_limits = current_limits + self._fit_y_axis(current_limits[0], current_limits[1]) + + while self._results_deque: # handle downsampled results in main thread + results = self._results_deque.popleft() for series_path, downsampled_time, downsampled_values in results: series_tag = f"series_{self.panel_id}_{series_path}" if dpg.does_item_exist(series_tag): - dpg.set_value(series_tag, [downsampled_time, downsampled_values]) + dpg.set_value(series_tag, (downsampled_time, downsampled_values.astype(float))) # update timeline current_time_s = self.playback_manager.current_time_s @@ -96,10 +155,45 @@ class TimeSeriesPanel(ViewPanel): if dpg.does_item_exist(series_tag): dpg.configure_item(series_tag, label=f"{series_path}: {formatted_value}") - # downsample if plot zoom changed significantly - plot_duration = dpg.get_axis_limits(self.x_axis_tag)[1] - dpg.get_axis_limits(self.x_axis_tag)[0] - if plot_duration > self._last_plot_duration * 2 or plot_duration < self._last_plot_duration * 0.5: - self._downsample_all_series(plot_duration) + def _on_x_axis_sync(self, min_time: float, max_time: float, source_panel): + with self._update_lock: + if source_panel != self: + self._queued_x_sync = (min_time, max_time) + + def _fit_y_axis(self, x_min: float, x_max: float): + if not self._series_data: + dpg.set_axis_limits(self.y_axis_tag, -1, 1) + return + + global_min = float('inf') + global_max = float('-inf') + found_data = False + + for time_array, value_array in self._series_data.values(): + if len(time_array) == 0: + continue + start_idx, end_idx = np.searchsorted(time_array, [x_min, x_max]) + end_idx = min(end_idx, len(time_array) - 1) + if start_idx <= end_idx: + y_slice = value_array[start_idx:end_idx + 1] + series_min, series_max = np.min(y_slice), np.max(y_slice) + global_min = min(global_min, series_min) + global_max = max(global_max, series_max) + found_data = True + + if not found_data: + dpg.set_axis_limits(self.y_axis_tag, -1, 1) + return + + if global_min == global_max: + padding = max(abs(global_min) * 0.1, 1.0) + y_min, y_max = global_min - padding, global_max + padding + else: + range_size = global_max - global_min + padding = range_size * 0.1 + y_min, y_max = global_min - padding, global_max + padding + + dpg.set_axis_limits(self.y_axis_tag, y_min, y_max) def _downsample_all_series(self, plot_duration): plot_width = dpg.get_item_rect_size(self.plot_tag)[0] @@ -118,11 +212,11 @@ class TimeSeriesPanel(ViewPanel): target_points = max(int(target_points_per_second * series_duration), plot_width) work_items.append((series_path, time_array, value_array, target_points)) elif dpg.does_item_exist(f"series_{self.panel_id}_{series_path}"): - dpg.set_value(f"series_{self.panel_id}_{series_path}", [time_array, value_array]) + dpg.set_value(f"series_{self.panel_id}_{series_path}", (time_array, value_array.astype(float))) if work_items: self.worker_manager.submit_task( - TimeSeriesPanel._downsample_worker, work_items, callback=lambda results: self.results_deque.append(results), task_id=f"downsample_{self.panel_id}" + TimeSeriesPanel._downsample_worker, work_items, callback=lambda results: self._results_deque.append(results), task_id=f"downsample_{self.panel_id}" ) def add_series(self, series_path: str, update: bool = False): @@ -133,18 +227,18 @@ class TimeSeriesPanel(ViewPanel): time_array, value_array = self._series_data[series_path] series_tag = f"series_{self.panel_id}_{series_path}" if dpg.does_item_exist(series_tag): - dpg.set_value(series_tag, [time_array, value_array]) + dpg.set_value(series_tag, (time_array, value_array.astype(float))) else: - line_series_tag = dpg.add_line_series(x=time_array, y=value_array, label=series_path, parent=self.y_axis_tag, tag=series_tag) - dpg.bind_item_theme(line_series_tag, "global_line_theme") - dpg.fit_axis_data(self.x_axis_tag) - dpg.fit_axis_data(self.y_axis_tag) + line_series_tag = dpg.add_line_series(x=time_array, y=value_array.astype(float), label=series_path, parent=self.y_axis_tag, tag=series_tag) + dpg.bind_item_theme(line_series_tag, "line_theme") + self._fit_y_axis(*dpg.get_axis_limits(self.x_axis_tag)) plot_duration = dpg.get_axis_limits(self.x_axis_tag)[1] - dpg.get_axis_limits(self.x_axis_tag)[0] self._downsample_all_series(plot_duration) def destroy_ui(self): with self._update_lock: self.data_manager.remove_observer(self.on_data_loaded) + self.playback_manager.remove_x_axis_observer(self._on_x_axis_sync) if dpg.does_item_exist(self.plot_tag): dpg.delete_item(self.plot_tag) self._ui_created = False @@ -165,7 +259,12 @@ class TimeSeriesPanel(ViewPanel): del self._series_data[series_path] def on_data_loaded(self, data: dict): - self._new_data = True + with self._update_lock: + self._new_data = True + if data.get('metadata_loaded'): + self._total_segments = data.get('total_segments', 0) + limits = (-10, self._total_segments * 60 + 10) + self._queued_x_sync = limits def _on_series_drop(self, sender, app_data, user_data): self.add_series(app_data) diff --git a/tools/mac_setup.sh b/tools/mac_setup.sh index 19ef77e01c..0ae0b35359 100755 --- a/tools/mac_setup.sh +++ b/tools/mac_setup.sh @@ -34,13 +34,11 @@ fi brew bundle --file=- <<-EOS brew "git-lfs" -brew "zlib" brew "capnp" brew "coreutils" brew "eigen" brew "ffmpeg" brew "glfw" -brew "libarchive" brew "libusb" brew "libtool" brew "llvm" @@ -50,7 +48,6 @@ brew "zeromq" cask "gcc-arm-embedded" brew "portaudio" brew "gcc@13" -cask "font-noto-color-emoji" EOS echo "[ ] finished brew install t=$SECONDS" diff --git a/tools/op.sh b/tools/op.sh index 54ff8e97e9..ae12809eb9 100755 --- a/tools/op.sh +++ b/tools/op.sh @@ -366,9 +366,11 @@ function op_switch() { BRANCH="$1" git config --replace-all remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*" + git submodule deinit --all --force git fetch "$REMOTE" "$BRANCH" git checkout -f FETCH_HEAD git checkout -B "$BRANCH" --track "$REMOTE"/"$BRANCH" + git submodule deinit --all --force git reset --hard "${REMOTE}/${BRANCH}" git clean -df git submodule update --init --recursive diff --git a/tools/plotjuggler/layouts/torque-controller.xml b/tools/plotjuggler/layouts/torque-controller.xml index 606df03611..8e9a1a8526 100644 --- a/tools/plotjuggler/layouts/torque-controller.xml +++ b/tools/plotjuggler/layouts/torque-controller.xml @@ -1,193 +1,180 @@ - + - + - - + + - - + + - - + + - - + + - - + + - + - + - - + + - - + + - - - + + + - + - - - - + + + + - - - + + - - - + + + - - + + - - - + + + - - - + + - + - - - - + + + + - - + + + - - - + + + - - + + - - - + + + - - + + + - + - - - - + + + + - - - - - - - + - - - + + + - - - - + + + - - - + + + - + - + - - - - + + + + - - + + - - - + + + - - - + + + - + + - - - + + + - - - - - - + + + + - - - + + + - - - - - - + @@ -199,44 +186,62 @@ + + + + + + + + + + + + + - + - return (0) - /carState/canValid + return value * 3.6 + /carState/vEgo - + + + return value * 2.23694 + /carState/vEgo + + return (value * v1 ^ 2) - (v2 * 9.81) - /controlsState/curvature + /controlsState/desiredCurvature /carState/vEgo /liveParameters/roll - + return (value * v1 ^ 2) - (v2 * 9.81) - /controlsState/desiredCurvature + /controlsState/curvature /carState/vEgo /liveParameters/roll - + - return value * 2.23694 - /carState/vEgo + return value + 0.2 + /carParams/steerActuatorDelay - + - return value * 3.6 - /carState/vEgo + return (0) + /carState/canValid diff --git a/tools/replay/SConscript b/tools/replay/SConscript index 99c8263a8c..136c4119f6 100644 --- a/tools/replay/SConscript +++ b/tools/replay/SConscript @@ -12,7 +12,9 @@ else: base_libs.append('OpenCL') replay_lib_src = ["replay.cc", "consoleui.cc", "camera.cc", "filereader.cc", "logreader.cc", "framereader.cc", - "route.cc", "util.cc", "seg_mgr.cc", "timeline.cc", "api.cc", "qcom_decoder.cc"] + "route.cc", "util.cc", "seg_mgr.cc", "timeline.cc", "api.cc"] +if arch != "Darwin": + replay_lib_src.append("qcom_decoder.cc") replay_lib = replay_env.Library("replay", replay_lib_src, LIBS=base_libs, FRAMEWORKS=base_frameworks) Export('replay_lib') replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'zstd', 'curl', 'yuv', 'ncurses'] + base_libs diff --git a/tools/replay/framereader.cc b/tools/replay/framereader.cc index f2b1faf2c4..e9cd090446 100644 --- a/tools/replay/framereader.cc +++ b/tools/replay/framereader.cc @@ -39,9 +39,12 @@ struct DecoderManager { } std::unique_ptr decoder; + #ifndef __APPLE__ if (Hardware::TICI() && hw_decoder) { decoder = std::make_unique(); - } else { + } else + #endif + { decoder = std::make_unique(); } @@ -264,6 +267,7 @@ bool FFmpegVideoDecoder::copyBuffer(AVFrame *f, VisionBuf *buf) { return true; } +#ifndef __APPLE__ bool QcomVideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) { if (codecpar->codec_id != AV_CODEC_ID_HEVC) { rError("Hardware decoder only supports HEVC codec"); @@ -305,3 +309,4 @@ bool QcomVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { } return result; } +#endif diff --git a/tools/replay/framereader.h b/tools/replay/framereader.h index 1fb3cdfeb1..d8e86fce0f 100644 --- a/tools/replay/framereader.h +++ b/tools/replay/framereader.h @@ -6,7 +6,10 @@ #include "msgq/visionipc/visionbuf.h" #include "tools/replay/filereader.h" #include "tools/replay/util.h" + +#ifndef __APPLE__ #include "tools/replay/qcom_decoder.h" +#endif extern "C" { #include @@ -65,6 +68,7 @@ private: AVBufferRef *hw_device_ctx = nullptr; }; +#ifndef __APPLE__ class QcomVideoDecoder : public VideoDecoder { public: QcomVideoDecoder() {}; @@ -75,3 +79,4 @@ public: private: MsmVidc msm_vidc = MsmVidc(); }; +#endif diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py index 2681b26904..68ff3050db 100644 --- a/tools/sim/lib/simulated_car.py +++ b/tools/sim/lib/simulated_car.py @@ -11,7 +11,7 @@ from openpilot.tools.sim.lib.common import SimulatorState class SimulatedCar: """Simulates a honda civic 2022 (panda state + can messages) to OpenPilot""" - packer = CANPacker("honda_civic_ex_2022_can_generated") + packer = CANPacker("honda_bosch_radarless_generated") def __init__(self): self.pm = messaging.PubMaster(['can', 'pandaStates']) @@ -23,7 +23,7 @@ class SimulatedCar: @staticmethod def get_car_can_parser(): - dbc_f = 'honda_civic_ex_2022_can_generated' + dbc_f = 'honda_bosch_radarless_generated' checks = [] return CANParser(dbc_f, checks, 0)