diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 42feadb46..f53ae2c3c 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -12,6 +12,9 @@ class Maneuver(): self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0]) self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration]) + self.only_lead2 = kwargs.get("only_lead2", False) + self.only_radar = kwargs.get("only_radar", False) + self.duration = duration self.title = title @@ -19,7 +22,9 @@ class Maneuver(): plant = Plant( lead_relevancy=self.lead_relevancy, speed=self.speed, - distance_lead=self.distance_lead + distance_lead=self.distance_lead, + only_lead2=self.only_lead2, + only_radar=self.only_radar ) valid = True diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 92b176133..0ec4f89fd 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -11,7 +11,8 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState class Plant(): messaging_initialized = False - def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0): + def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, + only_lead2=False, only_radar=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: @@ -30,12 +31,28 @@ class Plant(): # lead car self.distance_lead = distance_lead self.lead_relevancy = lead_relevancy + 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 time.sleep(1) self.sm = messaging.SubMaster(['longitudinalPlan']) + # make sure planner has time to be fully initialized + # TODO planner should just be explicitly initialized + for i in range(10000): + assert i < 10000 + radar = messaging.new_message('radarState') + control = messaging.new_message('controlsState') + car_state = messaging.new_message('carState') + control.controlsState.longControlState = LongCtrlState.pid + control.controlsState.vCruise = speed*3.6 + car_state.carState.vEgo = self.speed + Plant.radar.send(radar.to_bytes()) + Plant.controls_state.send(control.to_bytes()) + Plant.car_state.send(car_state.to_bytes()) + def current_time(self): return float(self.rk.frame) / self.rate @@ -51,11 +68,16 @@ class Plant(): if self.lead_relevancy: d_rel = np.maximum(0., self.distance_lead - self.distance) v_rel = v_lead - self.speed - prob = 1.0 + if self.only_radar: + prob = 0.0 + else: + prob = 1.0 + status = True else: d_rel = 200. v_rel = 0. prob = 0.0 + status = False lead = log.RadarState.LeadData.new_message() lead.dRel = float(d_rel) @@ -66,9 +88,10 @@ class Plant(): lead.vLeadK = float(v_lead) lead.aLeadK = float(a_lead) lead.aLeadTau = float(1.5) - lead.status = True + lead.status = status lead.modelProb = prob - radar.radarState.leadOne = lead + if not self.only_lead2: + radar.radarState.leadOne = lead radar.radarState.leadTwo = lead diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 3026510d9..e9df74ed4 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -61,6 +61,26 @@ maneuvers = [ speed_lead_values=[0., 0.], speed_lead_breakpoints=[1., 11.], ), + Maneuver( + "approach slower cut-in car at 20m/s", + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=50., + speed_lead_values=[15., 15.], + speed_lead_breakpoints=[1., 11.], + only_lead2=True, + ), + Maneuver( + "stay stopped behind radar override lead", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=10., + speed_lead_values=[0., 0.], + speed_lead_breakpoints=[1., 11.], + only_radar=True, + ), ]