diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 79f23294ba..0c55e101db 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -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