From 400b36a57f8b013b6d18f0737d789b63862a1fa0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 11 Nov 2022 14:02:09 -0800 Subject: [PATCH] longitudinal tests: add disabled maneuver (#26470) * undeclared variable * run first second disabled * only test * test disabled as a new maneuver * bottom old-commit-hash: 9f80a97eee52d128559b7fef0593b8c328a6142d --- .../controls/lib/longitudinal_planner.py | 3 ++- .../test/longitudinal_maneuvers/maneuver.py | 12 +++++++----- .../test/longitudinal_maneuvers/plant.py | 19 ++++++++++--------- .../test_longitudinal.py | 9 ++++++++- 4 files changed, 27 insertions(+), 16 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c6621f606d..def0a1208a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -69,7 +69,8 @@ class LongitudinalPlanner: e2e = self.params.get_bool('ExperimentalMode') and self.CP.openpilotLongitudinalControl self.mpc.mode = 'blended' if e2e else 'acc' - def parse_model(self, model_msg, model_error): + @staticmethod + def parse_model(model_msg, model_error): if (len(model_msg.position.x) == 33 and len(model_msg.velocity.x) == 33 and len(model_msg.acceleration.x) == 33): diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index dad5d89844..071eaada12 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -17,6 +17,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.enabled = kwargs.get("enabled", True) self.duration = duration self.title = title @@ -26,23 +27,24 @@ class Maneuver(): lead_relevancy=self.lead_relevancy, speed=self.speed, distance_lead=self.distance_lead, + enabled=self.enabled, only_lead2=self.only_lead2, only_radar=self.only_radar, ) valid = True logs = [] - while plant.current_time() < self.duration: - speed_lead = np.interp(plant.current_time(), self.breakpoints, self.speed_lead_values) - prob = np.interp(plant.current_time(), self.breakpoints, self.prob_lead_values) - cruise = np.interp(plant.current_time(), self.breakpoints, self.cruise_values) + while plant.current_time < self.duration: + speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values) + prob = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values) + cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values) log = plant.step(speed_lead, prob, cruise) d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200. 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(), + logs.append(np.array([plant.current_time, log['distance'], log['distance_lead'], log['speed'], diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index e81510e9ba..c3af1eee03 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -10,11 +10,12 @@ from selfdrive.modeld.constants import T_IDXS from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU -class Plant(): + +class Plant: messaging_initialized = False def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, - only_lead2=False, only_radar=False): + enabled=True, only_lead2=False, only_radar=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: @@ -32,10 +33,11 @@ class Plant(): self.speeds = [] # lead car - self.distance_lead = distance_lead self.lead_relevancy = lead_relevancy - self.only_lead2=only_lead2 - self.only_radar=only_radar + self.distance_lead = distance_lead + self.enabled = enabled + 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 @@ -47,6 +49,7 @@ class Plant(): self.planner = LongitudinalPlanner(CarInterface.get_params(CAR.CIVIC), init_v=self.speed) + @property def current_time(self): return float(self.rk.frame) / self.rate @@ -104,9 +107,7 @@ class Plant(): acceleration.x = [float(x) for x in np.zeros_like(T_IDXS)] model.modelV2.acceleration = acceleration - - - control.controlsState.longControlState = LongCtrlState.pid + control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off control.controlsState.vCruise = float(v_cruise * 3.6) car_state.carState.vEgo = float(self.speed) car_state.carState.standstill = self.speed < 0.01 @@ -141,7 +142,7 @@ class Plant(): # print at 5hz if (self.rk.frame % (self.rate // 5)) == 0: print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" - % (self.current_time(), self.distance, self.speed, self.acceleration, d_rel, v_rel)) + % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel)) # ******** update prevs ******** diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 7cc95b104a..e859952445 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -10,7 +10,7 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver # TODO: make new FCW tests maneuvers = [ Maneuver( - 'approach stopped car at 20m/s, initial distance: 120m', + 'approach stopped car at 25m/s, initial distance: 120m', duration=20., initial_speed=25., lead_relevancy=True, @@ -118,6 +118,13 @@ maneuvers = [ breakpoints=[1., 10., 15.], ensure_start=True, ), + Maneuver( + 'cruising at 25 m/s while disabled', + duration=20., + initial_speed=25., + lead_relevancy=False, + enabled=False, + ), ]