model cruise control button (commaai#453)

* show model cruise control button

* handle in update function

* model-based pos/speed targets for cruise mpc

New model: 09633f87-a95c-4dfd-9992-ffbaa97b269f/600

Whoops

Fixed some more stuff

longitudinal_planner.update on modelV2 update

Retuned MPC

Clip speed and position to v_cruise

* split MPCs that require different tuning

* clean up

* ui fixes

* fix for cereal

* update releases

Co-authored-by: mitchellgoffpc <[email protected]>
Co-authored-by: Comma Device <[email protected]>

SA_RELEASES.md

@@ -1,6 +1,7 @@
 Stock Additions Update 2 - 2021-08-15 (0.8.8)
 ===
  * Update SA to 0.8.8 with new model from upstream!
+ * Experimental model cruise control button is back with a new model
 
 Stock Additions Update 1 - 2021-08-15 (0.8.7)
 ===

selfdrive/controls/lib/lead_mpc.py

@@ -49,7 +49,7 @@ def set_cur_state(self, v, a):
     self.cur_state[0].v_ego = v_safe
     self.cur_state[0].a_ego = a_safe
 
-  def update(self, CS, radarstate, v_cruise):
+  def update(self, CS, radarstate, modelstate, v_cruise):
     v_ego = CS.vEgo
     if self.lead_id == 0:
       lead = radarstate.leadOne

selfdrive/controls/lib/long_mpc.py

@@ -9,18 +9,23 @@
 
 
 class LongitudinalMpc():
-  def __init__(self):
+  def __init__(self, mpc_id):
+    self.mpc_id = mpc_id
+
     self.reset_mpc()
     self.last_cloudlog_t = 0.0
     self.ts = list(range(10))
     self.status = True
-    self.min_a = -1.2
+    self.min_a = -1.2 if self.mpc_id == 0 else -3.5
     self.max_a = 1.2
 
 
   def reset_mpc(self):
     self.libmpc = libmpc_py.libmpc
-    self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0)
+    if self.mpc_id == 0:
+      self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0)
+    else:
+      self.libmpc.init(1.0, 1.0, 0.0, 5.0, 10000.0)
 
     self.mpc_solution = libmpc_py.ffi.new("log_t *")
     self.cur_state = libmpc_py.ffi.new("state_t *")
@@ -45,10 +50,14 @@ def set_cur_state(self, v, a):
     self.cur_state[0].v_ego = v_safe
     self.cur_state[0].a_ego = a_safe
 
-  def update(self, carstate, radarstate, v_cruise):
+  def update(self, carstate, radarstate, modelstate, v_cruise):
     v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0)
-    poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
-    speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
+    if self.mpc_id == 0:
+      poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
+      speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
+    else:
+      poss = np.minimum(np.array(modelstate.position.x)[:LON_MPC_N+1], v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1]))
+      speeds = np.minimum(np.array(modelstate.velocity.x)[:LON_MPC_N+1], v_cruise_clipped)
     accels = np.zeros(LON_MPC_N+1)
     self.update_with_xva(poss, speeds, accels)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -15,8 +15,6 @@
 from selfdrive.controls.lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
 from selfdrive.swaglog import cloudlog
-# from selfdrive.controls.lib.long_mpc_model import LongitudinalMpcModel
-# from common.op_params import opParams
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
@@ -46,44 +44,17 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   return [a_target[0], min(a_target[1], a_x_allowed)]
 
 
-class ModelMpcHelper:
-  def __init__(self):
-    self.model_t = [i ** 2 / 102.4 for i in range(33)]  # the timesteps of the model predictions
-    self.mpc_t = list(range(10))  # the timesteps of what the LongMpcModel class takes in, 1 sec intervels to 10
-    self.model_t_idx = [sorted(range(len(self.model_t)), key=[abs(idx - t) for t in self.model_t].__getitem__)[0] for idx in self.mpc_t]  # matches 0 to 9 interval to idx from t
-    assert len(self.model_t_idx) == 10, 'Needs to be length 10 for mpc'
-
-  def convert_data(self, sm):
-    modelV2 = sm['modelV2']
-    distances, speeds, accelerations = [], [], []
-    if not sm.alive['modelV2'] or len(modelV2.position.x) == 0:
-      return distances, speeds, accelerations
-
-    speeds = [modelV2.velocity.x[t] for t in self.model_t_idx]
-    distances = [modelV2.position.x[t] for t in self.model_t_idx]
-    for t in self.mpc_t:  # todo these three in one loop
-      if 0 < t < 9:
-        accelerations.append((speeds[t + 1] - speeds[t - 1]) / 2)
-
-    # Extrapolate forward and backward at edges
-    accelerations.append(accelerations[-1] - (accelerations[-2] - accelerations[-1]))
-    accelerations.insert(0, accelerations[0] - (accelerations[1] - accelerations[0]))
-    return distances, speeds, accelerations
-
-
 class Planner():
   def __init__(self, CP):
     self.CP = CP
     self.mpcs = {}
     self.mpcs['lead0'] = LeadMpc(0)
     self.mpcs['lead1'] = LeadMpc(1)
