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 0000000000..159873301a 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 6b892a546c..cbd491f489 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -313,6 +313,8 @@ class FrogPilotPlanner: frogpilotPlan.maxAcceleration = self.max_accel frogpilotPlan.minAcceleration = self.min_accel + frogpilotPlan.roadCurvature = self.road_curvature + frogpilotPlan.slcOverridden = bool(self.override_slc) frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed) frogpilotPlan.slcSpeedLimit = self.slc_target diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 5c765f5ee7..04c1312e89 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; @@ -325,6 +325,31 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.setBrush(bg); painter.drawPolygon(scene.track_vertices); + if (scene.show_stopping_point) { + bool curve_detected = sqrt(1.0 / scene.road_curvature) < v_ego; + bool following_lead = scene.has_lead && (scene.lead_distance < fmax(scene.model_length, 25)); + bool model_stopping = scene.model_length < v_ego * (10 - 3); + + if (model_stopping && !curve_detected && !following_lead) { + QPointF last_point = scene.track_vertices.last(); + + QPointF adjusted_point = last_point - QPointF(stopSignImg.width() / 2, stopSignImg.height()); + painter.drawPixmap(adjusted_point, stopSignImg); + + 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); @@ -514,7 +539,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); @@ -581,6 +606,8 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { bottom_layout->addWidget(map_settings_btn_bottom); 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 685e5b4cf3..fe7c321318 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 5992753fc0..0b01fe90d8 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -50,6 +50,7 @@ void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model_data) { const auto &lead = model_data.getLeadsV3()[i]; if (s->scene.has_lead) { float d_rel = lead.getX()[0]; + s->scene.lead_distance = d_rel; float y_rel = lead.getY()[0]; float z = line.getZ()[get_path_length_idx(line, d_rel)]; calib_frame_to_full_frame(s, d_rel, y_rel, z + 1.22, &s->scene.lead_vertices[i]); @@ -84,6 +85,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 +253,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.road_curvature = frogpilotPlan.getRoadCurvature(); scene.speed_limit = frogpilotPlan.getSlcSpeedLimit(); scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset(); scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden(); @@ -322,6 +325,7 @@ void ui_update_frogpilot_params(UIState *s) { 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.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 035e7d9b3d..db84c6291e 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -153,6 +153,7 @@ typedef struct UIScene { bool show_cem_status_bar; bool show_slc_offset; bool show_slc_offset_ui; + bool show_stopping_point; bool speed_limit_changed; bool speed_limit_controller; bool speed_limit_overridden; @@ -170,6 +171,7 @@ typedef struct UIScene { float lane_width_left; float lane_width_right; float lead_detection_threshold; + float road_curvature; float speed_limit; float speed_limit_offset; float speed_limit_overridden_speed; @@ -180,6 +182,8 @@ typedef struct UIScene { int conditional_speed; int conditional_speed_lead; int conditional_status; + int lead_distance; + int model_length; int steering_angle_deg; int tethering_config; int wheel_icon;