mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-22 06:22:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Safe interpolation in lane_planner (#20995)

Browse Source 
* safe idxs

* make thread more safe
This commit is contained in:
HaraldSchafer
2021-05-21 16:34:17 -07:00
committed by GitHub
parent dc40631e97
commit d6fcf6fa84
1 changed files with 7 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/lane_planner.py
+7 -2
View File
@@ -1,6 +1,7 @@
from common.numpy_fast import interp
import numpy as np
from selfdrive.hardware import EON, TICI
from selfdrive.swaglog import cloudlog
from cereal import log
@@ -91,6 +92,10 @@ class LanePlanner:
self.d_prob = l_prob + r_prob - l_prob * r_prob
lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
lane_path_y_interp = np.interp(path_t, self.ll_t, lane_path_y)
path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
safe_idxs = np.isfinite(self.ll_t)
if safe_idxs[0]:
lane_path_y_interp = np.interp(path_t, self.ll_t[safe_idxs], lane_path_y[safe_idxs])
path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
else:
cloudlog.warning("Lateral mpc - NaNs in laneline times, ignoring")
return path_xyz