longitudinal tests: add disabled maneuver (#26470)

* undeclared variable

* run first second disabled

* only test

* test disabled as a new maneuver

* bottom

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):

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'],

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 ********

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,
+  ),
 ]