diff --git a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py index d647419b38..f00214307f 100644 --- a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -21,20 +21,37 @@ def is_preferred_radar_source(source) -> bool: return source.startAddress == RADAR_3A5_3C4.start_addr and source.endAddress == RADAR_3A5_3C4.end_addr -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 - ] +def radar_source_sort_key(source) -> tuple[bool, int, int, int]: + return ( + not is_preferred_radar_source(source), + int(source.startAddress), + int(source.endAddress), + int(source.bus), + ) - # 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 sorted_radar_sources(live_tracks): + return sorted(live_tracks.trackSources, key=radar_source_sort_key) + + +def radar_track_source_index(track, sources) -> int: + address = int(track.sourceAddress) + bus = int(track.sourceBus) + if address != 0: + return next(( + index for index, source in enumerate(sources) + if source.startAddress <= address <= source.endAddress and source.bus == bus + ), 0) + return 0 + + +def draw_radar_source_marker(center: rl.Vector2, radius: float, color: rl.Color, source_index: int) -> None: + if source_index <= 0: + rl.draw_circle(int(center.x), int(center.y), radius, color) + return + sides = (4, 3, 5, 6)[(source_index - 1) % 4] + rotation = 45.0 if sides == 4 else -90.0 + rl.draw_poly(center, sides, radius, rotation, color) def radar_track_display(motion_state: int) -> tuple[rl.Color, bool]: @@ -47,16 +64,13 @@ def radar_track_display(motion_state: int) -> tuple[rl.Color, bool]: def format_radar_tracks_onroad_columns(live_tracks, v_ego: float = 0.0) -> tuple[str, str, str, str, str, str]: - sources = sorted( - (source for source in live_tracks.trackSources if is_preferred_radar_source(source)), - key=lambda source: (source.startAddress, source.endAddress, source.bus), - ) + sources = sorted_radar_sources(live_tracks) 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 preferred_radar_tracks(live_tracks)] + motion_states = [int(track.motionState) for track in live_tracks.points] moving_count = sum(state == DBC_MOTION_MOVING for state in motion_states) stationary_count = sum(state == DBC_MOTION_STATIONARY for state in motion_states) @@ -67,6 +81,7 @@ def format_radar_tracks_onroad_columns(live_tracks, v_ego: float = 0.0) -> tuple class RadarTracksStatus: HORIZONTAL_PADDING = 8 COLUMN_GAP = 8 + SOURCE_MARKER_WIDTH = 18 def __init__(self, settings_callback=None, right_margin: int = 12): self._settings_callback = settings_callback @@ -89,6 +104,7 @@ class RadarTracksStatus: ) self._status = ("", "none", "", "", "", "") self._status_colors: tuple[tuple[int, int, int], ...] = () + self._source_count = 0 self._layout_key: tuple[str, str, str, str, str, str, int] | None = None self._column_widths = [0, 36, 0, 0, 0, 0] self._width = 52 @@ -98,13 +114,15 @@ class RadarTracksStatus: if live_tracks.radarTracksAvailable and radar_mode != 2: status = ("", "radar detected\ntap to enable", "", "", "", "") status_colors = () + source_count = 0 else: status = format_radar_tracks_onroad_columns(live_tracks, v_ego) if valid else ("", "none", "", "", "", "") status_colors = (DBC_MOVING_COLOR, NEUTRAL_COLOR, DBC_UNKNOWN_COLOR, DBC_UNKNOWN_COLOR) - self._set_status(status, status_colors) + source_count = len(live_tracks.trackSources) if valid else 0 + self._set_status(status, status_colors, source_count) def reset(self) -> None: - self._set_status(("", "none", "", "", "", ""), ()) + self._set_status(("", "none", "", "", "", ""), (), 0) def handle_mouse(self, mouse_pos) -> bool: if self._settings_callback is None or not rl.check_collision_point_rec(mouse_pos, self._rect): @@ -123,18 +141,34 @@ class RadarTracksStatus: ) rl.draw_rectangle_rounded(self._rect, 0.5, 8, rl.Color(0, 0, 0, 170)) x = self._rect.x + self.HORIZONTAL_PADDING - active_columns = [(label, width) for label, width in zip(self._labels, self._column_widths, strict=True) if width] - for index, (label, width) in enumerate(active_columns): - label.render(rl.Rectangle(x, self._rect.y + 5, width, self._rect.height - 10)) - x += width + (self.COLUMN_GAP if index < len(active_columns) - 1 else 0) + active_columns = [ + (column_index, label, width) + for column_index, (label, width) in enumerate(zip(self._labels, self._column_widths, strict=True)) + if width + ] + for active_index, (column_index, label, width) in enumerate(active_columns): + label_x = x + label_width = width + if column_index == 0 and self._source_count: + marker_height = (self._rect.height - 10) / self._source_count + for source_index in range(self._source_count): + draw_radar_source_marker( + rl.Vector2(x + 6, self._rect.y + 5 + marker_height * (source_index + 0.5)), + 5.0, rl.Color(0, 255, 64, 255), source_index, + ) + label_x += self.SOURCE_MARKER_WIDTH + label_width -= self.SOURCE_MARKER_WIDTH + label.render(rl.Rectangle(label_x, self._rect.y + 5, label_width, self._rect.height - 10)) + x += width + (self.COLUMN_GAP if active_index < len(active_columns) - 1 else 0) def _set_status(self, status: tuple[str, str, str, str, str, str], - status_colors: tuple[tuple[int, int, int], ...]) -> None: - if status == self._status and status_colors == self._status_colors: + status_colors: tuple[tuple[int, int, int], ...], source_count: int) -> None: + if status == self._status and status_colors == self._status_colors and source_count == self._source_count: return self._status = status self._status_colors = status_colors + self._source_count = source_count for label, text in zip(self._labels, status, strict=True): label.set_text(text) for label, color in zip(self._labels[2:], status_colors, strict=False): @@ -152,6 +186,8 @@ class RadarTracksStatus: math.ceil(label.text_width) if text else 0 for label, text in zip(self._labels, self._status, strict=True) ] + if self._column_widths[0] and self._source_count: + self._column_widths[0] += self.SOURCE_MARKER_WIDTH self._column_widths[1] = max(36, self._column_widths[1]) for index in (2, 3, 4): if self._column_widths[index]: @@ -172,25 +208,32 @@ class RadarTracks: highlighted_tracks=None): highlighted_tracks = highlighted_tracks or {} highlighted_positions = {} + sources = sorted_radar_sources(live_tracks) - for track in preferred_radar_tracks(live_tracks): + for track in live_tracks.points: d_rel, y_rel, v_rel = track.dRel, track.yRel, track.vRel if not (math.isfinite(d_rel) and math.isfinite(y_rel) and math.isfinite(v_rel)): continue + motion_state = int(track.motionState) + if motion_state not in (DBC_MOTION_STATIONARY, DBC_MOTION_MOVING): + continue + pt = map_to_screen(d_rel, -y_rel, path_offset_z) if pt is None: continue x, y = pt[0] + screen_offset[0], pt[1] + screen_offset[1] - color, stationary = radar_track_display(int(track.motionState)) - radius = max(1, track_size - 4) if stationary else track_size + color, stationary = radar_track_display(motion_state) + radius = max(1, track_size - 5) 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) + draw_radar_source_marker( + rl.Vector2(x, y), radius, color, radar_track_source_index(track, sources), + ) return highlighted_positions diff --git a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py index e657273e38..939514986b 100644 --- a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py @@ -38,7 +38,7 @@ def test_format_radar_tracks_columns_range_and_count(): assert format_radar_tracks_onroad_columns(live_tracks) == ("3A5-3C4", "2", "1", "1", "0", "") -def test_format_radar_tracks_columns_ignores_non_dbc_ranges(): +def test_format_radar_tracks_columns_stacks_all_ranges_with_preferred_first(): live_tracks = car.RadarData.new_message() live_tracks.trackSources = [ {"startAddress": 0x500, "endAddress": 0x51F, "bus": 2, "trackCount": 3}, @@ -53,16 +53,16 @@ def test_format_radar_tracks_columns_ignores_non_dbc_ranges(): points[2].sourceAddress = 0x500 assert format_radar_tracks_onroad_columns(live_tracks) == ( - "3A5-3C4", - "2", + "3A5-3C4\n500-51F", + "2\n3", + "1", "1", "1", - "0", "", ) -def test_format_radar_tracks_columns_hides_non_dbc_source(): +def test_format_radar_tracks_columns_shows_non_motion_source(): live_tracks = car.RadarData.new_message() live_tracks.trackSources = [{"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 4}] points = live_tracks.init("points", 4) @@ -71,7 +71,7 @@ def test_format_radar_tracks_columns_hides_non_dbc_source(): point.motionState = 0 point.sourceAddress = 0x500 - assert format_radar_tracks_onroad_columns(live_tracks, v_ego=20.0) == ("", "none", "", "", "", "") + assert format_radar_tracks_onroad_columns(live_tracks, v_ego=20.0) == ("500-51F", "4", "0", "0", "4", "") def test_draw_radar_tracks_applies_screen_offset(monkeypatch): @@ -81,6 +81,7 @@ def test_draw_radar_tracks_applies_screen_offset(monkeypatch): points[0].yRel = 1 points[0].vRel = 2 points[0].aRel = 0 + points[0].motionState = radar_tracks.DBC_MOTION_MOVING drawn_circles = [] monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_circles.append((x, y, size))) @@ -95,22 +96,23 @@ def test_draw_radar_tracks_applies_screen_offset(monkeypatch): assert drawn_circles == [(120, 37, 3)] -def test_draw_radar_tracks_allows_unknown_acceleration(monkeypatch): +def test_draw_radar_tracks_hides_unknown_motion(monkeypatch): live_tracks = car.RadarData.new_message() point = live_tracks.init("points", 1)[0] point.dRel = 10 point.yRel = 1 point.vRel = -5 point.aRel = float("nan") + point.motionState = 0 drawn_colors = [] monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_colors.append(color_tuple(color))) radar_tracks.RadarTracks().draw_radar_tracks(live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2) - assert drawn_colors == [(*radar_tracks.DBC_UNKNOWN_COLOR, 255)] + assert drawn_colors == [] -def test_draw_radar_tracks_hides_non_dbc_source(monkeypatch): +def test_draw_radar_tracks_hides_unknown_motion_from_other_source(monkeypatch): live_tracks = car.RadarData.new_message() point = live_tracks.init("points", 1)[0] point.dRel = 10 @@ -128,6 +130,37 @@ def test_draw_radar_tracks_hides_non_dbc_source(monkeypatch): assert drawn_circles == [] +def test_draw_radar_tracks_uses_source_shapes_with_preferred_circle(monkeypatch): + live_tracks = car.RadarData.new_message() + live_tracks.trackSources = [ + {"startAddress": 0x500, "endAddress": 0x51F, "bus": 1, "trackCount": 1}, + {"startAddress": 0x3A5, "endAddress": 0x3C4, "bus": 1, "trackCount": 1}, + ] + points = live_tracks.init("points", 2) + for point, address in zip(points, (0x500, 0x3A5), strict=True): + point.dRel = address + point.yRel = 1 + point.vRel = 2 + point.motionState = radar_tracks.DBC_MOTION_MOVING + point.sourceAddress = address + point.sourceBus = 1 + + circles = [] + polygons = [] + monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, radius, color: circles.append((x, radius))) + monkeypatch.setattr( + radar_tracks.rl, "draw_poly", + lambda center, sides, radius, rotation, color: polygons.append((center.x, sides, radius, rotation)), + ) + + radar_tracks.RadarTracks().draw_radar_tracks( + live_tracks, lambda d_rel, y_rel, z: (d_rel, 30), path_offset_z=1.2, track_size=6, + ) + + assert circles == [(0x3A5, 6)] + assert polygons == [(0x500, 4, 6, 45.0)] + + def test_draw_radar_tracks_shrinks_stationary_dots(monkeypatch): live_tracks = car.RadarData.new_message() point = live_tracks.init("points", 1)[0] @@ -143,7 +176,7 @@ def test_draw_radar_tracks_shrinks_stationary_dots(monkeypatch): 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] + assert drawn_sizes == [1] def test_draw_radar_tracks_keeps_matched_speed_dots_large(monkeypatch): @@ -153,6 +186,7 @@ def test_draw_radar_tracks_keeps_matched_speed_dots_large(monkeypatch): point.yRel = 1 point.vRel = 0.5 point.aRel = 0 + point.motionState = radar_tracks.DBC_MOTION_MOVING drawn_sizes = [] monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_sizes.append(size)) @@ -172,6 +206,7 @@ def test_draw_radar_tracks_highlights_and_returns_matched_track(monkeypatch): point.yRel = 1 point.vRel = 2 point.aRel = 0 + point.motionState = radar_tracks.DBC_MOTION_MOVING highlight_color = radar_tracks.rl.Color(255, 215, 0, 255) drawn_rings = []