-    self.mpcs['cruise'] = LongitudinalMpc()
-    # self.mpcs['model'] = LongitudinalMpcModel()
+    self.mpcs['cruise'] = LongitudinalMpc(0)
+    self.mpcs['e2e'] = LongitudinalMpc(1)
 
     self.fcw = False
     self.fcw_checker = FCWChecker()
-    # self.op_params = opParams()
-    self.model_mpc_helper = ModelMpcHelper()
 
     self.v_desired = 0.0
     self.a_desired = 0.0
@@ -134,9 +105,9 @@ def update(self, sm, CP):
     next_a = np.inf
     for key in self.mpcs:
       self.mpcs[key].set_cur_state(self.v_desired, self.a_desired)
-      self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise)
-      # TODO: handle model long enabled check
-      if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a:  # picks slowest solution from accel in ~0.2 seconds
+      self.mpcs[key].update(sm['carState'], sm['radarState'], sm['modelV2'], v_cruise)
+      if (self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a and  # picks slowest solution from accel in ~0.2 seconds
+              ((key == 'e2e' and sm['modelLongButton'].enabled) or key != 'e2e')):
         self.longitudinalPlanSource = key
         self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N]
         self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N]

selfdrive/controls/plannerd.py

@@ -38,7 +38,6 @@ def plannerd_thread(sm=None, pm=None):
     if sm.updated['modelV2']:
       lateral_planner.update(sm, CP)
       lateral_planner.publish(sm, pm)
-    if sm.updated['radarState']:
       longitudinal_planner.update(sm, CP)
       longitudinal_planner.publish(sm, pm)
 

selfdrive/controls/tests/test_following_distance.py

@@ -6,6 +6,8 @@
 import cereal.messaging as messaging
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.lead_mpc import LeadMpc
+from selfdrive.controls.lib.drive_helpers import LON_MPC_N
+from selfdrive.modeld.constants import T_IDXS
 
 
 def RW(v_ego, v_l):
@@ -39,7 +41,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
     CS.carState.vEgo = v_ego
     CS.carState.aEgo = a_ego
 
-    # Setup model packet
+    # Setup radarstate packet
     radarstate = messaging.new_message('radarState')
     lead = log.RadarState.LeadData.new_message()
     lead.modelProb = .75
@@ -49,12 +51,21 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
     lead.status = True
     radarstate.radarState.leadOne = lead
 
+    # Setup model packet
+    modelstate = messaging.new_message('modelV2')
+    positions = log.ModelDataV2.XYZTData.new_message()
+    velocities = log.ModelDataV2.XYZTData.new_message()
+    positions.x = (v_ego * np.array(T_IDXS[:LON_MPC_N+1])).tolist()
+    velocities.x = (v_ego * np.ones(LON_MPC_N+1)).tolist()
+    modelstate.modelV2.position = positions
+    modelstate.modelV2.velocity = velocities
+
     # Run MPC
     mpc.set_cur_state(v_ego, a_ego)
     if first:  # Make sure MPC is converged on first timestep
       for _ in range(20):
-        mpc.update(CS.carState, radarstate.radarState, 0)
-    mpc.update(CS.carState, radarstate.radarState, 0)
+        mpc.update(CS.carState, radarstate.radarState, modelstate.modelV2, 0)
+    mpc.update(CS.carState, radarstate.radarState, modelstate.modelV2, 0)
 
     # Choose slowest of two solutions
     v_ego, a_ego = mpc.mpc_solution.v_ego[5], mpc.mpc_solution.a_ego[5]

selfdrive/test/process_replay/process_replay.py

