From f0be9a57ac1cf4931d7ebbfeb80a25e5d185167b Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Thu, 16 Sep 2021 20:45:13 -0700 Subject: [PATCH] Dont run plannerd for long tests (#22260) --- .../controls/lib/longitudinal_planner.py | 6 +-- .../test/longitudinal_maneuvers/maneuver.py | 4 +- .../test/longitudinal_maneuvers/plant.py | 39 +++++++------------ .../test_longitudinal.py | 15 +------ 4 files changed, 20 insertions(+), 44 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 30840d2951..339b9cf781 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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() diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index f53ae2c3c9..9de546a6c0 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/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) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 0ec4f89fd2..02df860ccd 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/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 ******** diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index c7241aab0e..8ea1af572f 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/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