diff --git a/common/params.cc b/common/params.cc index c5dd932..a8eefa1 100644 --- a/common/params.cc +++ b/common/params.cc @@ -211,6 +211,8 @@ std::unordered_map keys = { // FrogPilot parameters {"AccelerationPath", PERSISTENT}, {"AccelerationProfile", PERSISTENT}, + {"AdjacentPath", PERSISTENT}, + {"AdjacentPathMetrics", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, {"AggressiveFollow", PERSISTENT}, {"AggressiveJerk", PERSISTENT}, diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 71bdf20..a982f19 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -57,7 +57,7 @@ class FrogPilotPlanner: self.cem.update(carState, enabled, sm['frogpilotNavigation'], modelData, sm['radarState'], self.road_curvature, self.stop_distance, mpc.t_follow, v_ego) # Update the current lane widths - check_lane_width = self.blind_spot_path + check_lane_width = self.adjacent_lanes or self.blind_spot_path if check_lane_width and v_ego >= LANE_CHANGE_SPEED_MIN: self.lane_width_left = float(self.fpf.calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])) self.lane_width_right = float(self.fpf.calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])) @@ -121,6 +121,7 @@ class FrogPilotPlanner: self.relaxed_jerk = params.get_float("RelaxedJerk") custom_ui = params.get_bool("CustomUI") + self.adjacent_lanes = custom_ui and params.get_bool("AdjacentPath") self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath") longitudinal_tune = params.get_bool("LongitudinalTune") diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index 1af72fd..1900145 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -23,6 +23,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"}, {"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""}, + {"AdjacentPath", "Adjacent Paths", "Display paths to the left and right of your car, visualizing where the model detects lanes.", ""}, {"BlindSpotPath", "Blind Spot Path", "Visualize your blind spots with a red path when another vehicle is detected nearby.", ""}, {"LeadInfo", "Lead Info and Logics", "Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance.", ""}, @@ -111,6 +112,10 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot std::vector leadInfoToggles{"UseSI"}; std::vector leadInfoToggleNames{tr("Use SI Values")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, leadInfoToggles, leadInfoToggleNames); + } else if (param == "AdjacentPath") { + std::vector adjacentPathToggles{"AdjacentPathMetrics"}; + std::vector adjacentPathToggleNames{tr("Display Metrics")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, adjacentPathToggles, adjacentPathToggleNames); } else if (param == "ModelUI") { FrogPilotParamManageControl *modelUIToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); diff --git a/selfdrive/frogpilot/ui/visual_settings.h b/selfdrive/frogpilot/ui/visual_settings.h index 01b94ac..a03cee0 100644 --- a/selfdrive/frogpilot/ui/visual_settings.h +++ b/selfdrive/frogpilot/ui/visual_settings.h @@ -30,7 +30,7 @@ private: std::set alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"}; std::set customAlertsKeys = {}; - std::set customOnroadUIKeys = {"AccelerationPath", "BlindSpotPath", "LeadInfo"}; + std::set customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"}; std::set modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"}; std::set qolKeys = {"DriveStats"}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index d8c5812..a94e91f 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -621,6 +621,51 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { } } + // Paint adjacent lane paths + if (scene.adjacent_path && (laneWidthLeft != 0 || laneWidthRight != 0)) { + // Set up the units + double distanceValue = is_metric ? 1.0 : METER_TO_FOOT; + QString unit_d = is_metric ? " meters" : " feet"; + + // Declare the lane width thresholds + constexpr float minLaneWidth = 2.0f; + constexpr float maxLaneWidth = 4.0f; + + // Set gradient colors based on laneWidth and blindspot + auto setGradientColors = [](QLinearGradient &gradient, float laneWidth, bool blindspot) { + // Make the path red for smaller paths or if there's a car in the blindspot and green for larger paths + double hue = (laneWidth < minLaneWidth || laneWidth > maxLaneWidth || blindspot) + ? 0.0 : 120.0 * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth); + double hue_ratio = hue / 360.0; + gradient.setColorAt(0.0, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.6)); + gradient.setColorAt(0.5, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.4)); + gradient.setColorAt(1.0, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.2)); + }; + + // Paint the lanes + auto paintLane = [&](QPainter &painter, const QPolygonF &lane, float laneWidth, bool blindspot) { + QLinearGradient gradient(0, height(), 0, 0); + setGradientColors(gradient, laneWidth, blindspot); + + painter.setFont(InterFont(30, QFont::DemiBold)); + painter.setBrush(gradient); + painter.setPen(Qt::transparent); + painter.drawPolygon(lane); + painter.setPen(Qt::white); + + QRectF boundingRect = lane.boundingRect(); + if (scene.adjacent_path_metrics) { + painter.drawText(boundingRect.center(), + blindspot ? "Vehicle in blind spot" : + QString("%1%2").arg(laneWidth * distanceValue, 0, 'f', 2).arg(unit_d)); + } + painter.setPen(Qt::NoPen); + }; + + paintLane(painter, scene.track_adjacent_vertices[4], laneWidthLeft, blindSpotLeft); + paintLane(painter, scene.track_adjacent_vertices[5], laneWidthRight, blindSpotRight); + } + painter.restore(); } @@ -882,6 +927,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { experimentalMode = scene.experimental_mode; + laneWidthLeft = scene.lane_width_left; + laneWidthRight = scene.lane_width_right; + leadInfo = scene.lead_info; obstacleDistance = scene.obstacle_distance; obstacleDistanceStock = scene.obstacle_distance_stock; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 9e2ca12..94f2d3a 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -134,6 +134,8 @@ private: bool turnSignalRight; float distanceConversion; + float laneWidthLeft; + float laneWidthRight; float speedConversion; int cameraView; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 7583ba4..cca56b2 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -291,6 +291,8 @@ void ui_update_frogpilot_params(UIState *s) { bool custom_onroad_ui = params.getBool("CustomUI"); scene.acceleration_path = custom_onroad_ui && params.getBool("AccelerationPath"); + scene.adjacent_path = custom_onroad_ui && params.getBool("AdjacentPath"); + scene.adjacent_path_metrics = scene.adjacent_path && params.getBool("AdjacentPathMetrics"); scene.blind_spot_path = custom_onroad_ui && params.getBool("BlindSpotPath"); scene.lead_info = custom_onroad_ui && params.getBool("LeadInfo"); scene.use_si = scene.lead_info && params.getBool("UseSI"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 7b59ef4..0a70d34 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -170,6 +170,8 @@ typedef struct UIScene { // FrogPilot variables bool acceleration_path; + bool adjacent_path; + bool adjacent_path_metrics; bool always_on_lateral; bool always_on_lateral_active; bool blind_spot_left;