diff --git a/selfdrive/frogpilot/assets/other_images/stop_sign.png b/selfdrive/frogpilot/assets/other_images/stop_sign.png new file mode 100644 index 000000000..159873301 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/stop_sign.png differ diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index f2c5e07cf..20450ce9e 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -94,7 +94,7 @@ class FrogPilotPlanner: lead_distance = self.lead_one.dRel - distance_offset stopping_distance = STOP_DISTANCE + distance_offset - run_cem = frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.force_stops + run_cem = frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.force_stops or frogpilot_toggles.show_stopping_point if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateral) and driving_gear: self.cem.update(carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles) @@ -331,6 +331,7 @@ class FrogPilotPlanner: frogpilotPlan.forcingStop = self.forcing_stop frogpilotPlan.greenLight = not self.model_stopped + frogpilotPlan.redLight = self.cem.stop_light_detected frogpilotPlan.laneWidthLeft = self.lane_width_left frogpilotPlan.laneWidthRight = self.lane_width_right diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 06fef937d..fd015b1cd 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -269,7 +269,7 @@ void AnnotatedCameraWidget::updateFrameMat() { .translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); } -void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { +void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s, const float v_ego) { painter.save(); const UIScene &scene = s->scene; @@ -326,6 +326,27 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.setBrush(bg); painter.drawPolygon(scene.track_vertices); + if (scene.show_stopping_point && scene.red_light && v_ego > 1 && !(conditionalStatus == 1 || conditionalStatus == 3 || conditionalStatus == 5)) { + QPointF last_point = scene.track_vertices.last(); + + QPointF adjusted_point = last_point - QPointF(stopSignImg.width() / 2, stopSignImg.height()); + painter.drawPixmap(adjusted_point, stopSignImg); + + if (scene.show_stopping_point_metrics) { + QString text = QString::number(scene.model_length * distanceConversion) + leadDistanceUnit; + QFont font = InterFont(35, QFont::DemiBold); + QFontMetrics fm(font); + int text_width = fm.horizontalAdvance(text); + QPointF text_position = last_point - QPointF(text_width / 2, stopSignImg.height() + 35); + + painter.save(); + painter.setFont(font); + painter.setPen(Qt::white); + painter.drawText(text_position, text); + painter.restore(); + } + } + // Paint blindspot path if (scene.blind_spot_path) { QLinearGradient bs(0, height(), 0, 0); @@ -519,7 +540,7 @@ void AnnotatedCameraWidget::paintGL() { if (s->scene.world_objects_visible) { update_model(s, model, sm["uiPlan"].getUiPlan()); - drawLaneLines(painter, s); + drawLaneLines(painter, s, v_ego); if (s->scene.longitudinal_control && sm.rcv_frame("modelV2") > s->scene.started_frame) { update_leads(s, model); @@ -585,6 +606,8 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { bottom_layout->addWidget(map_settings_btn_bottom, 0, Qt::AlignBottom | Qt::AlignRight); main_layout->addLayout(bottom_layout); + + stopSignImg = loadPixmap("../frogpilot/assets/other_images/stop_sign.png", QSize(img_size, img_size)); } void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UIScene &scene) { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 685e5b4cf..fe7c32131 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -137,6 +137,8 @@ private: int conditionalSpeedLead; int conditionalStatus; + QPixmap stopSignImg; + QString accelerationUnit; QString leadDistanceUnit; QString leadSpeedUnit; @@ -149,7 +151,7 @@ protected: void initializeGL() override; void showEvent(QShowEvent *event) override; void updateFrameMat() override; - void drawLaneLines(QPainter &painter, const UIState *s); + void drawLaneLines(QPainter &painter, const UIState *s, const float v_ego); void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd, const float v_ego); void drawHud(QPainter &p); void drawDriverState(QPainter &painter, const UIState *s); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 91f158e8f..0658a70dd 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -84,6 +84,7 @@ void update_model(UIState *s, const cereal::UiPlan::Reader &plan) { UIScene &scene = s->scene; auto plan_position = plan.getPosition(); + scene.model_length = model.getPosition().getX()[33 - 1]; if (plan_position.getX().size() < model.getPosition().getX().size()) { plan_position = model.getPosition(); } @@ -251,6 +252,7 @@ static void update_state(UIState *s) { scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); scene.lane_width_left = frogpilotPlan.getLaneWidthLeft(); scene.lane_width_right = frogpilotPlan.getLaneWidthRight(); + scene.red_light = frogpilotPlan.getRedLight(); scene.speed_limit = frogpilotPlan.getSlcSpeedLimit(); scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset(); scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden(); @@ -323,6 +325,8 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { scene.static_pedals_on_ui = scene.pedals_on_ui && params.getBool("StaticPedalsOnUI"); scene.road_name_ui = custom_onroad_ui && params.getBool("RoadNameUI"); scene.rotating_wheel = custom_onroad_ui && params.getBool("RotatingWheel"); + scene.show_stopping_point = custom_onroad_ui && params.getBool("ShowStoppingPoint"); + scene.show_stopping_point_metrics = scene.show_stopping_point && params.getBool("ShowStoppingPointMetrics"); scene.wheel_icon = custom_onroad_ui ? params.getInt("WheelIcon") : 0; scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 7a45e53c8..47330870e 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -145,6 +145,7 @@ typedef struct UIScene { bool onroad_distance_button; bool parked; bool pedals_on_ui; + bool red_light; bool reverse; bool reverse_cruise; bool reverse_cruise_ui; @@ -155,6 +156,8 @@ typedef struct UIScene { bool show_cem_status_bar; bool show_slc_offset; bool show_slc_offset_ui; + bool show_stopping_point; + bool show_stopping_point_metrics; bool speed_limit_changed; bool speed_limit_controller; bool speed_limit_overridden; @@ -182,6 +185,7 @@ typedef struct UIScene { int conditional_speed; int conditional_speed_lead; int conditional_status; + int model_length; int steering_angle_deg; int tethering_config; int wheel_icon;