why drift

selfdrive/ui/mici/onroad/model_renderer.py

@@ -1,5 +1,4 @@
 import colorsys
-import math
 import numpy as np
 import pyray as rl
 from cereal import messaging, car
@@ -14,6 +13,7 @@ from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
 from openpilot.system.ui.widgets import Widget
 
 from openpilot.selfdrive.ui.sunnypilot.mici.onroad.model_renderer import LANE_LINE_COLORS_SP, ModelRendererSP
+from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, radar_lead_track_colors
 
 CLIP_MARGIN = 500
 MIN_DRAW_DISTANCE = 10.0
@@ -38,12 +38,6 @@ LANE_LINE_COLORS = {
   **LANE_LINE_COLORS_SP,
 }
 
-LEAD_TRACK_COLORS = (
-  rl.Color(255, 215, 0, 255),
-  rl.Color(255, 140, 0, 220),
-)
-
-
 @dataclass
 class ModelPoints:
   raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32))
@@ -163,14 +157,17 @@ class ModelRenderer(Widget, ModelRendererSP):
       self._draw_path(sm)
 
     if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
-      highlighted_tracks = self._lead_track_colors(radar_state) if render_lead_indicator else {}
+      highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {}
       matched_positions = self.radar_tracks.draw_radar_tracks(
         sm['liveTracks'], self._map_to_screen, self._path_offset_z,
         screen_offset=(self._rect.x, self._rect.y), v_ego=sm['carState'].vEgo,
         highlighted_tracks=highlighted_tracks,
       )
       if render_lead_indicator:
-        self._draw_lead_connectors(matched_positions, highlighted_tracks)
+        draw_radar_lead_connectors(
+          self._lead_vehicles, matched_positions, highlighted_tracks,
+          screen_offset=(self._rect.x, self._rect.y),
+        )
 
     if render_lead_indicator:
       self._draw_lead_indicator()
@@ -407,30 +404,6 @@ class ModelRenderer(Widget, ModelRendererSP):
       rl.draw_triangle_fan(offset_glow, len(offset_glow), rl.Color(218, 202, 37, 255))
       rl.draw_triangle_fan(offset_chevron, len(offset_chevron), rl.Color(201, 34, 49, lead.fill_alpha))
 
-  @staticmethod
-  def _lead_track_colors(radar_state):
-    highlighted_tracks = {}
-    if radar_state is None:
-      return highlighted_tracks
-
-    for lead, color in zip((radar_state.leadOne, radar_state.leadTwo), LEAD_TRACK_COLORS, strict=True):
-      if lead.status and lead.radar and lead.radarTrackId >= 0:
-        highlighted_tracks.setdefault(int(lead.radarTrackId), color)
-    return highlighted_tracks
-
-  def _draw_lead_connectors(self, matched_positions, highlighted_tracks):
-    for lead in self._lead_vehicles:
-      if not lead.radar or lead.position is None or lead.radar_track_id not in matched_positions:
-        continue
-
-      radar_position = matched_positions[lead.radar_track_id]
-      lead_position = (lead.position[0] + self._rect.x, lead.position[1] + self._rect.y)
-      if math.dist(lead_position, radar_position) < 4:
-        continue
-
-      rl.draw_line_ex(rl.Vector2(*lead_position), rl.Vector2(*radar_position), 2,
-                      highlighted_tracks[lead.radar_track_id])
-
   @staticmethod
   def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int:
     """Get the index corresponding to the given path height"""

selfdrive/ui/onroad/model_renderer.py

@@ -12,6 +12,7 @@ from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
 from openpilot.system.ui.widgets import Widget
 
 from openpilot.selfdrive.ui.sunnypilot.onroad.model_renderer import ChevronMetrics, ModelRendererSP
+from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, radar_lead_track_colors
 
 CLIP_MARGIN = 500
 MIN_DRAW_DISTANCE = 10.0
@@ -41,6 +42,9 @@ class LeadVehicle:
   glow: list[tuple[float, float]] = field(default_factory=list)
   chevron: list[tuple[float, float]] = field(default_factory=list)
   fill_alpha: int = 0
+  position: tuple[float, float] | None = None
+  radar_track_id: int = -1
+  radar: bool = False
 
 
 class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
@@ -114,7 +118,7 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
     model = sm['modelV2']
     radar_state = sm['radarState'] if sm.valid['radarState'] else None
     lead_one = radar_state.leadOne if radar_state else None
-    render_lead_indicator = self._longitudinal_control and radar_state is not None
+    render_lead_indicator = ui_state.radar_tracks != 0 and radar_state is not None
 
     # Update model data when needed
     model_updated = sm.updated['modelV2']
@@ -136,7 +140,13 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
     self._draw_path(sm)
 
     if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
