Longitudinal tests: test forceDecel (#26765)

* test with forceDecel

* test all combos

* fix

* fix

* fix

* ...

* remove print

* clean up

* just set cruise to 0

* update ref commit

Co-authored-by: Bruce Wayne <harald.the.engineer@gmail.com>

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -306,7 +306,7 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, carstate, radarstate, v_cruise, x, v, a, j):
+  def update(self, radarstate, v_cruise, x, v, a, j):
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -15,7 +15,6 @@ from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
 from system.swaglog import cloudlog
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
-AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
 A_CRUISE_MIN = -1.2
 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
@@ -111,9 +110,7 @@ class LongitudinalPlanner:
       self.v_model_error = sm['modelV2'].temporalPose.trans[0] - v_ego
 
     if force_slow_decel:
-      # if required so, force a smooth deceleration
-      accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
-      accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
+      v_cruise = 0.0
     # clip limits, cannot init MPC outside of bounds
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
@@ -122,7 +119,7 @@ class LongitudinalPlanner:
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    self.mpc.update(sm['carState'], sm['radarState'], v_cruise, x, v, a, j)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j)
 
     self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -19,6 +19,7 @@ class Maneuver:
     self.ensure_start = kwargs.get("ensure_start", False)
     self.enabled = kwargs.get("enabled", True)
     self.e2e = kwargs.get("e2e", False)
+    self.force_decel = kwargs.get("force_decel", False)
 
     self.duration = duration
     self.title = title
@@ -32,6 +33,7 @@ class Maneuver:
       only_lead2=self.only_lead2,
       only_radar=self.only_radar,
       e2e=self.e2e,
+      force_decel=self.force_decel,
     )
 
     valid = True
@@ -60,6 +62,10 @@ class Maneuver:
       if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1:
         print('LongitudinalPlanner not starting!')
         valid = False
+    if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04:
+      print('Not stopping with force decel')
+      valid = False
+
 
     print("maneuver end", valid)
     return valid, np.array(logs)

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -15,7 +15,7 @@ class Plant:
   messaging_initialized = False
 
   def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
-               enabled=True, only_lead2=False, only_radar=False, e2e=False):
+               enabled=True, only_lead2=False, only_radar=False, e2e=False, force_decel=False):
     self.rate = 1. / DT_MDL
 
     if not Plant.messaging_initialized:
@@ -39,6 +39,7 @@ class Plant:
     self.only_lead2 = only_lead2
     self.only_radar = only_radar
     self.e2e = e2e
+    self.force_decel = force_decel
 
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
     self.ts = 1. / self.rate
@@ -111,6 +112,7 @@ class Plant:
     control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
     control.controlsState.vCruise = float(v_cruise * 3.6)
     control.controlsState.experimentalMode = self.e2e
+    control.controlsState.forceDecel = self.force_decel
     car_state.carState.vEgo = float(self.speed)
     car_state.carState.standstill = self.speed < 0.01
 

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
+import itertools
 import os
-from parameterized import parameterized
+from parameterized import parameterized_class
 import unittest
 
 from common.params import Params
@@ -9,8 +10,8 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
 # TODO: make new FCW tests
