Dont run plannerd for long tests (#22260)

selfdrive/controls/lib/longitudinal_planner.py

@@ -45,7 +45,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
 
 
 class Planner():
-  def __init__(self, CP):
+  def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.CP = CP
     self.mpcs = {}
     self.mpcs['lead0'] = LeadMpc(0)
@@ -55,8 +55,8 @@ class Planner():
     self.fcw = False
     self.fcw_checker = FCWChecker()
 
-    self.v_desired = 0.0
-    self.a_desired = 0.0
+    self.v_desired = init_v
+    self.a_desired = init_a
     self.longitudinalPlanSource = 'cruise'
     self.alpha = np.exp(-DT_MDL/2.0)
     self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -28,6 +28,7 @@ class Maneuver():
     )
 
     valid = True
+    logs = []
     while plant.current_time() < self.duration:
       speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
       log = plant.step(speed_lead)
@@ -36,10 +37,11 @@ class Maneuver():
       v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
       log['d_rel'] = d_rel
       log['v_rel'] = v_rel
+      logs.append(np.array([plant.current_time(), log['distance'], log['distance_lead'], log['speed'], speed_lead, log['acceleration']]))
 
       if d_rel < 1.0:
         print("Crashed!!!!")
         valid = False
 
     print("maneuver end", valid)
-    return valid
+    return valid, np.array(logs)

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -6,6 +6,7 @@ from cereal import log
 import cereal.messaging as messaging
 from common.realtime import Ratekeeper, DT_MDL
 from selfdrive.controls.lib.longcontrol import LongCtrlState
+from selfdrive.controls.lib.longitudinal_planner import Planner
 
 
 class Plant():
@@ -39,19 +40,10 @@ class Plant():
     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())
+    from selfdrive.car.honda.values import CAR
+    from selfdrive.car.honda.interface import CarInterface
+    self.CP = CarInterface.get_params(CAR.CIVIC)
+    self.planner = Planner(self.CP, init_v=self.speed)
 
   def current_time(self):
     return float(self.rk.frame) / self.rate
@@ -97,22 +89,17 @@ class Plant():
 
     control.controlsState.longControlState = LongCtrlState.pid
     control.controlsState.vCruise = 130
-    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())
+    car_state.carState.vEgo = float(self.speed)
 
 
     # ******** get controlsState messages for plotting ***
-    self.sm.update()
-    while True:
-      time.sleep(0.01)
-      if self.sm.updated['longitudinalPlan']:
-        plan = self.sm['longitudinalPlan']
-        self.speed = plan.speeds[5]
-        self.acceleration = plan.accels[5]
-        fcw = plan.fcw
-        break
+    sm = {'radarState': radar.radarState,
+          'carState': car_state.carState,
+          'controlsState': control.controlsState}
+    self.planner.update(sm, self.CP)
+    self.speed = self.planner.v_desired
+    self.acceleration = self.planner.a_desired
+    fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts
 
     # ******** run the car ********

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -4,14 +4,6 @@ import unittest
 
 from common.params import Params
 from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
-from selfdrive.manager.process_config import managed_processes
-
-
-def put_default_car_params():
-  from selfdrive.car.honda.values import CAR
-  from selfdrive.car.honda.interface import CarInterface
-  cp = CarInterface.get_params(CAR.CIVIC)
-  Params().put("CarParams", cp.to_bytes())
 
 
 # TODO: make new FCW tests
@@ -124,12 +116,7 @@ def run_maneuver_worker(k):
   def run(self):
     man = maneuvers[k]
     print(man.title)
-    put_default_car_params()
-    managed_processes['plannerd'].start()
-
-    valid = man.evaluate()
-
-    managed_processes['plannerd'].stop()
+    valid, _ = man.evaluate()
     self.assertTrue(valid)
   return run