From 692fca7879660171828ce99be646553641ecdf51 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 12 Jun 2024 16:38:21 -0700 Subject: [PATCH] Visuals - Custom Onroad UI - Paths - Adjacent Show the detected adjacent lanes. --- .../frogpilot/controls/frogpilot_planner.py | 2 +- selfdrive/modeld/fill_model_msg.py | 27 ++++++++++-- selfdrive/ui/qt/onroad/annotated_camera.cc | 42 +++++++++++++++++++ selfdrive/ui/qt/onroad/annotated_camera.h | 5 +++ selfdrive/ui/ui.cc | 14 +++++++ selfdrive/ui/ui.h | 9 ++++ 6 files changed, 95 insertions(+), 4 deletions(-) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index ef233dc40..4f4471454 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -98,7 +98,7 @@ class FrogPilotPlanner: if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateral) and driving_gear: self.cem.update(carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles) - check_lane_width = frogpilot_toggles.lane_detection + check_lane_width = frogpilot_toggles.adjacent_lanes or frogpilot_toggles.lane_detection if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed: self.lane_width_left = calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]) self.lane_width_right = calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 99d7a9360..7ccc2250f 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -94,10 +94,31 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx] # lane lines - modelV2.init('laneLines', 4) - for i in range(4): + modelV2.init('laneLines', 6) + for i in range(6): lane_line = modelV2.laneLines[i] - fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) + if i < 4: + fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) + else: + far_lane, near_lane, road_edge = (0, 1, 0) if i == 4 else (3, 2, 1) + + near_lane_y = net_output_data['lane_lines'][0,near_lane,:,0] + road_edge_y = net_output_data['road_edges'][0,road_edge,:,0] + far_lane_y = net_output_data['lane_lines'][0,far_lane,:,0] + + road_edge_distance = abs(np.linalg.norm(road_edge_y - near_lane_y)) + far_lane_distance = abs(np.linalg.norm(far_lane_y - near_lane_y)) + + if road_edge_distance < far_lane_distance: + closest_lane_y = road_edge_y + else: + closest_lane_y = far_lane_y + + diff_y = closest_lane_y - near_lane_y + new_lane_y = near_lane_y + diff_y / 2 + + fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), new_lane_y, net_output_data['lane_lines'][0,near_lane,:,1]) + modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist() modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist() diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index ef93f54e4..eab6c5f3a 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -313,6 +313,41 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.setBrush(bg); painter.drawPolygon(scene.track_vertices); + // Paint adjacent lane paths + if (scene.adjacent_path && (laneWidthLeft != 0 || laneWidthRight != 0)) { + const float minLaneWidth = laneDetectionWidth * 0.5f; + const float maxLaneWidth = laneDetectionWidth * 1.5f; + + auto paintLane = [&](const QPolygonF &lane, float laneWidth, bool blindspot) { + QLinearGradient gradient(0, height(), 0, 0); + + bool redPath = laneWidth < minLaneWidth || laneWidth > maxLaneWidth || blindspot; + float hue = redPath ? 0.0f : 120.0f * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth); + float hueF = hue / 360.0f; + + gradient.setColorAt(0.0, QColor::fromHslF(hueF, 0.75f, 0.50f, 0.6f)); + gradient.setColorAt(0.5, QColor::fromHslF(hueF, 0.75f, 0.50f, 0.4f)); + gradient.setColorAt(1.0, QColor::fromHslF(hueF, 0.75f, 0.50f, 0.2f)); + + painter.setBrush(gradient); + painter.drawPolygon(lane); + + if (scene.adjacent_path_metrics) { + painter.setFont(InterFont(30, QFont::DemiBold)); + painter.setPen(Qt::white); + + QRectF boundingRect = lane.boundingRect(); + QString text = blindspot ? tr("Vehicle in blind spot") : QString::number(laneWidth * distanceConversion, 'f', 2) + leadDistanceUnit; + painter.drawText(boundingRect, Qt::AlignCenter, text); + + painter.setPen(Qt::NoPen); + } + }; + + paintLane(scene.track_adjacent_vertices[4], laneWidthLeft, blindSpotLeft); + paintLane(scene.track_adjacent_vertices[5], laneWidthRight, blindSpotRight); + } + painter.restore(); } @@ -549,6 +584,9 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce drawStatusBar(painter); } + blindSpotLeft = scene.blind_spot_left; + blindSpotRight = scene.blind_spot_right; + compass = scene.compass; bool enableCompass = compass && !hideBottomIcons; compass_img->setVisible(enableCompass); @@ -568,6 +606,10 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce experimentalMode = scene.experimental_mode; + laneDetectionWidth = scene.lane_detection_width; + laneWidthLeft = scene.lane_width_left; + laneWidthRight = scene.lane_width_right; + mapOpen = scene.map_open; map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled()); if (map_settings_btn_bottom->isEnabled()) { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 5297769c4..f8d28b9c4 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -80,6 +80,8 @@ private: QHBoxLayout *bottom_layout; bool alwaysOnLateralActive; + bool blindSpotLeft; + bool blindSpotRight; bool compass; bool experimentalMode; bool mapOpen; @@ -98,6 +100,9 @@ private: float accelerationConversion; float cruiseAdjustment; float distanceConversion; + float laneDetectionWidth; + float laneWidthLeft; + float laneWidthRight; float slcSpeedLimitOffset; float speedConversion; float unconfirmedSpeedLimit; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index ee379bc15..f3d634894 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -107,6 +107,11 @@ void update_model(UIState *s, update_line_data(s, road_edges[i], 0.025, 0, &scene.road_edge_vertices[i], max_idx); } + // Update adjacent paths + for (int i = 4; i <= 5; i++) { + update_line_data(s, lane_lines[i], (i == 4 ? scene.lane_width_left : scene.lane_width_right) / 2.0f, 0, &scene.track_adjacent_vertices[i], max_idx, false); + } + // update path auto lead_count = model.getLeadsV3().size(); if (lead_count > 0) { @@ -214,6 +219,8 @@ static void update_state(UIState *s) { } if (sm.updated("carState")) { auto carState = sm["carState"].getCarState(); + scene.blind_spot_left = carState.getLeftBlindspot(); + scene.blind_spot_right = carState.getRightBlindspot(); scene.parked = carState.getGearShifter() == cereal::CarState::GearShifter::PARK; } if (sm.updated("controlsState")) { @@ -238,6 +245,8 @@ static void update_state(UIState *s) { if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); + scene.lane_width_left = frogpilotPlan.getLaneWidthLeft(); + scene.lane_width_right = frogpilotPlan.getLaneWidthRight(); scene.speed_limit = frogpilotPlan.getSlcSpeedLimit(); scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset(); scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden(); @@ -301,6 +310,8 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { bool custom_onroad_ui = params.getBool("CustomUI"); bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths"); 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.compass = custom_onroad_ui && params.getBool("Compass"); scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); @@ -312,6 +323,9 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { scene.experimental_mode_via_screen = scene.longitudinal_control && params.getBool("ExperimentalModeActivation") && params.getBool("ExperimentalModeViaTap"); + bool lane_detection = params.getBool("NudgelessLaneChange") && params.getInt("LaneDetectionWidth") != 0; + scene.lane_detection_width = lane_detection ? params.getInt("LaneDetectionWidth") * (scene.is_metric ? 1 : FOOT_TO_METER) / 10.0f : 2.75f; + bool longitudinal_tune = scene.longitudinal_control && params.getBool("LongitudinalTune"); bool radarless_model = params.get("Model") == "radical-turtle"; scene.lead_detection_threshold = longitudinal_tune && !radarless_model ? params.getInt("LeadDetectionThreshold") / 100.0f : 0.5; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index a34677b3b..a5c95f9c2 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -123,7 +123,11 @@ 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_right; bool compass; bool conditional_experimental; bool disable_smoothing_mtsc; @@ -155,6 +159,9 @@ typedef struct UIScene { bool vtsc_controlling_curve; float adjusted_cruise; + float lane_detection_width; + float lane_width_left; + float lane_width_right; float lead_detection_threshold; float speed_limit; float speed_limit_offset; @@ -168,6 +175,8 @@ typedef struct UIScene { int conditional_status; int tethering_config; + QPolygonF track_adjacent_vertices[6]; + } UIScene; class UIState : public QObject {