zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Visuals - Custom Onroad UI - Paths - Adjacent

Browse Source 
Show the detected adjacent lanes.
This commit is contained in:
FrogAi 2024-05-22 00:05:50 -07:00
parent 358d37f8ee
commit 532048a2e9
6 changed files with 96 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
selfdrive/frogpilot/controls/frogpilot_planner.py
View File

@ -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

27
selfdrive/modeld/fill_model_msg.py
View File

@ -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()

41
selfdrive/ui/qt/onroad.cc
View File

@ -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;

5
selfdrive/ui/qt/onroad.h
View File

@ -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;

14
selfdrive/ui/ui.cc
View File

@ -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;

9
selfdrive/ui/ui.h
View File

@ -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 {