diff --git a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py index 5375fff109..e598bf3110 100644 --- a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py @@ -159,10 +159,10 @@ class ModelRenderer(Widget, ModelRendererSP): self._draw_path(sm) if ui_state.draw_radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: - if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or transform_updated or + if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or model_updated or transform_updated or not self.radar_tracks.projection_initialized): self.radar_tracks.update_radar_tracks( - sm['liveTracks'], self._map_to_screen, self._path_offset_z, + sm['liveTracks'], self._map_to_screen, self._path_offset_z, lane_lines=model.laneLines, ) highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {} matched_positions = self.radar_tracks.draw_cached_radar_tracks( diff --git a/openpilot/selfdrive/ui/onroad/model_renderer.py b/openpilot/selfdrive/ui/onroad/model_renderer.py index f129b85c88..57f69c9fa7 100644 --- a/openpilot/selfdrive/ui/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/onroad/model_renderer.py @@ -142,10 +142,10 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): self._draw_path(sm) if ui_state.draw_radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: - if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or transform_updated or + if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or model_updated or transform_updated or not self.radar_tracks.projection_initialized): self.radar_tracks.update_radar_tracks( - sm['liveTracks'], self._map_to_screen, self._path_offset_z, + sm['liveTracks'], self._map_to_screen, self._path_offset_z, lane_lines=model.laneLines, ) highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {} matched_positions = self.radar_tracks.draw_cached_radar_tracks( diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/radar_tracks.py b/openpilot/selfdrive/ui/sunnypilot/onroad/radar_tracks.py index d3ca1daf96..c2f6d38780 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/radar_tracks.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -31,6 +31,7 @@ class ProjectedRadarTrack: source_index: int camera_object: bool track_id: int + lane_index: int def is_preferred_radar_source(source) -> bool: @@ -103,6 +104,46 @@ def radar_track_display(motion_state: int) -> tuple[rl.Color, bool]: return rl.Color(*DBC_UNKNOWN_COLOR, 255), False +def lane_line_lateral_at_distance(lane_line, distance: float) -> float | None: + points = sorted( + (float(x), float(y)) + for x, y in zip(lane_line.x, lane_line.y, strict=True) + if math.isfinite(x) and math.isfinite(y) + ) + if len(points) < 2 or not points[0][0] <= distance <= points[-1][0]: + return None + + for (x_0, y_0), (x_1, y_1) in zip(points, points[1:], strict=True): + if x_0 <= distance <= x_1: + if x_0 == x_1: + return y_1 + return y_0 + (y_1 - y_0) * (distance - x_0) / (x_1 - x_0) + return None + + +def radar_track_lane_index(track, lane_lines) -> int | None: + if lane_lines is None or len(lane_lines) < 4: + return None + + boundaries = [ + lane_line_lateral_at_distance(lane_lines[lane_index], float(track.dRel)) + for lane_index in range(4) + ] + if any(boundary is None for boundary in boundaries): + return None + ordered_boundaries = sorted(float(boundary) for boundary in boundaries if boundary is not None) + + track_lateral = -float(track.yRel) + if not ordered_boundaries[0] <= track_lateral <= ordered_boundaries[-1]: + return None + + lane_centers = [ + (left_boundary + right_boundary) / 2.0 + for left_boundary, right_boundary in zip(ordered_boundaries, ordered_boundaries[1:], strict=False) + ] + return min(range(3), key=lambda lane_index: abs(track_lateral - lane_centers[lane_index])) + + def radar_lead_track_colors(radar_state) -> dict[int, rl.Color]: highlighted_tracks = {} if radar_state is None: @@ -283,7 +324,7 @@ class RadarTracks: self._projected_tracks = () self._projection_initialized = False - def update_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7) -> None: + def update_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7, lane_lines=None) -> None: projected_tracks = [] sources = sorted_radar_sources(live_tracks) @@ -295,6 +336,9 @@ class RadarTracks: motion_state = int(track.motionState) if motion_state not in (DBC_MOTION_STATIONARY, DBC_MOTION_MOVING): continue + lane_index = radar_track_lane_index(track, lane_lines) + if lane_index is None: + continue pt = map_to_screen(d_rel, -y_rel, path_offset_z) if pt is None: @@ -311,6 +355,7 @@ class RadarTracks: source_index=radar_track_source_index(track, sources), camera_object=source is not None and is_camera_object_source(source), track_id=int(track.trackId), + lane_index=lane_index, )) self._projected_tracks = tuple(projected_tracks) @@ -334,6 +379,6 @@ class RadarTracks: return highlighted_positions 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): - self.update_radar_tracks(live_tracks, map_to_screen, path_offset_z, track_size) + highlighted_tracks=None, lane_lines=None): + self.update_radar_tracks(live_tracks, map_to_screen, path_offset_z, track_size, lane_lines) return self.draw_cached_radar_tracks(screen_offset, highlighted_tracks) diff --git a/openpilot/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py b/openpilot/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py index 7ea053f9ed..00c0600695 100644 --- a/openpilot/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py +++ b/openpilot/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py @@ -7,6 +7,13 @@ from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lea radar_lead_track_colors, radar_track_display +def lane_line(lateral): + return SimpleNamespace(x=[0.0, 2000.0], y=[lateral, lateral]) + + +THREE_LANE_LINES = [lane_line(-5.4), lane_line(-1.8), lane_line(1.8), lane_line(5.4)] + + def color_tuple(color): return color.r, color.g, color.b, color.a @@ -140,6 +147,7 @@ def test_draw_radar_tracks_applies_screen_offset(monkeypatch): path_offset_z=1.2, track_size=3, screen_offset=(100, 7), + lane_lines=THREE_LANE_LINES, ) assert drawn_circles == [(120, 37, 3)] @@ -155,7 +163,9 @@ def test_draw_radar_tracks_hides_unknown_motion(monkeypatch): 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) + radar_tracks.RadarTracks().draw_radar_tracks( + live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2, lane_lines=THREE_LANE_LINES, + ) assert drawn_colors == [] @@ -172,12 +182,45 @@ def test_draw_radar_tracks_hides_unknown_motion_from_other_source(monkeypatch): 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, + live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2, lane_lines=THREE_LANE_LINES, ) assert drawn_circles == [] +def test_draw_radar_tracks_only_shows_tofus_in_three_lanes(monkeypatch): + live_tracks = car.RadarData.new_message() + points = live_tracks.init("points", 5) + for point, y_rel in zip(points, (3.6, 0.0, -3.6, -6.1, 6.1), strict=True): + point.dRel = 50 + point.yRel = y_rel + point.vRel = 2 + point.motionState = radar_tracks.DBC_MOTION_MOVING + + 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, lane_lines=THREE_LANE_LINES, + ) + + assert len(drawn_circles) == 3 + + +def test_radar_track_picks_lane_with_closest_center(): + def track(y_rel): + return SimpleNamespace(dRel=50, yRel=y_rel) + + assert radar_tracks.radar_track_lane_index(track(3.6), THREE_LANE_LINES) == 0 + assert radar_tracks.radar_track_lane_index(track(2.5), THREE_LANE_LINES) == 0 + assert radar_tracks.radar_track_lane_index(track(0.0), THREE_LANE_LINES) == 1 + assert radar_tracks.radar_track_lane_index(track(-1.0), THREE_LANE_LINES) == 1 + assert radar_tracks.radar_track_lane_index(track(-3.6), THREE_LANE_LINES) == 2 + assert radar_tracks.radar_track_lane_index(track(6.1), THREE_LANE_LINES) is None + assert radar_tracks.radar_track_lane_index(track(-6.1), THREE_LANE_LINES) is None + assert radar_tracks.radar_track_lane_index(track(0.0), None) is None + + def test_draw_radar_tracks_uses_source_shapes_with_preferred_circle(monkeypatch): live_tracks = car.RadarData.new_message() live_tracks.trackSources = [ @@ -203,6 +246,7 @@ def test_draw_radar_tracks_uses_source_shapes_with_preferred_circle(monkeypatch) 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, + lane_lines=THREE_LANE_LINES, ) assert circles == [(0x3A5, 6)] @@ -227,6 +271,7 @@ def test_draw_camera_objects_uses_triangle(monkeypatch): 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, + lane_lines=THREE_LANE_LINES, ) assert polygons == [(25, 3, 6, -90.0)] @@ -244,6 +289,7 @@ def test_draw_radar_tracks_shrinks_stationary_dots(monkeypatch): 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, + lane_lines=THREE_LANE_LINES, ) assert drawn_sizes == [1] @@ -261,6 +307,7 @@ def test_draw_radar_tracks_keeps_matched_speed_dots_large(monkeypatch): 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, + lane_lines=THREE_LANE_LINES, ) assert drawn_sizes == [6] @@ -289,7 +336,7 @@ def test_draw_radar_tracks_highlights_and_returns_matched_track(monkeypatch): 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}, + screen_offset=(100, 7), highlighted_tracks={11: highlight_color}, lane_lines=THREE_LANE_LINES, ) assert drawn_rings == [((111, 37), 9, 12, (255, 215, 0, 255))] @@ -316,7 +363,7 @@ def test_cached_radar_tracks_only_reproject_on_update(monkeypatch): lambda x, y, radius, color: drawn.append((x, y)), ) renderer = radar_tracks.RadarTracks() - renderer.update_radar_tracks(live_tracks, map_to_screen, path_offset_z=1.2) + renderer.update_radar_tracks(live_tracks, map_to_screen, path_offset_z=1.2, lane_lines=THREE_LANE_LINES) renderer.draw_cached_radar_tracks(screen_offset=(100, 7)) renderer.draw_cached_radar_tracks(screen_offset=(200, 9))