08 e2e (#94)

* e2e

Co-Authored-By: Shane Smiskol <shane@smiskol.com>

* enable alpha e2e long with 0.8. doesn't work very well yet

* add modelv2

Co-Authored-By: Shane Smiskol <shane@smiskol.com>

* spelling

* forgot

* fix

* dfix

* syntax

* fix

* how about this

* gix

* fix

* fix

* how about this.

* fix

* hmm

* revert

* fix

Co-authored-by: Shane Smiskol <shane@smiskol.com>

cereal/log.capnp

@@ -2066,6 +2066,10 @@ struct Sentinel {
   type @0 :SentinelType;
 }
 
+struct ModelLongButton {
+  enabled @0 :Bool;
+}
+
 struct Event {
   # in nanoseconds?
   logMonoTime @0 :UInt64;
@@ -2150,6 +2154,7 @@ struct Event {
     wideEncodeIdx @77 :EncodeIndex;
     dragonConf @78 :DragonConf;
     liveTrafficData @79:LiveTrafficData;
+    modelLongButton @80 :ModelLongButton;
   }
 }
 

cereal/service_list.yaml

@@ -83,6 +83,8 @@ modelV2: [8077, true, 20., 20]
 
 dragonConf: [8088, false, 2.]
 
+modelLongButton: [8084, false, 0.]
+
 testModel: [8040, false, 0.]
 testLiveLocation: [8045, false, 0.]
 testJoystick: [8056, false, 0.]

selfdrive/controls/lib/planner.py

@@ -17,6 +17,7 @@
 from selfdrive.controls.lib.fcw import FCWChecker
 from selfdrive.controls.lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from selfdrive.controls.lib.long_mpc_model import LongitudinalMpcModel
 from common.op_params import opParams
 op_params = opParams()
 osm = op_params.get('osm')
@@ -117,6 +118,7 @@ def __init__(self, CP):
 
     self.mpc1 = LongitudinalMpc(1)
     self.mpc2 = LongitudinalMpc(2)
+    self.mpc_model = LongitudinalMpcModel()
 
     self.v_acc_start = 0.0
     self.a_acc_start = 0.0
@@ -126,6 +128,8 @@ def __init__(self, CP):
     self.a_acc = 0.0
     self.v_cruise = 0.0
     self.a_cruise = 0.0
+    self.v_model = 0.0
+    self.a_model = 0.0
     self.osm = True
 
     self.longitudinalPlanSource = 'cruise'
@@ -144,7 +148,8 @@ def __init__(self, CP):
     self.v_model = 0.0
     self.a_model = 0.0
 
-  def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle):
+  def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle, model_enabled):
+    #possible_futures = [self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]
     center_x = -2.5 # Wheel base 2.5m
     lead1_check = True
     lead2_check = True
@@ -166,6 +171,9 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn
         solutions['mpc1'] = self.mpc1.v_mpc
       if self.mpc2.prev_lead_status and lead2_check:
         solutions['mpc2'] = self.mpc2.v_mpc
+      if self.mpc_model.valid and model_enabled:
+        solutions['model'] = self.mpc_model.v_mpc
+      solutions['cruise'] = self.v_cruise
 
       slowest = min(solutions, key=solutions.get)
 
@@ -180,16 +188,39 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn
       elif slowest == 'cruise':
         self.v_acc = self.v_cruise
         self.a_acc = self.a_cruise
+      elif slowest == 'model':
+        self.v_acc = self.mpc_model.v_mpc
+        self.a_acc = self.mpc_model.a_mpc
       # dp - slow on curve from 0.7.6.1
       elif self.dp_slow_on_curve and slowest == 'model':
         self.v_acc = self.v_model
         self.a_acc = self.a_model
 
     self.v_acc_future = v_cruise_setpoint
     if lead1_check:
-      self.v_acc_future = min([self.mpc1.v_mpc_future, self.v_acc_future])
+      self.v_acc_future = min([self.mpc1.v_mpc_future, self.v_acc_future, self.mpc_model.v_mpc_future])
     if lead2_check:
