Clean up planner files (#23031)

* clean up planner files

clean up planner files

* fix plant instance
old-commit-hash: 93fd662adf

selfdrive/controls/lib/lateral_planner.py

@@ -38,7 +38,7 @@ DESIRES = {
 }
 
 
-class LateralPlanner():
+class LateralPlanner:
   def __init__(self, CP, use_lanelines=True, wide_camera=False):
     self.use_lanelines = use_lanelines
     self.LP = LanePlanner(wide_camera)
@@ -67,12 +67,8 @@ class LateralPlanner():
   def reset_mpc(self, x0=np.zeros(6)):
     self.x0 = x0
     self.lat_mpc.reset(x0=self.x0)
-    self.desired_curvature = 0.0
-    self.safe_desired_curvature = 0.0
-    self.desired_curvature_rate = 0.0
-    self.safe_desired_curvature_rate = 0.0
 
-  def update(self, sm, CP):
+  def update(self, sm):
     v_ego = sm['carState'].vEgo
     active = sm['controlsState'].active
     measured_curvature = sm['controlsState'].curvature
@@ -167,13 +163,13 @@ class LateralPlanner():
       self.LP.rll_prob *= self.lane_change_ll_prob
     if self.use_lanelines:
       d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
-      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
+      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
     else:
       d_path_xyz = self.path_xyz
       path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH
       # Heading cost is useful at low speed, otherwise end of plan can be off-heading
       heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
-      self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
+      self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost)
     y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
     heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
     self.y_pts = y_pts
@@ -189,8 +185,7 @@ class LateralPlanner():
     # init state for next
     self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
 
-
-    #  Check for infeasable MPC solution
+    #  Check for infeasible MPC solution
     mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:, 3])
     t = sec_since_boot()
     if mpc_nans or self.lat_mpc.solution_status != 0:

selfdrive/controls/lib/longitudinal_planner.py

@@ -41,7 +41,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   return [a_target[0], min(a_target[1], a_x_allowed)]
 
 
-class Planner():
+class Planner:
   def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.CP = CP
     self.mpc = LongitudinalMpc()
@@ -56,8 +56,7 @@ class Planner():
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
 
-
-  def update(self, sm, CP):
+  def update(self, sm):
     v_ego = sm['carState'].vEgo
     a_ego = sm['carState'].aEgo
 

selfdrive/controls/plannerd.py

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

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -42,8 +42,7 @@ class Plant():
 
     from selfdrive.car.honda.values import CAR
     from selfdrive.car.honda.interface import CarInterface
-    self.CP = CarInterface.get_params(CAR.CIVIC)
-    self.planner = Planner(self.CP, init_v=self.speed)
+    self.planner = Planner(CarInterface.get_params(CAR.CIVIC), init_v=self.speed)
 
   def current_time(self):
     return float(self.rk.frame) / self.rate
@@ -97,7 +96,7 @@ class Plant():
     sm = {'radarState': radar.radarState,
           'carState': car_state.carState,
           'controlsState': control.controlsState}
-    self.planner.update(sm, self.CP)
+    self.planner.update(sm)
     self.speed = self.planner.v_desired
     self.acceleration = self.planner.a_desired
     fcw = self.planner.fcw