diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 9b4d016430..16ba8492cc 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -16,6 +16,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.duration = duration self.title = title @@ -52,5 +53,10 @@ class Maneuver(): print("Crashed!!!!") valid = False + if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1: + print('Planner not starting!') + 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 13025a9f03..a1f53ad2db 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -28,6 +28,7 @@ class Plant(): self.distance = 0. self.speed = speed self.acceleration = 0.0 + self.speeds = [] # lead car self.distance_lead = distance_lead @@ -98,6 +99,7 @@ class Plant(): self.planner.update(sm) self.speed = self.planner.v_desired_filter.x self.acceleration = self.planner.a_desired + self.speeds = self.planner.v_desired_trajectory.tolist() fcw = self.planner.fcw self.distance_lead = self.distance_lead + v_lead * self.ts diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 698877dd3a..5ed8096e7b 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -3,6 +3,7 @@ import os import unittest from common.params import Params +from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -106,6 +107,17 @@ maneuvers = [ breakpoints=[1., 1.01, 11.], cruise_values=[float("nan"), 15., 15.], ), + # controls relies on planner commanding to move for stock-ACC resume spamming + Maneuver( + "resume from a stop", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=STOP_DISTANCE, + speed_lead_values=[0., 0., 5.], + breakpoints=[1., 10., 15.], + ensure_start=True, + ), ]