Extra longitudinal unit tests (#21601)

* linter needs this

* unit test radar override
old-commit-hash: 9e8d99b530

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -12,6 +12,9 @@ class Maneuver():
     self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0])
     self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
 
+    self.only_lead2 = kwargs.get("only_lead2", False)
+    self.only_radar = kwargs.get("only_radar", False)
+
     self.duration = duration
     self.title = title
 
@@ -19,7 +22,9 @@ class Maneuver():
     plant = Plant(
       lead_relevancy=self.lead_relevancy,
       speed=self.speed,
-      distance_lead=self.distance_lead
+      distance_lead=self.distance_lead,
+      only_lead2=self.only_lead2,
+      only_radar=self.only_radar
     )
 
     valid = True

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -11,7 +11,8 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState
 class Plant():
   messaging_initialized = False
 
-  def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0):
+  def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
+               only_lead2=False, only_radar=False):
     self.rate = 1. / DT_MDL
 
     if not Plant.messaging_initialized:
@@ -30,12 +31,28 @@ class Plant():
     # lead car
     self.distance_lead = distance_lead
     self.lead_relevancy = lead_relevancy
+    self.only_lead2=only_lead2
+    self.only_radar=only_radar
 
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
     self.ts = 1. / self.rate
     time.sleep(1)
     self.sm = messaging.SubMaster(['longitudinalPlan'])
 
+    # make sure planner has time to be fully initialized
+    # TODO planner should just be explicitly initialized
+    for i in range(10000):
+      assert i < 10000
+      radar = messaging.new_message('radarState')
+      control = messaging.new_message('controlsState')
+      car_state = messaging.new_message('carState')
+      control.controlsState.longControlState = LongCtrlState.pid
+      control.controlsState.vCruise = speed*3.6
+      car_state.carState.vEgo = self.speed
+      Plant.radar.send(radar.to_bytes())
+      Plant.controls_state.send(control.to_bytes())
+      Plant.car_state.send(car_state.to_bytes())
+
   def current_time(self):
     return float(self.rk.frame) / self.rate
 
@@ -51,11 +68,16 @@ class Plant():
     if self.lead_relevancy:
       d_rel = np.maximum(0., self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
-      prob = 1.0
+      if self.only_radar:
+        prob = 0.0
+      else:
+        prob = 1.0
+      status = True
     else:
       d_rel = 200.
       v_rel = 0.
       prob = 0.0
+      status = False
 
     lead = log.RadarState.LeadData.new_message()
     lead.dRel = float(d_rel)
@@ -66,9 +88,10 @@ class Plant():
     lead.vLeadK = float(v_lead)
     lead.aLeadK = float(a_lead)
     lead.aLeadTau = float(1.5)
-    lead.status = True
+    lead.status = status
     lead.modelProb = prob
-    radar.radarState.leadOne = lead
+    if not self.only_lead2:
+      radar.radarState.leadOne = lead
     radar.radarState.leadTwo = lead
 
 

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -61,6 +61,26 @@ maneuvers = [
     speed_lead_values=[0., 0.],
     speed_lead_breakpoints=[1., 11.],
   ),
+  Maneuver(
+    "approach slower cut-in car at 20m/s",
+    duration=20.,
+    initial_speed=20.,
+    lead_relevancy=True,
+    initial_distance_lead=50.,
+    speed_lead_values=[15., 15.],
+    speed_lead_breakpoints=[1., 11.],
+    only_lead2=True,
+  ),
+  Maneuver(
+    "stay stopped behind radar override lead",
+    duration=20.,
+    initial_speed=0.,
+    lead_relevancy=True,
+    initial_distance_lead=10.,
+    speed_lead_values=[0., 0.],
+    speed_lead_breakpoints=[1., 11.],
+    only_radar=True,
+  ),
 ]