-def create_maneuvers(e2e):
-  return [
+def create_maneuvers(kwargs):
+  maneuvers = [
     Maneuver(
       'approach stopped car at 25m/s, initial distance: 120m',
       duration=20.,
@@ -19,7 +20,7 @@ def create_maneuvers(e2e):
       initial_distance_lead=120.,
       speed_lead_values=[30., 0.],
       breakpoints=[0., 1.],
-      e2e=e2e,
+      **kwargs,
     ),
     Maneuver(
       'approach stopped car at 20m/s, initial distance 90m',
@@ -29,7 +30,7 @@ def create_maneuvers(e2e):
       initial_distance_lead=90.,
       speed_lead_values=[20., 0.],
       breakpoints=[0., 1.],
-      e2e=e2e,
+      **kwargs,
     ),
     Maneuver(
       'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
@@ -39,7 +40,7 @@ def create_maneuvers(e2e):
       initial_distance_lead=35.,
       speed_lead_values=[20., 20., 0.],
       breakpoints=[0., 15., 35.0],
-      e2e=e2e,
+      **kwargs,
     ),
     Maneuver(
       'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
@@ -49,7 +50,7 @@ def create_maneuvers(e2e):
       initial_distance_lead=35.,
       speed_lead_values=[20., 20., 0.],
       breakpoints=[0., 15., 25.0],
-      e2e=e2e,
+      **kwargs,
     ),
     Maneuver(
       'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
@@ -59,7 +60,7 @@ def create_maneuvers(e2e):
       initial_distance_lead=35.,
       speed_lead_values=[20., 20., 0.],
       breakpoints=[0., 15., 21.66],
-      e2e=e2e,
+      **kwargs,
     ),
     Maneuver(
       'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2',
@@ -71,7 +72,7 @@ def create_maneuvers(e2e):
       prob_lead_values=[0., 1., 1.],
       cruise_values=[20., 20., 20.],
       breakpoints=[2., 2.01, 8.8],
-      e2e=e2e,
+      **kwargs,
     ),
     Maneuver(
       "approach stopped car at 20m/s, with prob_lead_values",
@@ -83,7 +84,7 @@ def create_maneuvers(e2e):
       prob_lead_values=[0.0, 0., 1.],
       cruise_values=[20., 20., 20.],
       breakpoints=[0.0, 2., 2.01],
-      e2e=e2e,
+      **kwargs,
     ),
     Maneuver(
       "approach slower cut-in car at 20m/s",
@@ -94,7 +95,7 @@ def create_maneuvers(e2e):
       speed_lead_values=[15., 15.],
       breakpoints=[1., 11.],
       only_lead2=True,
-      e2e=e2e,
+      **kwargs,
     ),
     Maneuver(
       "stay stopped behind radar override lead",
@@ -106,7 +107,7 @@ def create_maneuvers(e2e):
       prob_lead_values=[0., 0.],
       breakpoints=[1., 11.],
       only_radar=True,
-      e2e=e2e,
+      **kwargs,
     ),
     Maneuver(
       "NaN recovery",
@@ -117,10 +118,20 @@ def create_maneuvers(e2e):
       speed_lead_values=[0., 0., 0.0],
       breakpoints=[1., 1.01, 11.],
       cruise_values=[float("nan"), 15., 15.],
-      e2e=e2e,
+      **kwargs,
     ),
-    # controls relies on planner commanding to move for stock-ACC resume spamming
     Maneuver(
+      'cruising at 25 m/s while disabled',
+      duration=20.,
+      initial_speed=25.,
+      lead_relevancy=False,
+      enabled=False,
+      **kwargs,
+    ),
+  ]
+  if not kwargs['force_decel']:
+    # controls relies on planner commanding to move for stock-ACC resume spamming
+    maneuvers.append(Maneuver(
       "resume from a stop",
       duration=20.,
       initial_speed=0.,
@@ -129,20 +140,16 @@ def create_maneuvers(e2e):
       speed_lead_values=[0., 0., 2.],
       breakpoints=[1., 10., 15.],
       ensure_start=True,
-      e2e=e2e,
-    ),
-    Maneuver(
-      'cruising at 25 m/s while disabled',
-      duration=20.,
-      initial_speed=25.,
-      lead_relevancy=False,
-      enabled=False,
-      e2e=e2e,
-    ),
-  ]
+      **kwargs,
+    ))
+  return maneuvers
 
 
+@parameterized_class(("e2e", "force_decel"), itertools.product([True, False], repeat=2))
 class LongitudinalControl(unittest.TestCase):
+  e2e: bool
+  force_decel: bool
+
   @classmethod
   def setUpClass(cls):
     os.environ['SIMULATION'] = "1"
@@ -154,11 +161,12 @@ class LongitudinalControl(unittest.TestCase):
     params.put_bool("Passive", bool(os.getenv("PASSIVE")))
     params.put_bool("OpenpilotEnabledToggle", True)
 
-  @parameterized.expand([(man,) for e2e in [True, False] for man in create_maneuvers(e2e)])
-  def test_maneuver(self, maneuver):
-    print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode')
-    valid, _ = maneuver.evaluate()
-    self.assertTrue(valid, msg=maneuver.title)
+  def test_maneuver(self):
+    for maneuver in create_maneuvers({"e2e": self.e2e, "force_decel": self.force_decel}):
+      with self.subTest(title=maneuver.title, e2e=maneuver.e2e, force_decel=maneuver.force_decel):
+        print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode')
+        valid, _ = maneuver.evaluate()
+        self.assertTrue(valid)
 
 
 if __name__ == "__main__":

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-6d30c77af7b3210b03f65b433c0a043a96ee39bc
+c39b74fdc14bb22a1c95cd5deedd4f4fcadf8494