diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f4268877e..737a3352a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -46,6 +46,15 @@ LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25 LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2 LEAD_DEPART_CONFIDENT_CONFIRM_TIME = 0.35 +STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED = 0.5 +STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED = 0.45 +STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA = 0.35 +STANDSTILL_STOPPED_LEAD_GUARD_MIN_MODEL_PROB = 0.95 +STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET = 1.75 +STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE = 3.0 +STANDSTILL_STOPPED_LEAD_GUARD_DISTANCE_MARGIN = 3.0 +STANDSTILL_STOPPED_LEAD_GUARD_MIN_BRAKE = 0.16 +STANDSTILL_STOPPED_LEAD_GUARD_MAX_BRAKE = 0.26 RADAR_DEPART_CONFLICT_MAX_EGO_SPEED = 1.6 RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL = 1.5 RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE = 18.0 @@ -1098,6 +1107,50 @@ class LongitudinalPlanner: 0.55 * lead_factor + 0.45 * gap_factor, 0.0, 1.0) return min(accel_cap, max(float(model_desired_accel), LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL)) + def get_standstill_stopped_lead_guard_cap(self, lead, v_ego, accel_min, stop_distance, + release_ready, confident_depart_ready): + if lead is None or not lead.status or release_ready or confident_depart_ready: + return None + if float(v_ego) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED: + return None + + 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 < STANDSTILL_STOPPED_LEAD_GUARD_MIN_MODEL_PROB: + return None + + if abs(float(getattr(lead, "yRel", 0.0))) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET: + return None + + lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) + lead_delta = lead_speed - float(v_ego) + max_distance = max( + STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE, + float(stop_distance) + STANDSTILL_STOPPED_LEAD_GUARD_DISTANCE_MARGIN, + ) + if ( + float(getattr(lead, "dRel", float("inf"))) > max_distance or + lead_speed > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED or + lead_delta > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA + ): + return None + + distance_factor = float(np.clip((max_distance - float(lead.dRel)) / + max(max_distance - STANDSTILL_STOPPED_LEAD_GUARD_MIN_DISTANCE, 0.1), + 0.0, 1.0)) + speed_factor = float(np.clip(lead_speed / max(STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_SPEED, 0.1), 0.0, 1.0)) + delta_factor = float(np.clip((STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA - lead_delta) / + max(STANDSTILL_STOPPED_LEAD_GUARD_MAX_LEAD_DELTA, 0.1), + 0.0, 1.0)) + hold_brake = STANDSTILL_STOPPED_LEAD_GUARD_MIN_BRAKE + 0.06 * distance_factor + 0.02 * speed_factor + 0.02 * delta_factor + hold_brake = float(np.clip( + hold_brake, + STANDSTILL_STOPPED_LEAD_GUARD_MIN_BRAKE, + STANDSTILL_STOPPED_LEAD_GUARD_MAX_BRAKE, + )) + brake_floor = -hold_brake + return brake_floor if accel_min >= 0.0 else max(accel_min, brake_floor) + def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow): if lead is None or not lead.status: return None @@ -2135,6 +2188,33 @@ class LongitudinalPlanner: self.confident_lead_depart_elapsed >= LEAD_DEPART_CONFIDENT_CONFIRM_TIME ) + 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): + release_ready = bool(lead_depart_ready or confident_depart_ready) + standstill_stopped_lead_guard_caps = [ + cap for cap in ( + self.get_standstill_stopped_lead_guard_cap( + self.lead_one, + float(sm['carState'].vEgo), + output_accel_min, + standstill_nudge_gap, + release_ready, + confident_depart_ready, + ), + self.get_standstill_stopped_lead_guard_cap( + self.lead_two, + float(sm['carState'].vEgo), + output_accel_min, + standstill_nudge_gap, + release_ready, + confident_depart_ready, + ), + ) if cap is not None + ] + if standstill_stopped_lead_guard_caps: + standstill_stopped_lead_guard_cap = min(standstill_stopped_lead_guard_caps) + output_should_stop = True + if lead_control_active and sm['carState'].standstill and moving_leads and not depart_safety_veto: output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL) @@ -2362,6 +2442,10 @@ class LongitudinalPlanner: self.a_desired = min(self.a_desired, close_stop_hold_cap) output_a_target = min(output_a_target, close_stop_hold_cap) + if standstill_stopped_lead_guard_cap is not None: + self.a_desired = min(self.a_desired, standstill_stopped_lead_guard_cap) + output_a_target = min(output_a_target, standstill_stopped_lead_guard_cap) + if close_settle_cap is not None: self.a_desired = min(self.a_desired, close_settle_cap) output_a_target = min(output_a_target, close_settle_cap) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 34eac31dc..18d1d6cd3 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1120,6 +1120,78 @@ def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_vers assert planner.output_a_target >= 0.2 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_stopped_lead_guard_blocks_false_release_at_longer_gap(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + + sm = make_sm( + 0.03, + desired_accel=1.85, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=8.15, v_lead=0.04, a_lead=0.0, radar=False, model_prob=0.99), + ) + sm["carState"].standstill = True + 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 planner.output_a_target <= 0.0 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_stopped_lead_guard_does_not_block_radar_depart_at_longer_gap(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.45, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=8.2, v_lead=0.72, a_lead=0.32, radar=True, model_prob=0.999, y_rel=0.1), + ) + 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 not planner.output_should_stop + assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_stopped_lead_guard_blocks_false_release_during_creep_frame(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.2) + + sm = make_sm( + 0.2, + desired_accel=1.15, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=7.9, v_lead=0.03, a_lead=0.0, radar=False, model_prob=0.99), + ) + 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 planner.output_a_target <= 0.0 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_confident_departing_lead_clears_stop_without_waiting_for_model_accel(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)