@@ -264,8 +264,8 @@ def ublox_rcv_callback(msg):
   ProcessConfig(
     proc_name="plannerd",
     pub_sub={
-      "modelV2": ["lateralPlan"], "radarState": ["longitudinalPlan"],
-      "carState": [], "controlsState": [], 'modelLongButton': [],
+      "modelV2": ["lateralPlan", "longitudinalPlan"],
+      "carState": [], "controlsState": [], "radarState": [],
     },
     ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay"],
     init_callback=get_car_params,

selfdrive/ui/qt/onroad.cc

@@ -113,27 +113,26 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) {
   QWidget *btns_wrapper = new QWidget;
   QHBoxLayout *btns_layout  = new QHBoxLayout(btns_wrapper);
   btns_layout->setSpacing(0);
-  btns_layout->setContentsMargins(0, 0, 30, 30);
+  btns_layout->setContentsMargins(30, 0, 30, 30);
 
   main_layout->addWidget(btns_wrapper, 0, Qt::AlignBottom);
 
-//  mlButton = new QPushButton("Toggle Model Long");
-//  mlButton->setStyleSheet("font-size: 50px; border-radius: 25px; border-color: #b83737;");
-//  QObject::connect(mlButton, &QPushButton::clicked, [=]() {
-//    mlButton->setStyleSheet("font-size: 50px; border-radius: 25px; border-color: #37b868;");
-//  });
-//  mlButton->setFixedWidth(525);
-//  mlButton->setFixedHeight(150);
-//  btns_layout->addStretch();
-//  btns_layout->addWidget(mlButton, 0, Qt::AlignCenter);
+  mlButton = new QPushButton("Model Cruise Control");
+  QObject::connect(mlButton, &QPushButton::clicked, [=]() {
+    QUIState::ui_state.scene.mlButtonEnabled = !mlEnabled;
+  });
+  mlButton->setFixedWidth(575);
+  mlButton->setFixedHeight(150);
+  btns_layout->addStretch(4);
+  btns_layout->addWidget(mlButton, 0, Qt::AlignHCenter | Qt::AlignBottom);
+  btns_layout->addStretch(3);
 
   dfButton = new QPushButton("DF\nprofile");
   QObject::connect(dfButton, &QPushButton::clicked, [=]() {
     QUIState::ui_state.scene.dfButtonStatus = dfStatus < 3 ? dfStatus + 1 : 0;  // wrap back around
   });
   dfButton->setFixedWidth(200);
   dfButton->setFixedHeight(200);
-//  btns_layout->addStretch();
   btns_layout->addWidget(dfButton, 0, Qt::AlignRight);
 
   setStyleSheet(R"(
@@ -149,20 +148,25 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) {
 }
 
 void ButtonsWindow::updateState(const UIState &s) {
-  updateDfButton(s.scene.dfButtonStatus);  // update dynamic follow profile button
-//  updateMlButton(s.scene.dfButtonStatus);  // update model longitudinal button  // TODO: add model long back first
-}
-
-void ButtonsWindow::updateDfButton(int status) {
-  if (dfStatus != status) {  // updates (resets) on car start, or on button press
-    dfStatus = status;
+  if (dfStatus != s.scene.dfButtonStatus) {  // update dynamic follow profile button
+    dfStatus = s.scene.dfButtonStatus;
     dfButton->setStyleSheet(QString("font-size: 45px; border-radius: 100px; border-color: %1").arg(dfButtonColors.at(dfStatus)));
 
     MessageBuilder msg;
     auto dfButtonStatus = msg.initEvent().initDynamicFollowButton();
     dfButtonStatus.setStatus(dfStatus);
     QUIState::ui_state.pm->send("dynamicFollowButton", msg);
   }
+
+  if (mlEnabled != s.scene.mlButtonEnabled) {  // update model longitudinal button
+    mlEnabled = s.scene.mlButtonEnabled;
+    mlButton->setStyleSheet(QString("font-size: 50px; border-radius: 25px; border-color: %1").arg(mlButtonColors.at(mlEnabled)));
+
+    MessageBuilder msg;
+    auto mlButtonEnabled = msg.initEvent().initModelLongButton();
+    mlButtonEnabled.setEnabled(mlEnabled);
+    QUIState::ui_state.pm->send("modelLongButton", msg);
+  }
 }
 
 void OnroadAlerts::updateAlert(const Alert &a, const QColor &color) {

selfdrive/ui/qt/onroad.h

@@ -21,11 +21,15 @@ class ButtonsWindow : public QWidget {
 
 private:
   QPushButton *dfButton;
-//  QPushButton *mlButton;
+  QPushButton *mlButton;
 
+  // dynamic follow button
   int dfStatus = -1;  // always initialize style sheet and send msg
   const QStringList dfButtonColors = {"#044389", "#24a8bc", "#fcff4b", "#37b868"};
-  void updateDfButton(int status);
+
+  // model long button
+  bool mlEnabled = true;  // triggers initialization
+  const QStringList mlButtonColors = {"#b83737", "#37b868"};
 
 public slots:
   void updateState(const UIState &s);

selfdrive/ui/ui.h

@@ -102,7 +102,7 @@ typedef struct UIScene {
 
   int dfButtonStatus = 0;
   int lsButtonStatus = 0;
-  bool mlButtonEnabled;
+  bool mlButtonEnabled = false;
 
   mat3 view_from_calib;
   bool world_objects_visible;