diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 22633defd..bf7a67ea9 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -23,6 +23,8 @@ struct FrogPilotNavigation @0xda96579883444c35 { struct FrogPilotPlan @0x80ae746ee2596b11 { jerk @7 :Float32; + laneWidthLeft @8 :Float32; + laneWidthRight @9 :Float32; minAcceleration @10 :Float32; maxAcceleration @11 :Float32; tFollow @20 :Float32; diff --git a/common/params.cc b/common/params.cc index dd4e96072..187eb2983 100644 --- a/common/params.cc +++ b/common/params.cc @@ -217,6 +217,7 @@ std::unordered_map keys = { {"AlwaysOnLateralMain", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, {"AutomaticUpdates", PERSISTENT}, + {"BlindSpotPath", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomPaths", PERSISTENT}, {"CustomUI", PERSISTENT}, diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 0b2a41eab..5fa0743d5 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -7,6 +7,7 @@ from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX +from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel @@ -75,6 +76,14 @@ class FrogPilotPlanner: else: self.min_accel = ACCEL_MIN + check_lane_width = 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])) + else: + self.lane_width_left = 0 + self.lane_width_right = 0 + road_curvature = calculate_road_curvature(modelData, v_ego) if radarState.leadOne.status and self.CP.openpilotLongitudinalControl: @@ -124,6 +133,8 @@ class FrogPilotPlanner: frogpilotPlan = frogpilot_plan_send.frogpilotPlan frogpilotPlan.jerk = float(self.jerk) + frogpilotPlan.laneWidthLeft = self.lane_width_left + frogpilotPlan.laneWidthRight = self.lane_width_right frogpilotPlan.minAcceleration = self.min_accel frogpilotPlan.maxAcceleration = self.max_accel frogpilotPlan.tFollow = float(self.t_follow) @@ -137,6 +148,7 @@ class FrogPilotPlanner: custom_alerts = self.params.get_bool("CustomAlerts") custom_ui = self.params.get_bool("CustomUI") + self.blind_spot_path = custom_ui and self.params.get_bool("BlindSpotPath") longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune") self.acceleration_profile = self.params.get_int("AccelerationProfile") if longitudinal_tune else 0 diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc index 9ac224e41..71a954931 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc @@ -62,8 +62,8 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot }); toggle = customUIToggle; } else if (param == "CustomPaths") { - std::vector pathToggles{"AccelerationPath"}; - std::vector pathToggleNames{tr("Acceleration")}; + std::vector pathToggles{"AccelerationPath", "BlindSpotPath"}; + std::vector pathToggleNames{tr("Acceleration"), tr("Blind Spot")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, pathToggles, pathToggleNames); } else if (param == "QOLVisuals") { diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index c39ec2da3..c3d66b45a 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 936ae859e..f5ddf5ec5 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -538,6 +538,22 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.setBrush(bg); painter.drawPolygon(scene.track_vertices); + // Paint blindspot path + if (scene.blind_spot_path) { + QLinearGradient bs(0, height(), 0, 0); + bs.setColorAt(0.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6)); + bs.setColorAt(0.5, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.4)); + bs.setColorAt(1.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.2)); + + painter.setBrush(bs); + if (blindSpotLeft) { + painter.drawPolygon(scene.track_adjacent_vertices[4]); + } + if (blindSpotRight) { + painter.drawPolygon(scene.track_adjacent_vertices[5]); + } + } + painter.restore(); } @@ -737,6 +753,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; + experimentalMode = scene.experimental_mode; mapOpen = scene.map_open; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index f7fee7d3e..eef6dcf6f 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -119,6 +119,8 @@ private: QHBoxLayout *bottom_layout; bool alwaysOnLateralActive; + bool blindSpotLeft; + bool blindSpotRight; bool experimentalMode; bool mapOpen; bool showAlwaysOnLateralStatusBar; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4b00d75b5..9bc9e857c 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -115,6 +115,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) { @@ -207,6 +212,8 @@ static void update_state(UIState *s) { if (sm.updated("carState")) { auto carState = sm["carState"].getCarState(); scene.acceleration = carState.getAEgo(); + scene.blind_spot_left = carState.getLeftBlindspot(); + scene.blind_spot_right = carState.getRightBlindspot(); } if (sm.updated("controlsState")) { auto controlsState = sm["controlsState"].getControlsState(); @@ -223,6 +230,8 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); + scene.lane_width_left = frogpilotPlan.getLaneWidthLeft(); + scene.lane_width_right = frogpilotPlan.getLaneWidthRight(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -257,6 +266,7 @@ 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.blind_spot_path = custom_paths && params.getBool("BlindSpotPath"); bool quality_of_life_controls = params.getBool("QOLControls"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index c193612dc..72acd0f47 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -173,6 +173,9 @@ typedef struct UIScene { // FrogPilot variables bool acceleration_path; bool always_on_lateral_active; + bool blind_spot_left; + bool blind_spot_path; + bool blind_spot_right; bool enabled; bool experimental_mode; bool map_open; @@ -180,9 +183,13 @@ typedef struct UIScene { bool show_aol_status_bar; float acceleration; + float lane_width_left; + float lane_width_right; int alert_size; + QPolygonF track_adjacent_vertices[6]; + } UIScene; class UIState : public QObject {