|
|
|
@ -2,6 +2,7 @@ import numpy as np |
|
|
|
|
from common.realtime import sec_since_boot, DT_MDL |
|
|
|
|
from common.numpy_fast import interp |
|
|
|
|
from system.swaglog import cloudlog |
|
|
|
|
from selfdrive.modeld.constants import T_IDXS |
|
|
|
|
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc |
|
|
|
|
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N |
|
|
|
|
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED |
|
|
|
@ -35,6 +36,7 @@ class LateralPlanner: |
|
|
|
|
self.solution_invalid_cnt = 0 |
|
|
|
|
|
|
|
|
|
self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) |
|
|
|
|
self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3)) |
|
|
|
|
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) |
|
|
|
|
self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,)) |
|
|
|
|
self.t_idxs = np.arange(TRAJECTORY_SIZE) |
|
|
|
@ -47,9 +49,13 @@ class LateralPlanner: |
|
|
|
|
self.x0 = x0 |
|
|
|
|
self.lat_mpc.reset(x0=self.x0) |
|
|
|
|
|
|
|
|
|
def update(self, sm): |
|
|
|
|
def update(self, sm, v_plan): |
|
|
|
|
# clip speed , lateral planning is not possible at 0 speed |
|
|
|
|
self.v_ego = max(MIN_SPEED, sm['carState'].vEgo) |
|
|
|
|
v_plan = self.v_ego * np.ones(len(T_IDXS)) |
|
|
|
|
self.v_plan = v_plan |
|
|
|
|
plan_odo = np.concatenate(([0], np.cumsum(((v_plan[0:-1] + v_plan[1:])/2) * np.diff(T_IDXS)))) |
|
|
|
|
plan_odo = np.clip(plan_odo, MIN_SPEED, np.inf)[:LAT_MPC_N + 1] |
|
|
|
|
measured_curvature = sm['controlsState'].curvature |
|
|
|
|
|
|
|
|
|
# Parse model predictions |
|
|
|
@ -59,6 +65,9 @@ class LateralPlanner: |
|
|
|
|
self.t_idxs = np.array(md.position.t) |
|
|
|
|
self.plan_yaw = np.array(md.orientation.z) |
|
|
|
|
self.plan_yaw_rate = np.array(md.orientationRate.z) |
|
|
|
|
self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z]) |
|
|
|
|
car_speed = np.linalg.norm(self.velocity_xyz, axis=1) |
|
|
|
|
self.model_odo = np.concatenate(([0], np.cumsum(((car_speed[0:-1] + car_speed[1:])/2) * np.diff(self.t_idxs)))) |
|
|
|
|
|
|
|
|
|
# Lane change logic |
|
|
|
|
desire_state = md.meta.desireState |
|
|
|
@ -68,21 +77,23 @@ class LateralPlanner: |
|
|
|
|
lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob |
|
|
|
|
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) |
|
|
|
|
|
|
|
|
|
d_path_xyz = self.path_xyz |
|
|
|
|
self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST, |
|
|
|
|
LATERAL_ACCEL_COST, LATERAL_JERK_COST, |
|
|
|
|
STEERING_RATE_COST) |
|
|
|
|
|
|
|
|
|
y_pts = np.interp(self.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(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) |
|
|
|
|
yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate) |
|
|
|
|
#y_pts = np.interp(plan_odo, self.model_odo, self.path_xyz[:, 1]) |
|
|
|
|
#heading_pts = np.interp(plan_odo, self.model_odo, self.plan_yaw) |
|
|
|
|
#yaw_rate_pts = np.interp(plan_odo, self.model_odo, self.plan_yaw_rate) |
|
|
|
|
y_pts = np.interp(plan_odo, np.linalg.norm(self.path_xyz, axis=1), self.path_xyz[:, 1]) |
|
|
|
|
heading_pts = np.interp(plan_odo, np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) |
|
|
|
|
yaw_rate_pts = np.interp(plan_odo, np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate) |
|
|
|
|
self.y_pts = y_pts |
|
|
|
|
|
|
|
|
|
assert len(y_pts) == LAT_MPC_N + 1 |
|
|
|
|
assert len(heading_pts) == LAT_MPC_N + 1 |
|
|
|
|
assert len(yaw_rate_pts) == LAT_MPC_N + 1 |
|
|
|
|
lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2)) |
|
|
|
|
p = np.array([self.v_ego, lateral_factor]) |
|
|
|
|
lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf) |
|
|
|
|
p = np.column_stack([self.v_plan, lateral_factor]) |
|
|
|
|
self.lat_mpc.run(self.x0, |
|
|
|
|
p, |
|
|
|
|
y_pts, |
|
|
|
|