From 532048a2e9b8b5dc9cf5b139f9d37f3ee63172f6 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 22 May 2024 00:05:50 -0700 Subject: [PATCH] Visuals - Custom Onroad UI - Paths - Adjacent Show the detected adjacent lanes. --- .../frogpilot/controls/frogpilot_planner.py | 5 ++- selfdrive/modeld/fill_model_msg.py | 27 ++++++++++-- selfdrive/ui/qt/onroad.cc | 41 +++++++++++++++++++ selfdrive/ui/qt/onroad.h | 5 +++ selfdrive/ui/ui.cc | 14 +++++++ selfdrive/ui/ui.h | 9 ++++ 6 files changed, 96 insertions(+), 5 deletions(-) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 35e5c12..e4b4f3b 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -129,7 +129,7 @@ class FrogPilotPlanner: else: self.min_accel = A_CRUISE_MIN - 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 = 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])) @@ -277,6 +277,9 @@ class FrogPilotPlanner: frogpilotPlan.conditionalExperimental = self.cem.experimental_mode frogpilotPlan.redLight = self.cem.red_light_detected + frogpilotPlan.laneWidthLeft = self.lane_width_left + frogpilotPlan.laneWidthRight = self.lane_width_right + frogpilotPlan.leadDeparting = self.lead_departing frogpilotPlan.maxAcceleration = self.max_accel diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index c39ec2d..c3d66b4 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -94,10 +94,29 @@ 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): - 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]) + modelV2.init('laneLines', 6) + for i in range(6): + if i < 4: + 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]) + else: + lane_line = modelV2.laneLines[i] + far_lane, near_lane, road_edge = (0, 1, 0) if i == 4 else (3, 2, 1) + + lane_diff = np.abs(net_output_data['lane_lines'][0,near_lane] - net_output_data['lane_lines'][0,far_lane]) + road_edge_diff = np.abs(net_output_data['lane_lines'][0,near_lane] - net_output_data['road_edges'][0,road_edge]) + + y_min = net_output_data['lane_lines'][0, near_lane,:,0] + z_min = net_output_data['lane_lines'][0, near_lane,:,1] + + y_min += np.where(lane_diff[:,0] < road_edge_diff[:,0], net_output_data['lane_lines'][0,far_lane,:,0], net_output_data['road_edges'][0,road_edge,:,0]) + z_min += np.where(lane_diff[:,1] < road_edge_diff[:,1], net_output_data['lane_lines'][0,far_lane,:,1], net_output_data['road_edges'][0,road_edge,:,1]) + + y_min /= 2 + z_min /= 2 + + fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), y_min, z_min) + 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.cc b/selfdrive/ui/qt/onroad.cc index d107e6e..aae66dc 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -641,6 +641,40 @@ 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)) { + float minLaneWidth = laneDetectionWidth * 0.5; + float maxLaneWidth = laneDetectionWidth * 1.5; + + auto paintLane = [&](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); + + 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("%1%2").arg(laneWidth * distanceConversion, 0, 'f', 2).arg(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(); } @@ -865,6 +899,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { alwaysOnLateralActive = scene.always_on_lateral_active; showAlwaysOnLateralStatusBar = scene.show_aol_status_bar; + blindSpotLeft = scene.blind_spot_left; + blindSpotRight = scene.blind_spot_right; + compass = scene.compass; conditionalStatus = scene.conditional_status; @@ -876,6 +913,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { experimentalMode = scene.experimental_mode; + laneDetectionWidth = scene.lane_detection_width; + laneWidthLeft = scene.lane_width_left; + laneWidthRight = scene.lane_width_right; + mapOpen = scene.map_open; onroadDistanceButton = scene.onroad_distance_button; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 425dd4a..f20d34f 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -173,6 +173,8 @@ private: QHBoxLayout *bottom_layout; bool alwaysOnLateralActive; + bool blindSpotLeft; + bool blindSpotRight; bool compass; bool experimentalMode; bool mapOpen; @@ -189,6 +191,9 @@ private: float accelerationConversion; float cruiseAdjustment; float distanceConversion; + float laneDetectionWidth; + float laneWidthLeft; + float laneWidthRight; float slcSpeedLimitOffset; float speedConversion; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 97467b8..ba291cb 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -121,6 +121,11 @@ void update_model(UIState *s, } max_idx = get_path_length_idx(plan_position, max_distance); update_line_data(s, plan_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); + + // 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); + } } void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd) { @@ -215,6 +220,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")) { @@ -239,6 +246,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(); @@ -291,6 +300,8 @@ void ui_update_frogpilot_params(UIState *s) { 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"); @@ -302,6 +313,9 @@ void ui_update_frogpilot_params(UIState *s) { 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 068a8d3..342f382 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -187,7 +187,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; @@ -217,6 +221,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; @@ -229,6 +236,8 @@ typedef struct UIScene { int conditional_speed_lead; int conditional_status; + QPolygonF track_adjacent_vertices[6]; + } UIScene; class UIState : public QObject {