MPC retune for laneless fix (#20616)
* was making wrong policy more aggresive * allow to be set from simulator * update refs * put params togetheralbatross
parent
015973474e
commit
0e10b74a61
|
@ -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,
|
||||
|
|
|
@ -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'],
|
||||
|
|
|
@ -1 +1 @@
|
|||
8f1c3a315c4cce62459c48bfb405b5d8c0fc2760
|
||||
fe6473297e919d459effbe4a9888a083c9a84ee5
|
Loading…
Reference in New Issue