diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 28c0308..27ff215 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -19,6 +19,8 @@ struct FrogPilotNavigation @0xf35cc4560bbf6ec2 { } struct FrogPilotPlan @0xda96579883444c35 { + laneWidthLeft @3 :Float32; + laneWidthRight @4 :Float32; } struct CustomReserved4 @0x80ae746ee2596b11 { diff --git a/common/params.cc b/common/params.cc index c37f989..3fe344c 100644 --- a/common/params.cc +++ b/common/params.cc @@ -216,6 +216,7 @@ std::unordered_map keys = { {"AlwaysOnLateral", PERSISTENT}, {"AlwaysOnLateralMain", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, + {"BlindSpotPath", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomUI", PERSISTENT}, {"DecelerationProfile", PERSISTENT}, diff --git a/selfdrive/frogpilot/functions/frogpilot_functions.py b/selfdrive/frogpilot/functions/frogpilot_functions.py index e8df939..3f45dff 100644 --- a/selfdrive/frogpilot/functions/frogpilot_functions.py +++ b/selfdrive/frogpilot/functions/frogpilot_functions.py @@ -43,3 +43,17 @@ class FrogPilotFunctions: @staticmethod def get_max_accel_sport(v_ego): return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT) + + @staticmethod + def calculate_lane_width(lane, current_lane, road_edge): + lane_x, lane_y = np.array(lane.x), np.array(lane.y) + edge_x, edge_y = np.array(road_edge.x), np.array(road_edge.y) + current_x, current_y = np.array(current_lane.x), np.array(current_lane.y) + + lane_y_interp = np.interp(current_x, lane_x[lane_x.argsort()], lane_y[lane_x.argsort()]) + road_edge_y_interp = np.interp(current_x, edge_x[edge_x.argsort()], edge_y[edge_x.argsort()]) + + distance_to_lane = np.mean(np.abs(current_y - lane_y_interp)) + distance_to_road_edge = np.mean(np.abs(current_y - road_edge_y_interp)) + + return min(distance_to_lane, distance_to_road_edge) diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 5bf0a37..ef13bcb 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -2,6 +2,7 @@ import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV 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.longitudinal_planner import A_CRUISE_MIN, get_max_accel from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions @@ -44,6 +45,15 @@ class FrogPilotPlanner: self.accel_limits = [min_accel, max_accel] + # Update the current lane widths + check_lane_width = 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])) + else: + self.lane_width_left = 0 + self.lane_width_right = 0 + # Update the max allowed speed self.v_cruise = self.update_v_cruise(carState, controlsState, enabled, modelData, v_cruise, v_ego) @@ -65,12 +75,16 @@ class FrogPilotPlanner: frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan + frogpilotPlan.laneWidthLeft = self.lane_width_left + frogpilotPlan.laneWidthRight = self.lane_width_right + pm.send('frogpilotPlan', frogpilot_plan_send) def update_frogpilot_params(self, params): self.is_metric = params.get_bool("IsMetric") custom_ui = params.get_bool("CustomUI") + self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath") longitudinal_tune = params.get_bool("LongitudinalTune") self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0 diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index 8ba0fe5..6ef5ecf 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -15,6 +15,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.", ""}, + {"BlindSpotPath", "Blind Spot Path", "Visualize your blind spots with a red path when another vehicle is detected nearby.", ""}, {"QOLVisuals", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"}, {"DriveStats", "Drive Stats In Home Screen", "Display your device's drive stats in the home screen.", ""}, diff --git a/selfdrive/frogpilot/ui/visual_settings.h b/selfdrive/frogpilot/ui/visual_settings.h index 389b816..36e7bba 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"}; + std::set customOnroadUIKeys = {"AccelerationPath", "BlindSpotPath"}; std::set customThemeKeys = {}; std::set modelUIKeys = {}; std::set qolKeys = {"DriveStats"}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 4354a45..99e28d5 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -537,6 +537,24 @@ 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); + if (blindSpotLeft || blindSpotRight) { + 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 +755,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { alwaysOnLateral = scene.always_on_lateral; alwaysOnLateralActive = scene.always_on_lateral_active; + blindSpotLeft = scene.blind_spot_left; + blindSpotRight = scene.blind_spot_right; + experimentalMode = scene.experimental_mode; if (alwaysOnLateral) { diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 9e5809b..feba8ba 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -120,6 +120,8 @@ private: bool alwaysOnLateral; bool alwaysOnLateralActive; + bool blindSpotLeft; + bool blindSpotRight; bool experimentalMode; protected: diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 23d01bf..7ca7958 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -114,6 +114,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], scene.blind_spot_path ? (i == 4 ? scene.lane_width_left : scene.lane_width_right) / 2 : 0, 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) { @@ -202,6 +207,10 @@ static void update_state(UIState *s) { } if (sm.updated("carState")) { auto carState = sm["carState"].getCarState(); + if (scene.blind_spot_path) { + scene.blind_spot_left = carState.getLeftBlindspot(); + scene.blind_spot_right = carState.getRightBlindspot(); + } } if (sm.updated("controlsState")) { auto controlsState = sm["controlsState"].getControlsState(); @@ -216,6 +225,10 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); + if (scene.blind_spot_path) { + scene.lane_width_left = frogpilotPlan.getLaneWidthLeft(); + scene.lane_width_right = frogpilotPlan.getLaneWidthRight(); + } } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -250,6 +263,7 @@ void ui_update_frogpilot_params(UIState *s) { bool custom_onroad_ui = params.getBool("CustomUI"); scene.acceleration_path = custom_onroad_ui && params.getBool("AccelerationPath"); + scene.blind_spot_path = custom_onroad_ui && params.getBool("BlindSpotPath"); bool quality_of_life_controls = params.getBool("QOLControls"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 9f603a8..b5610d0 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -172,9 +172,17 @@ typedef struct UIScene { bool acceleration_path; bool always_on_lateral; bool always_on_lateral_active; + bool blind_spot_left; + bool blind_spot_path; + bool blind_spot_right; bool enabled; bool experimental_mode; + float lane_width_left; + float lane_width_right; + + QPolygonF track_adjacent_vertices[6]; + } UIScene; class UIState : public QObject {