mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 18:12:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

planner

Browse Source
This commit is contained in:
firestar5683
2026-05-04 09:55:38 -05:00
parent 41fef69a57
commit d7ec15bddb
2 changed files with 89 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+32 -7
View File
@@ -54,8 +54,14 @@ VISION_UNTRACKED_SLOW_LEAD_FULL_TTC = 8.0
VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME = 4.4
VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE = 80.0
VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE = 120.0
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_DISTANCE_TIME = 5.7
VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL = 0.85
VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL = 0.1
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MODEL_PROB = 0.68
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED = 8.0
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC = 10.0
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED = 10.0
VISION_UNTRACKED_SLOW_LEAD_RELAXED_FULL_CLOSING_SPEED = 16.0
VISION_SLOW_LEAD_MAX_SPEED = 5.0
VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5
VISION_SLOW_LEAD_TRIGGER_TTC = 4.5
@@ -363,8 +369,6 @@ class LongitudinalPlanner:
return None
lead_prob = float(getattr(lead, "modelProb", 0.0))
if lead_prob < VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB:
return None
lead_brake = max(0.0, -float(lead.aLeadK))
reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt)
@@ -377,21 +381,42 @@ class LongitudinalPlanner:
if closing_ratio < VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO:
return None
max_distance = float(np.clip(VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME * v_ego,
projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1)
if projected_ttc > VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC:
return None
min_model_prob = VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB
max_distance_time = VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME
if float(lead.vLead) <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED and \
projected_ttc <= VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC:
closing_relax = float(np.clip((projected_closing_speed - VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED) /
(VISION_UNTRACKED_SLOW_LEAD_RELAXED_FULL_CLOSING_SPEED -
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED), 0.0, 1.0))
ttc_relax = float(np.clip((VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC - projected_ttc) /
(VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC -
VISION_UNTRACKED_SLOW_LEAD_FULL_TTC), 0.0, 1.0))
relax_factor = closing_relax * ttc_relax
min_model_prob = float(np.interp(relax_factor, [0.0, 1.0],
[VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB,
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MODEL_PROB]))
max_distance_time = float(np.interp(relax_factor, [0.0, 1.0],
[VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME,
VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_DISTANCE_TIME]))
max_distance = float(np.clip(max_distance_time * v_ego,
VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE,
VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE))
if float(lead.dRel) > max_distance:
return None
projected_ttc = float(lead.dRel) / max(projected_closing_speed, 0.1)
if projected_ttc > VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC:
if lead_prob < min_model_prob:
return None
time_factor = float(np.clip((VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - projected_ttc) /
(VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC - VISION_UNTRACKED_SLOW_LEAD_FULL_TTC),
0.0, 1.0))
prob_factor = float(np.clip((lead_prob - VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB) /
(VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB - VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB),
prob_factor = float(np.clip((lead_prob - min_model_prob) /
(VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB - min_model_prob),
0.0, 1.0))
closing_factor = float(np.clip((closing_ratio - VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO) /
(VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO - VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO),
selfdrive/controls/tests/test_longitudinal_planner.py
+57
View File
@@ -268,6 +268,29 @@ def test_vision_untracked_slow_lead_cap_reaches_high_confidence_far_slower_lead_
assert not planner.raw_close_lead_needs_control(route_like_lead, v_ego)
def test_vision_untracked_slow_lead_cap_relaxes_confidence_for_near_stopped_high_closure_lead():
v_ego = 20.35
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
route_like_lead = make_lead(status=True, d_rel=115.4, v_lead=3.76, a_lead=0.0, radar=False, model_prob=0.70)
route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0)
assert route_cap is not None
assert route_cap < -0.55
def test_vision_untracked_slow_lead_cap_keeps_low_confidence_floor_for_less_threatening_lead():
v_ego = 20.35
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
less_threatening_lead = make_lead(status=True, d_rel=115.4, v_lead=9.5, a_lead=0.0, radar=False, model_prob=0.75)
assert planner.get_vision_untracked_slow_lead_cap(less_threatening_lead, v_ego, -1.0) is None
def test_vision_slow_stopped_lead_cap_brakes_earlier_for_confident_stop():
v_ego = 13.207
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
@@ -477,6 +500,40 @@ def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_track
assert lead_outputs[-1] < no_lead_outputs[-1] - 0.15
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
def test_acc_mode_pretracking_near_stopped_vision_lead_does_not_relax_when_confidence_is_midrange(model_version):
v_ego = 20.35
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego)
planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego)
sm_no_lead = make_sm(
v_ego,
desired_accel=0.2,
min_accel=-1.0,
experimental_mode=False,
tracking_lead=False,
)
sm_with_lead = make_sm(
v_ego,
desired_accel=0.2,
min_accel=-1.0,
experimental_mode=False,
tracking_lead=False,
lead_one=make_lead(status=True, d_rel=115.4, v_lead=3.76, a_lead=0.0, radar=False, model_prob=0.70),
)
sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0
sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0
for _ in range(8):
planner_no_lead.update(sm_no_lead, make_toggles(model_version))
planner_with_lead.update(sm_with_lead, make_toggles(model_version))
assert planner_with_lead.mode == "acc"
assert planner_with_lead.output_a_target < planner_no_lead.output_a_target - 0.12
assert planner_with_lead.output_a_target < -0.45
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
def test_acc_mode_tracked_pace_matched_lead_caps_positive_catchup(model_version):
v_ego = 28.7