diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py old mode 100755 new mode 100644 index f2dcb006..401d0492 --- a/selfdrive/controls/radard.py +++ b/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): diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 7f30d608..7aeec196 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/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(lead_left), adjacent_lead_vertices[0], surface_rect, frogpilot_nvg->blueColor(), true); + } + if (lead_right.getStatus()) { + drawLead(painter, reinterpret_cast(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]); + } + } +} diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h index 71d2624b..dd371d88 100644 --- a/selfdrive/ui/qt/onroad/model.h +++ b/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] = {}; };