-      self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z, v_ego=sm['carState'].vEgo)
+      highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {}
+      matched_positions = self.radar_tracks.draw_radar_tracks(
+        sm['liveTracks'], self._map_to_screen, self._path_offset_z,
+        v_ego=sm['carState'].vEgo, highlighted_tracks=highlighted_tracks,
+      )
+      if render_lead_indicator:
+        draw_radar_lead_connectors(self._lead_vehicles, matched_positions, highlighted_tracks)
 
     if render_lead_indicator and radar_state:
       self._draw_lead_indicator()
@@ -170,7 +180,11 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
         z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
         point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
         if point:
-          self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
+          lead_vehicle = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
+          lead_vehicle.position = lead_vehicle.chevron[1]
+          lead_vehicle.radar_track_id = int(lead_data.radarTrackId)
+          lead_vehicle.radar = lead_data.radar
+          self._lead_vehicles[i] = lead_vehicle
 
   def _update_model(self, lead, path_x_array):
     """Update model visualization data based on model message"""

selfdrive/ui/sunnypilot/onroad/radar_tracks.py

@@ -17,6 +17,10 @@ DBC_MOTION_STATIONARY = 1
 DBC_MOTION_MOVING = 2
 COASTED_ALPHA = 96
 COASTED_RADIUS_REDUCTION = 2
+LEAD_TRACK_COLORS = (
+  rl.Color(255, 215, 0, 255),
+  rl.Color(255, 140, 0, 220),
+)
 
 
 def is_preferred_radar_source(source) -> bool:
@@ -66,6 +70,33 @@ def radar_track_display(motion_state: int, measured: bool = True) -> tuple[rl.Co
   return rl.Color(*DBC_UNKNOWN_COLOR, alpha), False
 
 
+def radar_lead_track_colors(radar_state) -> dict[int, rl.Color]:
+  highlighted_tracks = {}
+  if radar_state is None:
+    return highlighted_tracks
+
+  for lead, color in zip((radar_state.leadOne, radar_state.leadTwo), LEAD_TRACK_COLORS, strict=True):
+    if lead.status and lead.radar and lead.radarTrackId >= 0:
+      highlighted_tracks.setdefault(int(lead.radarTrackId), color)
+  return highlighted_tracks
+
+
+def draw_radar_lead_connectors(lead_vehicles, matched_positions, highlighted_tracks, screen_offset=(0, 0)) -> None:
+  for lead in lead_vehicles:
+    if not lead.radar or lead.position is None or lead.radar_track_id not in matched_positions:
+      continue
+
+    radar_position = matched_positions[lead.radar_track_id]
+    lead_position = (lead.position[0] + screen_offset[0], lead.position[1] + screen_offset[1])
+    if math.dist(lead_position, radar_position) < 4:
+      continue
+
+    rl.draw_line_ex(
+      rl.Vector2(*lead_position), rl.Vector2(*radar_position), 2,
+      highlighted_tracks[lead.radar_track_id],
+    )
+
+
 def format_radar_tracks_onroad_columns(live_tracks, v_ego: float = 0.0) -> tuple[str, str, str, str, str, str]:
   sources = sorted_radar_sources(live_tracks)
   if not sources:

selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py

@@ -1,7 +1,10 @@
+from types import SimpleNamespace
+
 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_display
+from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, format_radar_tracks_onroad_columns, \
+  radar_lead_track_colors, radar_track_display
 
 
 def color_tuple(color):
@@ -29,6 +32,36 @@ def test_unknown_dbc_motion_uses_neutral_dbc_color():
   assert not stationary
 
 
+def test_radar_lead_track_colors_only_highlight_radar_matches():
+  radar_state = SimpleNamespace(
+    leadOne=SimpleNamespace(status=True, radar=True, radarTrackId=7),
+    leadTwo=SimpleNamespace(status=True, radar=False, radarTrackId=9),
+  )
+
+  colors = radar_lead_track_colors(radar_state)
+
+  assert list(colors) == [7]
+  assert color_tuple(colors[7]) == color_tuple(radar_tracks.LEAD_TRACK_COLORS[0])
+
+
+def test_draw_radar_lead_connectors_applies_screen_offset(monkeypatch):
+  lead = SimpleNamespace(radar=True, position=(10, 20), radar_track_id=7)
+  color = radar_tracks.LEAD_TRACK_COLORS[0]
+  drawn = []
+  monkeypatch.setattr(
+    radar_tracks.rl, "draw_line_ex",
+    lambda start, end, width, line_color: drawn.append(
+      ((start.x, start.y), (end.x, end.y), width, color_tuple(line_color))
+    ),
+  )
+
+  draw_radar_lead_connectors(
+    [lead], {7: (120, 30)}, {7: color}, screen_offset=(100, 5),
+  )
+
+  assert drawn == [((110, 25), (120, 30), 2, color_tuple(color))]
+
+
 def test_format_radar_tracks_columns_none():
   live_tracks = car.RadarData.new_message()