Add e2e long toggle back

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -194,9 +194,8 @@ def gen_long_ocp():
 
 
 class LongitudinalMpc:
-  def __init__(self, e2e=False, desired_TR=T_FOLLOW):
+  def __init__(self, desired_TR=T_FOLLOW):
     self.dynamic_follow = DynamicFollow()
-    self.e2e = e2e
     self.desired_TR = desired_TR
     self.v_ego = 0.
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
@@ -232,8 +231,8 @@ def reset(self):
     self.x0 = np.zeros(X_DIM)
     self.set_weights()
 
-  def set_weights(self, prev_accel_constraint=True):
-    if self.e2e:
+  def set_weights(self, prev_accel_constraint=True, e2e=False):
+    if e2e:
       self.set_weights_for_xva_policy()
       self.params[:,0] = -10.
       self.params[:,1] = 10.
@@ -269,7 +268,7 @@ def set_weights_for_lead_policy(self, prev_accel_constraint=True):
       self.solver.cost_set(i, 'Zl', Zl)
 
   def set_weights_for_xva_policy(self):
-    W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.]))
+    W = np.diag([0., 0., .0, 1., 0.0, 1.])
     for i in range(N):
       self.solver.cost_set(i, 'W', W)
     # Setting the slice without the copy make the array not contiguous,
@@ -368,15 +367,14 @@ def update(self, carstate, radarstate, v_cruise):
     self.params[:,3] = np.copy(self.prev_a)
     self.params[:, 4] = self.desired_TR
 
-
     self.run()
     if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
             radarstate.leadOne.modelProb > 0.9):
       self.crash_cnt += 1
     else:
       self.crash_cnt = 0
 
-  def update_with_xva(self, x, v, a):
+  def update_with_xva(self, carstate, radarstate, v_cruise, x, v, a):
     # v, and a are in local frame, but x is wrt the x[0] position
     # In >90degree turns, x goes to 0 (and may even be -ve)
     # So, we use integral(v) + x[0] to obtain the forward-distance
@@ -388,7 +386,32 @@ def update_with_xva(self, x, v, a):
     for i in range(N):
       self.solver.cost_set(i, "yref", self.yref[i])
     self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
+    # set accel limits in params
+    self.params[:, 0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL])
+    self.params[:, 1] = self.cruise_max_a
+    self.params[:, 2] = 1e3
     self.params[:,3] = np.copy(self.prev_a)
+
+    lead_xv_0 = self.process_lead(radarstate.leadOne)
+    lead_xv_1 = self.process_lead(radarstate.leadTwo)
+
+    # To estimate a safe distance from a moving lead, we calculate how much stopping
+    # distance that lead needs as a minimum. We can add that to the current distance
+    # and then treat that as a stopped car/obstacle at this new distance.
+    lead_0_obstacle = lead_xv_0[:, 0] + get_stopped_equivalence_factor(lead_xv_0[:, 1])
+    lead_1_obstacle = lead_xv_1[:, 0] + get_stopped_equivalence_factor(lead_xv_1[:, 1])
+
+    cruise_target = T_IDXS * v_cruise + x[0]
+    x_targets = np.column_stack([x,
+                                 lead_0_obstacle - (3 / 4) * get_safe_obstacle_distance(v),
+                                 lead_1_obstacle - (3 / 4) * get_safe_obstacle_distance(v),
+                                 cruise_target])
+
+    self.yref[:, 1] = np.min(x_targets, axis=1)
+    for i in range(N):
+      self.solver.set(i, "yref", self.yref[i])
+    self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
+
     self.run()
 
   def run(self):

selfdrive/controls/lib/longitudinal_planner.py

@@ -7,7 +7,7 @@
 from common.conversions import Conversions as CV
 from common.filter_simple import FirstOrderFilter
 from common.realtime import DT_MDL
-from selfdrive.modeld.constants import T_IDXS
+from selfdrive.modeld.constants import T_IDXS, IDX_N
 from selfdrive.controls.lib.longcontrol import LongCtrlState
 from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
@@ -48,6 +48,7 @@ class Planner:
   def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.CP = CP
     self.mpc = LongitudinalMpc()
+    self.last_e2e = False
 
     self.fcw = False
 
@@ -60,6 +61,10 @@ def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.solverExecutionTime = 0.0
 
   def update(self, sm):
