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>
3 years ago · b45dda2d0a
parent 4e9bddee5c
commit b45dda2d0a
6 changed files with 51 additions and 38 deletions
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/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

--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/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)
--- a/selfdrive/test/longitudinal_maneuvers/maneuver.py
+++ b/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)
--- a/selfdrive/test/longitudinal_maneuvers/plant.py
+++ b/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

--- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
+++ b/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__":
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@ -1 +1 @@
-6d30c77af7b3210b03f65b433c0a043a96ee39bc
+c39b74fdc14bb22a1c95cd5deedd4f4fcadf8494