should_stop: less creepy

Author: haraschax

selfdrive/controls/lib/drive_helpers.py

@@ -39,19 +39,17 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, b
   return float(new_curvature), limited_accel or limited_max_curv
 
 
-def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05):
+def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.1):
   if len(speeds) == len(t_idxs):
     v_now = speeds[0]
     a_now = accels[0]
     v_target = np.interp(action_t, t_idxs, speeds)
     a_target = 2 * (v_target - v_now) / (action_t) - a_now
-    v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds)
   else:
     v_target = 0.0
-    v_target_1sec = 0.0
     a_target = 0.0
   should_stop = (v_target < vEgoStopping and
-                 v_target_1sec < vEgoStopping)
+                 a_target < 0.2) # even teslas cannot a<0.2 when starting from stop
   return a_target, should_stop
 
 def curv_from_psis(psi_target, psi_rate, vego, action_t):