Planner cleanup (#25969)

selfdrive/controls/lib/lateral_planner.py

@@ -22,7 +22,6 @@ class LateralPlanner:
     self.solution_invalid_cnt = 0
 
     self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
-    self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
     self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
     self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,))
     self.t_idxs = np.arange(TRAJECTORY_SIZE)
@@ -45,8 +44,6 @@ class LateralPlanner:
       self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
       self.t_idxs = np.array(md.position.t)
       self.plan_yaw = np.array(md.orientation.z)
-    if len(md.position.xStd) == TRAJECTORY_SIZE:
-      self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
 
     # Lane change logic
     desire_state = md.meta.desireState

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -255,9 +255,6 @@ class LongitudinalMpc:
     elif self.mode == 'blended':
       cost_weights = [0., 0.2, 0.25, 1.0, 0.0, 1.0]
       constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0]
-    elif self.mode == 'e2e':
-      cost_weights = [0., 0.2, 0.25, 1., 0.0, .1]
-      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0]
     else:
       raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
     self.set_cost_weights(cost_weights, constraint_cost_weights)
@@ -386,28 +383,6 @@ class LongitudinalMpc:
          (lead_1_obstacle[0] - lead_0_obstacle[0]):
         self.source = 'lead1'
 
-
-  def update_with_xva(self, x, v, a):
-    self.params[:,0] = -10.
-    self.params[:,1] = 10.
-    self.params[:,2] = 1e5
-    self.params[:,4] = T_FOLLOW
-    self.params[:,5] = LEAD_DANGER_FACTOR
-
-    # 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
-    xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
-    x = np.cumsum(np.insert(xforward, 0, x[0]))
-    self.yref[:,1] = x
-    self.yref[:,2] = v
-    self.yref[:,3] = 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])
-    self.params[:,3] = np.copy(self.prev_a)
-    self.run()
-
   def run(self):
     # t0 = sec_since_boot()
     # reset = 0

selfdrive/controls/lib/longitudinal_planner.py

@@ -87,8 +87,8 @@ class LongitudinalPlanner:
       j = np.zeros(len(T_IDXS_MPC))
     return x, v, a, j
 
-  def update(self, sm):
-    if self.param_read_counter % 50 == 0:
+  def update(self, sm, read=True):
+    if self.param_read_counter % 50 == 0 and read:
       self.read_param()
     self.param_read_counter += 1