+    use_e2e_long = sm['modelLongButton'].enabled
+    if use_e2e_long and not self.last_e2e:
+      self.mpc.set_weights(e2e=True)
+      self.last_e2e = use_e2e_long
     v_ego = sm['carState'].vEgo
 
     v_cruise_kph = sm['controlsState'].vCruise
@@ -93,9 +98,26 @@ def update(self, sm):
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
     self.mpc.set_weights(prev_accel_constraint)
-    self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
+    if use_e2e_long:
+      self.mpc.set_accel_limits(-3.0, 2.0)
+    else:
+      self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
+
+    if use_e2e_long:
+      if (len(sm['modelV2'].position.x) == IDX_N and
+        len(sm['modelV2'].velocity.x) == IDX_N and
+        len(sm['modelV2'].acceleration.x) == IDX_N):
+        x = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].position.x)
+        v = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].velocity.x)
+        a = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].acceleration.x)
+      else:
+        x = np.zeros(len(T_IDXS_MPC))
+        v = np.zeros(len(T_IDXS_MPC))
+        a = np.zeros(len(T_IDXS_MPC))
+      self.mpc.update_with_xva(sm['carState'], sm['radarState'], v_cruise, x, v, a)
+    else:
+      self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
 
     self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
@@ -125,7 +147,8 @@ def publish(self, sm, pm):
     longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
 
     longitudinalPlan.hasLead = sm['radarState'].leadOne.status
-    longitudinalPlan.longitudinalPlanSource = self.mpc.source
+    if not sm['modelLongButton'].enabled:
+      longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
     longitudinalPlan.solverExecutionTime = self.mpc.solve_time

selfdrive/modeld/models/driving.cc

@@ -199,6 +199,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
   std::array<float, TRAJECTORY_SIZE> pos_x, pos_y, pos_z;
   std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std;
   std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
+  std::array<float, TRAJECTORY_SIZE> accel_x, accel_y, accel_z;
   std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z;
   std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z;
 
@@ -212,6 +213,9 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
     vel_x[i] = plan.mean[i].velocity.x;
     vel_y[i] = plan.mean[i].velocity.y;
     vel_z[i] = plan.mean[i].velocity.z;
+    accel_x[i] = plan.mean[i].acceleration.x;
+    accel_y[i] = plan.mean[i].acceleration.y;
+    accel_z[i] = plan.mean[i].acceleration.z;
     rot_x[i] = plan.mean[i].rotation.x;
     rot_y[i] = plan.mean[i].rotation.y;
     rot_z[i] = plan.mean[i].rotation.z;
@@ -222,6 +226,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
 
   fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std);
   fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z);
+  fill_xyzt(framed.initAcceleration(), T_IDXS_FLOAT, accel_x, accel_y, accel_z);
   fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z);
   fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
 }

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -53,6 +53,7 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
     radar = messaging.new_message('radarState')
     control = messaging.new_message('controlsState')
     car_state = messaging.new_message('carState')
+    model_long_button = messaging.new_message('modelLongButton')
     a_lead = (v_lead - self.v_lead_prev)/self.ts
     self.v_lead_prev = v_lead
 
@@ -94,7 +95,8 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
     # ******** get controlsState messages for plotting ***
     sm = {'radarState': radar.radarState,
           'carState': car_state.carState,
-          'controlsState': control.controlsState}
+          'controlsState': control.controlsState,
+          'modelLongButton': model_long_button.modelLongButton}
     self.planner.update(sm)
     self.speed = self.planner.v_desired_filter.x
     self.acceleration = self.planner.a_desired

selfdrive/ui/qt/onroad.cc

@@ -122,7 +122,7 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) {
   main_layout->addWidget(btns_wrapper, 0, Qt::AlignBottom);
 
   // Model long button
-  mlButton = new QPushButton("Model Cruise Control");
+  mlButton = new QPushButton("Toggle Model Long");
   QObject::connect(mlButton, &QPushButton::clicked, [=]() {
     uiState()->scene.mlButtonEnabled = !mlEnabled;
   });
@@ -132,7 +132,7 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) {
   btns_layout->addWidget(mlButton, 0, Qt::AlignHCenter | Qt::AlignBottom);
   btns_layout->addStretch(3);
 
-  std::string hide_model_long = "true";  // util::read_file("/data/community/params/hide_model_long");
+  std::string hide_model_long = util::read_file("/data/community/params/hide_model_long");
   if (hide_model_long == "true"){
     mlButton->hide();
   }