-      self.v_acc_future = min([self.mpc2.v_mpc_future, self.v_acc_future])
+      self.v_acc_future = min([self.mpc2.v_mpc_future, self.v_acc_future, self.mpc_model.v_mpc_future ])
+
+  def parse_modelV2_data(self, sm):
+    modelV2 = sm['modelV2']
+    distances, speeds, accelerations = [], [], []
+    if not sm.updated['modelV2'] or len(modelV2.position.x) == 0:
+      return distances, speeds, accelerations
+
+    model_t = modelV2.position.t
+    mpc_times = list(range(10))
+
+    model_t_idx = [sorted(range(len(model_t)), key=[abs(idx - t) for t in model_t].__getitem__)[0] for idx in mpc_times]  # matches 0 to 9 interval to idx from t
+
+    for t in model_t_idx:  # everything is derived from x position since velocity is outputting weird values
+      speeds.append(modelV2.velocity.x[t])
+      distances.append(modelV2.position.x[t])
+      if model_t_idx.index(t) > 0:  # skip first since we can't calculate (and don't want to use v_ego)
+        accelerations.append((speeds[-1] - speeds[-2]) / model_t[t])
+
+    accelerations.insert(0, accelerations[1] - (accelerations[2] - accelerations[1]))  # extrapolate back first accel from second and third, less weight
+    return distances, speeds, accelerations
 
   def update(self, sm, pm, CP, VM, PP):
     """Gets called when new radarState is available"""
@@ -360,11 +391,18 @@ def update(self, sm, pm, CP, VM, PP):
 
     self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
     self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
+    self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
 
     self.mpc1.update(pm, sm['carState'], lead_1)
     self.mpc2.update(pm, sm['carState'], lead_2)
 
-    self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle)
+    distances, speeds, accelerations = self.parse_modelV2_data(sm)
+    self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo,
+                          distances,
+                          speeds,
+                          accelerations)
+
+    self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle, sm['modelLongButton'].enabled)
 
     # determine fcw
     if self.mpc1.new_lead:

selfdrive/controls/plannerd.py

@@ -23,7 +23,7 @@ def plannerd_thread(sm=None, pm=None):
   VM = VehicleModel(CP)
 
   if sm is None:
-    sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData'],
+    sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData', 'modelLongButton', 'modelV2'],
                              poll=['radarState', 'model'])
 
   if pm is None:

selfdrive/ui/android/ui.cc

@@ -2,6 +2,7 @@
 #include <stdlib.h>
 #include <math.h>
 #include <sys/resource.h>
+#include <iostream>
 
 #include <algorithm>
 
@@ -29,6 +30,29 @@ static void ui_set_brightness(UIState *s, int brightness) {
   }
 }
 
+static void send_ml(UIState *s, bool enabled) {
+  MessageBuilder msg;
+  auto mlStatus = msg.initEvent().initModelLongButton();
+  mlStatus.setEnabled(enabled);
+  s->pm->send("modelLongButton", msg);
+}
+
+static bool handle_ml_touch(UIState *s, int touch_x, int touch_y) {
+  //mlButton manager
+  int padding = 40;
+  int btn_w = 500;
+  int btn_h = 138;
+  int xs[2] = {1920 / 2 - btn_w / 2, 1920 / 2 + btn_w / 2};
+  int y_top = 915 - btn_h / 2;
+  if (xs[0] <= touch_x + padding && touch_x - padding <= xs[1] && y_top - padding <= touch_y) {
+    s->scene.mlButtonEnabled = !s->scene.mlButtonEnabled;
+    send_ml(s, s->scene.mlButtonEnabled);
+    printf("ml button: %d\n", s->scene.mlButtonEnabled);
+    return true;
+  }
+  return false;
+}
+
 static void handle_display_state(UIState *s, bool user_input) {
 
   static int awake_timeout = 0;
@@ -103,6 +127,10 @@ static bool handle_dp_btn_touch(UIState *s, int touch_x, int touch_y) {
       Params().write_db_value("dp_last_modified", time_str, 11);
       return true;
     }
+    if (handle_ml_touch(s, touch_x, touch_y)) {
+      s->scene.uilayout_sidebarcollapsed = true;  // collapse sidebar when tapping any SA button
+      return true;  // only allow one button to be pressed at a time
+    }
   }
   return false;
 }
