mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-22 16:53:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

mica

Browse Source
This commit is contained in:
firestar5683
2026-06-23 20:00:29 -05:00
parent ea0ef444de
commit d6dc906ff8
2 changed files with 110 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/tests/test_longitudinal_planner.py
+59
View File
@@ -3052,6 +3052,65 @@ def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads():
assert lead_1_bias == 0.0
def test_duplicate_vision_comfort_lead_prefers_centered_candidate_before_closer_offset_candidate():
v_ego = 25.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10)
lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05)
lead_one.vRel = lead_one.vLead - v_ego
lead_two.vRel = lead_two.vLead - v_ego
planner.lead_one = lead_one
planner.lead_two = lead_two
selected = planner.get_duplicate_vision_comfort_lead(v_ego)
assert selected is lead_one
assert planner.duplicate_vision_comfort_lead_source == "lead0"
def test_duplicate_vision_comfort_lead_latches_source_until_duplicate_cluster_resolves():
v_ego = 25.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10)
lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05)
lead_one.vRel = lead_one.vLead - v_ego
lead_two.vRel = lead_two.vLead - v_ego
planner.lead_one = lead_one
planner.lead_two = lead_two
initial = planner.get_duplicate_vision_comfort_lead(v_ego)
lead_one.yRel = 1.25
lead_two.yRel = 0.05
lead_one.dRel = 44.4
lead_two.dRel = 44.5
latched = planner.get_duplicate_vision_comfort_lead(v_ego)
assert initial is lead_one
assert latched is lead_one
assert planner.duplicate_vision_comfort_lead_source == "lead0"
def test_duplicate_vision_comfort_lead_resets_when_duplicate_cluster_clears():
v_ego = 25.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10)
lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05)
lead_one.vRel = lead_one.vLead - v_ego
lead_two.vRel = lead_two.vLead - v_ego
planner.lead_one = lead_one
planner.lead_two = lead_two
assert planner.get_duplicate_vision_comfort_lead(v_ego) is lead_one
lead_two.dRel = 49.5
assert planner.get_duplicate_vision_comfort_lead(v_ego) is None
assert planner.duplicate_vision_comfort_lead_source is None
def test_near_duplicate_lead_transition_target_damps_same_source_sign_flip():
v_ego = 25.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)