New KL model + laneless toggle (#20454)

* New model: 0bf38240-6b55-42b0-bcfb-da08ff5cd0b9/650

* laneless baby

* hardcode model difference for now

* kale toggle

* kale emoji

* sensible description

* make clear it is not just for ui

* emojis dont work yet

* 9671a8fe-539f-4a04-a163-54571df21139/650

* new model

* only read param on rising edge of ignition

* update toggle text

* fix that

* update refs

Co-authored-by: mitchell <mitchellgoffpc@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

common/params_pyx.pyx

@@ -22,6 +22,7 @@ keys = {
   b"CarParamsCache": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
   b"CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
   b"CommunityFeaturesToggle": [TxType.PERSISTENT],
+  b"EndToEndToggle": [TxType.PERSISTENT],
   b"CompletedTrainingVersion": [TxType.PERSISTENT],
   b"DisablePowerDown": [TxType.PERSISTENT],
   b"DisableUpdates": [TxType.PERSISTENT],

selfdrive/controls/lib/lane_planner.py

@@ -8,10 +8,13 @@ TRAJECTORY_SIZE = 33
 # camera offset is meters from center car to camera
 if EON:
   CAMERA_OFFSET = 0.06
+  PATH_OFFSET = 0.0
 elif TICI:
   CAMERA_OFFSET = -0.04
+  PATH_OFFSET = -0.04
 else:
   CAMERA_OFFSET = 0.0
+  PATH_OFFSET = 0.0
 
 
 
@@ -56,6 +59,7 @@ class LanePlanner:
   def get_d_path(self, v_ego, path_t, path_xyz):
     # Reduce reliance on lanelines that are too far apart or
     # will be in a few seconds
+    path_xyz[:,1] -= PATH_OFFSET
     l_prob, r_prob = self.lll_prob, self.rll_prob
     width_pts = self.rll_y - self.lll_y
     prob_mods = []

selfdrive/controls/lib/lateral_planner.py

@@ -1,6 +1,7 @@
 import os
 import math
 import numpy as np
+from common.params import Params
 from common.realtime import sec_since_boot, DT_MDL
 from common.numpy_fast import interp, clip
 from selfdrive.swaglog import cloudlog
@@ -53,6 +54,7 @@ class LateralPlanner():
 
     self.setup_mpc()
     self.solution_invalid_cnt = 0
+    self.use_lanelines = Params().get('EndToEndToggle') != b'1'
     self.lane_change_state = LaneChangeState.off
     self.lane_change_direction = LaneChangeDirection.none
     self.lane_change_timer = 0.0
@@ -158,7 +160,10 @@ class LateralPlanner():
     if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
       self.LP.lll_prob *= self.lane_change_ll_prob
       self.LP.rll_prob *= self.lane_change_ll_prob
-    d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
+    if self.use_lanelines:
+      d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
+    else:
+      d_path_xyz = self.path_xyz
     y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
     heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
     self.y_pts = y_pts

selfdrive/manager/manager.py

@@ -30,6 +30,7 @@ def manager_init(spinner=None):
 
   default_params = [
     ("CommunityFeaturesToggle", "0"),
+    ("EndToEndToggle", "0"),
     ("CompletedTrainingVersion", "0"),
     ("IsRHD", "0"),
     ("IsMetric", "0"),

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-1ed77847fe3f53b14595bab8f1f47134fee03c9b
+6c2409d2b1a93b675e4cd4ae7e67fc56ec3824dc

selfdrive/ui/paint.cc

@@ -140,21 +140,26 @@ static void draw_frame(UIState *s) {
 
 static void ui_draw_vision_lane_lines(UIState *s) {
   const UIScene &scene = s->scene;
-  // paint lanelines
+  NVGpaint track_bg;
-  for (int i = 0; i < std::size(scene.lane_line_vertices); i++) {
+  if (!scene.end_to_end) {
-    NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene.lane_line_probs[i]);
+    // paint lanelines
-    ui_draw_line(s, scene.lane_line_vertices[i], &color, nullptr);
+    for (int i = 0; i < std::size(scene.lane_line_vertices); i++) {
-  }
+      NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene.lane_line_probs[i]);
+      ui_draw_line(s, scene.lane_line_vertices[i], &color, nullptr);
+    }
 
-  // paint road edges
+    // paint road edges
-  for (int i = 0; i < std::size(scene.road_edge_vertices); i++) {
+    for (int i = 0; i < std::size(scene.road_edge_vertices); i++) {
-    NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp<float>(1.0 - scene.road_edge_stds[i], 0.0, 1.0));
+      NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp<float>(1.0 - scene.road_edge_stds[i], 0.0, 1.0));
-    ui_draw_line(s, scene.road_edge_vertices[i], &color, nullptr);
+      ui_draw_line(s, scene.road_edge_vertices[i], &color, nullptr);
+    }
+    track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
+                                          COLOR_WHITE, COLOR_WHITE_ALPHA(0));
+  } else {
+    track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
+                                          COLOR_RED, COLOR_RED_ALPHA(0));
   }
