add test for resuming

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -16,6 +16,7 @@ class Maneuver():
 
     self.only_lead2 = kwargs.get("only_lead2", False)
     self.only_radar = kwargs.get("only_radar", False)
+    self.ensure_start = kwargs.get("ensure_start", False)
 
     self.duration = duration
     self.title = title
@@ -52,5 +53,10 @@ class Maneuver():
         print("Crashed!!!!")
         valid = False
 
+      if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1:
+        print('Planner not starting!')
+        valid = False
+
+
     print("maneuver end", valid)
     return valid, np.array(logs)

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -28,6 +28,7 @@ class Plant():
     self.distance = 0.
     self.speed = speed
     self.acceleration = 0.0
+    self.speeds = []
 
     # lead car
     self.distance_lead = distance_lead
@@ -98,6 +99,7 @@ class Plant():
     self.planner.update(sm)
     self.speed = self.planner.v_desired_filter.x
     self.acceleration = self.planner.a_desired
+    self.speeds = self.planner.v_desired_trajectory.tolist()
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts
 

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -3,6 +3,7 @@ import os
 import unittest
 
 from common.params import Params
+from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
 from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
@@ -106,6 +107,17 @@ maneuvers = [
     breakpoints=[1., 1.01, 11.],
     cruise_values=[float("nan"), 15., 15.],
   ),
+  # controls relies on planner commanding to move for stock-ACC resume spamming
+  Maneuver(
+    "resume from a stop",
+    duration=20.,
+    initial_speed=0.,
+    lead_relevancy=True,
+    initial_distance_lead=STOP_DISTANCE,
+    speed_lead_values=[0., 0., 5.],
+    breakpoints=[1., 10., 15.],
+    ensure_start=True,
+  ),
 ]