Compare commits

...

114 Commits

Author SHA1 Message Date
royjr f00db2734f no hud 2026-07-16 02:19:45 -04:00
royjr b9f2e5fdbc Merge branch 'master' into hyundai-radar-tracks 2026-07-16 02:16:41 -04:00
royjr 2cf5aa05a4 Update opendbc_repo 2026-07-16 02:16:35 -04:00
royjr dc4e10c3cd Update opendbc_repo 2026-07-16 01:27:08 -04:00
royjr 69581a38d4 fix 2026-07-16 01:27:06 -04:00
royjr 5b687a0b62 fixes 2026-07-16 01:27:06 -04:00
royjr 964d9b757c fixes 2026-07-16 01:27:06 -04:00
royjr 10f2392fc4 hud 2026-07-16 01:27:04 -04:00
royjr 05dbbc4831 lead 2026-07-16 01:27:02 -04:00
royjr 94c26e869b neu 2026-07-16 01:27:02 -04:00
royjr 1f0bfa1f05 better tracks 2026-07-16 01:27:02 -04:00
royjr 78327b8204 Update opendbc_repo 2026-07-16 01:26:31 -04:00
royjr 06f12d242d Update opendbc_repo 2026-07-15 16:13:48 -04:00
royjr f1dec1d074 non mici 2026-07-15 15:49:52 -04:00
royjr f05e0c5fc5 cleanup 2026-07-15 15:41:59 -04:00
royjr ae63b6fced cleanup 2026-07-15 15:37:35 -04:00
royjr bbc7135a31 pop 2026-07-15 15:28:04 -04:00
royjr 6cb5381bab small white 2026-07-15 15:28:04 -04:00
royjr f0f3889a62 nofade 2026-07-15 15:28:04 -04:00
royjr b23441aed7 revert proc 2026-07-15 15:28:04 -04:00
royjr 65169fc718 Update opendbc_repo 2026-07-15 15:26:29 -04:00
royjr ff05c6c6f7 Update opendbc_repo 2026-07-15 15:03:34 -04:00
royjr dbc5ac4b42 bright 2026-07-15 15:03:31 -04:00
royjr d89459a42d clean 2026-07-15 15:03:31 -04:00
royjr 9db3e95940 ego 2026-07-15 15:03:31 -04:00
royjr 40ad364a19 more 2026-07-15 15:03:31 -04:00
royjr 6447b61766 doppler 2026-07-15 15:03:31 -04:00
royjr efc9439350 proclag 2026-07-15 15:03:31 -04:00
royjr 69e7a74835 lagless 2026-07-15 15:03:31 -04:00
royjr 5542996fbf Update opendbc_repo 2026-07-15 14:59:48 -04:00
royjr f6bb2585af Update opendbc_repo 2026-07-15 12:02:26 -04:00
royjr 28d04ee39d Update opendbc_repo 2026-07-15 12:02:11 -04:00
royjr 9becb33534 RadarTracks 2026-07-15 12:02:09 -04:00
royjr 54408ee249 pill click always 2026-07-15 12:02:07 -04:00
royjr 1ec4b02c15 better toggle 2026-07-15 12:02:07 -04:00
royjr 4134e1a056 remove RadarTracks 2026-07-15 12:02:05 -04:00
royjr 8bf9fb9893 ui 2026-07-15 12:02:05 -04:00
royjr 7e61a09717 ui 2026-07-15 12:02:05 -04:00
royjr 55d1328b66 we are one 2026-07-15 12:02:05 -04:00
royjr 1d168cd409 better count 2026-07-15 12:02:05 -04:00
royjr 0077d7954f live 2026-07-15 12:02:05 -04:00
royjr b05f5ed70f ui 2026-07-15 12:02:03 -04:00
royjr 5f7294c3b7 big 2026-07-15 12:02:01 -04:00
royjr d82ae31a7d Update augmented_road_view.py 2026-07-15 12:02:01 -04:00
royjr 9098e623c8 ui 2026-07-15 12:02:01 -04:00
royjr 412971fddd Update toggles.py 2026-07-15 12:01:58 -04:00
royjr 6bda274413 Update model_renderer.py 2026-07-14 12:42:30 -04:00
royjr b1b658c506 Update opendbc_repo 2026-07-14 00:32:24 -04:00
royjr be8abfe8f8 Update opendbc_repo 2026-07-10 21:57:26 -04:00
royjr 453e1d9514 Update opendbc_repo 2026-07-08 00:33:38 -04:00
royjr 98a40ed15c Update opendbc_repo 2026-07-07 23:43:44 -04:00
royjr 267db8b817 Update opendbc_repo 2026-07-07 23:40:07 -04:00
royjr b9ce161fff Update opendbc_repo 2026-07-01 22:00:01 -04:00
royjr 8562e39b0b Merge branch 'master' into hyundai-radar-tracks 2026-06-30 21:55:22 -04:00
royjr be4223fe13 visuals-radar-tracks
commit 1e8d692804
Merge: 5d5dc6c50c 097dd9b5f2
Author: royjr <royjr96@gmail.com>
Date:   Tue Jun 9 00:13:12 2026 -0400

    Merge branch 'master' into visuals-radar-tracks

commit 5d5dc6c50c
Author: royjr <royjr96@gmail.com>
Date:   Tue Jun 9 00:12:06 2026 -0400

    yaml

commit 5c64fe0695
Merge: 37eeb121cd 066ba92e77
Author: royjr <royjr96@gmail.com>
Date:   Mon Jun 8 23:19:24 2026 -0400

    Merge branch 'master' into visuals-radar-tracks

commit 37eeb121cd
Merge: 19a5202f4c 1658898498
Author: royjr <royjr96@gmail.com>
Date:   Fri Mar 27 16:27:10 2026 -0700

    Merge branch 'master' into visuals-radar-tracks

commit 19a5202f4c
Merge: 920ef49b32 a17a38d8c3
Author: royjr <royjr96@gmail.com>
Date:   Mon Mar 2 02:44:58 2026 -0500

    Merge branch 'master' into visuals-radar-tracks

commit 920ef49b32
Author: royjr <royjr96@gmail.com>
Date:   Thu Feb 5 01:01:07 2026 -0500

    no return

commit c7a37ca89e
Merge: 45ca076aef 5c12a7cfc3
Author: royjr <royjr96@gmail.com>
Date:   Thu Feb 5 00:52:12 2026 -0500

    Merge branch 'master' into visuals-radar-tracks

commit 45ca076aef
Merge: 70cf5e6d52 19a7d1d5d7
Author: royjr <royjr96@gmail.com>
Date:   Wed Dec 31 15:26:12 2025 -0500

    Merge branch 'master' into visuals-radar-tracks

commit 70cf5e6d52
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 12:03:57 2025 -0500

    Update params_metadata.json

commit f4c71e4918
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 10:05:17 2025 -0500

    lint

commit 8408241495
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 02:23:10 2025 -0500

    Update toggles.py

commit 640988673e
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 02:22:16 2025 -0500

    mici toggle

commit 1429975f1e
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 02:22:01 2025 -0500

    Revert "force enable for now"

    This reverts commit efd499448a.

commit efd499448a
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 00:42:38 2025 -0500

    force enable for now

commit f7e636f089
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 00:42:19 2025 -0500

    use params

commit 00187dc59e
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 00:37:19 2025 -0500

    use checks

commit cde6368cf7
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 00:30:15 2025 -0500

    cleaner

commit 08b56f9b33
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 00:28:12 2025 -0500

    better sp

commit 78efe3a476
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 00:21:08 2025 -0500

    init sp

commit 54578675c2
Author: royjr <royjr96@gmail.com>
Date:   Mon Dec 29 00:19:17 2025 -0500

    init
