Fix division by zero in lateral_planner.py (#20071)

* Fix division by zero * Change fix logic
5 years ago · 8f9ee10b8a
parent 9e746c6dd8
commit 8f9ee10b8a
1 changed files with 1 additions and 1 deletions
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@ -188,7 +188,7 @@ class LateralPlanner():
    next_curvature = interp(delay, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature)
    psi = interp(delay, self.t_idxs[:MPC_N+1], self.mpc_solution.psi)
    next_curvature_rate = self.mpc_solution.curvature_rate[0]
-    next_curvature_from_psi = psi/(v_ego*delay)
+    next_curvature_from_psi = psi/(max(v_ego, 1e-1) * delay)
    if psi > self.mpc_solution.curvature[0] * delay * v_ego:
      next_curvature = max(next_curvature_from_psi, next_curvature)
    else: