diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index eb939c58..7cb686f5 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -130,6 +130,8 @@ class FrogPilotPlanner: frogpilotPlan.cscSpeed = float(self.frogpilot_vcruise.csc_target) frogpilotPlan.cscTraining = self.frogpilot_vcruise.csc.enable_training + frogpilotPlan.desiredFollowDistance = int(self.frogpilot_following.desired_follow_distance) + frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop diff --git a/frogpilot/controls/lib/frogpilot_following.py b/frogpilot/controls/lib/frogpilot_following.py index 67385e8a..c3ce7b33 100644 --- a/frogpilot/controls/lib/frogpilot_following.py +++ b/frogpilot/controls/lib/frogpilot_following.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import numpy as np -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, STOP_DISTANCE, get_jerk_factor, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, STOP_DISTANCE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW class FrogPilotFollowing: def __init__(self, FrogPilotPlanner): @@ -11,6 +11,7 @@ class FrogPilotFollowing: self.acceleration_jerk = 0 self.danger_jerk = 0 + self.desired_follow_distance = 0 self.speed_jerk = 0 self.t_follow = 0 @@ -53,6 +54,9 @@ class FrogPilotFollowing: if long_control_active and self.frogpilot_planner.tracking_lead: if frogpilot_toggles.human_following: self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead) + self.desired_follow_distance = desired_follow_distance(v_ego, self.frogpilot_planner.lead_one.vLead, self.t_follow) + else: + self.desired_follow_distance = 0 def update_follow_values(self, lead_distance, v_ego, v_lead): # Offset by FrogAi for FrogPilot for a more natural approach to a faster lead diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index d730df71..4294b16a 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -154,6 +154,7 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil cscControllingSpeed = frogpilotPlan.getCscControllingSpeed(); cscSpeed = frogpilotPlan.getCscSpeed(); cscTraining = frogpilotPlan.getCscTraining(); + desiredFollowDistance = frogpilotPlan.getDesiredFollowDistance(); experimentalMode = selfdriveState.getExperimentalMode(); forceCoast = frogpilotCarState.getForceCoast(); laneWidthLeft = frogpilotPlan.getLaneWidthLeft(); @@ -525,6 +526,73 @@ void FrogPilotAnnotatedCameraWidget::paintCurveSpeedControlTraining(QPainter &p) p.restore(); } +void FrogPilotAnnotatedCameraWidget::paintLeadMetrics(QPainter &p, bool adjacent, QPointF *chevron, const cereal::RadarState::LeadData::Reader &lead_data) { + float leadDistance = lead_data.getDRel() + (adjacent ? std::abs(lead_data.getYRel()) : 0.0f); + float leadSpeed = std::max(lead_data.getVLead(), 0.0f); + + QString distanceString = QString::number(qRound(leadDistance * distanceConversion)); + QString speedString = QString::number(qRound(leadSpeed * speedConversionMetrics)); + + QVector textLines; + textLines.reserve(3); + if (adjacent) { + textLines.append(QString("%1 %2").arg(distanceString, leadDistanceUnit)); + textLines.append(QString("%1 %2").arg(speedString, leadSpeedUnit)); + } else { + if (frogpilot_toggles.value("openpilot_longitudinal").toBool()) { + int desiredDistance = std::max(0, qRound(desiredFollowDistance * distanceConversion)); + textLines.append(QString("%1 %2 (%3)").arg(distanceString, leadDistanceUnit, tr("Desired: %1").arg(desiredDistance))); + } else { + textLines.append(QString("%1 %2").arg(distanceString, leadDistanceUnit)); + } + textLines.append(QString("%1 %2").arg(speedString, leadSpeedUnit)); + + float timeGap = leadDistance / std::max(speed / speedConversion, 1.0f); + textLines.append(QString("%1 %2").arg(QString::number(timeGap, 'f', 2), tr("seconds"))); + } + + p.setFont(InterFont(45, QFont::DemiBold)); + p.setPen(whiteColor()); + + QFontMetrics metrics(p.font()); + int lineHeight = metrics.lineSpacing(); + + int maxTextWidth = 0; + for (QString &line : textLines) { + maxTextWidth = std::max(maxTextWidth, metrics.horizontalAdvance(line)); + } + + int centerX = (chevron[2].x() + chevron[0].x()) / 2; + int startY = chevron[0].y() + lineHeight + 5; + + int xMargin = maxTextWidth * 0.1; + int yMargin = lineHeight * 0.1; + + QRect textRect(centerX - maxTextWidth / 2, startY - lineHeight, maxTextWidth, textLines.size() * lineHeight); + textRect.adjust(-xMargin, -yMargin, xMargin, yMargin); + + if (adjacent) { + if (textRect.intersects(adjacentLeadTextRect) || textRect.intersects(leadTextRect)) { + return; + } + adjacentLeadTextRect = textRect; + } else { + leadTextRect = textRect; + } + + for (int i = 0; i < textLines.size(); ++i) { + int lineX = centerX - metrics.horizontalAdvance(textLines[i]) / 2; + int lineY = startY + (i * lineHeight); + + QPainterPath path; + path.addText(lineX, lineY, p.font(), textLines[i]); + p.strokePath(path, QPen(Qt::black, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + p.setPen(whiteColor()); + p.drawText(lineX, lineY, textLines[i]); + } +} + void FrogPilotAnnotatedCameraWidget::paintLongitudinalPaused(QPainter &p) { if (dmIconPosition == QPoint(0, 0)) { return; diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index 6caf4349..57148aaf 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h @@ -15,6 +15,7 @@ public: void paintAdjacentPaths(QPainter &p); void paintBlindSpotPath(QPainter &p); void paintFrogPilotWidgets(QPainter &p, UIState &s); + void paintLeadMetrics(QPainter &p, bool adjacent, QPointF *chevron, const cereal::RadarState::LeadData::Reader &lead_data); void updateState(const UIState &s, const FrogPilotUIState &fs); bool hideBottomIcons; @@ -41,6 +42,8 @@ public: QPolygonF track_adjacent_vertices[2]; + QRect adjacentLeadTextRect; + QRect leadTextRect; QRect setSpeedRect; QSize defaultSize; @@ -76,6 +79,7 @@ private: bool redLight; int animationFrameIndex; + int desiredFollowDistance; int frogHopCount; int signalAnimationLength; int signalHeight; diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index bbba61d0..21936bab 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -35,7 +35,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { drawLaneLines(painter); drawPath(painter, model, surface_rect.height()); - if (longitudinal_control && sm.alive("radarState") && !frogpilot_toggles.value("hide_lead_marker").toBool()) { + if ((longitudinal_control || frogpilot_toggles.value("lead_info").toBool()) && sm.alive("radarState") && !frogpilot_toggles.value("hide_lead_marker").toBool()) { update_leads(radar_state, model.getPosition()); const auto &lead_two = radar_state.getLeadTwo(); if (lead_one.getStatus()) { @@ -46,6 +46,8 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { } } else { // FrogPilot variables + frogpilot_nvg->leadTextRect = QRect(); + } if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) { drawLead(painter, lead_two, lead_vertices[1], surface_rect, QColor(frogpilot_toggles.value("lead_marker_color").toString())); } @@ -59,11 +61,18 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { updateAdjacentLeads(frogpilot_radar_state, model.getPosition()); - if (lead_left.getStatus()) { + frogpilot_nvg->adjacentLeadTextRect = QRect(); + + if (lead_left.getStatus() && lead_right.getStatus() && (lead_left.getDRel() < lead_right.getDRel())) { 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); + } else { + 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); + } } } @@ -277,6 +286,9 @@ void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadDa painter.drawPolygon(chevron, std::size(chevron)); // FrogPilot variables + if (frogpilot_toggles.value("lead_info").toBool()) { + frogpilot_nvg->paintLeadMetrics(painter, adjacent, chevron, lead_data); + } } // Projects a point in car to space to the corresponding point in full frame image space.