diff --git a/frogpilot/common/frogpilot_utilities.py b/frogpilot/common/frogpilot_utilities.py index 8e052426..3f45cb22 100644 --- a/frogpilot/common/frogpilot_utilities.py +++ b/frogpilot/common/frogpilot_utilities.py @@ -85,6 +85,31 @@ def calculate_distance_to_point(lat1, lon1, lat2, lon2): return EARTH_RADIUS * c +def calculate_lane_width(lane_line1, lane_line2, road_edge=None): + lane_line1_x = np.asarray(lane_line1.x) + lane_line1_y = np.asarray(lane_line1.y) + + lane_line2_x = np.asarray(lane_line2.x) + lane_line2_y = np.asarray(lane_line2.y) + + lane_y_interp = np.interp(lane_line2_x, lane_line1_x, lane_line1_y) + distance_to_lane = np.median(np.abs(lane_line2_y - lane_y_interp)) + + if road_edge is None: + return float(distance_to_lane) + + edge_line_x = np.asarray(road_edge.x) + edge_line_y = np.asarray(road_edge.y) + + edge_y_interp = np.interp(lane_line2_x, edge_line_x, edge_line_y) + distance_to_road_edge = np.median(np.abs(lane_line2_y - edge_y_interp)) + + if distance_to_road_edge < distance_to_lane: + return 0.0 + + return float(distance_to_lane) + + # Credit goes to Pfeiferj! def calculate_road_curvature(modelData): orientation_rate = np.array(modelData.orientationRate.z) diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index cafb0445..eb939c58 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -11,7 +11,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_MAX from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE -from openpilot.frogpilot.common.frogpilot_utilities import calculate_road_curvature +from openpilot.frogpilot.common.frogpilot_utilities import calculate_lane_width, calculate_road_curvature from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD from openpilot.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode from openpilot.frogpilot.controls.lib.frogpilot_acceleration import FrogPilotAcceleration @@ -36,6 +36,8 @@ class FrogPilotPlanner: self.road_curvature_detected = False self.tracking_lead = False + self.lane_width_left = 0 + self.lane_width_right = 0 self.lateral_acceleration = 0 self.model_length = 0 self.road_curvature = 0 @@ -82,6 +84,13 @@ class FrogPilotPlanner: } self.params_memory.put("LastGPSPosition", json.dumps(self.gps_position)) + if v_ego >= frogpilot_toggles.minimum_lane_change_speed: + self.lane_width_left = calculate_lane_width(sm["modelV2"].laneLines[0], sm["modelV2"].laneLines[1], sm["modelV2"].roadEdges[0]) + self.lane_width_right = calculate_lane_width(sm["modelV2"].laneLines[3], sm["modelV2"].laneLines[2], sm["modelV2"].roadEdges[1]) + else: + self.lane_width_left = 0 + self.lane_width_right = 0 + self.lateral_acceleration = v_ego**2 * sm["controlsState"].curvature self.lateral_check |= sm["carState"].standstill @@ -132,6 +141,9 @@ class FrogPilotPlanner: frogpilotPlan.increasedStoppedDistance = frogpilot_toggles.increase_stopped_distance + frogpilotPlan.laneWidthLeft = self.lane_width_left + frogpilotPlan.laneWidthRight = self.lane_width_right + frogpilotPlan.lateralCheck = self.lateral_check frogpilotPlan.maxAcceleration = float(self.frogpilot_acceleration.max_accel) diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 78e7c1dd..d730df71 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -156,6 +156,8 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil cscTraining = frogpilotPlan.getCscTraining(); experimentalMode = selfdriveState.getExperimentalMode(); forceCoast = frogpilotCarState.getForceCoast(); + laneWidthLeft = frogpilotPlan.getLaneWidthLeft(); + laneWidthRight = frogpilotPlan.getLaneWidthRight(); redLight = frogpilotPlan.getRedLight(); roadCurvature = frogpilotPlan.getRoadCurvature(); roadName = QString::fromStdString(params_memory.get("RoadName")); @@ -256,6 +258,63 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState } } +void FrogPilotAnnotatedCameraWidget::paintAdjacentPaths(QPainter &p) { + std::function paintPath = [&](const QPolygonF &path, bool isLeft, bool isBlindSpot, float laneWidth) { + if (laneWidth == 0.0f) { + return; + } + + p.save(); + + QLinearGradient gradient(0, height(), 0, 0); + if (isBlindSpot && frogpilot_toggles.value("blind_spot_path").toBool()) { + gradient.setColorAt(0.0f, QColor::fromHslF(0.0f, 0.75f, 0.5f, 0.4f)); + gradient.setColorAt(0.5f, QColor::fromHslF(0.0f, 0.75f, 0.5f, 0.35f)); + gradient.setColorAt(1.0f, QColor::fromHslF(0.0f, 0.75f, 0.5f, 0.0f)); + } else { + float ratio = std::clamp(laneWidth / frogpilot_toggles.value("lane_detection_width").toDouble(), 0.0, 1.0); + float hue = (ratio * ratio) * (120.0f / 360.0f); + + gradient.setColorAt(0.0f, QColor::fromHslF(hue, 0.75f, 0.5f, 0.4f)); + gradient.setColorAt(0.5f, QColor::fromHslF(hue, 0.75f, 0.5f, 0.35f)); + gradient.setColorAt(1.0f, QColor::fromHslF(hue, 0.75f, 0.5f, 0.0f)); + } + + p.setBrush(gradient); + p.drawPolygon(path); + + if (frogpilot_toggles.value("adjacent_path_metrics").toBool()) { + QString text; + if (isBlindSpot && frogpilot_toggles.value("blind_spot_path").toBool()) { + text = tr("Vehicle in blind spot"); + } else { + text = QString::number(laneWidth * distanceConversion, 'f', 2) + leadDistanceUnit; + } + + int midIndex = path.size() / 2; + QPointF anchorPoint = isLeft ? path[midIndex / 2] : path[midIndex + (path.size() - midIndex) / 2]; + + p.setFont(InterFont(45, QFont::DemiBold)); + QFontMetrics metrics(p.font()); + + int textXPosition = isLeft ? anchorPoint.x() - metrics.horizontalAdvance(text) : anchorPoint.x(); + int textYPosition = anchorPoint.y() - metrics.height() / 2 + metrics.ascent(); + + QPainterPath textPath; + textPath.addText(textXPosition, textYPosition, p.font(), text); + p.strokePath(textPath, QPen(Qt::black, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + p.setPen(whiteColor()); + p.drawText(textXPosition, textYPosition, text); + } + + p.restore(); + }; + + paintPath(track_adjacent_vertices[0], true, blindspotLeft, laneWidthLeft); + paintPath(track_adjacent_vertices[1], false, blindspotRight, laneWidthRight); +} + void FrogPilotAnnotatedCameraWidget::paintBlindSpotPath(QPainter &p) { p.save(); diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index 6e3d6477..6caf4349 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h @@ -12,6 +12,7 @@ public: explicit FrogPilotAnnotatedCameraWidget(QWidget *parent = 0); void mousePressEvent(QMouseEvent *mouseEvent) override; + void paintAdjacentPaths(QPainter &p); void paintBlindSpotPath(QPainter &p); void paintFrogPilotWidgets(QPainter &p, UIState &s); void updateState(const UIState &s, const FrogPilotUIState &fs); @@ -38,6 +39,8 @@ public: QPoint dmIconPosition; QPoint experimentalButtonPosition; + QPolygonF track_adjacent_vertices[2]; + QRect setSpeedRect; QSize defaultSize; @@ -83,6 +86,8 @@ private: float accelerationEgo; float cscSpeed; float distanceConversion; + float laneWidthLeft; + float laneWidthRight; float roadCurvature; float setSpeed; float speedConversion; diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 304865e8..bbba61d0 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -31,7 +31,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { const auto &radar_state = sm["radarState"].getRadarState(); const auto &lead_one = radar_state.getLeadOne(); - update_model(model, lead_one); + update_model(model, lead_one, surface_rect.height()); drawLaneLines(painter); drawPath(painter, model, surface_rect.height()); @@ -85,7 +85,7 @@ void ModelRenderer::update_leads(const cereal::RadarState::Reader &radar_state, } } -void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) { +void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead, float height) { const auto &model_position = model.getPosition(); float max_distance = *(model_position.getX().end() - 1); @@ -125,6 +125,9 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const SubMaster &fpsm = *(fs->sm); const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan(); + + mapAveragedLineToPolygon(lane_lines[0], lane_lines[1], frogpilotPlan.getLaneWidthLeft() / 2.0f, 0, &frogpilot_nvg->track_adjacent_vertices[0], max_idx, height, false); + mapAveragedLineToPolygon(lane_lines[2], lane_lines[3], frogpilotPlan.getLaneWidthRight() / 2.0f, 0, &frogpilot_nvg->track_adjacent_vertices[1], max_idx, height, false); } void ModelRenderer::drawLaneLines(QPainter &painter) { @@ -190,7 +193,9 @@ void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reade painter.drawPolygon(track_vertices); // FrogPilot variables - if (frogpilot_toggles.value("blind_spot_path").toBool()) { + if (frogpilot_toggles.value("adjacent_paths").toBool() || frogpilot_toggles.value("adjacent_path_metrics").toBool()) { + frogpilot_nvg->paintAdjacentPaths(painter); + } else if (frogpilot_toggles.value("blind_spot_path").toBool()) { frogpilot_nvg->paintBlindSpotPath(painter); } } @@ -305,6 +310,49 @@ void ModelRenderer::mapLineToPolygon(const cereal::XYZTData::Reader &line, float } // FrogPilot variables +void ModelRenderer::mapAveragedLineToPolygon(const cereal::XYZTData::Reader &line1, const cereal::XYZTData::Reader &line2, float y_off, float z_off, + QPolygonF *pvd, int max_idx, float height, bool allow_invert) { + const auto line_x1 = line1.getX(), line_y1 = line1.getY(), line_z1 = line1.getZ(); + const auto line_y2 = line2.getY(); + QPointF left, right; + pvd->clear(); + for (int i = 0; i <= max_idx; i++) { + // highly negative x positions are drawn above the frame and cause flickering, clip to zy plane of camera + if (line_x1[i] < 0) continue; + + bool l = mapToScreen(line_x1[i], ((line_y1[i] + line_y2[i]) / 2.0f) - y_off, line_z1[i] + z_off, &left); + bool r = mapToScreen(line_x1[i], ((line_y1[i] + line_y2[i]) / 2.0f) + y_off, line_z1[i] + z_off, &right); + if (l && r) { + // For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts + if (!allow_invert && pvd->size() && left.y() > pvd->back().y()) { + continue; + } + pvd->push_back(left); + pvd->push_front(right); + } + } + + // Ground the path + if (pvd->size() >= 4) { + int mid = pvd->size() / 2; + + std::function extendToBottom = [&](int idx1, int idx2) { + QPointF &p0 = (*pvd)[idx1]; + QPointF &p1 = (*pvd)[idx2]; + + float dy = p0.y() - p1.y(); + if (std::abs(dy) > 0.1f) { + float slope = (p0.x() - p1.x()) / dy; + p0.setX(p0.x() + (height - p0.y()) * slope); + p0.setY(height); + } + }; + + extendToBottom(mid, mid + 1); + extendToBottom(mid - 1, mid - 2); + } +} + 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(); diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h index df0421bc..53afad22 100644 --- a/selfdrive/ui/qt/onroad/model.h +++ b/selfdrive/ui/qt/onroad/model.h @@ -26,7 +26,7 @@ private: 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, 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 update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead, float height); void drawLaneLines(QPainter &painter); void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height); void updatePathGradient(QLinearGradient &bg); @@ -47,6 +47,8 @@ private: QRectF clip_region; // FrogPilot variables + void mapAveragedLineToPolygon(const cereal::XYZTData::Reader &line1, const cereal::XYZTData::Reader &line2, float y_off, float z_off, + QPolygonF *pvd, int max_idx, float height, bool allow_invert = true); void updateAdjacentLeads(const cereal::FrogPilotRadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); void updateRadarTracks(const cereal::XYZTData::Reader &line);