diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 87c53a822..06e92ccc8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -40,6 +40,7 @@ RAW_LEAD_LOW_SPEED_HOLD_MAX_LATERAL_OFFSET = 1.75 RAW_LEAD_LOW_SPEED_HOLD_MIN_CLOSING_SPEED = 0.15 STANDSTILL_LEAD_NUDGE_ACCEL = 0.05 STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0 +STANDSTILL_LEAD_NUDGE_MIN_LEAD_ACCEL = 0.2 STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.35 STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5 STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6 @@ -1363,7 +1364,7 @@ class LongitudinalPlanner: return None lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) - if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE: + if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE and not tracking_lead_active: return None if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET: @@ -2366,6 +2367,13 @@ class LongitudinalPlanner: moving_leads = [lead for lead in (self.lead_one, self.lead_two) if lead.status and lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap] + accelerating_nudge_lead = any( + lead.status and + float(getattr(lead, "vLead", 0.0)) > STANDSTILL_LEAD_NUDGE_MIN_SPEED and + float(getattr(lead, "aLeadK", 0.0)) >= STANDSTILL_LEAD_NUDGE_MIN_LEAD_ACCEL and + float(getattr(lead, "dRel", 0.0)) >= standstill_nudge_gap + for lead in (self.lead_one, self.lead_two) + ) confident_depart_detected = any(self.is_confident_lead_depart(lead, float(sm['carState'].vEgo)) for lead in (self.lead_one, self.lead_two)) lead_depart_ready = any( @@ -2396,7 +2404,8 @@ class LongitudinalPlanner: ) standstill_stopped_lead_guard_cap = None - if lead_control_active and (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED): + standstill_guard_lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two)) + if standstill_guard_lead_present and (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED): release_ready = bool(lead_depart_ready or confident_depart_ready) standstill_stopped_lead_guard_caps = [ cap for cap in ( @@ -2540,12 +2549,14 @@ class LongitudinalPlanner: if vision_brake_cap_active: output_accel_min = min(output_accel_min, vision_cap_accel_min) - follow_control_lead = self.get_follow_control_lead( - lead_control_active, - scene_v_ego, - effective_t_follow, - allow_optional_far_lead_logic=allow_complex_follow_logic, - ) + follow_control_lead = None + if allow_complex_follow_logic: + follow_control_lead = self.get_follow_control_lead( + lead_control_active, + scene_v_ego, + effective_t_follow, + allow_optional_far_lead_logic=True, + ) if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass: if not output_should_stop and not vision_low_speed_stop_active: tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor( @@ -2701,6 +2712,15 @@ class LongitudinalPlanner: self.a_desired = min(self.a_desired, low_speed_weak_lead_accel_cap) output_a_target = min(output_a_target, low_speed_weak_lead_accel_cap) + if ( + lead_control_active and + (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED) and + output_should_stop and + accelerating_nudge_lead and + not depart_safety_veto + ): + output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL) + force_stop_handoff = bool( getattr(sm['starpilotPlan'], 'forcingStop', False) and not lead_control_active and diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 14ca2ba1a..a1c636e16 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1149,6 +1149,78 @@ def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_ve assert planner.output_a_target < 0.1 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_accelerating_lead_keeps_small_nudge_while_stop_holds(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + + sm = make_sm( + 0.0, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=5.55, v_lead=0.02, a_lead=0.40, radar=False, model_prob=1.0), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_should_stop + assert longitudinal_planner_module.STANDSTILL_LEAD_NUDGE_ACCEL <= planner.output_a_target < 0.1 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_near_standstill_accelerating_lead_keeps_nudge_during_creep_frame(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.03) + + sm = make_sm( + 0.03, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=5.60, v_lead=0.06, a_lead=0.40, radar=False, model_prob=1.0), + ) + sm["carState"].standstill = False + sm["controlsState"].longControlState = LongCtrlState.pid + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_should_stop + assert longitudinal_planner_module.STANDSTILL_LEAD_NUDGE_ACCEL <= planner.output_a_target < 0.1 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_tiny_opening_lead_without_accel_does_not_get_nudge_floor(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + + sm = make_sm( + 0.0, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=5.55, v_lead=0.02, a_lead=0.0, radar=False, model_prob=1.0), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_should_stop + assert planner.output_a_target <= 0.0 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version): v_ego = 0.0