-
   // paint path
-  NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
-                                        COLOR_WHITE, COLOR_WHITE_ALPHA(0));
   ui_draw_line(s, scene.track_vertices, nullptr, &track_bg);
 }
 

selfdrive/ui/qt/offroad/settings.cc

@@ -50,13 +50,17 @@ QWidget * toggles_panel() {
                                             "Use features from the open source community that are not maintained or supported by comma.ai and have not been confirmed to meet the standard safety model. These features include community supported cars and community supported hardware. Be extra cautious when using these features",
                                             "../assets/offroad/icon_shell.png"
                                             ));
-
+  toggles_list->addWidget(horizontal_line());
   ParamControl *record_toggle = new ParamControl("RecordFront",
                                             "Record and Upload Driver Camera",
                                             "Upload data from the driver facing camera and help improve the driver monitoring algorithm.",
                                             "../assets/offroad/icon_network.png");
-  toggles_list->addWidget(horizontal_line());
   toggles_list->addWidget(record_toggle);
+  toggles_list->addWidget(horizontal_line());
+  toggles_list->addWidget(new ParamControl("EndToEndToggle",
+                                           "Ignore lanelines (Experimental)",
+                                           "In this mode openpilot will ignore lanelines and just drive how it thinks a human would.",
+                                           "../assets/offroad/icon_road.png"));
 
   bool record_lock = Params().read_db_bool("RecordFrontLock");
   record_toggle->setEnabled(!record_lock);

selfdrive/ui/ui.cc

@@ -340,6 +340,7 @@ static void update_status(UIState *s) {
       s->scene.started_frame = s->sm->frame;
 
       read_param(&s->scene.is_rhd, "IsRHD");
+      read_param(&s->scene.end_to_end, "EndToEndToggle");
       s->active_app = cereal::UiLayoutState::App::NONE;
       s->sidebar_collapsed = true;
       s->scene.alert_size = cereal::ControlsState::AlertSize::NONE;

selfdrive/ui/ui.hpp

@@ -33,6 +33,7 @@
 #define COLOR_BLACK_ALPHA(x) nvgRGBA(0, 0, 0, x)
 #define COLOR_WHITE nvgRGBA(255, 255, 255, 255)
 #define COLOR_WHITE_ALPHA(x) nvgRGBA(255, 255, 255, x)
+#define COLOR_RED_ALPHA(x) nvgRGBA(201, 34, 49, x)
 #define COLOR_YELLOW nvgRGBA(218, 202, 37, 255)
 #define COLOR_RED nvgRGBA(201, 34, 49, 255)
 
@@ -132,7 +133,7 @@ typedef struct UIScene {
   vertex_data lead_vertices[2];
 
   float light_sensor, accel_sensor, gyro_sensor;
-  bool started, ignition, is_metric, longitudinal_control;
+  bool started, ignition, is_metric, longitudinal_control, end_to_end;
   uint64_t started_frame;
 } UIScene;