From 6ca32b9f25c6261bb317d13805c8ebeacfaa5a27 Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 17 Jul 2026 09:09:18 -0400 Subject: [PATCH] motionState --- .../ui/mici/onroad/augmented_road_view.py | 4 +- selfdrive/ui/onroad/augmented_road_view.py | 4 +- .../ui/sunnypilot/onroad/radar_tracks.py | 123 ++++++++++++------ .../ui/sunnypilot/onroad/test_radar_tracks.py | 47 ++++++- 4 files changed, 131 insertions(+), 47 deletions(-) diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index 6377740d7c..04795b1ceb 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -173,7 +173,9 @@ class AugmentedRoadView(CameraView): 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) + self._radar_tracks_status.update( + ui_state.sm["liveTracks"], ui_state.sm.valid["liveTracks"], ui_state.radar_tracks, ui_state.sm["carState"].vEgo, + ) elif not ui_state.sm.alive["liveTracks"]: self._radar_tracks_status.reset() diff --git a/selfdrive/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py index cd8423ef7b..84b2286355 100644 --- a/selfdrive/ui/onroad/augmented_road_view.py +++ b/selfdrive/ui/onroad/augmented_road_view.py @@ -64,7 +64,9 @@ class AugmentedRoadView(CameraView, AugmentedRoadViewSP): 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) + self._radar_tracks_status.update( + ui_state.sm["liveTracks"], ui_state.sm.valid["liveTracks"], ui_state.radar_tracks, ui_state.sm["carState"].vEgo, + ) elif not ui_state.sm.alive["liveTracks"]: self._radar_tracks_status.reset() diff --git a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py index 4a843e6749..3dfa6da43f 100644 --- a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -15,6 +15,10 @@ 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: @@ -32,14 +36,41 @@ 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]: +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.""" + 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) + + +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)) if not sources: - return "", "none" + 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 + 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}" + + 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}", "" class RadarTracksStatus: @@ -53,29 +84,41 @@ class RadarTracksStatus: 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._labels = ( + UnifiedLabel("", text_color=rl.Color(0, 255, 64, 255), **text_args), + UnifiedLabel("none", text_color=rl.Color(0, 255, 64, 255), **text_args), + UnifiedLabel("", text_color=rl.Color(*DBC_MOVING_COLOR, 255), **text_args), + UnifiedLabel("", text_color=rl.Color(*NEUTRAL_COLOR, 255), **text_args), + UnifiedLabel("", text_color=rl.Color(*DBC_UNKNOWN_COLOR, 255), **text_args), + UnifiedLabel("", text_color=rl.Color(*DBC_UNKNOWN_COLOR, 255), **text_args), + ) + self._status = ("", "none", "", "", "", "") + self._status_colors: tuple[tuple[int, int, int], ...] = () + 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 self._height = 42 - def update(self, live_tracks, valid: bool, radar_mode: int) -> None: + def update(self, live_tracks, valid: bool, radar_mode: int, v_ego: float = 0.0) -> None: if live_tracks.radarTracksAvailable and radar_mode != 2: - status = ("", "radar detected\ntap to enable") + status = ("", "radar detected\ntap to enable", "", "", "", "") + status_colors = () else: - status = format_radar_tracks_onroad_columns(live_tracks) if valid else ("", "none") - self._set_status(status) + 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) + ) + self._set_status(status, status_colors) def reset(self) -> None: - self._set_status(("", "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): @@ -93,26 +136,23 @@ class RadarTracksStatus: 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, - )) + 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) - def _set_status(self, status: tuple[str, str]) -> None: - if status == self._status: + 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: return self._status = status - self._ranges_label.set_text(status[0]) - self._counts_label.set_text(status[1]) + self._status_colors = status_colors + 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): + label.set_text_color(rl.Color(*color, 255)) self._layout_key = None def _update_layout(self, max_inner_width: int) -> None: @@ -120,16 +160,20 @@ class RadarTracksStatus: 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) + for label in self._labels: + label.get_content_height(max_inner_width) + self._column_widths = [ + math.ceil(label.text_width) if text else 0 + for label, text in zip(self._labels, self._status, strict=True) + ] + self._column_widths[1] = max(36, self._column_widths[1]) + 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 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, + *(label.get_content_height(max(width, 1)) + 10 + for label, width in zip(self._labels, self._column_widths, strict=True) if width), ) self._layout_key = layout_key @@ -150,8 +194,7 @@ class RadarTracks: 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) + color, stationary = radar_track_display(v_rel, v_ego, 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) diff --git a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py index 821d700a9e..6e9e30d554 100644 --- a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py @@ -1,7 +1,8 @@ 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 +from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import format_radar_tracks_onroad_columns, radar_track_color, \ + radar_track_display def color_tuple(color): @@ -29,18 +30,34 @@ def test_radar_track_stationary_world_object_is_white(): 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 + + def test_format_radar_tracks_columns_none(): live_tracks = car.RadarData.new_message() - assert format_radar_tracks_onroad_columns(live_tracks) == ("", "none") + 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) + 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") + assert format_radar_tracks_onroad_columns(live_tracks) == ("500-51F", "2", "M 1", "S 1", "U 0", "") def test_format_radar_tracks_columns_sorts_ranges(): @@ -50,11 +67,31 @@ def test_format_radar_tracks_columns_sorts_ranges(): {"startAddress": 0x210, "endAddress": 0x21F, "bus": 1, "trackCount": 1}, {"startAddress": 0x500, "endAddress": 0x51F, "bus": 0, "trackCount": 2}, ] - live_tracks.init("points", 1) + points = live_tracks.init("points", 3) + points[0].motionState = 2 + points[1].motionState = 0 + points[2].motionState = 7 assert format_radar_tracks_onroad_columns(live_tracks) == ( "210-21F\n500-51F\n500-51F", "1\n2\n3", + "M 1", + "S 0", + "U 2", + "", + ) + + +def test_format_radar_tracks_columns_uses_implementation_when_dbc_motion_is_unavailable(): + 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 + + assert format_radar_tracks_onroad_columns(live_tracks, v_ego=20.0) == ( + "500-51F", "4", "A 1", "= 1", "S 1", "R 1", )