declanwu
/
dp
forked from mawei/dp
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #134 from Hikari1023/patch-9 

Fix DLP laneless bug
This commit is contained in:
Kumar 2022-04-16 08:18:59 -07:00 committed by GitHub
parent af80cdd94a b03486fa10
commit 458dd1a527
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
selfdrive/controls/lib/lateral_planner.py
View File

@ -203,17 +203,17 @@ class LateralPlanner:
if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.lane_change_ll_prob self.LP.lll_prob *= self.lane_change_ll_prob
self.LP.rll_prob *= self.lane_change_ll_prob self.LP.rll_prob *= self.lane_change_ll_prob
if self.get_dlp_laneless_mode(): if not self.get_dlp_laneless_mode():
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, self.steer_rate_cost)
self.laneless_mode_is_e2e = True self.laneless_mode_is_e2e = False
else: else:
d_path_xyz = self.path_xyz 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 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 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]) heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost) self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost)
self.laneless_mode_is_e2e = False self.laneless_mode_is_e2e = True
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)
self.y_pts = y_pts self.y_pts = y_pts