diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index dc27fd27a9..c017951232 100644 --- 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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a0f6318323..0febfbafd9 100755 --- 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) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 65935f8979..00ddfe627e 100644 --- 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) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 8e150d800c..59cb0e1fe3 100755 --- 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 diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 686b35e456..bc477ca9fe 100755 --- 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__": diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 24ec1c62de..c23ff8a140 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -6d30c77af7b3210b03f65b433c0a043a96ee39bc \ No newline at end of file +c39b74fdc14bb22a1c95cd5deedd4f4fcadf8494