@@ -161,6 +189,7 @@ int main(int argc, char* argv[]) {
   UIState uistate = {};
   UIState *s = &uistate;
   ui_init(s);
+  sa_init(s, true);
   s->sound = &sound;
 
   TouchState touch = {0};
@@ -196,6 +225,12 @@ int main(int argc, char* argv[]) {
     if (!s->started) {
       usleep(50 * 1000);
     }
+
+    if (s->started) {
+      sa_init(s, false);  // reset ml button and regrab params
+    }
+
+
     double u1 = millis_since_boot();
 
     ui_update(s);

selfdrive/ui/paint.cc

@@ -548,6 +548,34 @@ static void ui_draw_driver_view(UIState *s) {
   ui_draw_circle_image(s->vg, x, y, face_size, s->img_face, scene->dmonitoring_state.getFaceDetected());
 }
 
+static void ui_draw_ml_button(UIState *s) {
+  int btn_w = 500;
+  int btn_h = 138;
+  int x = 1920 / 2;
+  int y = 915;
+  int btn_x = x - btn_w / 2;
+  int btn_y = y - btn_h / 2;
+
+  nvgBeginPath(s->vg);
+  nvgRoundedRect(s->vg, btn_x, btn_y, btn_w, btn_h, 25);
+  if (s->scene.mlButtonEnabled) {  // change outline color based on status of button
+    nvgStrokeColor(s->vg, nvgRGBA(55, 184, 104, 255));
+  } else {
+    nvgStrokeColor(s->vg, nvgRGBA(184, 55, 55, 255));
+  }
+  nvgStrokeWidth(s->vg, 12);
+  nvgStroke(s->vg);
+
+  nvgBeginPath(s->vg);  // dark background for readability
+  nvgRoundedRect(s->vg, btn_x, btn_y, btn_w, btn_h, 25);
+  nvgFillColor(s->vg, nvgRGBA(75, 75, 75, 75));
+  nvgFill(s->vg);
+
+  nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
+  nvgFontSize(s->vg, 65);
+  nvgText(s->vg, x, y + btn_h / 8, "Toggle Model Long", NULL);
+}
+
 static void ui_draw_vision_header(UIState *s) {
   const Rect &viz_rect = s->scene.viz_rect;
   if (!s->scene.dpFullScreenApp) {
@@ -567,6 +595,7 @@ static void ui_draw_vision_header(UIState *s) {
   if (s->scene.dpUiEvent) {
   ui_draw_vision_event(s);
   }
+  ui_draw_ml_button(s);
 }
 
 static void ui_draw_vision_footer(UIState *s) {

selfdrive/ui/ui.cc

@@ -8,6 +8,8 @@
 #include <math.h>
 #include <poll.h>
 #include <sys/mman.h>
+//#include "json11.hpp"
+//#include <fstream>
 
 #include "common/util.h"
 #include "common/swaglog.h"
@@ -24,6 +26,34 @@ int write_param_float(float param, const char* param_name, bool persistent_param
   return Params(persistent_param).write_db_value(param_name, s, size < sizeof(s) ? size : sizeof(s));
 }
 
+void sa_init(UIState *s, bool full_init) {
+  if (full_init) {
+    s->pm = new PubMaster({"modelLongButton"});
+  }
+
+  //s->ui_debug = false;  // change to true while debugging
+
+  // stock additions todo: run opparams first (in main()?) to ensure json values exist
+  //std::ifstream op_params_file("/data/op_params.json");
+  //std::string op_params_content((std::istreambuf_iterator<char>(op_params_file)),
+                                //(std::istreambuf_iterator<char>()));
+
+  //std::string err;
+  //auto json = json11::Json::parse(op_params_content, err);
+  //if (!json.is_null() && err.empty()) {
+    //printf("successfully parsed opParams json\n");
+    //s->scene.dfButtonStatus = DF_TO_IDX[json["dynamic_follow"].string_value()];
+    //s->scene.lsButtonStatus = LS_TO_IDX[json["lane_speed_alerts"].string_value()];
+//    printf("dfButtonStatus: %d\n", s->scene.dfButtonStatus);
+//    printf("lsButtonStatus: %d\n", s->scene.lsButtonStatus);
+  //} else {  // error parsing json
+    //printf("ERROR PARSING OPPARAMS JSON!\n");
+    //s->scene.dfButtonStatus = 0;
+    //s->scene.lsButtonStatus = 0;
+  //}
+  s->scene.mlButtonEnabled = false;  // state isn't saved yet
+}
+
 void ui_init(UIState *s) {
   s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "liveMapData",
                          "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents",

selfdrive/ui/ui.hpp

@@ -107,6 +107,8 @@ typedef struct {
 
 typedef struct UIScene {
 
+  bool mlButtonEnabled;
+
   mat4 extrinsic_matrix;      // Last row is 0 so we can use mat4.
   bool world_objects_visible;
 
@@ -232,7 +234,7 @@ typedef struct UIState {
   int img_speed;
 
   SubMaster *sm;
-
+  PubMaster *pm;
   Sound *sound;
   UIStatus status;
   UIScene scene;
@@ -278,6 +280,7 @@ typedef struct UIState {
 } UIState;
 
 void ui_init(UIState *s);
+void sa_init(UIState *s, bool full_init);
 void ui_update(UIState *s);
 
 int write_param_float(float param, const char* param_name, bool persistent_param = false);