2026-06-09 00:15:59 -04:00
royjr 2b492098e9 Update opendbc_repo 2026-06-09 00:14:58 -04:00
royjr 00fee3ee88 Merge branch 'master' into hyundai-radar-tracks 2026-06-09 00:14:36 -04:00
royjr 930ab1a84d fixes 2026-06-08 22:37:07 -04:00
royjr 0c77f1b41b auto_radar 2026-06-08 22:26:08 -04:00
royjr 9fd4e5b9f8 Merge branch 'master' into hyundai-radar-tracks 2026-06-08 22:20:43 -04:00
royjr 818dd7c4e3 Update opendbc_repo 2026-06-08 22:20:39 -04:00
royjr 9fe6d20249 Merge branch 'master' into hyundai-radar-tracks 2026-03-27 16:11:57 -07:00
royjr e31b28d0b3 Update opendbc_repo 2026-03-27 16:11:51 -07:00
royjr 06498600bf Update opendbc_repo 2026-03-11 23:28:33 -04:00
royjr 05649045b7 Update opendbc_repo 2026-03-01 23:02:25 -05:00
royjr 0ee21708ac Merge branch 'master' into hyundai-radar-tracks 2026-03-01 22:39:12 -05:00
royjr 1492941483 Update opendbc_repo 2026-03-01 22:39:07 -05:00
royjr 8794178ccf Merge branch 'master' into hyundai-radar-tracks 2026-02-28 17:02:32 -05:00
royjr 86303e7a3c Update opendbc_repo 2026-02-28 17:01:42 -05:00
royjr e189678033 Update opendbc_repo 2026-02-25 21:25:16 -05:00
royjr 99bae4f475 Merge branch 'master' into hyundai-radar-tracks 2026-02-25 21:24:58 -05:00
royjr ef7046c2ed Merge branch 'master' into hyundai-radar-toggle 2026-02-13 22:28:03 -05:00
royjr 734121b517 Update opendbc_repo 2026-02-13 22:27:58 -05:00
royjr b738769c22 Update opendbc_repo 2026-02-05 22:59:22 -05:00
royjr 0bfd9e5cf2 Merge branch 'master' into hyundai-radar-toggle 2026-02-05 22:50:16 -05:00
royjr 20fadbcaa8 Update opendbc_repo 2026-02-05 22:50:08 -05:00
royjr 0eeb052241 hyundai_radar toggle mici 2025-12-29 02:20:27 -05:00
royjr edc2d16ae4 Update params_metadata.json 2025-12-28 17:47:10 -05:00
royjr e72fa9453d Merge branch 'master' into hyundai-radar-toggle 2025-12-28 17:20:20 -05:00
royjr 1c56f0b0a3 Update opendbc_repo 2025-12-28 17:19:47 -05:00
royjr 80d15212e2 Merge branch 'master' into hyundai-radar-toggle 2025-12-20 11:47:50 -05:00
royjr ab2863a859 Update opendbc_repo 2025-12-20 11:45:59 -05:00
royjr 743aa8e736 Merge branch 'master' into hyundai-radar-toggle 2025-10-10 00:09:54 -04:00
royjr 5847256371 Update opendbc_repo 2025-10-10 00:09:37 -04:00
royjr c13f909390 Update opendbc_repo 2025-10-09 01:30:54 -04:00
royjr f1de835d17 Update opendbc_repo 2025-10-09 01:11:07 -04:00
royjr 76ddb20cd5 Merge branch 'master' into hyundai-radar-toggle 2025-10-09 01:10:40 -04:00
royjr ede7f70ddc Merge branch 'master' into hyundai-radar-toggle 2025-10-08 23:04:21 -04:00
royjr 46c3047fa7 Update opendbc_repo 2025-10-08 23:04:16 -04:00
royjr cf5b5c666d Merge branch 'master' into hyundai-radar-toggle 2025-09-30 13:57:57 -04:00
royjr 03b56d6f09 Update opendbc_repo 2025-09-30 13:57:52 -04:00
royjr ab180ce1e5 Update opendbc_repo 2025-09-22 23:13:42 -04:00
royjr e0d8bc88b2 Update opendbc_repo 2025-09-22 00:33:12 -04:00
royjr 1dc45adb1c Merge branch 'master' into hyundai-radar-toggle 2025-09-22 00:28:40 -04:00
royjr c3bd613885 Merge branch 'master' into hyundai-radar-toggle 2025-09-20 01:56:28 -04:00
royjr 55edb4efcb Update opendbc_repo 2025-09-20 01:56:23 -04:00
royjr 7ff9b28c94 Merge branch 'master' into hyundai-radar-toggle 2025-08-24 16:57:46 -04:00
royjr 5f9d340d7b Update opendbc_repo 2025-08-24 16:57:37 -04:00
royjr e82b47cb70 Update opendbc_repo 2025-08-22 12:03:48 -04:00
royjr 10b5e58558 Merge branch 'master' into hyundai-radar-toggle 2025-08-22 11:47:43 -04:00
royjr 116936341f Update opendbc_repo 2025-08-22 11:41:48 -04:00
royjr 8d5bd92d51 Update opendbc_repo 2025-08-14 22:13:26 -04:00
royjr 4453dba6ed Update opendbc_repo 2025-08-13 23:03:43 -04:00
royjr cc20313815 fix param 2025-08-13 20:38:33 -04:00
royjr 3b30b93bbb Merge branch 'master' into hyundai-radar-toggle 2025-08-13 18:34:52 -04:00
royjr 5d9de36d40 Update opendbc_repo 2025-08-13 18:31:10 -04:00
royjr a91b97b89a Set RADAR_OFF flag for Hyundai when radar is off
Adds logic to set the RADAR_OFF flag in CP_SP.flags when the Hyundai radar type is set to OFF. This ensures the correct flag is applied for vehicles with radar disabled.
2025-07-01 02:09:53 -04:00
royjr ec4e2ec3c1 Update opendbc_repo 2025-07-01 02:09:48 -04:00
royjr 1f49367380 Merge branch 'master-new' into hyundai-radar-toggle 2025-07-01 01:10:33 -04:00
royjr 040f81fc91 Update opendbc_repo 2025-07-01 01:10:02 -04:00
royjr 5253b33b7a buttonClicked 2025-06-03 01:34:18 -04:00
royjr fc46a0ed7f Update opendbc_repo 2025-06-03 01:18:21 -04:00
royjr 8056a797c7 Merge branch 'master-new' into hyundai-radar-toggle 2025-06-03 01:17:26 -04:00
royjr a492625927 init 2025-05-20 01:34:50 -04:00
23 changed files with 2001 additions and 14 deletions
+1
View File
@@ -180,6 +180,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"OnroadUploads", {PERSISTENT | BACKUP, BOOL, "1"}},
{"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
{"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
{"RadarTracks", {PERSISTENT | BACKUP, INT, "0"}},
{"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
{"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
+17
View File
@@ -181,6 +181,8 @@ class Car:
self.is_metric = self.params.get_bool("IsMetric")
self.experimental_mode = self.params.get_bool("ExperimentalMode")
self.radar_tracks = int(self.params.get("RadarTracks", return_default=True))
self._applied_radar_tracks = getattr(self.RI, "radar_mode", self.radar_tracks)
# card is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
@@ -198,6 +200,8 @@ class Car:
CS, CS_SP = self.CI.update(can_list)
CS_SP = convert_to_capnp(CS_SP)
self._update_radar_tracks()
# Update radar tracks from CAN
RD: structs.RadarDataT | None = self.RI.update(can_list)
@@ -224,6 +228,18 @@ class Car:
return CS, CS_SP, RD
def _update_radar_tracks(self) -> None:
if self.CP.brand != "hyundai" or self.radar_tracks == self._applied_radar_tracks:
return
if self.RI.set_radar_mode(self.radar_tracks):
self.CP_SP_capnp = convert_to_capnp(self.CP_SP)
cp_sp_bytes = self.CP_SP_capnp.to_bytes()
self.params.put("CarParamsSP", cp_sp_bytes)
self.params.put("CarParamsSPCache", cp_sp_bytes)
self.params.put("CarParamsSPPersistent", cp_sp_bytes)
self._applied_radar_tracks = self.radar_tracks
def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, RD: structs.RadarDataT | None):
"""carState and carParams publish loop"""
@@ -302,6 +318,7 @@ class Car:
while not evt.is_set():
self.is_metric = self.params.get_bool("IsMetric")
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
self.radar_tracks = int(self.params.get("RadarTracks", return_default=True))
# sunnypilot
self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
+5 -1
View File
@@ -34,7 +34,11 @@ class MainLayout(Widget):
# Initialize layouts
self._home_layout = HomeLayout()
self._home_body_layout = BodyLayout()
self._layouts = {MainState.HOME: self._home_layout, MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView()}
self._layouts = {
MainState.HOME: self._home_layout,
MainState.SETTINGS: SettingsLayout(),
MainState.ONROAD: AugmentedRoadView(radar_tracks_settings_callback=lambda: self.open_settings(PanelType.TOGGLES)),
}
self._sidebar_rect = rl.Rectangle(0, 0, 0, 0)
self._content_rect = rl.Rectangle(0, 0, 0, 0)
+16
View File
@@ -106,7 +106,18 @@ class TogglesLayout(Widget):
icon="speed_limit.png"
)
self._radar_tracks_setting = None
self._toggles = {}
if gui_app.sunnypilot_ui():
self._radar_tracks_setting = multiple_button_item(
lambda: tr("Radar Tracks"),
"",
buttons=[lambda: tr("Off"), lambda: tr("Lead Only"), lambda: tr("Full Radar")],
button_width=250,
callback=self._set_radar_tracks,
selected_index=self._params.get("RadarTracks", return_default=True),
)
self._toggles["RadarTracks"] = self._radar_tracks_setting
self._locked_toggles = set()
for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
toggle = toggle_item(
@@ -203,6 +214,8 @@ class TogglesLayout(Widget):
# refresh toggles from params to mirror external changes
for param in self._toggle_defs:
self._toggles[param].action_item.set_state(self._params.get_bool(param))
if self._radar_tracks_setting is not None:
self._radar_tracks_setting.action_item.set_selected_button(self._params.get("RadarTracks", return_default=True))
# these toggles need restart, block while engaged
for toggle_def in self._toggle_defs:
@@ -247,3 +260,6 @@ class TogglesLayout(Widget):
def _set_longitudinal_personality(self, button_index: int):
self._params.put("LongitudinalPersonality", button_index, block=True)
def _set_radar_tracks(self, button_index: int):
self._params.put("RadarTracks", button_index, block=True)
+2 -1
View File
@@ -32,7 +32,8 @@ class MiciMainLayout(Scroller):
self._home_layout = MiciHomeLayout()
self._alerts_layout = MiciOffroadAlerts()
self._settings_layout = SettingsLayout()
self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked)
self._car_onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked,
radar_tracks_settings_callback=self._settings_layout.open_toggles)
self._body_onroad_layout = BodyLayout()
# Initialize widget rects
@@ -20,9 +20,9 @@ class SettingsLayout(NavScroller):
super().__init__()
self._params = Params()
toggles_panel = TogglesLayoutMici()
self._toggles_panel = TogglesLayoutMici()
toggles_btn = SettingsBigButton("toggles", "", gui_app.texture("icons_mici/settings.png", 64, 64))
toggles_btn.set_click_callback(lambda: gui_app.push_widget(toggles_panel))
toggles_btn.set_click_callback(lambda: gui_app.push_widget(self._toggles_panel))
network_panel = NetworkLayoutMici()
network_btn = SettingsBigButton("network", "", gui_app.texture("icons_mici/settings/network/wifi_strength_full.png", 76, 56))
@@ -56,3 +56,8 @@ class SettingsLayout(NavScroller):
])
self._font_medium = gui_app.font(FontWeight.MEDIUM)
def open_toggles(self) -> None:
if not gui_app.widget_in_stack(self):
gui_app.push_widget(self)
gui_app.push_widget(self._toggles_panel)
@@ -21,8 +21,10 @@ class TogglesLayoutMici(NavScroller):
record_front = BigParamControl("record & upload driver camera", "RecordFront", toggle_callback=restart_needed_callback)
record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
radar_tracks = BigMultiParamToggle("radar tracks", "RadarTracks", ["off", "lead only", "full radar"])
self._scroller.add_widgets([
radar_tracks,
self._personality_toggle,
self._experimental_btn,
is_metric_toggle,
@@ -35,6 +37,7 @@ class TogglesLayoutMici(NavScroller):
# Toggle lists
self._refresh_toggles = (
("RadarTracks", radar_tracks),
("ExperimentalMode", self._experimental_btn),
("IsMetric", is_metric_toggle),
("IsLdwEnabled", ldw_toggle),
@@ -10,6 +10,7 @@ from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer
from openpilot.selfdrive.ui.mici.onroad.model_renderer import ModelRenderer
from openpilot.selfdrive.ui.mici.onroad.confidence_ball import ConfidenceBall
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracksStatus
from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent
from openpilot.system.ui.widgets.label import UnifiedLabel
from openpilot.system.ui.widgets import Widget
@@ -133,9 +134,11 @@ class BookmarkIcon(Widget):
class AugmentedRoadView(CameraView):
def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD):
def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD,
radar_tracks_settings_callback=None):
super().__init__("camerad", stream_type)
self._bookmark_callback = bookmark_callback
self._radar_tracks_status = RadarTracksStatus(radar_tracks_settings_callback)
self._set_placeholder_color(rl.BLACK)
self.device_camera: DeviceCameraConfig | None = None
@@ -169,6 +172,11 @@ class AugmentedRoadView(CameraView):
def _update_state(self):
super()._update_state()
if ui_state.sm.updated["liveTracks"]:
self._radar_tracks_status.update(ui_state.sm["liveTracks"], ui_state.sm.valid["liveTracks"], ui_state.radar_tracks)
elif not ui_state.sm.alive["liveTracks"]:
self._radar_tracks_status.reset()
# update offroad label
if ui_state.panda_type == log.PandaState.PandaType.unknown:
self._offroad_label.set_text("system booting")
@@ -178,6 +186,9 @@ class AugmentedRoadView(CameraView):
self._offroad_label.set_text("start the car to\nuse sunnypilot")
def _handle_mouse_release(self, mouse_pos: MousePos):
if self._radar_tracks_status.handle_mouse(mouse_pos):
return
# Don't trigger click callback if bookmark was triggered
if not self._bookmark_icon.interacting():
super()._handle_mouse_release(mouse_pos)
@@ -220,6 +231,8 @@ class AugmentedRoadView(CameraView):
# Fade out bottom of overlays for looks
rl.draw_texture_ex(self._fade_texture, rl.Vector2(self._content_rect.x, self._content_rect.y), 0.0, 1.0, rl.WHITE)
self._radar_tracks_status.render(self._content_rect)
alert_to_render, not_animating_out = self._alert_renderer.will_render()
# Hide DMoji when disengaged unless AlwaysOnDM is enabled
+55 -6
View File
@@ -1,4 +1,5 @@
import colorsys
import math
import numpy as np
import pyray as rl
from cereal import messaging, car
@@ -37,6 +38,11 @@ LANE_LINE_COLORS = {
**LANE_LINE_COLORS_SP,
}
LEAD_TRACK_COLORS = (
rl.Color(255, 215, 0, 255),
rl.Color(255, 140, 0, 220),
)
@dataclass
class ModelPoints:
@@ -49,6 +55,9 @@ class LeadVehicle:
glow: list[float] = field(default_factory=list)
chevron: list[float] = field(default_factory=list)
fill_alpha: int = 0
position: tuple[float, float] | None = None
radar_track_id: int = -1
radar: bool = False
class ModelRenderer(Widget, ModelRendererSP):
@@ -131,7 +140,7 @@ class ModelRenderer(Widget, ModelRendererSP):
model = sm['modelV2']
radar_state = sm['radarState'] if sm.valid['radarState'] else None
lead_one = radar_state.leadOne if radar_state else None
render_lead_indicator = self._longitudinal_control and radar_state is not None
render_lead_indicator = ui_state.radar_tracks != 0 and radar_state is not None
# Update model data when needed
model_updated = sm.updated['modelV2']
@@ -153,8 +162,18 @@ class ModelRenderer(Widget, ModelRendererSP):
self._draw_lane_lines()
self._draw_path(sm)
# if render_lead_indicator and radar_state:
# self._draw_lead_indicator()
if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
highlighted_tracks = self._lead_track_colors(radar_state) if render_lead_indicator else {}
matched_positions = self.radar_tracks.draw_radar_tracks(
sm['liveTracks'], self._map_to_screen, self._path_offset_z,
screen_offset=(self._rect.x, self._rect.y), v_ego=sm['carState'].vEgo,
highlighted_tracks=highlighted_tracks,
)
if render_lead_indicator:
self._draw_lead_connectors(matched_positions, highlighted_tracks)
if render_lead_indicator:
self._draw_lead_indicator()
def _update_raw_points(self, model):
"""Update raw 3D points from model data"""
@@ -184,7 +203,11 @@ class ModelRenderer(Widget, ModelRendererSP):
z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
if point:
self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
lead_vehicle = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
lead_vehicle.position = lead_vehicle.chevron[1]
lead_vehicle.radar_track_id = int(lead_data.radarTrackId)
lead_vehicle.radar = lead_data.radar
self._lead_vehicles[i] = lead_vehicle
def _update_model(self, lead, path_x_array):
"""Update model visualization data based on model message"""
@@ -379,8 +402,34 @@ class ModelRenderer(Widget, ModelRendererSP):
if not lead.glow or not lead.chevron:
continue
rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255))
rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha))
offset_glow = [(x + self._rect.x, y + self._rect.y) for x, y in lead.glow]
offset_chevron = [(x + self._rect.x, y + self._rect.y) for x, y in lead.chevron]
rl.draw_triangle_fan(offset_glow, len(offset_glow), rl.Color(218, 202, 37, 255))
rl.draw_triangle_fan(offset_chevron, len(offset_chevron), rl.Color(201, 34, 49, lead.fill_alpha))
@staticmethod
def _lead_track_colors(radar_state):
highlighted_tracks = {}
if radar_state is None:
return highlighted_tracks
for lead, color in zip((radar_state.leadOne, radar_state.leadTwo), LEAD_TRACK_COLORS, strict=True):
if lead.status and lead.radar and lead.radarTrackId >= 0:
highlighted_tracks.setdefault(int(lead.radarTrackId), color)
return highlighted_tracks
def _draw_lead_connectors(self, matched_positions, highlighted_tracks):
for lead in self._lead_vehicles:
if not lead.radar or lead.position is None or lead.radar_track_id not in matched_positions:
continue
radar_position = matched_positions[lead.radar_track_id]
lead_position = (lead.position[0] + self._rect.x, lead.position[1] + self._rect.y)
if math.dist(lead_position, radar_position) < 4:
continue
rl.draw_line_ex(rl.Vector2(*lead_position), rl.Vector2(*radar_position), 2,
highlighted_tracks[lead.radar_track_id])
@staticmethod
def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int:
+17 -2
View File
@@ -18,6 +18,7 @@ if gui_app.sunnypilot_ui():
from openpilot.selfdrive.ui.sunnypilot.onroad.augmented_road_view import BORDER_COLORS_SP, AugmentedRoadViewSP
from openpilot.selfdrive.ui.sunnypilot.onroad.driver_state import DriverStateRendererSP as DriverStateRenderer
from openpilot.selfdrive.ui.sunnypilot.onroad.hud_renderer import HudRendererSP as HudRenderer
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracksStatus
from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
OpState = log.SelfdriveState.OpenpilotState
@@ -39,7 +40,7 @@ INF_POINT = np.array([1000.0, 0.0, 0.0])
class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD):
def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD, radar_tracks_settings_callback=None):
CameraView.__init__(self, "camerad", stream_type)
AugmentedRoadViewSP.__init__(self)
self._set_placeholder_color(BORDER_COLORS[UIStatus.DISENGAGED])
@@ -51,12 +52,22 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
self._matrix_cache_key = (0, 0.0, 0.0, stream_type)
self._cached_matrix: np.ndarray | None = None
self._content_rect = rl.Rectangle()
self._radar_tracks_status = RadarTracksStatus(radar_tracks_settings_callback, right_margin=240) if gui_app.sunnypilot_ui() else None
self.model_renderer = ModelRenderer()
self._hud_renderer = HudRenderer()
self.alert_renderer = AlertRenderer()
self.driver_state_renderer = DriverStateRenderer()
def _update_state(self):
super()._update_state()
if self._radar_tracks_status is None:
return
if ui_state.sm.updated["liveTracks"]:
self._radar_tracks_status.update(ui_state.sm["liveTracks"], ui_state.sm.valid["liveTracks"], ui_state.radar_tracks)
elif not ui_state.sm.alive["liveTracks"]:
self._radar_tracks_status.reset()
def _render(self, rect):
# Only render when system is started to avoid invalid data access
if not ui_state.started:
@@ -90,6 +101,8 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
# Draw all UI overlays
self.model_renderer.render(self._content_rect)
AugmentedRoadViewSP.update_fade_out_bottom_overlay(self, self._content_rect)
if self._radar_tracks_status is not None:
self._radar_tracks_status.render(self._content_rect)
self._hud_renderer.render(self._content_rect)
self.alert_renderer.render(self._content_rect)
self.driver_state_renderer.render(self._content_rect)
@@ -103,7 +116,9 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP):
# Draw colored border based on driving state
self._draw_border(rect)
def _handle_mouse_press(self, _):
def _handle_mouse_press(self, mouse_pos):
if self._radar_tracks_status is not None and self._radar_tracks_status.handle_mouse(mouse_pos):
return
if not self._hud_renderer.user_interacting() and self._click_callback is not None:
self._click_callback()
+3
View File
@@ -135,6 +135,9 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
self._draw_lane_lines()
self._draw_path(sm)
if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z, v_ego=sm['carState'].vEgo)
if render_lead_indicator and radar_state:
self._draw_lead_indicator()
self.chevron_metrics.draw_lead_status(sm, radar_state, self._rect, self._lead_vehicles)
@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
import pyray as rl
from openpilot.selfdrive.ui.ui_state import UIStatus
from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracks
LANE_LINE_COLORS_SP = {
UIStatus.LAT_ONLY: rl.Color(0, 255, 64, 255),
@@ -17,3 +18,4 @@ LANE_LINE_COLORS_SP = {
class ModelRendererSP:
def __init__(self):
self.rainbow_path = RainbowPath()
self.radar_tracks = RadarTracks()
@@ -6,9 +6,12 @@ See the LICENSE.md file in the root directory for more details.
"""
from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics
from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracks
class ModelRendererSP:
def __init__(self):
self.rainbow_path = RainbowPath()
self.chevron_metrics = ChevronMetrics()
self.radar_tracks = RadarTracks()
@@ -0,0 +1,164 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import math
import pyray as rl
from openpilot.system.ui.lib.application import FontWeight
from openpilot.system.ui.widgets.label import UnifiedLabel
RELATIVE_SPEED_MOVING_THRESHOLD = 0.5 # m/s relative speed deadband
STATIONARY_SPEED_THRESHOLD = 1.0 # m/s estimated ground speed
APPROACHING_COLOR = (0, 140, 255)
NEUTRAL_COLOR = (255, 255, 255)
MATCHED_SPEED_COLOR = (0, 255, 64)
RECEDING_COLOR = (255, 45, 45)
def radar_track_color(v_rel: float, v_ego: float = 0.0) -> rl.Color:
"""Classify tracks as stationary, speed-matched, approaching, or receding with discrete colors."""
if radar_track_is_stationary(v_rel, v_ego):
return rl.Color(*NEUTRAL_COLOR, 255)
if abs(v_rel) <= RELATIVE_SPEED_MOVING_THRESHOLD:
return rl.Color(*MATCHED_SPEED_COLOR, 255)
color = APPROACHING_COLOR if v_rel < 0.0 else RECEDING_COLOR
return rl.Color(*color, 255)
def radar_track_is_stationary(v_rel: float, v_ego: float = 0.0) -> bool:
return abs(v_ego + v_rel) <= STATIONARY_SPEED_THRESHOLD
def format_radar_tracks_onroad_columns(live_tracks) -> tuple[str, str]:
sources = sorted(live_tracks.trackSources, key=lambda source: (source.startAddress, source.endAddress, source.bus))
if not sources:
return "", "none"
range_text = "\n".join(f"{source.startAddress:X}-{source.endAddress:X}" for source in sources)
count_text = "\n".join(str(source.trackCount) for source in sources)
return range_text, count_text
class RadarTracksStatus:
HORIZONTAL_PADDING = 8
COLUMN_GAP = 8
def __init__(self, settings_callback=None, right_margin: int = 12):
self._settings_callback = settings_callback
self._right_margin = right_margin
self._rect = rl.Rectangle()
text_args = {
"font_size": 26,
"font_weight": FontWeight.SEMI_BOLD,
"text_color": rl.Color(0, 255, 64, 255),
"alignment": rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
"alignment_vertical": rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
"wrap_text": False,
}
self._ranges_label = UnifiedLabel("", **text_args)
self._counts_label = UnifiedLabel("none", **text_args)
self._status = ("", "none")
self._layout_key: tuple[str, str, int] | None = None
self._range_width = 0
self._count_width = 36
self._width = 52
self._height = 42
def update(self, live_tracks, valid: bool, radar_mode: int) -> None:
if live_tracks.radarTracksAvailable and radar_mode != 2:
status = ("", "radar detected\ntap to enable")
else:
status = format_radar_tracks_onroad_columns(live_tracks) if valid else ("", "none")
self._set_status(status)
def reset(self) -> None:
self._set_status(("", "none"))
def handle_mouse(self, mouse_pos) -> bool:
if self._settings_callback is None or not rl.check_collision_point_rec(mouse_pos, self._rect):
return False
self._settings_callback()
return True
def render(self, content_rect: rl.Rectangle) -> None:
self._update_layout(int(content_rect.width - 40))
self._rect = rl.Rectangle(
content_rect.x + content_rect.width - self._width - self._right_margin,
content_rect.y + 8,
self._width,
self._height,
)
rl.draw_rectangle_rounded(self._rect, 0.5, 8, rl.Color(0, 0, 0, 170))
self._ranges_label.render(rl.Rectangle(
self._rect.x + self.HORIZONTAL_PADDING,
self._rect.y + 5,
self._range_width,
self._rect.height - 10,
))
self._counts_label.render(rl.Rectangle(
self._rect.x + self.HORIZONTAL_PADDING + self._range_width + (self.COLUMN_GAP if self._range_width else 0),
self._rect.y + 5,
self._count_width,
self._rect.height - 10,
))
def _set_status(self, status: tuple[str, str]) -> None:
if status == self._status:
return
self._status = status
self._ranges_label.set_text(status[0])
self._counts_label.set_text(status[1])
self._layout_key = None
def _update_layout(self, max_inner_width: int) -> None:
layout_key = (*self._status, max_inner_width)
if layout_key == self._layout_key:
return
self._ranges_label.get_content_height(max_inner_width - 36 - self.COLUMN_GAP)
self._counts_label.get_content_height(max_inner_width)
self._range_width = math.ceil(self._ranges_label.text_width)
self._count_width = max(36, math.ceil(self._counts_label.text_width))
inner_width = self._range_width + self._count_width + (self.COLUMN_GAP if self._range_width else 0)
self._width = inner_width + self.HORIZONTAL_PADDING * 2
self._height = max(
42,
self._ranges_label.get_content_height(max(self._range_width, 1)) + 10,
self._counts_label.get_content_height(self._count_width) + 10,
)
self._layout_key = layout_key
class RadarTracks:
def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7, screen_offset=(0, 0), v_ego=0.0,
highlighted_tracks=None):
highlighted_tracks = highlighted_tracks or {}
highlighted_positions = {}
for track in live_tracks.points:
d_rel, y_rel, v_rel = track.dRel, track.yRel, track.vRel
if not (math.isfinite(d_rel) and math.isfinite(y_rel) and math.isfinite(v_rel)):
continue
pt = map_to_screen(d_rel, -y_rel, path_offset_z)
if pt is None:
continue
x, y = pt[0] + screen_offset[0], pt[1] + screen_offset[1]
color = radar_track_color(v_rel, v_ego)
stationary = radar_track_is_stationary(v_rel, v_ego)
radius = max(1, track_size - 4) if stationary else track_size
track_id = int(track.trackId)
highlight_color = highlighted_tracks.get(track_id)
if highlight_color is not None:
center = rl.Vector2(int(x), int(y))
rl.draw_ring(center, radius + 2, radius + 5, 0, 360, 24, highlight_color)
highlighted_positions[track_id] = (x, y)
rl.draw_circle(int(x), int(y), radius, color)
return highlighted_positions
@@ -0,0 +1,158 @@
from cereal import car
from openpilot.selfdrive.ui.sunnypilot.onroad import radar_tracks
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import format_radar_tracks_onroad_columns, radar_track_color
def color_tuple(color):
return color.r, color.g, color.b, color.a
def test_radar_track_relative_speed_colors():
assert color_tuple(radar_track_color(-10.0)) == (0, 140, 255, 255)
assert color_tuple(radar_track_color(0.0)) == (255, 255, 255, 255)
assert color_tuple(radar_track_color(10.0)) == (255, 45, 45, 255)
assert color_tuple(radar_track_color(-5.0)) == (0, 140, 255, 255)
assert color_tuple(radar_track_color(5.0)) == (255, 45, 45, 255)
def test_radar_track_relative_speed_deadband_is_green():
assert color_tuple(radar_track_color(-0.5, v_ego=10.0)) == (0, 255, 64, 255)
assert color_tuple(radar_track_color(0.5, v_ego=10.0)) == (0, 255, 64, 255)
assert color_tuple(radar_track_color(-0.51, v_ego=10.0)) == (0, 140, 255, 255)
assert color_tuple(radar_track_color(0.51, v_ego=10.0)) == (255, 45, 45, 255)
def test_radar_track_stationary_world_object_is_white():
assert color_tuple(radar_track_color(-20.0, v_ego=20.0)) == (255, 255, 255, 255)
assert color_tuple(radar_track_color(-19.0, v_ego=20.0)) == (255, 255, 255, 255)
assert color_tuple(radar_track_color(-18.9, v_ego=20.0)) == (0, 140, 255, 255)
def test_format_radar_tracks_columns_none():
live_tracks = car.RadarData.new_message()
assert format_radar_tracks_onroad_columns(live_tracks) == ("", "none")
def test_format_radar_tracks_columns_range_and_count():
live_tracks = car.RadarData.new_message()
live_tracks.trackSources = [{"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 2}]
live_tracks.init("points", 2)
assert format_radar_tracks_onroad_columns(live_tracks) == ("500-51F", "2")
def test_format_radar_tracks_columns_sorts_ranges():
live_tracks = car.RadarData.new_message()
live_tracks.trackSources = [
{"startAddress": 0x500, "endAddress": 0x51F, "bus": 2, "trackCount": 3},
{"startAddress": 0x210, "endAddress": 0x21F, "bus": 1, "trackCount": 1},
{"startAddress": 0x500, "endAddress": 0x51F, "bus": 0, "trackCount": 2},
]
live_tracks.init("points", 1)
assert format_radar_tracks_onroad_columns(live_tracks) == (
"210-21F\n500-51F\n500-51F",
"1\n2\n3",
)
def test_draw_radar_tracks_applies_screen_offset(monkeypatch):
live_tracks = car.RadarData.new_message()
points = live_tracks.init("points", 1)
points[0].dRel = 10
points[0].yRel = 1
points[0].vRel = 2
points[0].aRel = 0
drawn_circles = []
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_circles.append((x, y, size)))
radar_tracks.RadarTracks().draw_radar_tracks(
live_tracks,
lambda d_rel, y_rel, z: (20, 30),
path_offset_z=1.2,
track_size=3,
screen_offset=(100, 7),
)
assert drawn_circles == [(120, 37, 3)]
def test_draw_radar_tracks_allows_unknown_acceleration(monkeypatch):
live_tracks = car.RadarData.new_message()
point = live_tracks.init("points", 1)[0]
point.dRel = 10
point.yRel = 1
point.vRel = -5
point.aRel = float("nan")
drawn_colors = []
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_colors.append(color_tuple(color)))
radar_tracks.RadarTracks().draw_radar_tracks(live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2)
assert drawn_colors == [color_tuple(radar_track_color(-5))]
def test_draw_radar_tracks_shrinks_stationary_dots(monkeypatch):
live_tracks = car.RadarData.new_message()
point = live_tracks.init("points", 1)[0]
point.dRel = 10
point.yRel = 1
point.vRel = -20
point.aRel = 0
drawn_sizes = []
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_sizes.append(size))
radar_tracks.RadarTracks().draw_radar_tracks(
live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2, track_size=6, v_ego=20,
)
assert drawn_sizes == [2]
def test_draw_radar_tracks_keeps_matched_speed_dots_large(monkeypatch):
live_tracks = car.RadarData.new_message()
point = live_tracks.init("points", 1)[0]
point.dRel = 10
point.yRel = 1
point.vRel = 0.5
point.aRel = 0
drawn_sizes = []
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_sizes.append(size))
radar_tracks.RadarTracks().draw_radar_tracks(
live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2, track_size=6, v_ego=20,
)
assert drawn_sizes == [6]
def test_draw_radar_tracks_highlights_and_returns_matched_track(monkeypatch):
live_tracks = car.RadarData.new_message()
points = live_tracks.init("points", 2)
for track_id, point in enumerate(points, start=10):
point.trackId = track_id
point.dRel = track_id
point.yRel = 1
point.vRel = 2
point.aRel = 0
highlight_color = radar_tracks.rl.Color(255, 215, 0, 255)
drawn_rings = []
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda *args: None)
monkeypatch.setattr(
radar_tracks.rl,
"draw_ring",
lambda center, inner, outer, start, end, segments, color: drawn_rings.append(
((center.x, center.y), inner, outer, color_tuple(color))
),
)
matched_positions = radar_tracks.RadarTracks().draw_radar_tracks(
live_tracks, lambda d_rel, y_rel, z: (d_rel, 30), path_offset_z=1.2,
screen_offset=(100, 7), highlighted_tracks={11: highlight_color},
)
assert drawn_rings == [((111, 37), 9, 12, (255, 215, 0, 255))]
assert matched_positions == {11: (111, 37)}
+2
View File
@@ -43,6 +43,7 @@ class UIStateSP:
self.custom_interactive_timeout: int = 0
self.developer_ui = None
self.hide_v_ego_ui: bool = False
self.radar_tracks: int = self.params.get("RadarTracks", return_default=True)
self.onroad_brightness: int = 0
self.onroad_brightness_timer: int = 0
self.onroad_brightness_timer_param: int = 0
@@ -151,6 +152,7 @@ class UIStateSP:
self.custom_interactive_timeout = self.params.get("InteractivityTimeout", return_default=True)
self.developer_ui = self.params.get("DevUIInfo")
self.hide_v_ego_ui = self.params.get_bool("HideVEgoUI")
self.radar_tracks = self.params.get("RadarTracks", return_default=True)
self.onroad_brightness = int(float(self.params.get("OnroadScreenOffBrightness", return_default=True)))
self.onroad_brightness_timer_param = self.params.get("OnroadScreenOffTimer", return_default=True)
self.rainbow_path = self.params.get_bool("RainbowMode")
+1
View File
@@ -62,6 +62,7 @@ class UIState(UIStateSP):
"liveParameters",
"testJoystick",
"rawAudioData",
"liveTracks",
] + self.sm_services_ext
)
+2
View File
@@ -109,6 +109,8 @@ def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
def initialize_params(params) -> list[dict[str, Any]]:
keys: list = []
keys.append("RadarTracks")
# hyundai
keys.extend([
"HyundaiLongitudinalTuning",
+343
View File
@@ -0,0 +1,343 @@
#!/usr/bin/env python3
import argparse
import concurrent.futures
import os
import signal
import subprocess
import sys
import time
from collections import Counter, defaultdict
import cereal.messaging as messaging
from msgq.ipc_pyx import IpcError
from opendbc.car.tests.routes import CarTestRoute, routes
from openpilot.tools.radar.custom_routes import CUSTOM_ROUTES
from openpilot.tools.radar.radar_helpers import (
build_seen_address_map,
get_radar_spec,
is_exclusive_full_range_match,
)
REPLAY_PATH = os.path.join(os.path.dirname(__file__), "..", "replay", "replay")
REPLAY_PLAYBACK_SPEED = "3.0"
REPLAY_ALLOW_SERVICES = "can"
DETECTION_TIMEOUT_SECONDS = 20.0
DETECTION_MIN_HITS = 10
SOCKET_WAIT_TIMEOUT_SECONDS = 10.0
CAR_MODEL_WIDTH = 38
RADAR_TYPE_WIDTH = 10
ANSI_RESET = "\033[0m"
ANSI_BOLD = "\033[1m"
ANSI_DIM = "\033[2m"
ANSI_RED = "\033[31m"
ANSI_GREEN = "\033[32m"
ANSI_YELLOW = "\033[33m"
ANSI_BLUE = "\033[34m"
ANSI_MAGENTA = "\033[35m"
ANSI_CYAN = "\033[36m"
RADAR_FAMILY_COLORS = {
"RADAR_500_51F": ANSI_CYAN,
"RADAR_210_21F": ANSI_YELLOW,
"RADAR_3A5_3C4": ANSI_GREEN,
"RADAR_602_611": ANSI_BLUE,
}
def build_arg_parser() -> argparse.ArgumentParser:
parser = argparse.ArgumentParser(
description="Replay Hyundai/Kia/Genesis test routes and auto-detect radar type.",
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
)
parser.add_argument("--routes", action="store_true",
help="Use Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.")
parser.add_argument("--custom-routes", action="store_true",
help="Use routes from tools/radar/custom_routes.py.")
parser.add_argument("--route", action="append", default=[],
help="Specific route(s) to test. If omitted, use Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.")
parser.add_argument("--data-dir", default=None,
help="Optional local directory of route data to pass through to replay.")
parser.add_argument("--timeout", type=float, default=DETECTION_TIMEOUT_SECONDS,
help="Seconds to wait for a radar-family detection before giving up on a route.")
parser.add_argument("--min-hits", type=int, default=DETECTION_MIN_HITS,
help="Minimum CAN hits in a radar-family address range before considering it detected.")
parser.add_argument("--playback", default=REPLAY_PLAYBACK_SPEED,
help="Replay playback speed.")
parser.add_argument("--limit", type=int, default=None,
help="Only process the first N matching routes.")
parser.add_argument("--jobs", type=int, default=1,
help="Number of routes to process in parallel.")
parser.add_argument("--prefix", default="auto-radar-detect",
help="Base OPENPILOT_PREFIX to isolate replay sockets.")
return parser
def get_hkg_routes() -> list[CarTestRoute]:
hkg_prefixes = ("HYUNDAI_", "KIA_", "GENESIS_")
return [
route for route in routes
if route.car_model is not None and getattr(route.car_model, "name", str(route.car_model)).startswith(hkg_prefixes)
]
def get_selected_routes(args: argparse.Namespace) -> list[CarTestRoute]:
selected_sources = int(bool(args.route)) + int(bool(args.routes)) + int(bool(args.custom_routes))
if selected_sources > 1:
raise ValueError("Use only one of --route, --routes, or --custom-routes.")
if args.route:
route_map = {route.route: route for route in get_hkg_routes()}
selected = []
for route_name in args.route:
selected.append(route_map.get(route_name, CarTestRoute(route_name, None)))
return selected[:args.limit] if args.limit is not None else selected
if args.custom_routes:
selected = [CarTestRoute(route.route, route.car_model) for route in CUSTOM_ROUTES]
return selected[:args.limit] if args.limit is not None else selected
selected = get_hkg_routes()
return selected[:args.limit] if args.limit is not None else selected
def terminate_process(proc: subprocess.Popen) -> None:
if proc.poll() is not None:
return
proc.terminate()
try:
proc.wait(timeout=5)
except subprocess.TimeoutExpired:
proc.kill()
proc.wait(timeout=5)
def start_replay(route: str, prefix: str, args: argparse.Namespace) -> subprocess.Popen:
cmd = [
REPLAY_PATH,
"--no-vipc",
"--no-loop",
"--playback", args.playback,
"--allow", REPLAY_ALLOW_SERVICES,
"--prefix", prefix,
]
if args.data_dir:
cmd.extend(["--data_dir", args.data_dir])
cmd.append(route)
env = os.environ.copy()
env["OPENPILOT_PREFIX"] = prefix
return subprocess.Popen(
cmd,
cwd=os.path.dirname(REPLAY_PATH),
env=env,
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL,
preexec_fn=os.setsid if os.name != "nt" else None,
)
def stop_replay(proc: subprocess.Popen) -> None:
if proc.poll() is not None:
return
if os.name != "nt":
try:
os.killpg(proc.pid, signal.SIGTERM)
except ProcessLookupError:
return
else:
proc.terminate()
try:
proc.wait(timeout=5)
except subprocess.TimeoutExpired:
if os.name != "nt":
try:
os.killpg(proc.pid, signal.SIGKILL)
except ProcessLookupError:
return
else:
proc.kill()
proc.wait(timeout=5)
def wait_for_can_socket(prefix: str, timeout: float) -> messaging.SubSocket:
socket_path = os.path.join("/tmp", f"msgq_{prefix}", "can")
started = time.monotonic()
while True:
if os.path.exists(socket_path):
break
if time.monotonic() - started > timeout:
raise TimeoutError(f"Timed out waiting for CAN socket at {socket_path}")
time.sleep(0.1)
while True:
try:
return messaging.sub_sock("can", conflate=False, timeout=100)
except IpcError:
if time.monotonic() - started > timeout:
raise
time.sleep(0.1)
def detect_radar_family(route: CarTestRoute, route_idx: int, args: argparse.Namespace) -> dict:
prefix = f"{args.prefix}-{os.getpid()}-{route_idx}"
os.environ["OPENPILOT_PREFIX"] = prefix
messaging.reset_context()
proc = start_replay(route.route, prefix, args)
logcan = wait_for_can_socket(prefix, min(args.timeout, SOCKET_WAIT_TIMEOUT_SECONDS))
counts = Counter()
buses = defaultdict(set)
seen_addresses = build_seen_address_map()
started = time.monotonic()
detected = None
try:
while True:
if proc.poll() is not None and (time.monotonic() - started) > 1.0:
break
if time.monotonic() - started > args.timeout:
break
msgs = messaging.drain_sock(logcan, wait_for_one=True)
for msg in msgs:
for can_msg in msg.can:
radar_spec = get_radar_spec(can_msg.address)
if radar_spec is not None:
counts[radar_spec.name] += 1
buses[radar_spec.name].add(can_msg.src)
seen_addresses[radar_spec.name].add(can_msg.address)
if counts[radar_spec.name] >= args.min_hits and is_exclusive_full_range_match(radar_spec, seen_addresses):
detected = radar_spec.name
if detected is not None:
break
if detected is not None:
break
if detected is not None:
break
finally:
stop_replay(proc)
dominant_family = detected
return {
"route": route.route,
"car_model": str(route.car_model) if route.car_model is not None else "UNKNOWN",
"detected": dominant_family or "NONE",
"counts": dict(counts),
"buses": {name: sorted(bus_set) for name, bus_set in buses.items()},
"seen_addresses": {name: sorted(addresses) for name, addresses in seen_addresses.items()},
}
def print_results(results: list[dict]) -> None:
print(f"{'Car Model':<{CAR_MODEL_WIDTH}} {'Detected':<{RADAR_TYPE_WIDTH}} Route")
print("-" * (CAR_MODEL_WIDTH + RADAR_TYPE_WIDTH + 9 + 60))
for result in results:
detected_plain = "" if result["detected"] == "NONE" else result["detected"]
model = format_car_model(result["car_model"])[:CAR_MODEL_WIDTH]
detected = f"{detected_plain:<{RADAR_TYPE_WIDTH}}"
if supports_color() and detected_plain:
detected = f"{colorize_detected(detected_plain)}" + " " * max(0, RADAR_TYPE_WIDTH - len(detected_plain))
print(f"{model:<{CAR_MODEL_WIDTH}} {detected} {result['route']}")
def supports_color() -> bool:
return sys.stdout.isatty() and os.getenv("TERM") not in (None, "dumb")
def format_car_model(car_model: object) -> str:
return str(car_model) if car_model is not None else "UNKNOWN"
def colorize_detected(detected: str) -> str:
if detected == "NONE":
return ""
if not supports_color():
return detected
color = RADAR_FAMILY_COLORS.get(detected, ANSI_MAGENTA)
return f"{color}{detected}{ANSI_RESET}"
def print_progress_line(index: int, total: int, route: CarTestRoute, detected: str) -> None:
left = f"[{index}/{total}]"
model = format_car_model(route.car_model)[:CAR_MODEL_WIDTH]
detected_plain = "" if detected == "NONE" else detected
route_text = route.route
if supports_color():
left = f"{ANSI_DIM}{left}{ANSI_RESET}"
model = f"{ANSI_CYAN}{model:<{CAR_MODEL_WIDTH}}{ANSI_RESET}"
detected_text = colorize_detected(detected_plain).ljust(len(colorize_detected(detected_plain)))
route_text = f"{ANSI_DIM}{route_text}{ANSI_RESET}"
else:
model = f"{model:<{CAR_MODEL_WIDTH}}"
detected_text = detected_plain
detected_text = f"{detected_plain:<{RADAR_TYPE_WIDTH}}" if not supports_color() else (
f"{colorize_detected(detected_plain)}" + " " * max(0, RADAR_TYPE_WIDTH - len(detected_plain))
)
print(f"{left} {model} {detected_text} {route_text}")
def main() -> int:
args = build_arg_parser().parse_args(sys.argv[1:])
try:
selected_routes = get_selected_routes(args)
except ValueError as e:
print(str(e), file=sys.stderr)
return 2
if not selected_routes:
print("No matching routes found.", file=sys.stderr)
return 1
results = []
total = len(selected_routes)
jobs = max(1, min(args.jobs, total))
header_model = "Car Model"
header_detected = "Radar"
if supports_color():
header_model = f"{ANSI_BOLD}{header_model:<{CAR_MODEL_WIDTH}}{ANSI_RESET}"
header_detected = f"{ANSI_BOLD}{header_detected:<{RADAR_TYPE_WIDTH}}{ANSI_RESET}"
else:
header_model = f"{header_model:<{CAR_MODEL_WIDTH}}"
header_detected = f"{header_detected:<{RADAR_TYPE_WIDTH}}"
print(f" {header_model} {header_detected} Route")
if jobs == 1:
for idx, route in enumerate(selected_routes):
result = detect_radar_family(route, idx, args)
print_progress_line(idx + 1, total, route, result['detected'])
results.append((idx, result))
else:
with concurrent.futures.ProcessPoolExecutor(max_workers=jobs) as executor:
future_to_job = {
executor.submit(detect_radar_family, route, idx, args): (idx, route)
for idx, route in enumerate(selected_routes)
}
completed = 0
for future in concurrent.futures.as_completed(future_to_job):
idx, route = future_to_job[future]
result = future.result()
completed += 1
print_progress_line(completed, total, route, result['detected'])
results.append((idx, result))
results = sorted(
(result for _, result in results),
key=lambda result: (result["car_model"], result["route"]),
)
print()
print_results(results)
return 0
if __name__ == "__main__":
raise SystemExit(main())
+64
View File
@@ -0,0 +1,64 @@
#!/usr/bin/env python3
from dataclasses import dataclass
@dataclass(frozen=True)
class CustomRoute:
car_model: str
route: str
CUSTOM_ROUTES = [
CustomRoute("HYUNDAI_ELANTRA_HEV_2024", "07a48901db7b2503|0000012d--61aba9f832"),
CustomRoute("HYUNDAI_IONIQ_5", "c5a5f79df9b6a084/0000005d--24c0a7a7ee"),
CustomRoute("HYUNDAI_IONIQ_5", "90950642f47cf05b/00000681--6a904aaa98"),
CustomRoute("HYUNDAI_IONIQ_5", "102161de13e822d1/0000000d--f74a52a624"),
CustomRoute("HYUNDAI_PALISADE_2023", "17ef028fec0ccf81/00000010--dba8455015"),
CustomRoute("HYUNDAI_PALISADE_2023", "6b8052042ba3ee05/00000199--3ed3fb5799"),
CustomRoute("KIA_EV6", "455a0ab75ce5c1e0/000000d1--098f32028c"),
CustomRoute("KIA_EV6", "6d2092783bf67457/0000008c--5caf525813"),
CustomRoute("KIA_EV6", "4961cb0f7bdd77a2/00000105--b636ed2f65"),
CustomRoute("KIA_K8_HEV_1ST_GEN", "78ad5150de133637|2023-09-13--16-15-57"),
CustomRoute("GENESIS_GV60_EV_1ST_GEN", "b1e441e63f1c99dd|0000009d--14cde5aeaf"),
CustomRoute("HYUNDAI_IONIQ_6", "faa815c21279fef7/00000187--ce36ae7c11"),
CustomRoute("HYUNDAI_IONIQ_6", "faa815c21279fef7/00000191--e5c3ffb55b"),
CustomRoute("HYUNDAI_KONA_2ND_GEN", "32025f26789d8fab/00000022--a499e8ffa3"),
CustomRoute("HYUNDAI_KONA_EV_2ND_GEN", "1618132d68afc876/00000021--bf0f957649"),
CustomRoute("HYUNDAI_KONA_HEV_2ND_GEN", "97ca61196eb73e0d/00000052--4555329470"),
CustomRoute("HYUNDAI_SANTA_FE_HEV_5TH_GEN", "d54302f1d5e7a7cc|00000656--5dae9f54a7"),
CustomRoute("HYUNDAI_SONATA_2024", "4267ea8a353cdb36/00000262--8a427003c7"),
CustomRoute("KIA_CARNIVAL_HEV_4TH_GEN_2026", "7b8cc7bb46000e53/0000000b--2abacaff78"),
CustomRoute("KIA_EV6_2025", "48c27f77f9fd1a9b|00000199--193ee1ba20"),
CustomRoute("KIA_EV9", "ccfd4a1af758ee73/00000091--7fa49719a5"),
CustomRoute("KIA_K4_2025", "baf39eeaba1217ca/00000002--b36e3fa031"),
CustomRoute("KIA_K5_2025", "c4a804b067623789/0000007c--163f831540"),
CustomRoute("KIA_NIRO_EV_2ND_GEN", "80b8e9a6ad0acec3/0000032d--8379197bd3"),
CustomRoute("KIA_NIRO_EV_2ND_GEN", "80b8e9a6ad0acec3/0000032c--0055eeee96"),
CustomRoute("KIA_NIRO_HEV_2ND_GEN", "0d4257d1c6741384/00000067--42230e4b8d"),
CustomRoute("HYUNDAI_KONA_EV_2022", "e174e1a0b92263ed/00000003--d0591c14ec"),
CustomRoute("HYUNDAI_KONA_EV_2022", "e174e1a0b92263ed/00000001--18955a967e"),
CustomRoute("KIA_CEED_PHEV", "26064e18b24ae44c/00000000--4eb95fc137"),
CustomRoute("HYUNDAI_IONIQ", "ac27c9808ac91710/00000002--e0aab81e4d"),
CustomRoute("KIA_K7_2017", "74fbff45aa20fe9e/00000010--6f173d5799"),
CustomRoute("KIA_NIRO_EV", "b27d9eafeb61976e/00000233--03a9c0858c"),
CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000157--ecaf77a801"),
CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000158--f7e70e1435"),
CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000154--3471fc4ab3"),
CustomRoute("KIA_SORENTO_4TH_GEN", "bf42073ef09e0af2/00000017--3edc1f8259"),
CustomRoute("KIA_SORENTO_HEV_4TH_GEN", "833262b0c9e4016a/0000004a--0227a058e3"),
CustomRoute("HYUNDAI_PALISADE", "662bedbf8453b81e/00000124--7dc358f20c"),
CustomRoute("HYUNDAI_SONATA", "8f52823c702cb300/00000177--177d1aa6ff"),
CustomRoute("HYUNDAI_SONATA_HYBRID", "e3ae9e987a2a4e57/0000002b--30a6963e88"),
CustomRoute("HYUNDAI_ELANTRA_HEV_2021", "7bc0de9da607c543/00000031--4cb0d8aca5"),
CustomRoute("HYUNDAI_ELANTRA_HEV_2021", "7bc0de9da607c543/00000042--302b371266"),
CustomRoute("HYUNDAI_IONIQ_HEV_2022", "4a975fc1d9e71801/00000005--50d176008a"),
CustomRoute("HYUNDAI_IONIQ_9", "71e4e67d29034771/00000014--25fc6b216f"),
CustomRoute("HYUNDAI_IONIQ_9", "71e4e67d29034771/0000000e--d3b19ad6f6/10"),
CustomRoute("HYUNDAI_SANTA_CRUZ_2025", "6e7904b03a4aafc2/00000010--31034184b6"),
CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "26da1db30eff4fcc/00000061--a60470e363"),
CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "a6d25e95d936fdc4/000002ed--89d7944c52"),
CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "6722665fbbf2a644/00000534--4ab7b733c5"),
CustomRoute("HYUNDAI_TUCSON_HEV_2025", "5868ec006e2bb61e/00000021--7d10df95e8"),
CustomRoute("KIA_SPORTAGE_5TH_GEN", "ce05e32158479f42/000003f4--9543087719"),
CustomRoute("KIA_SPORTAGE_HEV_2026", "343d5e350abaedcf/00000020--1c864750b6"),
]
+209
View File
@@ -0,0 +1,209 @@
#!/usr/bin/env python3
import math
import os
import tempfile
from dataclasses import dataclass
from opendbc.can.parser import CANParser
RADAR_500_51F_DBC_TEMPLATE = """
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 8 RADAR
SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
"""
RADAR_3A5_3C4_DBC_TEMPLATE = """
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 24 RADAR
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
SG_ NEW_SIGNAL_1 : 25|2@0+ (1,0) [0|3] "" XXX
SG_ NEW_SIGNAL_3 : 28|2@0+ (1,0) [0|3] "" XXX
SG_ COUNTER_3 : 31|2@0+ (1,0) [0|3] "" XXX
SG_ NEW_SIGNAL_2 : 38|7@0- (1,0) [0|127] "" XXX
SG_ COUNTER_256 : 47|8@0+ (1,0) [0|255] "" XXX
SG_ NEW_SIGNAL_6 : 51|4@0+ (1,0) [0|15] "" XXX
SG_ STATE : 54|3@0+ (1,0) [0|7] "" XXX
SG_ NEW_SIGNAL_8 : 62|7@0- (1,0) [0|127] "" XXX
SG_ LONG_DIST : 63|12@1+ (0.05,0) [0|8191] "m" XXX
SG_ LAT_DIST : 76|12@1- (0.05,0) [0|127] "" XXX
SG_ REL_SPEED : 88|14@1- (0.01,0) [0|16383] "" XXX
SG_ NEW_SIGNAL_4 : 103|2@0+ (1,0) [0|3] "" XXX
SG_ LAT_DIST_ACCEL : 104|13@1- (1,0) [0|8191] "" XXX
SG_ REL_ACCEL : 118|10@1- (0.02,0) [0|1023] "" XXX
SG_ NEW_SIGNAL_5 : 133|4@0+ (1,0) [0|15] "" XXX
"""
RADAR_210_21F_DBC_TEMPLATE = """
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 32 RADAR
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
SG_ 1_COUNTER_255 : 47|8@0+ (1,0) [0|255] "" XXX
SG_ 1_STATE_ALT : 51|4@0+ (1,0) [0|15] "" XXX
SG_ 1_STATE : 55|4@0+ (1,0) [0|15] "" XXX
SG_ 1_NEW_SIGNAL_3 : 63|8@0- (1,0) [0|255] "" XXX
SG_ 1_LONG_DIST : 64|12@1+ (0.05,0) [0|4095] "" XXX
SG_ 1_LAT_DIST : 76|12@1- (0.05,0) [0|4095] "" XXX
SG_ 1_REL_SPEED : 88|14@1- (0.01,0) [0|16383] "" XXX
SG_ 1_NEW_SIGNAL_1 : 102|2@1+ (1,0) [0|3] "" XXX
SG_ 1_LAT_ACCEL : 104|13@1- (1,0) [0|8191] "" XXX
SG_ 1_REL_ACCEL : 118|10@1- (1,0) [0|1023] "" XXX
SG_ 2_COUNTER_255 : 175|8@0+ (1,0) [0|255] "" XXX
SG_ 2_STATE_ALT : 179|4@0+ (1,0) [0|15] "" XXX
SG_ 2_STATE : 183|4@0+ (1,0) [0|15] "" XXX
SG_ 2_NEW_SIGNAL_3 : 191|8@0- (1,0) [0|255] "" XXX
SG_ 2_LONG_DIST : 192|12@1+ (0.05,0) [0|4095] "" XXX
SG_ 2_LAT_DIST : 204|12@1- (0.05,0) [0|4095] "" XXX
SG_ 2_REL_SPEED : 216|14@1- (0.01,0) [0|65535] "" XXX
SG_ 2_NEW_SIGNAL_1 : 230|2@1+ (1,0) [0|3] "" XXX
SG_ 2_LAT_ACCEL : 232|13@1- (1,0) [0|8191] "" XXX
SG_ 2_REL_ACCEL : 246|10@1- (1,0) [0|1023] "" XXX
"""
RADAR_602_611_DBC_TEMPLATE = """
BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 8 RADAR
SG_ 1_DISTANCE : 0|10@1+ (0.25,0) [0|255.75] "" XXX
SG_ 1_LATERAL : 10|11@1+ (0.03,-30.705) [-30.705|30.705] "" XXX
SG_ 1_SPEED : 21|10@1+ (0.25,-128) [-128|127.75] "" XXX
SG_ 2_DISTANCE : 31|10@1+ (0.25,0) [0|255.75] "" XXX
SG_ 2_LATERAL : 41|11@1+ (0.03,-30.705) [-30.705|30.705] "" XXX
SG_ 2_SPEED : 52|10@1+ (0.25,-128) [-128|127.75] "" XXX
SG_ COUNTER : 62|2@1+ (1,0) [0|3] "" XXX
"""
@dataclass(frozen=True)
class RadarSpec:
name: str
start_addr: int
msg_count: int
dbc_template: str
track_prefixes: tuple[str, ...]
@property
def end_addr(self) -> int:
return self.start_addr + self.msg_count - 1
def contains(self, address: int) -> bool:
return self.start_addr <= address <= self.end_addr
RADAR_SPECS = (
RadarSpec("RADAR_500_51F", 0x500, 32, RADAR_500_51F_DBC_TEMPLATE, ("",)),
RadarSpec("RADAR_210_21F", 0x210, 16, RADAR_210_21F_DBC_TEMPLATE, ("1_", "2_")),
RadarSpec("RADAR_3A5_3C4", 0x3A5, 32, RADAR_3A5_3C4_DBC_TEMPLATE, ("",)),
RadarSpec("RADAR_602_611", 0x602, 16, RADAR_602_611_DBC_TEMPLATE, ("1_", "2_")),
)
def build_seen_address_map() -> dict[str, set[int]]:
return {radar_spec.name: set() for radar_spec in RADAR_SPECS}
def get_radar_spec(address: int) -> RadarSpec | None:
for radar_spec in RADAR_SPECS:
if radar_spec.contains(address):
return radar_spec
return None
def is_exclusive_full_range_match(radar_spec: RadarSpec, seen_addresses: dict[str, set[int]]) -> bool:
expected_addresses = set(range(radar_spec.start_addr, radar_spec.end_addr + 1))
if seen_addresses[radar_spec.name] != expected_addresses:
return False
for other_spec in RADAR_SPECS:
if other_spec.name == radar_spec.name:
continue
other_expected_addresses = set(range(other_spec.start_addr, other_spec.end_addr + 1))
if seen_addresses[other_spec.name] == other_expected_addresses:
return False
return True
def get_radar_dbc_path(radar_spec: RadarSpec) -> str:
dbc_path = os.path.join(tempfile.gettempdir(), f"{radar_spec.name.lower()}_radar_ui.dbc")
dbc_content = "\n".join(
radar_spec.dbc_template.format(addr_dec=addr, addr_hex=f"{addr:x}")
for addr in range(radar_spec.start_addr, radar_spec.end_addr + 1)
)
if not os.path.exists(dbc_path) or open(dbc_path).read() != dbc_content:
with open(dbc_path, "w") as f:
f.write(dbc_content)
return dbc_path
def get_radar_can_parser(radar_spec: RadarSpec, bus: int) -> CANParser:
messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(radar_spec.start_addr, radar_spec.end_addr + 1)]
return CANParser(get_radar_dbc_path(radar_spec), messages, bus)
def get_track_storage_key(radar_spec: RadarSpec, bus: int, addr: int, track_prefix: str) -> tuple[str, int, int]:
if radar_spec.name in ("RADAR_500_51F", "RADAR_3A5_3C4"):
return (radar_spec.name, bus, addr)
track_index = int(track_prefix[0]) - 1
return (radar_spec.name, bus, addr * 2 + track_index)
def get_track_ts_nanos(parser: CANParser, msg_name: str, radar_spec: RadarSpec, track_prefix: str) -> int:
if radar_spec.name == "RADAR_602_611":
return parser.ts_nanos[msg_name][f"{track_prefix}DISTANCE"]
if radar_spec.name == "RADAR_210_21F":
return parser.ts_nanos[msg_name][f"{track_prefix}LONG_DIST"]
return parser.ts_nanos[msg_name]["LONG_DIST"]
def decode_radar_track(radar_spec: RadarSpec, track_msg, track_prefix: str) -> tuple[float, float, float, float]:
if radar_spec.name == "RADAR_602_611":
return (
track_msg[f"{track_prefix}DISTANCE"],
track_msg[f"{track_prefix}LATERAL"],
track_msg[f"{track_prefix}SPEED"],
float("nan"),
)
if radar_spec.name == "RADAR_210_21F":
return (
track_msg[f"{track_prefix}LONG_DIST"],
track_msg[f"{track_prefix}LAT_DIST"],
track_msg[f"{track_prefix}REL_SPEED"],
float("nan"),
)
if radar_spec.name == "RADAR_500_51F":
azimuth = math.radians(track_msg["AZIMUTH"])
long_dist = track_msg["LONG_DIST"]
return (
math.cos(azimuth) * long_dist,
0.5 * -math.sin(azimuth) * long_dist,
track_msg["REL_SPEED"],
track_msg["REL_ACCEL"],
)
return (
track_msg["LONG_DIST"],
track_msg["LAT_DIST"],
track_msg["REL_SPEED"],
track_msg["REL_ACCEL"],
)
def is_radar_track_valid(radar_spec: RadarSpec, track_msg, track_prefix: str) -> bool:
if radar_spec.name == "RADAR_602_611":
return track_msg[f"{track_prefix}DISTANCE"] != 255.75
if radar_spec.name == "RADAR_210_21F":
return track_msg[f"{track_prefix}STATE"] in (3, 4)
return track_msg["STATE"] in (3, 4)
+912
View File
@@ -0,0 +1,912 @@
#!/usr/bin/env python3
import argparse
import os
import signal
import subprocess
import sys
import time
from dataclasses import dataclass
import cv2
import numpy as np
import pyray as rl
import cereal.messaging as messaging
from opendbc.car.tests.routes import routes
from openpilot.common.basedir import BASEDIR
from openpilot.common.transformations.camera import DEVICE_CAMERAS
from openpilot.tools.radar.custom_routes import CUSTOM_ROUTES
from openpilot.tools.replay.lib.ui_helpers import (
UP,
BLACK,
GREEN,
Calibration,
get_blank_lid_overlay,
init_plots,
plot_lead,
plot_model,
to_topdown_pt,
)
from openpilot.tools.radar.radar_helpers import (
RADAR_SPECS,
build_seen_address_map,
decode_radar_track,
get_radar_can_parser,
get_radar_spec,
get_track_storage_key,
get_track_ts_nanos,
is_exclusive_full_range_match,
is_radar_track_valid,
)
from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
from msgq.visionipc import VisionIpcClient, VisionStreamType
os.environ['BASEDIR'] = BASEDIR
ANGLE_SCALE = 5.0
RADAR_TRACK_TIMEOUT_FRAMES = 10
RADAR_FORMAT_SWITCH_MISS_FRAMES = 30
RADAR_TRACK_RADIUS = 4
CAMERA_RADAR_Y_OFFSET = 25
RADAR_HEATMAP_DECAY = 0.975
RADAR_HEATMAP_ALPHA = 0.65
CAMERA_RADAR_HEATMAP_ALPHA = 0.45
REPLAY_PATH = os.path.join(os.path.dirname(__file__), "..", "replay", "replay")
REPLAY_SOCKET_WAIT_TIMEOUT_SECONDS = 10.0
REPLAY_SPEEDS = (0.2, 0.5, 1.0, 2.0, 4.0, 8.0)
CAMERA_DRAW_WIDTH = 640
CAMERA_DRAW_HEIGHT = 480
TOP_DOWN_DRAW_WIDTH = 384
PLOT_DRAW_WIDTH = 480
PLOT_DRAW_HEIGHT = 480
RADAR_HEATMAP_MODES = ("OFF", "TOP", "CAMERA", "BOTH")
CYAN = (90, 235, 255)
AMBER = (255, 210, 90)
SOFT_WHITE = (235, 235, 235)
SLATE = (140, 180, 210)
@dataclass
class RadarTrackPoint:
trackId: int
measured: bool = True
dRel: float = 0.0
yRel: float = 0.0
vRel: float = 0.0
aRel: float = 0.0
yvRel: float = float("nan")
def draw_radar_points(tracks, lid_overlay):
for track in tracks:
px, py = to_topdown_pt(track.dRel, -track.yRel)
if px != -1:
cv2.circle(lid_overlay, (py, px), RADAR_TRACK_RADIUS, 255, thickness=-1, lineType=cv2.LINE_AA)
def update_radar_heatmap(tracks, radar_heatmap):
radar_heatmap *= RADAR_HEATMAP_DECAY
for track in tracks:
px, py = to_topdown_pt(track.dRel, -track.yRel)
if px != -1:
cv2.circle(radar_heatmap, (py, px), RADAR_TRACK_RADIUS + 2, 255.0, thickness=-1, lineType=cv2.LINE_AA)
def update_radar_camera_heatmap(tracks, radar_heatmap, calibration, shape):
radar_heatmap *= RADAR_HEATMAP_DECAY
if calibration is None:
return
height, width = shape[:2]
for track in tracks:
if track.dRel <= 0.0:
continue
pt = calibration.car_space_to_bb(
np.asarray([track.dRel - RADAR_TO_CAMERA]),
np.asarray([-track.yRel]),
np.asarray([1.0]),
)
x, y = np.round(pt[0]).astype(int)
y += CAMERA_RADAR_Y_OFFSET
if 0 <= x < width and 0 <= y < height:
cv2.circle(radar_heatmap, (x, y), RADAR_TRACK_RADIUS + 3, 255.0, thickness=-1, lineType=cv2.LINE_AA)
def overlay_heatmap(img, radar_heatmap, alpha_scale):
radar_heat_uint8 = np.ascontiguousarray(np.clip(radar_heatmap, 0, 255).astype(np.uint8))
radar_heat_mask = radar_heat_uint8 > 0
if not np.any(radar_heat_mask):
return
heat_rgb = cv2.cvtColor(cv2.applyColorMap(radar_heat_uint8, cv2.COLORMAP_TURBO), cv2.COLOR_BGR2RGB).astype(np.float32)
img_rgb = img.astype(np.float32)
alpha = ((radar_heat_uint8.astype(np.float32) / 255.0) * alpha_scale)[..., None]
img[:] = np.where(
radar_heat_mask[..., None],
np.clip(img_rgb * (1.0 - alpha) + heat_rgb * alpha, 0, 255).astype(np.uint8),
img,
)
def draw_radar_points_camera(tracks, img, calibration):
if calibration is None:
return
for track in tracks:
if track.dRel <= 0.0:
continue
# Match the road-space projection convention used by other UI overlays.
pt = calibration.car_space_to_bb(
np.asarray([track.dRel - RADAR_TO_CAMERA]),
np.asarray([-track.yRel]),
np.asarray([1.0]),
)
x, y = np.round(pt[0]).astype(int)
y += CAMERA_RADAR_Y_OFFSET
if 0 <= x < img.shape[1] and 0 <= y < img.shape[0]:
cv2.circle(img, (x, y), RADAR_TRACK_RADIUS, (255, 255, 255), thickness=-1, lineType=cv2.LINE_AA)
def draw_loading_overlay(font, lines, camera_texture, top_down_texture, hor_mode, panel_x, panel_y):
rl.draw_texture_pro(
camera_texture,
rl.Rectangle(0, 0, camera_texture.width, camera_texture.height),
rl.Rectangle(0, 0, CAMERA_DRAW_WIDTH, CAMERA_DRAW_HEIGHT),
rl.Vector2(0, 0),
0.0,
rl.WHITE,
)
rl.draw_texture(top_down_texture, CAMERA_DRAW_WIDTH, 0, rl.WHITE) # noqa: TID251
if hor_mode:
panel_width = 620
panel_height = 300
else:
panel_width = 700
panel_height = 320
rl.draw_rectangle(panel_x - 20, panel_y - 30, panel_width, panel_height, rl.Color(0, 0, 0, 140))
rl.draw_rectangle_lines(panel_x - 20, panel_y - 30, panel_width, panel_height, rl.Color(255, 255, 255, 90))
for i, line in enumerate(lines):
if line:
rl.draw_text_ex(font, line, rl.Vector2(panel_x, panel_y + i * 40), 28 if i == 0 else 20, 0, rl.WHITE)
def start_replay(route: str, prefix: str, playback: str, data_dir: str | None, start_seconds: int) -> subprocess.Popen:
cmd = [
REPLAY_PATH,
"--playback", playback,
"--prefix", prefix,
]
if start_seconds > 0:
cmd.extend(["--start", str(start_seconds)])
if data_dir:
cmd.extend(["--data_dir", data_dir])
cmd.append(route)
env = os.environ.copy()
env["OPENPILOT_PREFIX"] = prefix
return subprocess.Popen(
cmd,
cwd=os.path.dirname(REPLAY_PATH),
env=env,
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL,
preexec_fn=os.setsid if os.name != "nt" else None,
)
def stop_replay(proc: subprocess.Popen | None) -> None:
if proc is not None and proc.poll() is None:
if os.name != "nt":
try:
os.killpg(proc.pid, signal.SIGTERM)
except ProcessLookupError:
pass
else:
proc.terminate()
try:
proc.wait(timeout=5)
except subprocess.TimeoutExpired:
if os.name != "nt":
try:
os.killpg(proc.pid, signal.SIGKILL)
except ProcessLookupError:
pass
else:
proc.kill()
proc.wait(timeout=5)
# `replay` can leave helper processes like `replayd` behind, so do one
# targeted cleanup pass on shutdown as well.
for pattern in ("/tools/replay/replay", "replayd"):
try:
subprocess.run(
["pkill", "-f", pattern],
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL,
check=False,
)
except FileNotFoundError:
pass
def wait_for_can_socket(prefix: str, timeout: float) -> None:
socket_path = os.path.join("/tmp", f"msgq_{prefix}", "can")
started = time.monotonic()
while not os.path.exists(socket_path):
if time.monotonic() - started > timeout:
raise TimeoutError(f"Timed out waiting for CAN socket at {socket_path}")
time.sleep(0.1)
def get_hkg_routes() -> list[tuple[str, str]]:
hkg_prefixes = ("HYUNDAI_", "KIA_", "GENESIS_")
return [
(route.route, getattr(route.car_model, "name", str(route.car_model)))
for route in routes
if route.car_model is not None and getattr(route.car_model, "name", str(route.car_model)).startswith(hkg_prefixes)
]
def get_custom_routes() -> list[tuple[str, str]]:
return [(route.route, route.car_model) for route in CUSTOM_ROUTES]
def make_submaster(addr):
return messaging.SubMaster(
[
'carState',
'longitudinalPlan',
'carControl',
'radarState',
'liveCalibration',
'controlsState',
'selfdriveState',
'liveTracks',
'modelV2',
'liveParameters',
'roadCameraState',
],
addr=addr,
)
def reset_radar_state():
return (
0,
None,
0,
{radar_spec.name: 0 for radar_spec in RADAR_SPECS},
build_seen_address_map(),
{},
0,
{},
{},
{},
)
def ui_thread(addr, route_entries=None, playback="1.0", data_dir=None, prefix="ui-replay", start_route_idx=0):
cv2.setNumThreads(1)
# Get monitor info before creating window
rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT)
rl.init_window(1, 1, "")
max_height = rl.get_monitor_height(0)
rl.close_window()
hor_mode = os.getenv("HORIZONTAL") is not None
hor_mode = True if max_height < 960 + 300 else hor_mode
if hor_mode:
size = (CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH + PLOT_DRAW_WIDTH, 960)
write_x = 5
write_y = 480
else:
size = (CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH, 960 + 300)
write_x = CAMERA_DRAW_WIDTH + 5
write_y = 970
rl.set_trace_log_level(rl.TraceLogLevel.LOG_ERROR)
rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT)
rl.init_window(size[0], size[1], "openpilot debug UI")
rl.set_target_fps(60)
# Load font
font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf")
font = rl.load_font_ex(font_path, 32, None, 0)
# Create textures for camera and top-down view
camera_image = rl.gen_image_color(640, 480, rl.BLACK)
camera_texture = rl.load_texture_from_image(camera_image)
rl.unload_image(camera_image)
# lid_overlay array is (lidar_x, lidar_y) = (384, 960)
# pygame treats first axis as width, so texture is 384 wide x 960 tall
# For raylib, we need to transpose to get (height, width) = (960, 384) for the RGBA array
top_down_image = rl.gen_image_color(UP.lidar_x, UP.lidar_y, rl.BLACK)
top_down_texture = rl.load_texture_from_image(top_down_image)
rl.unload_image(top_down_image)
current_route_idx = start_route_idx
current_route_name = None
current_route_model = None
replay_proc = None
current_start_seconds = 0
paused = False
state_checks_enabled = False
radar_heatmap_mode_idx = 0
current_playback = min(REPLAY_SPEEDS, key=lambda speed: abs(speed - float(playback)))
last_replay_started_at = time.monotonic()
playback_ready = False
loading_status = "Initializing UI"
def current_offset_seconds() -> int:
if paused or not playback_ready or replay_proc is None or replay_proc.poll() is not None:
return current_start_seconds
return max(0, int(current_start_seconds + (time.monotonic() - last_replay_started_at) * current_playback))
def connect_streams():
nonlocal replay_proc, current_route_name, current_route_model, current_route_idx, current_start_seconds, loading_status, paused, \
last_replay_started_at, current_playback, playback_ready
if route_entries:
current_route_name, current_route_model = route_entries[current_route_idx]
loading_status = f"Starting replay for route {current_route_idx + 1}/{len(route_entries)} at {current_start_seconds}s ({current_playback:.1f}x)"
stop_replay(replay_proc)
os.environ["OPENPILOT_PREFIX"] = prefix
messaging.reset_context()
replay_proc = start_replay(current_route_name, prefix, f"{current_playback:.1f}", data_dir, current_start_seconds)
paused = False
playback_ready = False
loading_status = "Waiting for CAN socket"
wait_for_can_socket(prefix, REPLAY_SOCKET_WAIT_TIMEOUT_SECONDS)
loading_status = "Connecting to messaging streams"
sm_local = make_submaster(addr)
logcan_local = messaging.sub_sock("can", addr=addr, conflate=False, timeout=100)
loading_status = "Waiting for road camera frames"
return sm_local, logcan_local
sm, logcan = connect_streams()
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = None
num_px = 0
calibration = None
(can_range_msg_count,
active_radar_format_name,
active_radar_format_miss_count,
radar_format_total_counts,
radar_format_seen_addresses,
radar_track_ids,
next_radar_track_id,
radar_tracks,
radar_track_last_seen,
radar_parsers) = reset_radar_state()
lid_overlay_blank = get_blank_lid_overlay(UP)
radar_heatmap = np.zeros_like(lid_overlay_blank, dtype=np.float32)
camera_radar_heatmap = np.zeros(img.shape[:2], dtype=np.float32)
# plots
name_to_arr_idx = {
"gas": 0,
"computer_gas": 1,
"user_brake": 2,
"computer_brake": 3,
"v_ego": 4,
"v_pid": 5,
"angle_steers_des": 6,
"angle_steers": 7,
"angle_steers_k": 8,
"steer_torque": 9,
"v_override": 10,
"v_cruise": 11,
"a_ego": 12,
"a_target": 13,
}
plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0.0, 75.0), (-3.0, 2.0)]
plot_names = [
["gas", "computer_gas", "user_brake", "computer_brake"],
["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"],
["v_ego", "v_override", "v_pid", "v_cruise"],
["a_ego", "a_target"],
]
plot_colors = [["b", "b", "g", "r", "y"], ["b", "g", "y", "r"], ["b", "g", "r", "y"], ["b", "r"]]
plot_styles = [["-", "-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-"]]
draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles)
# Palette for converting lid_overlay grayscale indices to RGBA colors
palette = np.zeros((256, 4), dtype=np.uint8)
palette[:, 3] = 255 # alpha
palette[1] = [255, 0, 0, 255] # RED
palette[2] = [0, 255, 0, 255] # GREEN
palette[3] = [0, 0, 255, 255] # BLUE
palette[4] = [255, 255, 0, 255] # YELLOW
palette[110] = [110, 110, 110, 255] # car_color (gray)
palette[255] = [255, 255, 255, 255] # WHITE
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
while not rl.window_should_close():
# ***** frame *****
if not vipc_client.is_connected():
vipc_client.connect(False)
rl.begin_drawing()
rl.clear_background(rl.Color(64, 64, 64, 255))
if rl.is_key_released(rl.KeyboardKey.KEY_Q):
rl.end_drawing()
stop_replay(replay_proc)
replay_proc = None
break
shift_down = rl.is_key_down(rl.KeyboardKey.KEY_LEFT_SHIFT) or rl.is_key_down(rl.KeyboardKey.KEY_RIGHT_SHIFT)
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_SPACE):
if paused:
sm, logcan = connect_streams()
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
else:
current_start_seconds = current_offset_seconds()
paused = True
loading_status = "Paused"
stop_replay(replay_proc)
replay_proc = None
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_RIGHT):
current_route_idx = (current_route_idx + 1) % len(route_entries)
current_start_seconds = 0
paused = False
(can_range_msg_count,
active_radar_format_name,
active_radar_format_miss_count,
radar_format_total_counts,
radar_format_seen_addresses,
radar_track_ids,
next_radar_track_id,
radar_tracks,
radar_track_last_seen,
radar_parsers) = reset_radar_state()
radar_heatmap.fill(0)
camera_radar_heatmap.fill(0)
sm, logcan = connect_streams()
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_LEFT):
current_route_idx = (current_route_idx - 1) % len(route_entries)
current_start_seconds = 0
paused = False
(can_range_msg_count,
active_radar_format_name,
active_radar_format_miss_count,
radar_format_total_counts,
radar_format_seen_addresses,
radar_track_ids,
next_radar_track_id,
radar_tracks,
radar_track_last_seen,
radar_parsers) = reset_radar_state()
radar_heatmap.fill(0)
camera_radar_heatmap.fill(0)
sm, logcan = connect_streams()
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_M):
current_start_seconds = max(0, current_offset_seconds() + (-60 if shift_down else 60))
paused = False
(can_range_msg_count,
active_radar_format_name,
active_radar_format_miss_count,
radar_format_total_counts,
radar_format_seen_addresses,
radar_track_ids,
next_radar_track_id,
radar_tracks,
radar_track_last_seen,
radar_parsers) = reset_radar_state()
radar_heatmap.fill(0)
camera_radar_heatmap.fill(0)
sm, logcan = connect_streams()
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_S):
current_start_seconds = max(0, current_offset_seconds() + (-10 if shift_down else 10))
paused = False
(can_range_msg_count,
active_radar_format_name,
active_radar_format_miss_count,
radar_format_total_counts,
radar_format_seen_addresses,
radar_track_ids,
next_radar_track_id,
radar_tracks,
radar_track_last_seen,
radar_parsers) = reset_radar_state()
sm, logcan = connect_streams()
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
if route_entries and (rl.is_key_released(rl.KeyboardKey.KEY_EQUAL) or rl.is_key_released(rl.KeyboardKey.KEY_KP_ADD)):
for speed in REPLAY_SPEEDS:
if speed > current_playback:
current_playback = speed
break
if not paused:
current_start_seconds = current_offset_seconds()
(can_range_msg_count,
active_radar_format_name,
active_radar_format_miss_count,
radar_format_total_counts,
radar_format_seen_addresses,
radar_track_ids,
next_radar_track_id,
radar_tracks,
radar_track_last_seen,
radar_parsers) = reset_radar_state()
radar_heatmap.fill(0)
camera_radar_heatmap.fill(0)
sm, logcan = connect_streams()
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
if route_entries and (rl.is_key_released(rl.KeyboardKey.KEY_MINUS) or rl.is_key_released(rl.KeyboardKey.KEY_KP_SUBTRACT)):
for speed in reversed(REPLAY_SPEEDS):
if speed < current_playback:
current_playback = speed
break
if not paused:
current_start_seconds = current_offset_seconds()
(can_range_msg_count,
active_radar_format_name,
active_radar_format_miss_count,
radar_format_total_counts,
radar_format_seen_addresses,
radar_track_ids,
next_radar_track_id,
radar_tracks,
radar_track_last_seen,
radar_parsers) = reset_radar_state()
radar_heatmap.fill(0)
camera_radar_heatmap.fill(0)
sm, logcan = connect_streams()
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
if rl.is_key_released(rl.KeyboardKey.KEY_C):
state_checks_enabled = not state_checks_enabled
if rl.is_key_released(rl.KeyboardKey.KEY_H):
radar_heatmap_mode_idx = (radar_heatmap_mode_idx + 1) % len(RADAR_HEATMAP_MODES)
if RADAR_HEATMAP_MODES[radar_heatmap_mode_idx] == "OFF":
radar_heatmap.fill(0)
camera_radar_heatmap.fill(0)
yuv_img_raw = vipc_client.recv()
if yuv_img_raw is None or not yuv_img_raw.data.any():
if replay_proc is not None and replay_proc.poll() is not None:
loading_status = f"Replay exited with code {replay_proc.poll()}"
loading_lines = [
f"{loading_status}{' (paused)' if paused else ''}",
f"Route {current_route_idx + 1}/{len(route_entries)}" if route_entries else "Connected to external replay",
f"Platform: {current_route_model}" if current_route_model is not None else "",
f"Route: {current_route_name}" if current_route_name is not None else "",
f"Offset: {current_offset_seconds()}s" if route_entries else "",
f"Playback: {current_playback:.1f}x" if route_entries else "",
f"Radar state checks: {'ON' if state_checks_enabled else 'OFF'}",
f"Radar heatmap: {RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]}",
"Keys: SPACE play/pause, RIGHT next, LEFT prev, M +/-60s, S +/-10s, +/- speed, C checks, H heatmap, Q quit" \
if route_entries else "Keys: C checks, H heatmap, Q quit",
]
draw_loading_overlay(font, loading_lines, camera_texture, top_down_texture, hor_mode, 80, 160)
rl.end_drawing()
continue
if not playback_ready:
playback_ready = True
last_replay_started_at = time.monotonic()
loading_status = "Streaming"
lid_overlay = lid_overlay_blank.copy()
top_down = top_down_texture, lid_overlay
sm.update(0)
camera = DEVICE_CAMERAS[("tici", str(sm['roadCameraState'].sensor))]
# Use received buffer dimensions (full HEVC can have stride != buffer_len/rows due to VENUS padding)
h, w, stride = yuv_img_raw.height, yuv_img_raw.width, yuv_img_raw.stride
nv12_size = h * 3 // 2 * stride
imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8, count=nv12_size).reshape((h * 3 // 2, stride))
num_px = w * h
rgb = cv2.cvtColor(imgff[: h * 3 // 2, : w], cv2.COLOR_YUV2RGB_NV12)
qcam = "QCAM" in os.environ
bb_scale = 0.825 if qcam else 0.8
calib_scale = camera.fcam.width / 640.0
zoom_matrix = np.asarray([[bb_scale, 0.0, 0.0], [0.0, bb_scale, 0.0], [0.0, 0.0, 1.0]])
cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
intrinsic_matrix = camera.fcam.intrinsics
w = sm['controlsState'].lateralControlState.which()
if w == 'lqrStateDEPRECATED':
angle_steers_k = sm['controlsState'].lateralControlState.lqrStateDEPRECATED.steeringAngleDeg
elif w == 'indiState':
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg
else:
angle_steers_k = np.inf
plot_arr[:-1] = plot_arr[1:]
plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gasDEPRECATED
# TODO gas is deprecated
plot_arr[-1, name_to_arr_idx['computer_gas']] = np.clip(sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.torque * ANGLE_SCALE
# TODO brake is deprecated
plot_arr[-1, name_to_arr_idx['computer_brake']] = np.clip(-sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo
plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
if len(sm['longitudinalPlan'].accels):
plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].accels[0]
if sm.recv_frame['modelV2']:
plot_model(sm['modelV2'], img, calibration, top_down)
if sm.recv_frame['radarState']:
plot_lead(sm['radarState'], top_down)
if sm.updated['liveCalibration'] and num_px:
rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
can_packets = messaging.drain_sock(logcan)
if can_packets:
can_strings = [
(can_packet.logMonoTime, [(msg.address, msg.dat, msg.src) for msg in can_packet.can])
for can_packet in can_packets
]
detected_format_counts = {radar_spec.name: 0 for radar_spec in RADAR_SPECS}
for can_packet in can_packets:
for msg in can_packet.can:
radar_spec = get_radar_spec(msg.address)
if radar_spec is not None:
can_range_msg_count += 1
detected_format_counts[radar_spec.name] += 1
radar_format_total_counts[radar_spec.name] += 1
radar_format_seen_addresses[radar_spec.name].add(msg.address)
if radar_spec.name not in radar_parsers:
radar_parsers[radar_spec.name] = {}
if msg.src not in radar_parsers[radar_spec.name]:
radar_parsers[radar_spec.name][msg.src] = get_radar_can_parser(radar_spec, msg.src)
matching_formats = [
radar_spec.name
for radar_spec in RADAR_SPECS
if is_exclusive_full_range_match(radar_spec, radar_format_seen_addresses)
]
if len(matching_formats) == 1:
if active_radar_format_name == matching_formats[0]:
active_radar_format_miss_count = 0
elif active_radar_format_name is None:
active_radar_format_name = matching_formats[0]
active_radar_format_miss_count = 0
else:
active_radar_format_miss_count += 1
if active_radar_format_miss_count >= RADAR_FORMAT_SWITCH_MISS_FRAMES:
active_radar_format_name = matching_formats[0]
active_radar_format_miss_count = 0
elif len(matching_formats) == 0 and active_radar_format_name is not None:
active_radar_format_miss_count += 1
if active_radar_format_miss_count >= RADAR_FORMAT_SWITCH_MISS_FRAMES:
active_radar_format_name = None
active_radar_format_miss_count = 0
active_radar_spec = next((spec for spec in RADAR_SPECS if spec.name == active_radar_format_name), None)
if active_radar_spec is not None:
for bus, parser in radar_parsers.get(active_radar_spec.name, {}).items():
updated_addrs = parser.update(can_strings)
relevant_updated_addrs = {
track_addr for track_addr in updated_addrs
if active_radar_spec.start_addr <= track_addr <= active_radar_spec.end_addr
}
if not relevant_updated_addrs:
continue
for track_addr in relevant_updated_addrs:
msg_name = f"RADAR_TRACK_{track_addr:x}"
track_msg = parser.vl[msg_name]
for track_prefix in active_radar_spec.track_prefixes:
track_key = get_track_storage_key(active_radar_spec, bus, track_addr, track_prefix)
ts_nanos = get_track_ts_nanos(parser, msg_name, active_radar_spec, track_prefix)
if ts_nanos == 0:
continue
if state_checks_enabled and not is_radar_track_valid(active_radar_spec, track_msg, track_prefix):
radar_tracks.pop(track_key, None)
radar_track_last_seen.pop(track_key, None)
continue
d_rel, y_rel, v_rel, a_rel = decode_radar_track(active_radar_spec, track_msg, track_prefix)
if track_key not in radar_track_ids:
radar_track_ids[track_key] = next_radar_track_id
next_radar_track_id += 1
radar_tracks[track_key] = RadarTrackPoint(
trackId=radar_track_ids[track_key],
dRel=d_rel,
yRel=y_rel,
vRel=v_rel,
aRel=a_rel,
)
radar_track_last_seen[track_key] = sm.frame
stale_tracks = [
track_key for track_key, last_seen in radar_track_last_seen.items()
if (sm.frame - last_seen) > RADAR_TRACK_TIMEOUT_FRAMES
]
for track_key in stale_tracks:
radar_track_last_seen.pop(track_key, None)
radar_tracks.pop(track_key, None)
active_radar_tracks = [
track for track_key, track in radar_tracks.items()
if active_radar_format_name is not None and track_key[0] == active_radar_format_name
]
active_radar_buses = sorted({
track_key[1] for track_key in radar_tracks
if active_radar_format_name is not None and track_key[0] == active_radar_format_name
})
if len(active_radar_tracks) == 0:
active_radar_tracks = sm['liveTracks'].points
active_radar_buses = []
radar_heatmap_mode = RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]
if radar_heatmap_mode in ("TOP", "BOTH"):
update_radar_heatmap(active_radar_tracks, radar_heatmap)
if radar_heatmap_mode in ("CAMERA", "BOTH"):
update_radar_camera_heatmap(active_radar_tracks, camera_radar_heatmap, calibration, img.shape)
overlay_heatmap(img, camera_radar_heatmap, CAMERA_RADAR_HEATMAP_ALPHA)
# draw decoded radar tracks when present, otherwise fall back to liveTracks
draw_radar_points(active_radar_tracks, top_down[1])
draw_radar_points_camera(active_radar_tracks, img, calibration)
# *** blits ***
# Update camera texture from numpy array
img_rgba = cv2.cvtColor(img, cv2.COLOR_RGB2RGBA)
rl.update_texture(camera_texture, rl.ffi.cast("void *", img_rgba.ctypes.data))
rl.draw_texture_pro(
camera_texture,
rl.Rectangle(0, 0, camera_texture.width, camera_texture.height),
rl.Rectangle(0, 0, CAMERA_DRAW_WIDTH, CAMERA_DRAW_HEIGHT),
rl.Vector2(0, 0),
0.0,
rl.WHITE,
)
# display alerts
rl.draw_text_ex(font, sm['selfdriveState'].alertText1, rl.Vector2(180, 150), 30, 0, rl.RED)
rl.draw_text_ex(font, sm['selfdriveState'].alertText2, rl.Vector2(180, 190), 20, 0, rl.RED)
# draw plots (texture is reused internally)
plot_texture = draw_plots(plot_arr)
if hor_mode:
rl.draw_texture_pro(
plot_texture,
rl.Rectangle(0, 0, plot_texture.width, plot_texture.height),
rl.Rectangle(CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH, 0, PLOT_DRAW_WIDTH, PLOT_DRAW_HEIGHT),
rl.Vector2(0, 0),
0.0,
rl.WHITE,
)
else:
rl.draw_texture(plot_texture, 0, 300, rl.WHITE) # noqa: TID251
# Convert lid_overlay to RGBA and update top_down texture
# lid_overlay is (384, 960), need to transpose to (960, 384) for row-major RGBA buffer
lid_rgba = palette[lid_overlay.T]
if radar_heatmap_mode in ("TOP", "BOTH"):
overlay_heatmap(lid_rgba[..., :3], radar_heatmap.T, RADAR_HEATMAP_ALPHA)
rl.update_texture(top_down_texture, rl.ffi.cast("void *", np.ascontiguousarray(lid_rgba).ctypes.data))
rl.draw_texture(top_down_texture, CAMERA_DRAW_WIDTH, 0, rl.WHITE) # noqa: TID251
SPACING = 20
lines = [
("ENABLED", GREEN if sm['selfdriveState'].enabled else BLACK),
("SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", CYAN),
("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), CYAN),
("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), CYAN),
(f"RADAR FORMAT: {active_radar_format_name or 'NONE'}", AMBER),
(f"RADAR CAN MSGS: {can_range_msg_count}", AMBER),
(f"RADAR TRACKS: {len(active_radar_tracks)}"
+ (f" (BUS {','.join(str(bus) for bus in active_radar_buses)})" if active_radar_buses else ""),
AMBER),
(f"RADAR STATE CHECKS: {'ON' if state_checks_enabled else 'OFF'}", AMBER),
(f"RADAR HEATMAP: {RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]}", AMBER),
(f"ROUTE: {current_route_name}" if current_route_name is not None else "", SLATE),
(f"PLATFORM: {current_route_model}" if current_route_model is not None else "", SLATE),
(f"OFFSET: {current_offset_seconds()}s" if route_entries else "", SOFT_WHITE),
(f"PLAYBACK: {current_playback:.1f}x" if route_entries else "", SOFT_WHITE),
(f"STATUS: {'PAUSED' if paused else 'PLAYING'}" if route_entries else "", SOFT_WHITE),
("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", SOFT_WHITE),
("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 2)) + " deg", SOFT_WHITE),
("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100.0, 2)) + " %", SOFT_WHITE),
("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), SOFT_WHITE),
]
hud_height = len(lines) * SPACING + 18
rl.draw_rectangle(write_x - 10, write_y - 12, 560, hud_height, rl.Color(8, 12, 18, 155))
rl.draw_rectangle(write_x - 10, write_y - 12, 4, hud_height, rl.Color(90, 235, 255, 255))
rl.draw_rectangle_lines(write_x - 10, write_y - 12, 560, hud_height, rl.Color(90, 235, 255, 80))
for i, line in enumerate(lines):
if line is not None:
color = rl.Color(line[1][0], line[1][1], line[1][2], 255)
rl.draw_text_ex(font, line[0], rl.Vector2(write_x, write_y + i * SPACING), 20, 0, color)
rl.end_drawing()
rl.unload_texture(camera_texture)
rl.unload_texture(top_down_texture)
rl.unload_font(font)
rl.close_window()
stop_replay(replay_proc)
def get_arg_parser():
parser = argparse.ArgumentParser(description="Show replay data in a UI.", formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("ip_address", nargs="?", default="127.0.0.1", help="The ip address on which to receive zmq messages.")
parser.add_argument("--route", default=None, help="Route to replay locally before opening the UI.")
parser.add_argument("--routes", action="store_true", help="Cycle Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.")
parser.add_argument("--custom-routes", nargs="?", type=int, const=1, default=None,
help="Cycle routes from tools/radar/custom_routes.py, optionally starting from a 1-based route index.")
parser.add_argument("--data-dir", default=None, help="Optional local route data directory to pass to replay.")
parser.add_argument("--playback", default="1.0", help="Replay playback speed when using --route.")
parser.add_argument("--prefix", default="ui-replay", help="OPENPILOT_PREFIX to use when launching replay from the UI.")
parser.add_argument("--frame-address", default=None, help="The frame address (fully qualified ZMQ endpoint for frames) on which to receive zmq messages.")
return parser
if __name__ == "__main__":
args = get_arg_parser().parse_args(sys.argv[1:])
selected_sources = int(args.route is not None) + int(args.routes) + int(args.custom_routes is not None)
if selected_sources > 1:
raise SystemExit("Use only one of --route, --routes, or --custom-routes.")
route_entries = None
start_route_idx = 0
if args.route is not None:
route_entries = [(args.route, "MANUAL_ROUTE")]
elif args.routes:
route_entries = get_hkg_routes()
elif args.custom_routes is not None:
route_entries = get_custom_routes()
start_route_idx = max(0, min(len(route_entries) - 1, args.custom_routes - 1))
if route_entries:
os.environ["OPENPILOT_PREFIX"] = args.prefix
messaging.reset_context()
elif args.ip_address != "127.0.0.1":
os.environ["ZMQ"] = "1"
messaging.reset_context()
ui_thread(args.ip_address, route_entries=route_entries, playback=args.playback, data_dir=args.data_dir, prefix=args.prefix, start_route_idx=start_route_idx)