mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-06 14:16:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ui: draw radarState leads (#26852)

Browse Source 
use radarState
old-commit-hash: 304796bafe0c0608e40b20fc44923f9edf462240
This commit is contained in:
Shane Smiskol
2022-12-31 19:51:03 -08:00
committed by GitHub
parent 6c8aca245a
commit 4d1e15b85c
2 changed files with 12 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/qt/onroad.cc
+11 -9
View File
@@ -498,13 +498,13 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
painter.restore();
}
void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd) {
void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd) {
painter.save();
const float speedBuff = 10.;
const float leadBuff = 40.;
const float d_rel = lead_data.getX()[0];
const float v_rel = lead_data.getV()[0];
const float d_rel = lead_data.getDRel();
const float v_rel = lead_data.getVRel();
float fillAlpha = 0;
if (d_rel < leadBuff) {
@@ -539,6 +539,7 @@ void AnnotatedCameraWidget::paintGL() {
SubMaster &sm = *(s->sm);
const double start_draw_t = millis_since_boot();
const cereal::ModelDataV2::Reader &model = sm["modelV2"].getModelV2();
const cereal::RadarState::Reader &radar_state = sm["radarState"].getRadarState();
// draw camera frame
{
@@ -589,19 +590,20 @@ void AnnotatedCameraWidget::paintGL() {
if (sm.rcv_frame("modelV2") > s->scene.started_frame) {
update_model(s, sm["modelV2"].getModelV2());
if (sm.rcv_frame("radarState") > s->scene.started_frame) {
update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition());
update_leads(s, radar_state, sm["modelV2"].getModelV2().getPosition());
}
}
drawLaneLines(painter, s);
if (s->scene.longitudinal_control) {
const auto leads = model.getLeadsV3();
if (leads[0].getProb() > .5) {
drawLead(painter, leads[0], s->scene.lead_vertices[0]);
auto lead_one = radar_state.getLeadOne();
auto lead_two = radar_state.getLeadTwo();
if (lead_one.getStatus()) {
drawLead(painter, lead_one, s->scene.lead_vertices[0]);
}
if (leads[1].getProb() > .5 && (std::abs(leads[1].getX()[0] - leads[0].getX()[0]) > 3.0)) {
drawLead(painter, leads[1], s->scene.lead_vertices[1]);
if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
drawLead(painter, lead_two, s->scene.lead_vertices[1]);
}
}
}
selfdrive/ui/qt/onroad.h
+1 -1
View File
@@ -80,7 +80,7 @@ protected:
void showEvent(QShowEvent *event) override;
void updateFrameMat() override;
void drawLaneLines(QPainter &painter, const UIState *s);
void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd);
void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd);
void drawHud(QPainter &p);
inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); }
inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); }