diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 216bd1d76..73fe63844 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -136,6 +136,17 @@ VISION_CLOSE_STOP_HOLD_MAX_DISTANCE = 3.5 VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB = 0.95 VISION_CLOSE_STOP_HOLD_MIN_BRAKE = 0.20 VISION_CLOSE_STOP_HOLD_MAX_BRAKE = 0.36 +VISION_CLOSE_SETTLE_MAX_EGO_SPEED = 0.75 +VISION_CLOSE_SETTLE_MAX_LEAD_SPEED = 2.75 +VISION_CLOSE_SETTLE_MAX_DISTANCE = 4.2 +VISION_CLOSE_SETTLE_MAX_LEAD_DELTA = 2.6 +VISION_CLOSE_SETTLE_MIN_BRAKE = 0.16 +VISION_CLOSE_SETTLE_MAX_BRAKE = 0.30 +VISION_CLOSE_FINAL_GUARD_MAX_EGO_SPEED = 0.5 +VISION_CLOSE_FINAL_GUARD_MAX_LEAD_SPEED = 2.75 +VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE = 4.5 +VISION_CLOSE_FINAL_GUARD_MIN_BRAKE = 0.18 +VISION_CLOSE_FINAL_GUARD_MAX_BRAKE = 0.28 VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED = 2.5 VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_SPEED = 3.5 VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE = 4.2 @@ -750,17 +761,27 @@ class LongitudinalPlanner: return None lead_speed = max(float(lead.vLead), 0.0) + near_standstill_settle = bool( + float(v_ego) <= VISION_CLOSE_SETTLE_MAX_EGO_SPEED and + lead_speed <= VISION_CLOSE_SETTLE_MAX_LEAD_SPEED and + float(lead.dRel) <= VISION_CLOSE_SETTLE_MAX_DISTANCE + ) + max_lead_speed = VISION_CLOSE_SETTLE_MAX_LEAD_SPEED if near_standstill_settle else VISION_CLOSE_STOP_HOLD_MAX_LEAD_SPEED + max_distance = VISION_CLOSE_SETTLE_MAX_DISTANCE if near_standstill_settle else VISION_CLOSE_STOP_HOLD_MAX_DISTANCE if ( float(v_ego) > VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED or - lead_speed > VISION_CLOSE_STOP_HOLD_MAX_LEAD_SPEED or - float(lead.dRel) > VISION_CLOSE_STOP_HOLD_MAX_DISTANCE + lead_speed > max_lead_speed or + float(lead.dRel) > max_distance ): return None - distance_factor = float(np.clip((VISION_CLOSE_STOP_HOLD_MAX_DISTANCE - float(lead.dRel)) / - max(VISION_CLOSE_STOP_HOLD_MAX_DISTANCE - 1.8, 0.1), 0.0, 1.0)) + distance_factor = float(np.clip((max_distance - float(lead.dRel)) / + max(max_distance - 1.8, 0.1), 0.0, 1.0)) speed_factor = float(np.clip(float(v_ego) / max(VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED, 0.1), 0.0, 1.0)) hold_brake = VISION_CLOSE_STOP_HOLD_MIN_BRAKE + 0.08 * distance_factor + 0.08 * speed_factor + if near_standstill_settle: + settle_brake = VISION_CLOSE_SETTLE_MIN_BRAKE + 0.10 * distance_factor + 0.04 * speed_factor + hold_brake = max(hold_brake, settle_brake) hold_brake = float(np.clip(hold_brake, VISION_CLOSE_STOP_HOLD_MIN_BRAKE, VISION_CLOSE_STOP_HOLD_MAX_BRAKE)) return max(accel_min, -hold_brake) @@ -774,25 +795,86 @@ class LongitudinalPlanner: lead_speed = max(float(lead.vLead), 0.0) lead_delta = lead_speed - float(v_ego) + near_standstill_settle = bool( + float(v_ego) <= VISION_CLOSE_SETTLE_MAX_EGO_SPEED and + lead_speed <= VISION_CLOSE_SETTLE_MAX_LEAD_SPEED and + float(lead.dRel) <= VISION_CLOSE_SETTLE_MAX_DISTANCE and + lead_delta <= VISION_CLOSE_SETTLE_MAX_LEAD_DELTA + ) + max_lead_speed = VISION_CLOSE_SETTLE_MAX_LEAD_SPEED if near_standstill_settle else VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_SPEED + max_distance = VISION_CLOSE_SETTLE_MAX_DISTANCE if near_standstill_settle else VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE + max_lead_delta = VISION_CLOSE_SETTLE_MAX_LEAD_DELTA if near_standstill_settle else VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA if ( float(v_ego) > VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED or - lead_speed > VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_SPEED or - float(lead.dRel) > VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE or + lead_speed > max_lead_speed or + float(lead.dRel) > max_distance or lead_delta < VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA or - lead_delta > VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA + lead_delta > max_lead_delta ): return None - distance_factor = float(np.clip((VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE - float(lead.dRel)) / - max(VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE - 2.8, 0.1), 0.0, 1.0)) + distance_factor = float(np.clip((max_distance - float(lead.dRel)) / + max(max_distance - 2.8, 0.1), 0.0, 1.0)) speed_factor = float(np.clip(float(v_ego) / max(VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED, 0.1), 0.0, 1.0)) - delta_factor = float(np.clip((VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA - lead_delta) / - max(VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA - VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA, 0.1), + delta_factor = float(np.clip((max_lead_delta - lead_delta) / + max(max_lead_delta - VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA, 0.1), 0.0, 1.0)) hold_brake = VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE + 0.12 * distance_factor + 0.06 * speed_factor + 0.04 * delta_factor + if near_standstill_settle: + settle_brake = VISION_CLOSE_SETTLE_MIN_BRAKE + 0.08 * distance_factor + 0.02 * delta_factor + hold_brake = max(hold_brake, settle_brake) hold_brake = float(np.clip(hold_brake, VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE, VISION_CLOSE_RELEASE_HOLD_MAX_BRAKE)) return max(accel_min, -hold_brake) + def get_vision_close_settle_cap(self, lead, v_ego, accel_min, stop_guard_active): + if lead is None or not lead.status or bool(getattr(lead, "radar", False)): + return None + + lead_prob = float(getattr(lead, "modelProb", 0.0)) + if lead_prob < VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB: + return None + + lead_speed = max(float(lead.vLead), 0.0) + lead_delta = lead_speed - float(v_ego) + if ( + float(v_ego) > VISION_CLOSE_SETTLE_MAX_EGO_SPEED or + lead_speed > VISION_CLOSE_SETTLE_MAX_LEAD_SPEED or + float(lead.dRel) > VISION_CLOSE_SETTLE_MAX_DISTANCE or + lead_delta > VISION_CLOSE_SETTLE_MAX_LEAD_DELTA + ): + return None + + if not stop_guard_active and lead_delta < 0.0: + return None + + distance_factor = float(np.clip((VISION_CLOSE_SETTLE_MAX_DISTANCE - float(lead.dRel)) / + max(VISION_CLOSE_SETTLE_MAX_DISTANCE - 2.8, 0.1), 0.0, 1.0)) + delta_factor = float(np.clip((VISION_CLOSE_SETTLE_MAX_LEAD_DELTA - lead_delta) / + max(VISION_CLOSE_SETTLE_MAX_LEAD_DELTA, 0.1), 0.0, 1.0)) + hold_brake = VISION_CLOSE_SETTLE_MIN_BRAKE + 0.10 * distance_factor + 0.04 * delta_factor + hold_brake = float(np.clip(hold_brake, VISION_CLOSE_SETTLE_MIN_BRAKE, VISION_CLOSE_SETTLE_MAX_BRAKE)) + return max(accel_min, -hold_brake) + + def get_vision_close_final_guard_cap(self, lead, v_ego, accel_min): + if lead is None or not lead.status or bool(getattr(lead, "radar", False)): + return None + + lead_prob = float(getattr(lead, "modelProb", 0.0)) + lead_speed = max(float(lead.vLead), 0.0) + if ( + lead_prob < VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB or + float(v_ego) > VISION_CLOSE_FINAL_GUARD_MAX_EGO_SPEED or + lead_speed > VISION_CLOSE_FINAL_GUARD_MAX_LEAD_SPEED or + float(lead.dRel) > VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE + ): + return None + + distance_factor = float(np.clip((VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE - float(lead.dRel)) / + max(VISION_CLOSE_FINAL_GUARD_MAX_DISTANCE - 2.8, 0.1), 0.0, 1.0)) + hold_brake = VISION_CLOSE_FINAL_GUARD_MIN_BRAKE + 0.10 * distance_factor + hold_brake = float(np.clip(hold_brake, VISION_CLOSE_FINAL_GUARD_MIN_BRAKE, VISION_CLOSE_FINAL_GUARD_MAX_BRAKE)) + return max(accel_min, -hold_brake) + def _update_manual_stop_resume_override(self, sm): now_t = time.monotonic() lead = sm["radarState"].leadOne @@ -1643,29 +1725,57 @@ class LongitudinalPlanner: float(sm['carState'].vEgo) <= LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED ) - if lead_control_active and output_should_stop: + close_stop_active = bool(output_should_stop or vision_low_speed_stop_active) + + close_stop_hold_cap = None + if lead_control_active and close_stop_active: close_stop_hold_caps = [ cap for cap in ( - self.get_vision_close_stop_hold_cap(self.lead_one, scene_v_ego, output_accel_min, output_should_stop), - self.get_vision_close_stop_hold_cap(self.lead_two, scene_v_ego, output_accel_min, output_should_stop), + self.get_vision_close_stop_hold_cap(self.lead_one, v_ego, output_accel_min, close_stop_active), + self.get_vision_close_stop_hold_cap(self.lead_two, v_ego, output_accel_min, close_stop_active), ) if cap is not None ] if close_stop_hold_caps: close_stop_hold_cap = min(close_stop_hold_caps) - self.a_desired = min(self.a_desired, close_stop_hold_cap) - output_a_target = min(output_a_target, close_stop_hold_cap) close_release_hold_cap = None - if lead_control_active and not output_should_stop: + if lead_control_active and not close_stop_active: close_release_hold_caps = [ cap for cap in ( - self.get_vision_close_release_hold_cap(self.lead_one, scene_v_ego, vision_cap_accel_min, output_should_stop), - self.get_vision_close_release_hold_cap(self.lead_two, scene_v_ego, vision_cap_accel_min, output_should_stop), + self.get_vision_close_release_hold_cap(self.lead_one, v_ego, vision_cap_accel_min, close_stop_active), + self.get_vision_close_release_hold_cap(self.lead_two, v_ego, vision_cap_accel_min, close_stop_active), ) if cap is not None ] if close_release_hold_caps: close_release_hold_cap = min(close_release_hold_caps) + close_settle_guard_active = bool( + output_should_stop or + vision_low_speed_stop_active or + sm['controlsState'].longControlState == LongCtrlState.stopping + ) + close_settle_cap = None + if lead_control_active: + close_settle_caps = [ + cap for cap in ( + self.get_vision_close_settle_cap(self.lead_one, v_ego, output_accel_min, close_settle_guard_active), + self.get_vision_close_settle_cap(self.lead_two, v_ego, output_accel_min, close_settle_guard_active), + ) if cap is not None + ] + if close_settle_caps: + close_settle_cap = min(close_settle_caps) + + close_final_guard_cap = None + if lead_control_active: + close_final_guard_caps = [ + cap for cap in ( + self.get_vision_close_final_guard_cap(self.lead_one, v_ego, output_accel_min), + self.get_vision_close_final_guard_cap(self.lead_two, v_ego, output_accel_min), + ) if cap is not None + ] + if close_final_guard_caps: + close_final_guard_cap = min(close_final_guard_caps) + if lead_one_active: lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(self.lead_one, scene_v_ego, effective_t_follow) if lead_catchup_accel_cap is not None: @@ -1736,6 +1846,18 @@ class LongitudinalPlanner: output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1] output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max)) + if close_stop_hold_cap is not None: + self.a_desired = min(self.a_desired, close_stop_hold_cap) + output_a_target = min(output_a_target, close_stop_hold_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) + + if close_final_guard_cap is not None: + self.a_desired = min(self.a_desired, close_final_guard_cap) + output_a_target = min(output_a_target, close_final_guard_cap) + if close_release_hold_cap is not None: self.a_desired = min(self.a_desired, close_release_hold_cap) output_a_target = min(output_a_target, close_release_hold_cap) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index bd4293c97..1095f2a72 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -862,6 +862,29 @@ def test_acc_mode_close_near_standstill_vision_lead_keeps_meaningful_brake_floor assert planner.output_a_target <= -0.20 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_acc_mode_close_near_standstill_moving_lead_keeps_brake_floor_while_should_stop(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.034) + + sm = make_sm( + 0.034, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=3.93, v_lead=1.61, a_lead=2.18, radar=False, model_prob=1.0), + ) + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = True + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_should_stop + assert planner.output_a_target <= -0.20 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_close_opening_vision_lead_does_not_drop_to_zero_after_stop_release(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -894,7 +917,41 @@ def test_acc_mode_close_opening_vision_lead_does_not_drop_to_zero_after_stop_rel sm_release["modelV2"].action.shouldStop = False planner.update(sm_release, toggles) - assert not planner.output_should_stop + assert planner.output_a_target <= -0.18 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_acc_mode_close_near_standstill_departing_lead_keeps_small_brake_after_stop_release(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.034) + toggles = make_toggles(model_version) + + sm_stop = make_sm( + 0.034, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=3.93, v_lead=1.61, a_lead=2.18, radar=False, model_prob=1.0), + ) + sm_stop["controlsState"].longControlState = LongCtrlState.stopping + sm_stop["starpilotPlan"].vCruise = 10.0 + sm_stop["modelV2"].action.shouldStop = True + planner.update(sm_stop, toggles) + + sm_release = make_sm( + 0.449, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=4.02, v_lead=2.45, a_lead=2.26, radar=False, model_prob=1.0), + ) + sm_release["controlsState"].longControlState = LongCtrlState.pid + sm_release["starpilotPlan"].vCruise = 10.0 + sm_release["modelV2"].action.shouldStop = False + planner.update(sm_release, toggles) + assert planner.output_a_target <= -0.18