mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-08 02:05:43 +08:00
no imp
This commit is contained in:
+1
-1
Submodule opendbc_repo updated: d922f8a875...372a1e7161
@@ -6,71 +6,62 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import math
|
||||
import pyray as rl
|
||||
from opendbc.car.hyundai.radar_interface import RADAR_3A5_3C4
|
||||
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)
|
||||
DBC_MOVING_COLOR = (190, 125, 255)
|
||||
DBC_UNKNOWN_COLOR = (154, 168, 184)
|
||||
DBC_MOTION_STATIONARY = 1
|
||||
DBC_MOTION_MOVING = 2
|
||||
|
||||
|
||||
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 is_preferred_radar_source(source) -> bool:
|
||||
return source.startAddress == RADAR_3A5_3C4.start_addr and source.endAddress == RADAR_3A5_3C4.end_addr
|
||||
|
||||
|
||||
def radar_track_is_stationary(v_rel: float, v_ego: float = 0.0) -> bool:
|
||||
return abs(v_ego + v_rel) <= STATIONARY_SPEED_THRESHOLD
|
||||
def preferred_radar_tracks(live_tracks):
|
||||
points = list(live_tracks.points)
|
||||
if any(int(track.sourceAddress) != 0 for track in points):
|
||||
return [
|
||||
track for track in points
|
||||
if RADAR_3A5_3C4.start_addr <= int(track.sourceAddress) <= RADAR_3A5_3C4.end_addr
|
||||
]
|
||||
|
||||
# Legacy messages have no per-point source metadata. They are unambiguous only
|
||||
# when 3A5-3C4 is the sole reported source (or tests provide no source list).
|
||||
sources = list(live_tracks.trackSources)
|
||||
if not sources or all(is_preferred_radar_source(source) for source in sources):
|
||||
return points
|
||||
return []
|
||||
|
||||
|
||||
def radar_track_display(v_rel: float, v_ego: float, motion_state: int) -> tuple[rl.Color, bool]:
|
||||
"""Prefer the radar's motion classification, falling back when it is unknown or unavailable."""
|
||||
def radar_track_display(motion_state: int) -> tuple[rl.Color, bool]:
|
||||
"""Color tracks exclusively from the radar's DBC motion classification."""
|
||||
if motion_state == DBC_MOTION_STATIONARY:
|
||||
return rl.Color(*NEUTRAL_COLOR, 255), True
|
||||
if motion_state == DBC_MOTION_MOVING:
|
||||
return rl.Color(*DBC_MOVING_COLOR, 255), False
|
||||
return radar_track_color(v_rel, v_ego), radar_track_is_stationary(v_rel, v_ego)
|
||||
return rl.Color(*DBC_UNKNOWN_COLOR, 255), False
|
||||
|
||||
|
||||
def format_radar_tracks_onroad_columns(live_tracks, v_ego: float = 0.0) -> tuple[str, str, str, str, str, str]:
|
||||
sources = sorted(live_tracks.trackSources, key=lambda source: (source.startAddress, source.endAddress, source.bus))
|
||||
sources = sorted(
|
||||
(source for source in live_tracks.trackSources if is_preferred_radar_source(source)),
|
||||
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)
|
||||
motion_states = [int(track.motionState) for track in live_tracks.points]
|
||||
if not any(state in (DBC_MOTION_STATIONARY, DBC_MOTION_MOVING) for state in motion_states):
|
||||
implementation_counts = [0, 0, 0, 0] # approaching, speed matched, stationary, receding
|
||||
for track in live_tracks.points:
|
||||
if radar_track_is_stationary(track.vRel, v_ego):
|
||||
implementation_counts[2] += 1
|
||||
elif abs(track.vRel) <= RELATIVE_SPEED_MOVING_THRESHOLD:
|
||||
implementation_counts[1] += 1
|
||||
elif track.vRel < 0.0:
|
||||
implementation_counts[0] += 1
|
||||
else:
|
||||
implementation_counts[3] += 1
|
||||
approaching, speed_matched, stationary, receding = implementation_counts
|
||||
return range_text, count_text, f"A {approaching}", f"= {speed_matched}", f"S {stationary}", f"R {receding}"
|
||||
motion_states = [int(track.motionState) for track in preferred_radar_tracks(live_tracks)]
|
||||
|
||||
moving_count = sum(state == DBC_MOTION_MOVING for state in motion_states)
|
||||
stationary_count = sum(state == DBC_MOTION_STATIONARY for state in motion_states)
|
||||
unknown_count = len(motion_states) - moving_count - stationary_count
|
||||
return range_text, count_text, f"M {moving_count}", f"S {stationary_count}", f"U {unknown_count}", ""
|
||||
return range_text, count_text, str(moving_count), str(stationary_count), str(unknown_count), ""
|
||||
|
||||
|
||||
class RadarTracksStatus:
|
||||
@@ -109,12 +100,7 @@ class RadarTracksStatus:
|
||||
status_colors = ()
|
||||
else:
|
||||
status = format_radar_tracks_onroad_columns(live_tracks, v_ego) if valid else ("", "none", "", "", "", "")
|
||||
has_dbc_motion = any(int(track.motionState) in (DBC_MOTION_STATIONARY, DBC_MOTION_MOVING) for track in live_tracks.points)
|
||||
status_colors = (
|
||||
(DBC_MOVING_COLOR, NEUTRAL_COLOR, DBC_UNKNOWN_COLOR, DBC_UNKNOWN_COLOR)
|
||||
if has_dbc_motion
|
||||
else (APPROACHING_COLOR, MATCHED_SPEED_COLOR, NEUTRAL_COLOR, RECEDING_COLOR)
|
||||
)
|
||||
status_colors = (DBC_MOVING_COLOR, NEUTRAL_COLOR, DBC_UNKNOWN_COLOR, DBC_UNKNOWN_COLOR)
|
||||
self._set_status(status, status_colors)
|
||||
|
||||
def reset(self) -> None:
|
||||
@@ -167,6 +153,9 @@ class RadarTracksStatus:
|
||||
for label, text in zip(self._labels, self._status, strict=True)
|
||||
]
|
||||
self._column_widths[1] = max(36, self._column_widths[1])
|
||||
for index in (2, 3, 4):
|
||||
if self._column_widths[index]:
|
||||
self._column_widths[index] = max(36, self._column_widths[index])
|
||||
active_widths = [width for width in self._column_widths if width]
|
||||
inner_width = sum(active_widths) + self.COLUMN_GAP * (len(active_widths) - 1)
|
||||
self._width = inner_width + self.HORIZONTAL_PADDING * 2
|
||||
@@ -184,7 +173,7 @@ class RadarTracks:
|
||||
highlighted_tracks = highlighted_tracks or {}
|
||||
highlighted_positions = {}
|
||||
|
||||
for track in live_tracks.points:
|
||||
for track in preferred_radar_tracks(live_tracks):
|
||||
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
|
||||
@@ -194,7 +183,7 @@ class RadarTracks:
|
||||
continue
|
||||
|
||||
x, y = pt[0] + screen_offset[0], pt[1] + screen_offset[1]
|
||||
color, stationary = radar_track_display(v_rel, v_ego, int(track.motionState))
|
||||
color, stationary = radar_track_display(int(track.motionState))
|
||||
radius = max(1, track_size - 4) if stationary else track_size
|
||||
track_id = int(track.trackId)
|
||||
highlight_color = highlighted_tracks.get(track_id)
|
||||
|
||||
@@ -1,47 +1,25 @@
|
||||
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, \
|
||||
radar_track_display
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import format_radar_tracks_onroad_columns, radar_track_display
|
||||
|
||||
|
||||
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_dbc_motion_colors():
|
||||
assert color_tuple(radar_track_display(2)[0]) == (190, 125, 255, 255)
|
||||
assert not radar_track_display(2)[1]
|
||||
assert color_tuple(radar_track_display(1)[0]) == (255, 255, 255, 255)
|
||||
assert radar_track_display(1)[1]
|
||||
|
||||
|
||||
def test_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_unknown_dbc_motion_uses_neutral_dbc_color():
|
||||
color, stationary = radar_track_display(0)
|
||||
|
||||
|
||||
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_dbc_motion_overrides_relative_speed_classification():
|
||||
assert color_tuple(radar_track_display(-20.0, 20.0, 2)[0]) == (190, 125, 255, 255)
|
||||
assert not radar_track_display(-20.0, 20.0, 2)[1]
|
||||
assert color_tuple(radar_track_display(0.0, 20.0, 1)[0]) == (255, 255, 255, 255)
|
||||
assert radar_track_display(0.0, 20.0, 1)[1]
|
||||
|
||||
|
||||
def test_unknown_dbc_motion_falls_back_to_relative_speed_classification():
|
||||
color, stationary = radar_track_display(-20.0, 20.0, 0)
|
||||
|
||||
assert color_tuple(color) == (255, 255, 255, 255)
|
||||
assert stationary
|
||||
assert color_tuple(color) == (*radar_tracks.DBC_UNKNOWN_COLOR, 255)
|
||||
assert not stationary
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_none():
|
||||
@@ -52,47 +30,48 @@ def test_format_radar_tracks_columns_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.trackSources = [{"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1, "trackCount": 2}]
|
||||
points = live_tracks.init("points", 2)
|
||||
points[0].motionState = 2
|
||||
points[1].motionState = 1
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("500-51F", "2", "M 1", "S 1", "U 0", "")
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == ("3A5-3C4", "2", "1", "1", "0", "")
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_sorts_ranges():
|
||||
def test_format_radar_tracks_columns_ignores_non_dbc_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},
|
||||
{"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1, "trackCount": 2},
|
||||
]
|
||||
points = live_tracks.init("points", 3)
|
||||
points[0].motionState = 2
|
||||
points[1].motionState = 0
|
||||
points[2].motionState = 7
|
||||
points[0].sourceAddress = 0x3A5
|
||||
points[1].motionState = 1
|
||||
points[1].sourceAddress = 0x3A6
|
||||
points[2].motionState = 0
|
||||
points[2].sourceAddress = 0x500
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks) == (
|
||||
"210-21F\n500-51F\n500-51F",
|
||||
"1\n2\n3",
|
||||
"M 1",
|
||||
"S 0",
|
||||
"U 2",
|
||||
"3A5-3C4",
|
||||
"2",
|
||||
"1",
|
||||
"1",
|
||||
"0",
|
||||
"",
|
||||
)
|
||||
|
||||
|
||||
def test_format_radar_tracks_columns_uses_implementation_when_dbc_motion_is_unavailable():
|
||||
def test_format_radar_tracks_columns_hides_non_dbc_source():
|
||||
live_tracks = car.RadarData.new_message()
|
||||
live_tracks.trackSources = [{"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 4}]
|
||||
points = live_tracks.init("points", 4)
|
||||
for point, v_rel in zip(points, (-5.0, 0.2, -20.0, 5.0), strict=True):
|
||||
point.vRel = v_rel
|
||||
point.motionState = 0
|
||||
point.sourceAddress = 0x500
|
||||
|
||||
assert format_radar_tracks_onroad_columns(live_tracks, v_ego=20.0) == (
|
||||
"500-51F", "4", "A 1", "= 1", "S 1", "R 1",
|
||||
)
|
||||
assert format_radar_tracks_onroad_columns(live_tracks, v_ego=20.0) == ("", "none", "", "", "", "")
|
||||
|
||||
|
||||
def test_draw_radar_tracks_applies_screen_offset(monkeypatch):
|
||||
@@ -128,7 +107,25 @@ def test_draw_radar_tracks_allows_unknown_acceleration(monkeypatch):
|
||||
|
||||
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))]
|
||||
assert drawn_colors == [(*radar_tracks.DBC_UNKNOWN_COLOR, 255)]
|
||||
|
||||
|
||||
def test_draw_radar_tracks_hides_non_dbc_source(monkeypatch):
|
||||
live_tracks = car.RadarData.new_message()
|
||||
point = live_tracks.init("points", 1)[0]
|
||||
point.dRel = 10
|
||||
point.yRel = 1
|
||||
point.vRel = -5
|
||||
point.motionState = 0
|
||||
point.sourceAddress = 0x500
|
||||
drawn_circles = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda *args: drawn_circles.append(args))
|
||||
|
||||
radar_tracks.RadarTracks().draw_radar_tracks(
|
||||
live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2,
|
||||
)
|
||||
|
||||
assert drawn_circles == []
|
||||
|
||||
|
||||
def test_draw_radar_tracks_shrinks_stationary_dots(monkeypatch):
|
||||
@@ -138,6 +135,7 @@ def test_draw_radar_tracks_shrinks_stationary_dots(monkeypatch):
|
||||
point.yRel = 1
|
||||
point.vRel = -20
|
||||
point.aRel = 0
|
||||
point.motionState = 1
|
||||
drawn_sizes = []
|
||||
monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_sizes.append(size))
|
||||
|
||||
|
||||
+37
-65
@@ -25,8 +25,6 @@ from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS, view_frame_from_device_frame
|
||||
from openpilot.common.transformations.orientation import rot_from_euler
|
||||
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RELATIVE_SPEED_MOVING_THRESHOLD, radar_track_color, \
|
||||
radar_track_is_stationary
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.tools.replay.lib.ui_helpers import Calibration, plot_model
|
||||
from opendbc.can import CANParser
|
||||
@@ -72,7 +70,7 @@ RADAR_DETAIL_SIGNALS = {
|
||||
"MOTION_STATE", "REL_LAT_SPEED", "ABS_SPEED", "WIDTH", "LENGTH", "ORIENTATION_ANGLE",
|
||||
"AGE", "COAST_AGE", "STATE_ALT", "TRACK_COUNTER",
|
||||
}
|
||||
TABLE_MODES = ("comparison", "kinematics", "object")
|
||||
TABLE_MODES = ("motion", "kinematics", "object")
|
||||
PLAYBACK_SPEEDS = (0.2, 0.5, 1.0, 2.0, 4.0, 8.0)
|
||||
TRACK_COUNT_FIELD_WIDTH = 3
|
||||
SOURCE_CIRCLE_RADIUS_SCALE = 0.8
|
||||
@@ -406,14 +404,6 @@ def toggle_source_filter(source_filters: dict[RadarSourceKey, tuple[bool, bool,
|
||||
source_filters[source_key] = (filters[0], filters[1], filters[2])
|
||||
|
||||
|
||||
def implementation_class(v_rel: float, v_ego: float) -> str:
|
||||
if radar_track_is_stationary(v_rel, v_ego):
|
||||
return "stationary"
|
||||
if abs(v_rel) <= RELATIVE_SPEED_MOVING_THRESHOLD:
|
||||
return "speed matched"
|
||||
return "approaching" if v_rel < 0.0 else "receding"
|
||||
|
||||
|
||||
def dbc_motion_class(motion_state: int | None) -> str:
|
||||
if motion_state is None:
|
||||
return "unknown"
|
||||
@@ -432,10 +422,8 @@ def dbc_unknown_raw_label(motion_state: int | None) -> str | None:
|
||||
return "DBC unknown" if motion_state is None else f"DBC raw={motion_state}"
|
||||
|
||||
|
||||
def display_track_color(track, v_ego: float, motion_states: dict[int, int], use_dbc_colors: bool) -> rl.Color:
|
||||
if use_dbc_colors:
|
||||
return dbc_motion_color(motion_states.get(int(track.trackId)))
|
||||
return radar_track_color(track.vRel, v_ego)
|
||||
def display_track_color(track, motion_states: dict[int, int]) -> rl.Color:
|
||||
return dbc_motion_color(motion_states.get(int(track.trackId)))
|
||||
|
||||
|
||||
def filter_tracks(tracks, motion_states: dict[int, int], track_locations: dict[int, tuple[int, int]], sources,
|
||||
@@ -823,8 +811,8 @@ def draw_track_popup(font, track, x: float, y: float, radius: float, bounds: rl.
|
||||
draw_text(font, label, label_x, label_y, 18, color)
|
||||
|
||||
|
||||
def draw_camera_tracks(font, calibration: Calibration | None, tracks, camera_rect: rl.Rectangle, v_ego: float,
|
||||
show_labels: bool, motion_states: dict[int, int], use_dbc_colors: bool,
|
||||
def draw_camera_tracks(font, calibration: Calibration | None, tracks, camera_rect: rl.Rectangle,
|
||||
show_labels: bool, motion_states: dict[int, int],
|
||||
track_locations: dict[int, tuple[int, int]], sources,
|
||||
projection_height: float, hovered_id: int | None, selected_id: int | None,
|
||||
preview_id: int | None = None) -> None:
|
||||
@@ -838,7 +826,7 @@ def draw_camera_tracks(font, calibration: Calibration | None, tracks, camera_rec
|
||||
x, y, radius = geometry
|
||||
dot_radius = max(4.5, radius * 0.68)
|
||||
|
||||
color = display_track_color(track, v_ego, motion_states, use_dbc_colors)
|
||||
color = display_track_color(track, motion_states)
|
||||
is_hovered = int(track.trackId) == hovered_id
|
||||
is_selected = int(track.trackId) == selected_id
|
||||
is_previewed = int(track.trackId) == preview_id and not is_selected
|
||||
@@ -858,8 +846,8 @@ def draw_camera_tracks(font, calibration: Calibration | None, tracks, camera_rec
|
||||
|
||||
def draw_fused_camera_mode(font, road_camera_view: CameraView, wide_camera_view: CameraView, device_camera,
|
||||
rpy_calib: np.ndarray, wide_from_device_euler: np.ndarray, full_rect: rl.Rectangle,
|
||||
tracks, v_ego: float, show_labels: bool, motion_states: dict[int, int],
|
||||
use_dbc_colors: bool, track_locations: dict[int, tuple[int, int]], sources,
|
||||
tracks, show_labels: bool, motion_states: dict[int, int],
|
||||
track_locations: dict[int, tuple[int, int]], sources,
|
||||
selected_id: int | None) -> int | None:
|
||||
wide_render_rect = rl.Rectangle(
|
||||
full_rect.x - full_rect.width * (FUSED_CAMERA_ZOOM - 1.0) / 2,
|
||||
@@ -901,10 +889,10 @@ def draw_fused_camera_mode(font, road_camera_view: CameraView, wide_camera_view:
|
||||
current_hovered_id = road_hovered_id if road_hovered_id is not None else wide_hovered_id
|
||||
selected_id = retain_selected_track_id(selected_id, current_hovered_id, tracks)
|
||||
|
||||
draw_camera_tracks(font, wide_calibration, wide_tracks, wide_content_rect, v_ego, show_labels, motion_states,
|
||||
use_dbc_colors, track_locations, sources, wide_projection_height, wide_hovered_id, selected_id)
|
||||
draw_camera_tracks(font, road_calibration, road_tracks, road_content_rect, v_ego, show_labels, motion_states,
|
||||
use_dbc_colors, track_locations, sources, road_projection_height, road_hovered_id, selected_id)
|
||||
draw_camera_tracks(font, wide_calibration, wide_tracks, wide_content_rect, show_labels, motion_states,
|
||||
track_locations, sources, wide_projection_height, wide_hovered_id, selected_id)
|
||||
draw_camera_tracks(font, road_calibration, road_tracks, road_content_rect, show_labels, motion_states,
|
||||
track_locations, sources, road_projection_height, road_hovered_id, selected_id)
|
||||
return selected_id
|
||||
|
||||
|
||||
@@ -923,8 +911,8 @@ def draw_model_line(points_x, points_y, center_x: float, car_y: float, longitudi
|
||||
previous = current
|
||||
|
||||
|
||||
def draw_top_down(font, rect: rl.Rectangle, tracks, v_ego: float, show_labels: bool, model,
|
||||
motion_states: dict[int, int], use_dbc_colors: bool, hide_moving: bool, hide_stationary: bool,
|
||||
def draw_top_down(font, rect: rl.Rectangle, tracks, show_labels: bool, model,
|
||||
motion_states: dict[int, int], hide_moving: bool, hide_stationary: bool,
|
||||
hide_unknown: bool, track_locations: dict[int, tuple[int, int]], sources,
|
||||
hovered_id: int | None, selected_id: int | None,
|
||||
preview_id: int | None = None) -> None:
|
||||
@@ -945,19 +933,11 @@ def draw_top_down(font, rect: rl.Rectangle, tracks, v_ego: float, show_labels: b
|
||||
label_size = measure_text_cached(font, label, 14)
|
||||
draw_text(font, label, scale_x - label_size.x - 10, y - 7, 14, MUTED)
|
||||
|
||||
if use_dbc_colors:
|
||||
legend = (
|
||||
(PURPLE, "moving", hide_moving),
|
||||
(WHITE, "stationary", hide_stationary),
|
||||
(MUTED, "unknown", hide_unknown),
|
||||
)
|
||||
else:
|
||||
legend = (
|
||||
(radar_track_color(-2.0, 10.0), "approaching", hide_moving),
|
||||
(radar_track_color(0.0, 10.0), "speed matched", hide_moving),
|
||||
(radar_track_color(2.0, 10.0), "receding", hide_moving),
|
||||
(radar_track_color(-10.0, 10.0), "stationary", hide_stationary),
|
||||
)
|
||||
legend = (
|
||||
(PURPLE, "moving", hide_moving),
|
||||
(WHITE, "stationary", hide_stationary),
|
||||
(MUTED, "unknown", hide_unknown),
|
||||
)
|
||||
legend_x = rect.x + rect.width - 150
|
||||
legend_y = rect.y + 50
|
||||
for index, (color, label, disabled) in enumerate(legend):
|
||||
@@ -996,7 +976,7 @@ def draw_top_down(font, rect: rl.Rectangle, tracks, v_ego: float, show_labels: b
|
||||
if geometry is None:
|
||||
continue
|
||||
x, y, radius = geometry
|
||||
color = display_track_color(track, v_ego, motion_states, use_dbc_colors)
|
||||
color = display_track_color(track, motion_states)
|
||||
is_hovered = int(track.trackId) == hovered_id
|
||||
is_selected = int(track.trackId) == selected_id
|
||||
is_previewed = int(track.trackId) == preview_id and not is_selected
|
||||
@@ -1036,7 +1016,7 @@ def dbc_track_state(state: int) -> str:
|
||||
return {0: "empty", 1: "tent 1", 2: "tent 2", 3: "measured", 4: "coasted", 7: "unresolved"}.get(state, str(state))
|
||||
|
||||
|
||||
def draw_track_table(font, rect: rl.Rectangle, tracks, v_ego: float, motion_states: dict[int, int], scroll: int,
|
||||
def draw_track_table(font, rect: rl.Rectangle, tracks, motion_states: dict[int, int], scroll: int,
|
||||
selected_id: int | None, hovered_id: int | None,
|
||||
track_signals: dict[int, DisplayTrackSignals], track_locations: dict[int, tuple[int, int]],
|
||||
table_mode: str) -> None:
|
||||
@@ -1062,8 +1042,8 @@ def draw_track_table(font, rect: rl.Rectangle, tracks, v_ego: float, motion_stat
|
||||
)
|
||||
else:
|
||||
columns = (
|
||||
("ID", 0.02), ("CAN", 0.09), ("DIST", 0.21), ("LAT", 0.32), ("REL V", 0.43),
|
||||
("IMPLEMENTATION", 0.56), ("DBC MOTION", 0.80),
|
||||
("ID", 0.02), ("CAN", 0.10), ("DIST", 0.24), ("LAT", 0.38), ("REL V", 0.52),
|
||||
("DBC MOTION", 0.70),
|
||||
)
|
||||
header_y = rect.y + 36
|
||||
for title, offset in columns:
|
||||
@@ -1109,12 +1089,11 @@ def draw_track_table(font, rect: rl.Rectangle, tracks, v_ego: float, motion_stat
|
||||
else:
|
||||
values = (
|
||||
(str(track.trackId), 0.02, TEXT),
|
||||
(can_location, 0.09, TEXT if can_address >= 0 else MUTED),
|
||||
(f"{track.dRel:6.1f}", 0.21, TEXT),
|
||||
(f"{track.yRel:+6.1f}", 0.32, TEXT),
|
||||
(f"{track.vRel:+6.1f}", 0.43, TEXT),
|
||||
(implementation_class(track.vRel, v_ego), 0.56, radar_track_color(track.vRel, v_ego)),
|
||||
(dbc_motion_class(motion_state), 0.80, dbc_motion_color(motion_state)),
|
||||
(can_location, 0.10, TEXT if can_address >= 0 else MUTED),
|
||||
(f"{track.dRel:6.1f}", 0.24, TEXT),
|
||||
(f"{track.yRel:+6.1f}", 0.38, TEXT),
|
||||
(f"{track.vRel:+6.1f}", 0.52, TEXT),
|
||||
(dbc_motion_class(motion_state), 0.70, dbc_motion_color(motion_state)),
|
||||
)
|
||||
for value, offset, color in values:
|
||||
draw_text(font, value, rect.x + rect.width * offset, y, 15, color)
|
||||
@@ -1464,7 +1443,7 @@ def fused_stream_loss_state(available_streams, missing_since: float | None,
|
||||
|
||||
|
||||
def draw_source_status(font, rect: rl.Rectangle, live_tracks, valid: bool, alive: bool, source_active: bool,
|
||||
show_labels: bool, data_source: str, use_dbc_colors: bool, camera_mode: str,
|
||||
show_labels: bool, data_source: str, camera_mode: str,
|
||||
both_cameras_available: bool,
|
||||
visible_tracks, motion_states: dict[int, int], track_locations: dict[int, tuple[int, int]],
|
||||
source_filters: dict[RadarSourceKey, tuple[bool, bool, bool]],
|
||||
@@ -1582,17 +1561,15 @@ def draw_source_status(font, rect: rl.Rectangle, live_tracks, valid: bool, alive
|
||||
chip_x = rect.x + 14
|
||||
chip_y = source_tooltip_anchor_y
|
||||
camera_label = {"fused": "FUSED", "wide 180": "WIDE"}.get(camera_mode, "ROAD")
|
||||
table_label = {"comparison": "COMP", "kinematics": "KIN", "object": "OBJ"}[table_mode]
|
||||
table_label = {"motion": "MOTION", "kinematics": "KIN", "object": "OBJ"}[table_mode]
|
||||
chip_specs = [
|
||||
("OPEN", GREEN, "route", "Open a route, or paste one with Cmd/Ctrl+V"),
|
||||
(source_title, status_color, "source", "Switch CAN / liveTracks source"),
|
||||
("DBC" if use_dbc_colors else "IMPL", PURPLE if use_dbc_colors else CYAN, "colors",
|
||||
"Switch DBC / implementation colors"),
|
||||
("LABELS ON" if show_labels else "LABELS", GREEN if show_labels else MUTED, "labels", "Show / hide all labels"),
|
||||
]
|
||||
if both_cameras_available:
|
||||
chip_specs.append((camera_label, CYAN, "camera", "Cycle road / wide / fused camera"))
|
||||
chip_specs.append((table_label, MUTED, "table", "Cycle comparison / kinematics / object table"))
|
||||
chip_specs.append((table_label, MUTED, "table", "Cycle motion / kinematics / object table"))
|
||||
clicked_action = source_clicked_action
|
||||
hovered_tooltip = source_hovered_tooltip or top_hovered_tooltip
|
||||
for label, color, action, tooltip in chip_specs:
|
||||
@@ -1669,7 +1646,6 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False, st
|
||||
show_labels = False
|
||||
source_filters: dict[RadarSourceKey, tuple[bool, bool, bool]] = {}
|
||||
use_can_source = True
|
||||
use_dbc_colors = True
|
||||
table_mode_index = 0
|
||||
table_scroll = 0
|
||||
selected_track_id = None
|
||||
@@ -1972,7 +1948,7 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False, st
|
||||
wide_from_device_euler = np.asarray(sm["liveCalibration"].wideFromDeviceEuler)
|
||||
selected_track_id = draw_fused_camera_mode(
|
||||
font, road_camera_view, wide_camera_view, fused_device_camera, rpy_calib, wide_from_device_euler,
|
||||
fused_bounds, tracks, v_ego, show_labels, motion_states, use_dbc_colors,
|
||||
fused_bounds, tracks, show_labels, motion_states,
|
||||
track_locations, selected_tracks.trackSources, selected_track_id,
|
||||
)
|
||||
fused_available_streams = set(road_camera_view.available_streams) | set(wide_camera_view.available_streams)
|
||||
@@ -1989,7 +1965,7 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False, st
|
||||
table_mode = TABLE_MODES[table_mode_index]
|
||||
clicked_action = draw_source_status(
|
||||
font, status_rect, selected_tracks, data_valid, data_alive,
|
||||
replay_process is not None or can_data_seen or live_data_seen, show_labels, data_source, use_dbc_colors,
|
||||
replay_process is not None or can_data_seen or live_data_seen, show_labels, data_source,
|
||||
"fused", both_camera_streams_available(fused_available_streams),
|
||||
tracks, motion_states, track_locations, source_filters, table_mode, rl.get_fps(),
|
||||
)
|
||||
@@ -2001,8 +1977,6 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False, st
|
||||
use_can_source = not use_can_source
|
||||
table_scroll = 0
|
||||
selected_track_id = None
|
||||
elif clicked_action == "colors":
|
||||
use_dbc_colors = not use_dbc_colors
|
||||
elif clicked_action == "labels":
|
||||
show_labels = not show_labels
|
||||
elif clicked_action == "camera":
|
||||
@@ -2089,20 +2063,20 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False, st
|
||||
WHITE,
|
||||
)
|
||||
|
||||
draw_camera_tracks(font, calibration, tracks, camera_draw_rect, v_ego, show_labels, motion_states, use_dbc_colors,
|
||||
draw_camera_tracks(font, calibration, tracks, camera_draw_rect, show_labels, motion_states,
|
||||
track_locations, selected_tracks.trackSources,
|
||||
camera_projection_height, camera_hovered_id, selected_track_id, table_hover_id)
|
||||
draw_top_down(
|
||||
font, radar_rect, tracks, v_ego, show_labels, model, motion_states, use_dbc_colors,
|
||||
font, radar_rect, tracks, show_labels, model, motion_states,
|
||||
hide_moving, hide_stationary, hide_unknown, track_locations, selected_tracks.trackSources,
|
||||
top_down_hovered_id, selected_track_id, table_hover_id,
|
||||
)
|
||||
table_mode = TABLE_MODES[table_mode_index]
|
||||
draw_track_table(font, table_rect, tracks, v_ego, motion_states, table_scroll, selected_track_id, table_hover_id,
|
||||
draw_track_table(font, table_rect, tracks, motion_states, table_scroll, selected_track_id, table_hover_id,
|
||||
track_signals, track_locations, table_mode)
|
||||
clicked_action = draw_source_status(
|
||||
font, status_rect, selected_tracks, data_valid, data_alive,
|
||||
replay_process is not None or can_data_seen or live_data_seen, show_labels, data_source, use_dbc_colors,
|
||||
replay_process is not None or can_data_seen or live_data_seen, show_labels, data_source,
|
||||
"wide 180" if camera_view.stream_type == VisionStreamType.VISION_STREAM_WIDE_ROAD else "road",
|
||||
both_camera_streams_available(camera_view.available_streams),
|
||||
tracks, motion_states, track_locations, source_filters, table_mode, rl.get_fps(),
|
||||
@@ -2115,8 +2089,6 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False, st
|
||||
use_can_source = not use_can_source
|
||||
table_scroll = 0
|
||||
selected_track_id = None
|
||||
elif clicked_action == "colors":
|
||||
use_dbc_colors = not use_dbc_colors
|
||||
elif clicked_action == "labels":
|
||||
show_labels = not show_labels
|
||||
elif clicked_action == "camera":
|
||||
|
||||
Reference in New Issue
Block a user