From 7b8bc36d3d291cb983bc8be542bb3d4c59ff5681 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 2 Apr 2024 14:18:23 -0700 Subject: [PATCH] Draw adjacent paths in onroad UI Added toggle to draw the adjacent paths in the onroad UI to visualize what other lanes the model may see. --- common/params.cc | 2 + .../frogpilot/controls/frogpilot_planner.py | 3 +- .../ui/qt/offroad/visual_settings.cc | 4 +- selfdrive/ui/qt/onroad.cc | 39 +++++++++++++++++++ selfdrive/ui/qt/onroad.h | 2 + selfdrive/ui/ui.cc | 2 + selfdrive/ui/ui.h | 2 + 7 files changed, 51 insertions(+), 3 deletions(-) diff --git a/common/params.cc b/common/params.cc index 2cc7f87d7..2b9dbbe32 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/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 56ab59361..62c1cc2b4 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -80,7 +80,7 @@ class FrogPilotPlanner: else: self.min_accel = ACCEL_MIN - 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(calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])) self.lane_width_right = float(calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])) @@ -182,6 +182,7 @@ class FrogPilotPlanner: self.relaxed_follow = self.params.get_float("RelaxedFollow") custom_ui = self.params.get_bool("CustomUI") + self.adjacent_lanes = custom_ui and self.params.get_bool("AdjacentPath") self.blind_spot_path = custom_ui and self.params.get_bool("BlindSpotPath") longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune") diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc index d667b31d1..49e79a3da 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc @@ -125,8 +125,8 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot std::vector leadInfoToggleNames{tr("Use SI Values")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, leadInfoToggles, leadInfoToggleNames); } else if (param == "CustomPaths") { - std::vector pathToggles{"AccelerationPath", "BlindSpotPath"}; - std::vector pathToggleNames{tr("Acceleration"), tr("Blind Spot")}; + std::vector pathToggles{"AccelerationPath", "AdjacentPath", "BlindSpotPath", "AdjacentPathMetrics"}; + std::vector pathToggleNames{tr("Acceleration"), tr("Adjacent"), tr("Blind Spot"), tr("Metrics")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, pathToggles, pathToggleNames); } else if (param == "ModelUI") { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 534440b70..751cadaaa 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -658,6 +658,42 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { } } + // Paint adjacent lane paths + if (scene.adjacent_path && (laneWidthLeft != 0 || laneWidthRight != 0)) { + QString unit_d = is_metric ? tr(" meters") : tr(" feet"); + + float minLaneWidth = laneDetectionWidth * 0.5; + float maxLaneWidth = laneDetectionWidth * 1.5; + + auto paintLane = [=](QPainter &painter, const QPolygonF &lane, float laneWidth, bool blindspot) { + QLinearGradient al(0, height(), 0, 0); + + bool redPath = laneWidth < minLaneWidth || laneWidth > maxLaneWidth || blindspot; + float hue = redPath ? 0.0 : 120.0 * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth); + + al.setColorAt(0.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.6)); + al.setColorAt(0.5, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.4)); + al.setColorAt(1.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.2)); + + painter.setBrush(al); + painter.drawPolygon(lane); + + painter.setFont(InterFont(30, QFont::DemiBold)); + painter.setPen(Qt::white); + + QRectF boundingRect = lane.boundingRect(); + if (scene.adjacent_path_metrics) { + QString text = blindspot ? tr("Vehicle in blind spot") : + QString("%1%2").arg(laneWidth * distanceConversion, 0, 'f', 2).arg(unit_d); + painter.drawText(boundingRect, Qt::AlignCenter, text); + } + painter.setPen(Qt::NoPen); + }; + + paintLane(painter, scene.track_adjacent_vertices[4], laneWidthLeft, blindSpotLeft); + paintLane(painter, scene.track_adjacent_vertices[5], laneWidthRight, blindSpotRight); + } + painter.restore(); } @@ -910,6 +946,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { 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 92470efb6..16e3b311d 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 alertSize; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index a334f5072..f2148f28e 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -303,6 +303,8 @@ void ui_update_frogpilot_params(UIState *s) { bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths"); bool developer_ui = !isRelease && custom_onroad_ui && params.getBool("DeveloperUI"); scene.acceleration_path = custom_paths && params.getBool("AccelerationPath"); + scene.adjacent_path = custom_paths && params.getBool("AdjacentPath"); + scene.adjacent_path_metrics = scene.adjacent_path && params.getBool("AdjacentPathMetrics"); scene.blind_spot_path = custom_paths && params.getBool("BlindSpotPath"); scene.lead_info = scene.longitudinal_control && custom_onroad_ui && params.getBool("LeadInfo"); scene.show_jerk = scene.longitudinal_control && developer_ui && params.getBool("ShowJerk"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index cbd7cd5e2..7449a7863 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -172,6 +172,8 @@ typedef struct UIScene { // FrogPilot variables bool acceleration_path; + bool adjacent_path; + bool adjacent_path_metrics; bool always_on_lateral_active; bool blind_spot_left; bool blind_spot_path;