MPC retune for laneless fix (#20616)

* was making wrong policy more aggresive

* allow to be set from simulator

* update refs

* put params together

selfdrive/controls/lib/lateral_planner.py

@@ -1,7 +1,6 @@
 import os
 import math
 import numpy as np
-from common.params import Params
 from common.realtime import sec_since_boot, DT_MDL
 from common.numpy_fast import interp, clip
 from selfdrive.swaglog import cloudlog
@@ -9,7 +8,6 @@ from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS
 from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
 from selfdrive.config import Conversions as CV
-from selfdrive.hardware import TICI
 import cereal.messaging as messaging
 from cereal import log
 
@@ -47,10 +45,8 @@ DESIRES = {
 
 
 class LateralPlanner():
-  def __init__(self, CP):
-    params = Params()
-
-    wide_camera = (params.get('EnableWideCamera') == b'1') if TICI else False
+  def __init__(self, CP, use_lanelines=True, wide_camera=False):
+    self.use_lanelines = use_lanelines
     self.LP = LanePlanner(wide_camera)
 
     self.last_cloudlog_t = 0
@@ -58,7 +54,6 @@ class LateralPlanner():
 
     self.setup_mpc()
     self.solution_invalid_cnt = 0
-    self.use_lanelines = not params.get_bool('EndToEndToggle')
     self.lane_change_state = LaneChangeState.off
     self.lane_change_direction = LaneChangeDirection.none
     self.lane_change_timer = 0.0
@@ -168,18 +163,20 @@ class LateralPlanner():
       self.LP.lll_prob *= self.lane_change_ll_prob
       self.LP.rll_prob *= self.lane_change_ll_prob
     if self.use_lanelines:
-      std_cost_mult = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 5.0)
       d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
+      self.libmpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
     else:
-      std_cost_mult = 1.0
       d_path_xyz = self.path_xyz
+      path_cost = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 5.0) * 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.libmpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
     y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
     heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
     self.y_pts = y_pts
 
     assert len(y_pts) == MPC_N + 1
     assert len(heading_pts) == MPC_N + 1
-    self.libmpc.set_weights(std_cost_mult*MPC_COST_LAT.PATH, 0.0, CP.steerRateCost)
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
                         float(v_ego),
                         CAR_ROTATION_RADIUS,

selfdrive/controls/plannerd.py

@@ -5,6 +5,7 @@ from common.realtime import Priority, config_realtime_process
 from selfdrive.swaglog import cloudlog
 from selfdrive.controls.lib.longitudinal_planner import Planner
 from selfdrive.controls.lib.lateral_planner import LateralPlanner
+from selfdrive.hardware import TICI
 import cereal.messaging as messaging
 
 
@@ -13,11 +14,14 @@ def plannerd_thread(sm=None, pm=None):
   config_realtime_process(2, Priority.CTRL_LOW)
 
   cloudlog.info("plannerd is waiting for CarParams")
-  CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
+  params = Params()
+  CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
+  use_lanelines = not params.get_bool('EndToEndToggle')
+  wide_camera = (params.get('EnableWideCamera') == b'1') if TICI else False
   cloudlog.info("plannerd got CarParams: %s", CP.carName)
 
   longitudinal_planner = Planner(CP)
-  lateral_planner = LateralPlanner(CP)
+  lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera)
 
   if sm is None:
     sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2'],

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-8f1c3a315c4cce62459c48bfb405b5d8c0fc2760
+fe6473297e919d459effbe4a9888a083c9a84ee5