Safe interpolation in lane_planner (#20995)

* safe idxs

* make thread more safe
old-commit-hash: d6fcf6fa84

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)
+    safe_idxs = np.isfinite(self.ll_t)
-    path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
+    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