diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6ded61183..f97353dfb 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -220,6 +220,12 @@ COMFORTABLE_PULLAWAY_FOLLOW_MIN_MODEL_PROB = 0.95 COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_DELTA = -0.05 COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_ACCEL = 0.20 COMFORTABLE_PULLAWAY_FOLLOW_MIN_HEADWAY_MARGIN = 0.20 +SPACIOUS_TRACKED_FOLLOW_MIN_MODEL_PROB = 0.98 +SPACIOUS_TRACKED_FOLLOW_MIN_HEADWAY_MARGIN = 0.45 +SPACIOUS_TRACKED_FOLLOW_MAX_CLOSING_SPEED = 0.60 +SPACIOUS_TRACKED_FOLLOW_MAX_LEAD_BRAKE = 0.10 +SPACIOUS_TRACKED_FOLLOW_LATCH_TIME = 1.25 +SPACIOUS_TRACKED_FOLLOW_LATCH_MIN_LEAD_DELTA = 0.90 LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_SPEED = 12.0 LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_MODEL_PROB = 0.85 LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_LEAD_BRAKE = 0.20 @@ -559,6 +565,7 @@ class LongitudinalPlanner: self.untracked_slow_lead_confirm_t = 0.0 self.manual_stop_resume_override_until = 0.0 self.lead_depart_accel_hold_until = 0.0 + self.spacious_follow_cap_bypass_until = 0.0 if self.is_preap: try: @@ -1297,6 +1304,38 @@ class LongitudinalPlanner: headway_margin = actual_headway - float(t_follow) return headway_margin >= COMFORTABLE_PULLAWAY_FOLLOW_MIN_HEADWAY_MARGIN + def is_spacious_low_closure_follow(self, lead, v_ego, t_follow): + if lead is None or not lead.status or float(v_ego) < CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED: + return False + + lead_radar = bool(getattr(lead, "radar", False)) + lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) + if not lead_radar and lead_prob < SPACIOUS_TRACKED_FOLLOW_MIN_MODEL_PROB: + return False + + if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET: + return False + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if lead_brake > SPACIOUS_TRACKED_FOLLOW_MAX_LEAD_BRAKE: + return False + + closing_speed = max(float(v_ego) - float(lead.vLead), 0.0) + if closing_speed > SPACIOUS_TRACKED_FOLLOW_MAX_CLOSING_SPEED: + return False + + if self.raw_close_lead_needs_control(lead, v_ego): + return False + + actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) + headway_margin = actual_headway - float(t_follow) + return headway_margin >= SPACIOUS_TRACKED_FOLLOW_MIN_HEADWAY_MARGIN + + def spacious_follow_cap_bypass_active(self, lead, v_ego, t_follow, tracking_lead_active): + if not tracking_lead_active or time.monotonic() > self.spacious_follow_cap_bypass_until: + return False + return self.is_spacious_low_closure_follow(lead, v_ego, t_follow) + def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow, current_source=None, tracking_lead_active=False): if lead is None or not lead.status: return None @@ -1331,6 +1370,8 @@ class LongitudinalPlanner: if current_source == "cruise" and tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow): return None + if current_source == "cruise" and self.spacious_follow_cap_bypass_active(lead, v_ego, t_follow, tracking_lead_active): + return None if not low_speed_follow_window and self.is_stable_post_departure_pullaway(lead, v_ego, t_follow): return None @@ -1405,6 +1446,8 @@ class LongitudinalPlanner: if tracking_lead_active and self.is_comfortable_accelerating_away_follow(lead, v_ego, t_follow): return None + if self.spacious_follow_cap_bypass_active(lead, v_ego, t_follow, tracking_lead_active): + return None closing_speed = max(float(v_ego) - float(lead.vLead), 0.0) raw_close_lead = self.raw_close_lead_needs_control(lead, v_ego) @@ -2100,6 +2143,21 @@ class LongitudinalPlanner: not recently_braked ) + if lead_one_active and self.mpc.source == "cruise": + lead_delta = float(self.lead_one.vLead) - float(scene_v_ego) + lead_brake = max(0.0, -float(getattr(self.lead_one, "aLeadK", 0.0))) + if ( + self.is_spacious_low_closure_follow(self.lead_one, scene_v_ego, effective_t_follow) and + lead_brake <= SPACIOUS_TRACKED_FOLLOW_MAX_LEAD_BRAKE and + ( + self.is_stable_post_departure_pullaway(self.lead_one, scene_v_ego, effective_t_follow) or + lead_delta >= SPACIOUS_TRACKED_FOLLOW_LATCH_MIN_LEAD_DELTA + ) + ): + self.spacious_follow_cap_bypass_until = now_t + SPACIOUS_TRACKED_FOLLOW_LATCH_TIME + elif not lead_one_active: + self.spacious_follow_cap_bypass_until = 0.0 + # Calculate scene uncertainty from model desire prediction entropy and disengage predictions uncertainty = 0.0 if hasattr(sm['modelV2'], 'meta'): diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 1b033bb9f..778da73ee 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1,3 +1,4 @@ +import time import sys import types from types import SimpleNamespace @@ -1932,6 +1933,46 @@ def test_route_8bc6_cruise_tracking_cap_skips_comfortable_accelerating_radar_fol assert cap is None +def test_route_687_voacc_catchup_cap_skips_spacious_low_closure_follow_with_flat_lead_accel(): + v_ego = 12.2 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10, + ) + planner.spacious_follow_cap_bypass_until = time.monotonic() + 1.0 + + cap = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.45, + current_source="cruise", + tracking_lead_active=True, + ) + + assert cap is None + + +def test_route_687_voacc_cruise_tracking_cap_skips_spacious_low_closure_follow_with_flat_lead_accel(): + v_ego = 12.2 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10, + ) + planner.spacious_follow_cap_bypass_until = time.monotonic() + 1.0 + + cap = planner.get_cruise_tracking_lead_accel_cap( + lead, + v_ego, + 1.45, + current_source="cruise", + tracking_lead_active=True, + ) + + assert cap is None + + def test_low_speed_follow_catchup_uses_raw_vehicle_speed_when_cluster_runs_high(): v_ego = 7.8 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)