mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-21 00:33:44 +08:00
feat(long): comfort stop
This commit is contained in:
rav4kumar
@@ -307,6 +307,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
decelTarget @4 :Float32; # early-soft comfort decel target (m/s^2, negative)
|
||||
smoothActive @5 :Bool; # early-soft braking currently shaping the target
|
||||
bypassed @6 :Bool; # passthrough to stock plan (hard brake / FCW / should_stop / closing lead / e2e)
|
||||
comfortStopActive @7 :Bool; # low-speed comfort decel-to-stop floor currently governing (behind a near-stopped lead)
|
||||
comfortStopFloor @8 :Float32; # comfort-stop floor commanded (m/s^2, negative; 0 when not engaged)
|
||||
}
|
||||
|
||||
enum AccelerationPersonality {
|
||||
|
||||
@@ -4,11 +4,13 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
|
||||
Acceleration personality: per-profile launch/cruise accel ceiling (ECO/NORMAL/SPORT) plus an
|
||||
anticipatory brake front-load. SAFETY INVARIANT: on the brake side the output is NEVER WEAKER than the
|
||||
plan -- it can only be EQUAL or DEEPER (front-load). It never softens, delays, or rate-limits a brake,
|
||||
so it can never under-brake a closing lead. Hard brakes, stops and low speed pass the plan straight
|
||||
through (stock). Disabled => byte-stock.
|
||||
Acceleration personality: per-profile launch/cruise accel ceiling (ECO/NORMAL/SPORT), an anticipatory
|
||||
brake front-load, and a low-speed comfort stop. SAFETY: a firm/closing brake -- emergency (raw <=
|
||||
HARD_BRAKE_TARGET_ACCEL or brake_need >= HARD_BRAKE_NEED), FCW/crash, should_stop, or blended/e2e -- passes
|
||||
the plan straight through at full strength and rate, never softened/delayed/rate-limited. Only on the
|
||||
NON-emergency comfort path may the onset arrive spread by at most ONSET_SPREAD_MAX (a tightly bounded,
|
||||
transient lag) so a gentle brake does not land as a step. The front-load and comfort stop only ever ADD
|
||||
braking (min(., plan)). Disabled => byte-stock.
|
||||
"""
|
||||
|
||||
from collections.abc import Sequence
|
||||
@@ -25,8 +27,9 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
|
||||
STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, SMOOTH_DECEL_BP, SMOOTH_DECEL_V, BRAKE_DEEPENING_JERK, \
|
||||
BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, MIN_SMOOTH_BRAKE_NEED, \
|
||||
HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, OVERBITE_CAP, STOP_PASSTHROUGH_V, \
|
||||
STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, \
|
||||
STOP_ENFORCE_V, STOP_ENFORCE_DIST, STOP_ENFORCE_RANGE, STOP_ENFORCE_LEAD_V, STOP_ENFORCE_MAX_DECEL, STOP_ENFORCE_MIN_GAP
|
||||
STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, ONSET_SPREAD_MAX, ONSET_SPREAD_JERK, \
|
||||
COMFORT_STOP_V, COMFORT_STOP_LEAD_V, COMFORT_STOP_GAP, COMFORT_STOP_MIN_GAP, \
|
||||
COMFORT_STOP_MAX_DECEL, COMFORT_STOP_JERK, COMFORT_STOP_RELEASE_V, COMFORT_STOP_HOLD_GAP
|
||||
|
||||
_ZERO_ACCEL_EPS = 1e-6
|
||||
|
||||
@@ -48,6 +51,7 @@ class AccelController:
|
||||
self._lead_status = False
|
||||
self._lead_d = 0.0
|
||||
self._lead_vlead = 0.0
|
||||
self._stop_floor = 0.0 # comfort-stop floor latch (monotone within a stop episode, eased on release)
|
||||
self._read_params()
|
||||
|
||||
def _read_params(self) -> None:
|
||||
@@ -88,11 +92,11 @@ class AccelController:
|
||||
self._smooth_active = False
|
||||
self._bypassed = False
|
||||
|
||||
out = self._shape(raw, should_stop, reset, speed_trajectory, t_idxs)
|
||||
out = self._stop_enforce(out) # never-weaker low-speed floor: no creep inside the target stop gap
|
||||
out = self._shape(raw, should_stop, reset, speed_trajectory, t_idxs, stock_brake)
|
||||
out = self._comfort_stop(out, reset) # low-speed monotone comfort decel-to-stop (replaces the self-releasing enforcer)
|
||||
return self._finalize(out)
|
||||
|
||||
def _shape(self, raw: float, should_stop: bool, reset: bool, speed_trajectory, t_idxs) -> float:
|
||||
def _shape(self, raw: float, should_stop: bool, reset: bool, speed_trajectory, t_idxs, stock_brake: bool) -> float:
|
||||
# --- Full stock passthroughs (output is exactly the plan, no shaping) ---
|
||||
if reset or not self._enabled:
|
||||
return raw # disabled / reset
|
||||
@@ -100,29 +104,61 @@ class AccelController:
|
||||
return raw # stop/creep regime: braking is stock (no coast-in)
|
||||
self._bypassed = self._emergency_bypass(raw, should_stop)
|
||||
if self._bypassed or self._stop_imminent(speed_trajectory, t_idxs):
|
||||
return raw # hard brake / coming stop: full strength, no delay
|
||||
return raw # emergency / coming stop: full strength, no delay
|
||||
|
||||
# Anticipatory front-load, capped at OVERBITE_CAP below the live plan (avoids an abrupt over-bite on a
|
||||
# cut-in brake_need spike). min(., raw) keeps the output never weaker than the plan -> never under-brakes.
|
||||
# cut-in brake_need spike).
|
||||
target = raw
|
||||
if self._brake_need >= MIN_SMOOTH_BRAKE_NEED:
|
||||
self._smooth_active = True
|
||||
self._decel_target = max(self.get_decel_target(self._brake_need), raw - OVERBITE_CAP)
|
||||
target = min(raw, self._decel_target)
|
||||
if raw > 0.0:
|
||||
target = max(target, 0.0) # plan wants throttle -> ease the gas early, never fabricate a brake
|
||||
slewed = self._slew(target)
|
||||
return min(slewed, raw) if raw < 0.0 else slewed
|
||||
if raw >= 0.0:
|
||||
return slewed
|
||||
if stock_brake:
|
||||
return min(slewed, raw) # blended/e2e: the model owns the brake -> strict never-weaker
|
||||
return self._onset_spread(slewed, raw) # non-emergency brake: bounded onset spread (<= ONSET_SPREAD_MAX weaker)
|
||||
|
||||
def _stop_enforce(self, out: float) -> float:
|
||||
# Never-weaker low-speed floor: bring the car to rest at STOP_ENFORCE_DIST behind a near-stopped lead,
|
||||
# so the stock MPC's crawl-creep cannot park us inside the target gap. Disabled => no-op (off==stock).
|
||||
if not (self._enabled and self._lead_status and 0.1 < self._v_ego < STOP_ENFORCE_V
|
||||
and self._lead_vlead < STOP_ENFORCE_LEAD_V
|
||||
and 0.1 < self._lead_d < STOP_ENFORCE_DIST + STOP_ENFORCE_RANGE): # only the final-approach creep zone
|
||||
def _onset_spread(self, shaped: float, raw: float) -> float:
|
||||
# Scoped softening: on a NON-emergency brake the onset may arrive spread instead of stepping to the plan.
|
||||
# The output deepens toward the plan jerk-limited at ONSET_SPREAD_JERK and may lag it by at most
|
||||
# ONSET_SPREAD_MAX -- a tightly bounded, transient weaker-than-plan window that smooths the felt onset.
|
||||
# Emergency brakes never reach here (raw passthrough in _shape), so a genuine hard brake is never softened.
|
||||
# The front-load still wins when it is deeper (anticipation preserved).
|
||||
spread = max(raw, self._last_target_accel - ONSET_SPREAD_JERK * DT_MDL) # deepen toward the plan, jerk-limited
|
||||
spread = min(spread, raw + ONSET_SPREAD_MAX) # never more than the bounded lag weaker
|
||||
return min(shaped, spread)
|
||||
|
||||
def _comfort_stop(self, out: float, reset: bool) -> float:
|
||||
# Low-speed comfort decel-to-stop behind a near-stopped lead. Unlike the old enforcer it slews IN (no entry
|
||||
# grab) and low-passes raw-radar dRel (deepening rate-limited). It tracks the kinematic decel a_req both ways
|
||||
# while approaching, BUT holds strictly monotone (never weakens) inside the final-approach window so it cannot
|
||||
# self-release into a roll; outside that window it may weaken at the release rate when a creeping lead pulls
|
||||
# away (no phantom brake into an opening gap). min(out, floor) keeps it never weaker than the plan. Off => no-op.
|
||||
if reset or not self._enabled:
|
||||
self._stop_floor = 0.0 # disengaged/disabled: drop the latch, pure passthrough
|
||||
return out
|
||||
gap = self._lead_d - STOP_ENFORCE_DIST # distance left before reaching the target gap
|
||||
floor = -(self._v_ego ** 2) / (2.0 * max(gap, STOP_ENFORCE_MIN_GAP)) # gentle decel to stop at the target
|
||||
floor = max(floor, STOP_ENFORCE_MAX_DECEL) # cap -> gentle hold, never a grab
|
||||
return min(out, floor) # never weaker than the plan
|
||||
engaged = (self._lead_status and self._lead_vlead < COMFORT_STOP_LEAD_V
|
||||
and self._lead_d > 0.1 and self._v_ego < COMFORT_STOP_V)
|
||||
if engaged and self._v_ego >= COMFORT_STOP_RELEASE_V:
|
||||
gap = self._lead_d - COMFORT_STOP_GAP
|
||||
a_req = max(-(self._v_ego ** 2) / (2.0 * max(gap, COMFORT_STOP_MIN_GAP)), COMFORT_STOP_MAX_DECEL)
|
||||
lo = self._stop_floor - COMFORT_STOP_JERK * DT_MDL # deepest allowed this frame (slew-in, no grab)
|
||||
hi = self._stop_floor + BRAKE_RELEASE_JERK * DT_MDL # shallowest allowed this frame (release rate)
|
||||
tracked = min(hi, max(lo, a_req)) # track a_req, rate-limited both directions
|
||||
if gap > COMFORT_STOP_HOLD_GAP:
|
||||
self._stop_floor = min(0.0, tracked) # gap still open -> may weaken if the lead pulls away
|
||||
else:
|
||||
self._stop_floor = min(tracked, self._stop_floor) # final approach -> strict monotone hold (no roll)
|
||||
else:
|
||||
# Stop episode over (lead moving / launched / standstill handoff): ease the floor toward 0 at the release
|
||||
# jerk. This matches _shape's own _slew_up release rate (BRAKE_RELEASE_JERK), so the floor decays in
|
||||
# lockstep with the natural output -> no added launch drag, and no release-direction step (no snap).
|
||||
self._stop_floor = min(0.0, self._stop_floor + BRAKE_RELEASE_JERK * DT_MDL)
|
||||
return min(out, self._stop_floor) if self._stop_floor < 0.0 else out
|
||||
|
||||
def _stop_imminent(self, speed_trajectory: Sequence[float] | None, t_idxs: Sequence[float]) -> bool:
|
||||
# plan predicts a near-stop within the lookahead -> a stop is coming (lead or light/sign).
|
||||
@@ -193,3 +229,9 @@ class AccelController:
|
||||
|
||||
def bypassed(self) -> bool:
|
||||
return self._bypassed
|
||||
|
||||
def comfort_stop_floor(self) -> float:
|
||||
return self._stop_floor
|
||||
|
||||
def comfort_stop_active(self) -> bool:
|
||||
return self._stop_floor < 0.0
|
||||
|
||||
@@ -28,20 +28,21 @@ A_CRUISE_MAX_V = {
|
||||
RISE_RATE = {ECO: 0.10, NORMAL: 0.15, SPORT: 0.22} # ceiling open-rate: all >> stock 0.05 for fast take-off
|
||||
|
||||
# Anticipatory front-load: predicted brake need (m/s^2) -> early decel target (m/s^2). Starts a gentle
|
||||
# decel early when a brake is predicted, so it arrives spread out, not as one late firm onset. One-sided
|
||||
# (never weaker than the plan).
|
||||
SMOOTH_DECEL_BP = [0.0, 0.4, 0.8, 1.2, 1.6, 2.0, 2.4]
|
||||
# decel early when a brake is predicted, so it arrives spread out, not as one late firm onset. The first
|
||||
# knot sits AT the MIN_SMOOTH_BRAKE_NEED gate (0.00 there): below the gate there is no front-load, so there
|
||||
# is no dead [0, gate) anchor and no step at the gate (the old [0.0 -> 0.00] knot was never evaluated).
|
||||
SMOOTH_DECEL_BP = [0.4, 0.8, 1.2, 1.6, 2.0, 2.4]
|
||||
SMOOTH_DECEL_V = {
|
||||
ECO: [0.00, -0.08, -0.20, -0.35, -0.55, -0.78, -1.00],
|
||||
NORMAL: [0.00, -0.13, -0.30, -0.55, -0.84, -1.12, -1.40],
|
||||
SPORT: [0.00, -0.17, -0.40, -0.72, -1.05, -1.35, -1.65],
|
||||
ECO: [0.00, -0.20, -0.35, -0.55, -0.78, -1.00],
|
||||
NORMAL: [0.00, -0.30, -0.55, -0.84, -1.12, -1.40],
|
||||
SPORT: [0.00, -0.40, -0.72, -1.05, -1.35, -1.65],
|
||||
}
|
||||
BRAKE_DEEPENING_JERK = {ECO: 0.5, NORMAL: 0.8, SPORT: 1.0}
|
||||
BRAKE_RELEASE_JERK = 2.0
|
||||
ACCEL_RISE_JERK = {ECO: 1.0, NORMAL: 1.5, SPORT: 2.2} # accel-onset jerk: higher = snappier take-off, stepped per tier
|
||||
|
||||
SMOOTH_DECEL_LOOKAHEAD_T = 3.0
|
||||
MIN_SMOOTH_BRAKE_NEED = 0.2
|
||||
MIN_SMOOTH_BRAKE_NEED = 0.4 # below this no front-load (kills the faint low-brake_need drag + the gate-crossing toggle)
|
||||
|
||||
# Cap how much DEEPER than the live plan the front-load may bite -> no abrupt over-bite on a cut-in
|
||||
# brake_need spike (binds only when the plan still wants throttle; once it brakes, the table wins).
|
||||
@@ -59,15 +60,29 @@ HARD_BRAKE_NEED = 2.6
|
||||
STOP_IMMINENT_VEGO = 1.0 # m/s plan-predicted speed below this within the lookahead == stop coming
|
||||
STOP_IMMINENT_LOOKAHEAD_T = 3.0 # s
|
||||
|
||||
# Below this ego speed the brake side is stock passthrough, so stop distance is byte-identical to off.
|
||||
# Below this ego speed the brake side is stock passthrough (the comfort stop below adds the only low-speed
|
||||
# shaping); the bounded onset-spread does not run here, so a stock stop is not rate-limited.
|
||||
STOP_PASSTHROUGH_V = 5.0 # m/s
|
||||
|
||||
# Low-speed stop-distance enforcer. The stock MPC loses gap-cost leverage at crawl and creeps inside
|
||||
# STOP_DISTANCE behind a stopped lead. This is a never-weaker floor: command the gentle decel that brings
|
||||
# the car to rest at STOP_ENFORCE_DIST and take min(plan, floor) -> only ever adds braking, self-targeting.
|
||||
STOP_ENFORCE_V = 5.0 # m/s: only enforce at/below this ego speed
|
||||
STOP_ENFORCE_DIST = 5.5 # m: target standstill gap (under STOP_DISTANCE=6 for the radar rear-of-lead offset)
|
||||
STOP_ENFORCE_RANGE = 3.0 # m: only within DIST+RANGE of the lead (final-approach creep zone)
|
||||
STOP_ENFORCE_LEAD_V = 1.5 # m/s: only behind a near-stopped lead
|
||||
STOP_ENFORCE_MAX_DECEL = -1.8 # m/s^2: cap -> always a gentle hold, never a grab
|
||||
STOP_ENFORCE_MIN_GAP = 0.5 # m: kinematic denominator floor
|
||||
# Scoped onset-spread -- the ONLY place the output may be transiently WEAKER than the plan. On a NON-emergency
|
||||
# brake the onset may arrive spread over a bounded ramp instead of stepping straight to the plan: the output
|
||||
# may lag the plan by at most ONSET_SPREAD_MAX, deepening toward it at ONSET_SPREAD_JERK. A firm/closing brake
|
||||
# (raw <= HARD_BRAKE_TARGET_ACCEL or brake_need >= HARD_BRAKE_NEED, FCW/crash, should_stop, blended/e2e) skips
|
||||
# this entirely (raw passthrough), so a real hard brake is never softened or delayed.
|
||||
ONSET_SPREAD_MAX = 0.25 # m/s^2: max the output may lag (be weaker than) the live plan, non-emergency only
|
||||
ONSET_SPREAD_JERK = 2.5 # m/s^3: rate the spread output deepens back toward the plan
|
||||
|
||||
# Low-speed comfort stop. Behind a (near-)stopped lead, bring the car to rest at COMFORT_STOP_GAP with a
|
||||
# MONOTONE decel that slews IN (no entry grab) and never self-releases early (so the car does not roll the
|
||||
# final metre). min(plan, floor) keeps it never weaker than the plan; the monotone + slew-in also low-passes
|
||||
# raw-radar dRel steps (a farther/noisier dRel can only be ignored, never injected as a deeper grab). Replaces
|
||||
# the old self-releasing v^2/(2*gap) enforcer, which grabbed at v~3 then released into a roll. Off => no-op.
|
||||
COMFORT_STOP_V = 4.0 # m/s: only engage at/below this ego speed
|
||||
COMFORT_STOP_LEAD_V = 1.0 # m/s: only behind a (near-)stopped lead
|
||||
COMFORT_STOP_GAP = 5.0 # m: target standstill gap (radar dRel); roomier than the stock crawl-in (~3.7-4.4m)
|
||||
COMFORT_STOP_MIN_GAP = 1.0 # m: kinematic denominator floor (gentle; no 1/x blow-up near the target)
|
||||
COMFORT_STOP_MAX_DECEL = -1.6 # m/s^2: gentle cap -> never a grab
|
||||
COMFORT_STOP_JERK = 1.0 # m/s^3: slew-IN / deepen rate of the comfort floor (no step on engage)
|
||||
COMFORT_STOP_RELEASE_V = 0.3 # m/s: below this, ease the floor out (release jerk) -> smooth stock standstill handoff
|
||||
COMFORT_STOP_HOLD_GAP = 2.0 # m: within this of the target gap = final approach -> strict monotone hold (no roll);
|
||||
# beyond it the floor may WEAKEN at the release rate if a creeping lead pulls away
|
||||
|
||||
+94
-24
@@ -14,7 +14,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
|
||||
ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE, \
|
||||
STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, OVERBITE_CAP, \
|
||||
STOP_PASSTHROUGH_V, AccelerationPersonality
|
||||
STOP_PASSTHROUGH_V, ONSET_SPREAD_MAX, COMFORT_STOP_MAX_DECEL, AccelerationPersonality
|
||||
|
||||
T_IDXS = [0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.25, 1.5, 1.75, 2.0, 2.5, 3.0, 4.0]
|
||||
_EPS = 1e-6
|
||||
@@ -95,12 +95,15 @@ def test_rise_rate_ordering():
|
||||
|
||||
@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT])
|
||||
def test_never_weaker_than_plan_sustained(personality):
|
||||
# Core safety invariant: on the brake side the output is NEVER weaker than the plan (only equal or deeper).
|
||||
# Safety: an EMERGENCY brake is never weaker than the plan (strict). A non-emergency brake may lag the plan
|
||||
# by at most ONSET_SPREAD_MAX (the bounded onset-spread) and no more.
|
||||
ctrl = make_controller(personality=personality)
|
||||
for raw in [0.0, -0.2, -0.5, -0.9, -1.2, -1.5, -2.0] + [-2.0] * 20:
|
||||
out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
|
||||
if raw < 0.0:
|
||||
assert out <= raw + _EPS
|
||||
if raw <= HARD_BRAKE_TARGET_ACCEL:
|
||||
assert out <= raw + _EPS # emergency: strict never-weaker
|
||||
elif raw < 0.0:
|
||||
assert out <= raw + ONSET_SPREAD_MAX + _EPS # non-emergency: bounded onset-spread only
|
||||
|
||||
|
||||
@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT])
|
||||
@@ -112,7 +115,7 @@ def test_never_weaker_random_walk(personality):
|
||||
traj = flat_traj(raw - float(rng.uniform(0.0, 0.6)))
|
||||
out = ctrl.smooth_target_accel(raw, traj, T_IDXS, should_stop=False)
|
||||
if raw < 0.0:
|
||||
assert out <= raw + _EPS
|
||||
assert out <= raw + ONSET_SPREAD_MAX + _EPS # never more than the bounded onset-spread weaker
|
||||
|
||||
|
||||
@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT])
|
||||
@@ -226,42 +229,109 @@ def test_stop_imminent_passthrough_but_moving_follow_shapes():
|
||||
assert ctrl.smooth_active()
|
||||
|
||||
|
||||
def test_stop_enforce_brakes_when_mpc_creeps_inside_target():
|
||||
# Crawl behind a stopped lead inside the target gap: the stock plan eases off (~ -0.1), but the enforcer
|
||||
# holds a gentle decel to stop at the target -> output is DEEPER than the easing plan (no creep-in).
|
||||
def test_comfort_stop_brakes_approaching_stopped_lead():
|
||||
# Approaching a near-stopped lead at low speed: the comfort floor adds a gentle decel below the easing plan
|
||||
# so the car stops cleanly instead of crawling in, and stays within the gentle cap (never a grab).
|
||||
ctrl = make_controller(personality=ECO)
|
||||
ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=4.0, lead_vlead=0.0)) # inside 5.5m target, moving
|
||||
out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
|
||||
assert out < -0.1 - _EPS # enforcer added braking vs the easing plan
|
||||
assert out >= -2.0 - _EPS # but gentle (capped), never a grab
|
||||
ctrl.update(make_sm(v_ego=2.5, lead_status=True, lead_d=6.0, lead_vlead=0.0))
|
||||
out = 0.0
|
||||
for _ in range(30):
|
||||
out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
|
||||
assert out < -0.1 - _EPS # deeper than the easing plan (no creep-in)
|
||||
assert out >= COMFORT_STOP_MAX_DECEL - _EPS # but gentle (capped), never a grab
|
||||
|
||||
|
||||
def test_stop_enforce_off_when_disabled():
|
||||
# Disabled controller: enforcer is a no-op (off == stock).
|
||||
def test_comfort_stop_slews_in_no_grab():
|
||||
# First engaged frame must NOT step to the cap -- the floor slews in from 0 (no entry grab / jerk).
|
||||
ctrl = make_controller(personality=ECO)
|
||||
ctrl.update(make_sm(v_ego=2.5, lead_status=True, lead_d=6.0, lead_vlead=0.0))
|
||||
first = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
|
||||
assert first > -0.2 # ~ -0.1 plan + a tiny slewed-in floor, not -1.6
|
||||
|
||||
|
||||
def test_comfort_stop_monotone_no_early_release():
|
||||
# While still moving, the comfort floor never WEAKENS frame-to-frame (the old enforcer self-released -> roll).
|
||||
ctrl = make_controller(personality=ECO)
|
||||
floors = []
|
||||
for v in [3.0, 2.6, 2.2, 1.8, 1.4, 1.0, 0.6]: # decelerating toward the lead
|
||||
ctrl.update(make_sm(v_ego=v, lead_status=True, lead_d=max(0.5, 7.0 - (3.0 - v) * 2), lead_vlead=0.0))
|
||||
ctrl.smooth_target_accel(-0.5, flat_traj(-0.5), T_IDXS, should_stop=False)
|
||||
floors.append(ctrl._stop_floor)
|
||||
for a, b in zip(floors, floors[1:], strict=False):
|
||||
assert b <= a + _EPS # monotone non-weakening while approaching
|
||||
|
||||
|
||||
def test_comfort_stop_off_when_disabled():
|
||||
ctrl = make_controller(enabled=False, personality=ECO)
|
||||
ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=4.0, lead_vlead=0.0))
|
||||
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=4.0, lead_vlead=0.0))
|
||||
out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
|
||||
assert out == pytest.approx(-0.1, abs=_EPS)
|
||||
|
||||
|
||||
def test_stop_enforce_no_op_past_target_and_moving_lead():
|
||||
# Past the target gap (lead far): no enforcement. Moving lead: no enforcement (only near-stopped leads).
|
||||
def test_comfort_stop_no_op_moving_lead():
|
||||
# Moving lead (vLead high): no comfort stop (only behind a near-stopped lead).
|
||||
ctrl = make_controller(personality=ECO)
|
||||
ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=12.0, lead_vlead=0.0)) # far -> no floor
|
||||
assert ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False) == pytest.approx(-0.1, abs=_EPS)
|
||||
ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=4.0, lead_vlead=4.0)) # moving lead -> no floor
|
||||
assert ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False) == pytest.approx(-0.1, abs=_EPS)
|
||||
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=6.0, lead_vlead=5.0))
|
||||
out = ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
|
||||
assert out == pytest.approx(-0.1, abs=_EPS)
|
||||
|
||||
|
||||
def test_stop_enforce_never_weaker():
|
||||
# The enforcer only ever ADDS braking: output is never weaker than the plan.
|
||||
def test_comfort_stop_never_weaker():
|
||||
# The comfort floor only ever ADDS braking: output never weaker than the plan.
|
||||
ctrl = make_controller(personality=ECO)
|
||||
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=4.5, lead_vlead=0.0))
|
||||
for raw in (-0.05, -0.3, -1.0, -2.5):
|
||||
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=5.5, lead_vlead=0.0))
|
||||
out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
|
||||
assert out <= raw + _EPS
|
||||
|
||||
|
||||
def test_comfort_stop_weakens_when_gap_opens():
|
||||
# Creeping stop-and-go lead (vLead stays < COMFORT_STOP_LEAD_V) that pulls away: once the gap opens well past
|
||||
# the target the floor must WEAKEN, not hold a phantom brake into an opening gap.
|
||||
ctrl = make_controller(personality=ECO)
|
||||
for _ in range(15): # approach close -> deep floor (final-approach hold)
|
||||
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=5.5, lead_vlead=0.3))
|
||||
ctrl.smooth_target_accel(-0.5, flat_traj(-0.5), T_IDXS, should_stop=False)
|
||||
deep = ctrl._stop_floor
|
||||
assert deep < -0.3
|
||||
for _ in range(25): # lead creeps away (still vLead<1): gap opens wide
|
||||
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=12.0, lead_vlead=0.5))
|
||||
ctrl.smooth_target_accel(-0.05, flat_traj(-0.05), T_IDXS, should_stop=False)
|
||||
assert ctrl._stop_floor > deep + 0.3 # floor weakened as the gap opened (no phantom brake)
|
||||
|
||||
|
||||
def test_comfort_stop_releases_on_launch():
|
||||
# Stop-and-go GO: after holding a comfort floor at a stop, once the lead moves and the plan wants throttle the
|
||||
# floor must release (track the plan up) and not hold the output below the natural plan -> the car launches.
|
||||
ctrl = make_controller(personality=ECO)
|
||||
for _ in range(20): # build a deep comfort floor approaching a stopped lead
|
||||
ctrl.update(make_sm(v_ego=1.5, lead_status=True, lead_d=6.0, lead_vlead=0.0))
|
||||
ctrl.smooth_target_accel(-0.1, flat_traj(-0.1), T_IDXS, should_stop=False)
|
||||
assert ctrl._stop_floor < -0.2 # floor is engaged/deep
|
||||
out = 0.0
|
||||
for _ in range(30): # lead launches, plan wants throttle
|
||||
ctrl.update(make_sm(v_ego=2.0, lead_status=True, lead_d=8.0, lead_vlead=4.0))
|
||||
out = ctrl.smooth_target_accel(0.8, flat_traj(0.8), T_IDXS, should_stop=False)
|
||||
assert out > 0.0 # launches (floor did not hold it back)
|
||||
assert ctrl._stop_floor == 0.0 # floor fully released
|
||||
|
||||
|
||||
def test_onset_spread_bounded_and_skipped_for_emergency():
|
||||
# Non-emergency brake onset is spread (lagged) but never by more than ONSET_SPREAD_MAX; an emergency brake
|
||||
# is instant full depth (no spread).
|
||||
ctrl = make_controller(personality=ECO)
|
||||
for _ in range(3):
|
||||
ctrl.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False)
|
||||
out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False) # non-emergency step
|
||||
assert out > -1.0 + _EPS # lagged (spread), not an instant step
|
||||
assert out <= -1.0 + ONSET_SPREAD_MAX + _EPS # but bounded
|
||||
ctrl2 = make_controller(personality=ECO)
|
||||
for _ in range(3):
|
||||
ctrl2.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False)
|
||||
out2 = ctrl2.smooth_target_accel(-2.0, flat_traj(-2.0), T_IDXS, should_stop=False) # emergency (<= -1.5)
|
||||
assert out2 == pytest.approx(-2.0, abs=_EPS)
|
||||
|
||||
|
||||
def test_disabled_hard_brake_is_instant_stock():
|
||||
ctrl = make_controller(enabled=False, personality=ECO)
|
||||
out = ctrl.smooth_target_accel(-3.0, flat_traj(-3.0), T_IDXS, should_stop=False)
|
||||
|
||||
@@ -156,6 +156,8 @@ class LongitudinalPlannerSP:
|
||||
acceleration.decelTarget = float(self.accel.decel_target())
|
||||
acceleration.smoothActive = self.accel.smooth_active()
|
||||
acceleration.bypassed = bool(self.accel.bypassed())
|
||||
acceleration.comfortStopActive = bool(self.accel.comfort_stop_active())
|
||||
acceleration.comfortStopFloor = float(self.accel.comfort_stop_floor())
|
||||
|
||||
|
||||
pm.send('longitudinalPlanSP', plan_sp_send)
|
||||
|
||||
Reference in New Issue
Block a user