radar vs voacc

selfdrive/controls/tests/test_leads.py

@@ -1,13 +1,34 @@
 from types import SimpleNamespace
 
 import cereal.messaging as messaging
+from cereal import log
 
 from opendbc.car.toyota.values import CAR as TOYOTA
 from openpilot.selfdrive.test.process_replay import replay_process_with_name
-from openpilot.selfdrive.controls.radard import g90_low_speed_radar_lead_sane, g90_radar_lead_lateral_sane
+from openpilot.selfdrive.controls.radard import (
+  g90_low_speed_radar_lead_sane,
+  g90_radar_lead_lateral_sane,
+  match_vision_to_track,
+)
 
 
 class TestLeads:
+  @staticmethod
+  def make_track(d_rel: float, y_rel: float, v_rel: float, cnt: int = 5):
+    return SimpleNamespace(dRel=d_rel, yRel=y_rel, vRel=v_rel, cnt=cnt)
+
+  @staticmethod
+  def make_lead(x: float, y: float, v: float, x_std: float = 1.5, y_std: float = 0.3, v_std: float = 1.0):
+    return SimpleNamespace(x=[x], y=[y], v=[v], xStd=[x_std], yStd=[y_std], vStd=[v_std])
+
+  @staticmethod
+  def make_model_data():
+    model = log.ModelDataV2.new_message()
+    model.init('laneLines', 4)
+    model.meta.laneChangeState = 0
+    model.meta.laneChangeDirection = 0
+    return model
+
   def test_g90_radar_filters_side_tracks(self):
     side_track = SimpleNamespace(dRel=13.0, yRel=-10.38, cnt=10)
     centered_track = SimpleNamespace(dRel=10.8, yRel=-0.21, cnt=5)
@@ -22,6 +43,40 @@ class TestLeads:
     assert g90_low_speed_radar_lead_sane(centered_track, 2.0)
     assert not g90_low_speed_radar_lead_sane(far_low_speed_track, 3.5)
 
+  def test_match_vision_to_track_sticky_holds_preferred_track(self):
+    v_ego = 21.2
+    lead = self.make_lead(x=37.72, y=0.07, v=20.03)
+    preferred_track = self.make_track(d_rel=46.95, y_rel=0.05, v_rel=-0.58, cnt=8)
+    tracks = {3647: preferred_track}
+
+    track = match_vision_to_track(
+      v_ego,
+      lead,
+      self.make_model_data(),
+      tracks,
+      SimpleNamespace(human_lane_changes=False),
+      preferred_track_id=3647,
+    )
+
+    assert track is preferred_track
+
+  def test_match_vision_to_track_sticky_rejects_bad_preferred_track(self):
+    v_ego = 21.2
+    lead = self.make_lead(x=37.72, y=0.07, v=20.03)
+    preferred_track = self.make_track(d_rel=58.0, y_rel=2.8, v_rel=-4.5, cnt=8)
+    tracks = {3647: preferred_track}
+
+    track = match_vision_to_track(
+      v_ego,
+      lead,
+      self.make_model_data(),
+      tracks,
+      SimpleNamespace(human_lane_changes=False),
+      preferred_track_id=3647,
+    )
+
+    assert track is None
+
   def test_radar_fault(self):
     # if there's no radar-related can traffic, radard should either not respond or respond with an error
     # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check