|
|
|
@ -11,13 +11,12 @@ from cereal import log |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class LateralPlanner: |
|
|
|
class LateralPlanner: |
|
|
|
def __init__(self, CP, use_lanelines=True, wide_camera=False): |
|
|
|
def __init__(self, use_lanelines=True, wide_camera=False): |
|
|
|
self.use_lanelines = use_lanelines |
|
|
|
self.use_lanelines = use_lanelines |
|
|
|
self.LP = LanePlanner(wide_camera) |
|
|
|
self.LP = LanePlanner(wide_camera) |
|
|
|
self.DH = DesireHelper() |
|
|
|
self.DH = DesireHelper() |
|
|
|
|
|
|
|
|
|
|
|
self.last_cloudlog_t = 0 |
|
|
|
self.last_cloudlog_t = 0 |
|
|
|
self.steer_rate_cost = CP.steerRateCost |
|
|
|
|
|
|
|
self.solution_invalid_cnt = 0 |
|
|
|
self.solution_invalid_cnt = 0 |
|
|
|
|
|
|
|
|
|
|
|
self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) |
|
|
|
self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) |
|
|
|
@ -59,12 +58,12 @@ class LateralPlanner: |
|
|
|
# Calculate final driving path and set MPC costs |
|
|
|
# Calculate final driving path and set MPC costs |
|
|
|
if self.use_lanelines: |
|
|
|
if self.use_lanelines: |
|
|
|
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) |
|
|
|
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, self.steer_rate_cost) |
|
|
|
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE) |
|
|
|
else: |
|
|
|
else: |
|
|
|
d_path_xyz = self.path_xyz |
|
|
|
d_path_xyz = self.path_xyz |
|
|
|
# Heading cost is useful at low speed, otherwise end of plan can be off-heading |
|
|
|
# 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.15]) |
|
|
|
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15]) |
|
|
|
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, self.steer_rate_cost) |
|
|
|
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE) |
|
|
|
|
|
|
|
|
|
|
|
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]) |
|
|
|
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) |
|
|
|
heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) |
|
|
|
@ -79,7 +78,7 @@ class LateralPlanner: |
|
|
|
y_pts, |
|
|
|
y_pts, |
|
|
|
heading_pts) |
|
|
|
heading_pts) |
|
|
|
# init state for next |
|
|
|
# init state for next |
|
|
|
# mpc.u_sol is the desired curvature rate given x0 curv state. |
|
|
|
# mpc.u_sol is the desired curvature rate given x0 curv state. |
|
|
|
# with x0[3] = measured_curvature, this would be the actual desired rate. |
|
|
|
# with x0[3] = measured_curvature, this would be the actual desired rate. |
|
|
|
# instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. |
|
|
|
# instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. |
|
|
|
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) |
|
|
|
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) |
|
|
|
|
Jason Shuler
3 years ago