only within

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(

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(

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)

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))