Adjacent leads tracking

selfdrive/controls/radard.py

@@ -62,6 +62,10 @@ class Track:
     self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
 
     # FrogPilot variables
+    self.leadLeft = False
+    self.leadRight = False
+
+    self.leadTrackID = 0
 
   def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
     # relative values, copy
@@ -115,6 +119,22 @@ class Track:
     return ret
 
   # FrogPilot variables
+  def potential_adjacent_lead(self, left: bool, model_data: capnp._DynamicStructReader):
+    if self.vLeadK < 1 or self.leadTrackID == self.identifier:
+      return False
+
+    far_left_lane = np.interp(self.dRel, model_data.laneLines[0].x, model_data.laneLines[0].y)
+    left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y)
+    right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y)
+    far_right_lane = np.interp(self.dRel, model_data.laneLines[3].x, model_data.laneLines[3].y)
+
+    self.leadLeft = far_left_lane < -self.yRel < left_lane and self.dRel < model_data.position.x[-1]
+    self.leadRight = right_lane < -self.yRel < far_right_lane and self.dRel < model_data.position.x[-1]
+
+    if left:
+      return self.leadLeft
+    else:
+      return self.leadRight
 
 
 def laplacian_pdf(x: float, mu: float, b: float):
@@ -191,11 +211,22 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
         lead_dict = closest_track.get_RadarState()
 
   # FrogPilot variables
+  for track in tracks.values():
+    track.leadTrackID = lead_dict.get('radarTrackId', -1)
 
   return lead_dict
 
 
 # FrogPilot variables
+def get_adjacent_lead(tracks: dict[int, Track], model_data: capnp._DynamicStructReader, left: bool = True) -> dict[str, Any]:
+  lead_dict = {'status': False}
+
+  adjacent_tracks = [c for c in tracks.values() if c.potential_adjacent_lead(left, model_data)]
+  if len(adjacent_tracks) > 0:
+    closest_track = min(adjacent_tracks, key=lambda c: c.dRel)
+    lead_dict = closest_track.get_RadarState()
+
+  return lead_dict
 
 
 class RadarD:
@@ -264,6 +295,10 @@ class RadarD:
       self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles, low_speed_override=False)
 
     # FrogPilot variables
+    if self.ready and (self.frogpilot_toggles.adjacent_lead_tracking):
+      self.frogpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['modelV2'], left=True)
+      self.frogpilot_radar_state.leadRight = get_adjacent_lead(self.tracks, sm['modelV2'], left=False)
+
     self.frogpilot_toggles = get_frogpilot_toggles(sm)
 
   def publish(self, pm: messaging.PubMaster):

selfdrive/ui/qt/onroad/model.cc

@@ -51,6 +51,18 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
     // FrogPilot variables
     SubMaster &fpsm = *(frogpilotUIState()->sm);
     const cereal::FrogPilotRadarState::Reader &frogpilot_radar_state = fpsm["frogpilotRadarState"].getFrogpilotRadarState();
+
+    const cereal::FrogPilotRadarState::LeadData::Reader &lead_left = frogpilot_radar_state.getLeadLeft();
+    const cereal::FrogPilotRadarState::LeadData::Reader &lead_right = frogpilot_radar_state.getLeadRight();
+
+    updateAdjacentLeads(frogpilot_radar_state, model.getPosition());
+
+    if (lead_left.getStatus()) {
+      drawLead(painter, reinterpret_cast<const cereal::RadarState::LeadData::Reader&>(lead_left), adjacent_lead_vertices[0], surface_rect, frogpilot_nvg->blueColor(), true);
+    }
+    if (lead_right.getStatus()) {
+      drawLead(painter, reinterpret_cast<const cereal::RadarState::LeadData::Reader&>(lead_right), adjacent_lead_vertices[1], surface_rect, frogpilot_nvg->purpleColor(), true);
+    }
   }
 
   // FrogPilot variables
@@ -203,10 +215,10 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float
 }
 
 void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data,
-                             const QPointF &vd, const QRect &surface_rect) {
+                             const QPointF &vd, const QRect &surface_rect, QColor marker_color, bool adjacent) {
   const float speedBuff = 10.;
   const float leadBuff = 40.;
-  const float d_rel = lead_data.getDRel();
+  const float d_rel = lead_data.getDRel() + (adjacent ? fabs(lead_data.getYRel()) : 0);
   const float v_rel = lead_data.getVRel();
 
   float fillAlpha = 0;
@@ -231,7 +243,7 @@ void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadDa
 
   // chevron
   QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}};
-  painter.setBrush(QColor(201, 34, 49, fillAlpha));
+  painter.setBrush(QColor(marker_color.red(), marker_color.green(), marker_color.blue(), fillAlpha));
   painter.drawPolygon(chevron, std::size(chevron));
 
   // FrogPilot variables
@@ -268,3 +280,12 @@ void ModelRenderer::mapLineToPolygon(const cereal::XYZTData::Reader &line, float
 }
 
 // FrogPilot variables
+void ModelRenderer::updateAdjacentLeads(const cereal::FrogPilotRadarState::Reader &radar_state, const cereal::XYZTData::Reader &line) {
+  for (int i = 0; i < 2; ++i) {
+    const auto &lead_data = (i == 0) ? radar_state.getLeadLeft() : radar_state.getLeadRight();
+    if (lead_data.getStatus()) {
+      float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())];
+      mapToScreen(lead_data.getDRel(), -lead_data.getYRel(), z + path_offset_z, &adjacent_lead_vertices[i]);
+    }
+  }
+}

selfdrive/ui/qt/onroad/model.h

@@ -24,7 +24,7 @@ private:
   bool mapToScreen(float in_x, float in_y, float in_z, QPointF *out);
   void mapLineToPolygon(const cereal::XYZTData::Reader &line, float y_off, float z_off,
                         QPolygonF *pvd, int max_idx, bool allow_invert = true);
-  void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect);
+  void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect, QColor marker_color, bool adjacent=false);
   void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
   void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead);
   void drawLaneLines(QPainter &painter);
@@ -47,4 +47,7 @@ private:
   QRectF clip_region;
 
   // FrogPilot variables
+  void updateAdjacentLeads(const cereal::FrogPilotRadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
+
+  QPointF adjacent_lead_vertices[2] = {};
 };