mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-20 22:47:02 +08:00
Smooth radar ACC handoffs and stop departures
This commit is contained in:
rav4kumar
@@ -17,6 +17,8 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
|
||||
DROPOUT_ACTION_ACCEL_MARGIN, HORIZON_DOWN_JERK, HORIZON_HOLD_TIME, HORIZON_SPEED_BUDGET, HORIZON_UP_JERK, MAX_LEAD_ACCEL_TAU,
|
||||
MIN_LEAD_SPEED, POSITIVE_MPC_HEADROOM, PROFILE_CONFIGS, PROFILE_TRANSITION_JERK, RADAR_STALE_TIMEOUT, RELIEF_CAP_MARGIN,
|
||||
RELIEF_CONFIRM_FRAMES, RELIEF_LEAD_SPEED_STEP, RELIEF_MPC_JERK, REQUIRED_DECEL_MARGIN, ROUTINE_DECEL_MAX, STOP_HOLD_EGO_SPEED,
|
||||
SHALLOW_BRAKE_BOUND, SHALLOW_BRAKE_RELIEF_TIME, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP, STOP_GAP_RESERVE_LEAD_SPEED,
|
||||
STOP_HOLD_CREEP_ABORT_FRAMES, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED,
|
||||
STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED,
|
||||
STOPPED_LEAD_SPEED, URGENT_CLOSING_SPEED, URGENT_RELEASE_ACCEL, URGENT_REQUIRED_DECEL, URGENT_TTC, URGENT_TTC_MIN_CLOSING,
|
||||
VEGO_NOISE_TOLERANCE, AccelProfile,
|
||||
@@ -38,8 +40,13 @@ class EnergyEnvelope:
|
||||
selected_lead: int = -1
|
||||
selected_lead_speed: float = math.inf
|
||||
selected_lead_decel: float = 0.0
|
||||
departure_lead_index: int = -1
|
||||
departure_lead_speed: float = math.inf
|
||||
departure_cap: float = math.inf
|
||||
departure_lead_speeds: tuple[float, float] = (math.inf, math.inf)
|
||||
departure_lead_separations: tuple[float, float] = (-math.inf, -math.inf)
|
||||
usable_gap: float = math.inf
|
||||
safety_usable_gap: float = math.inf
|
||||
closing_speed: float = 0.0
|
||||
required_decel: float = 0.0
|
||||
has_nearly_stopped_lead: bool = False
|
||||
@@ -77,11 +84,17 @@ class _ControllerPath:
|
||||
cap_samples: deque[float] = field(default_factory=lambda: deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
|
||||
required_samples: deque[float] = field(default_factory=lambda: deque(maxlen=CAP_FILTER_FRAMES))
|
||||
lead_decel_samples: deque[float] = field(default_factory=lambda: deque(maxlen=CAP_FILTER_FRAMES))
|
||||
departure_samples: tuple[deque[float], deque[float]] = field(
|
||||
default_factory=lambda: (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES)),
|
||||
)
|
||||
departure_references: list[float | None] = field(default_factory=lambda: [None, None])
|
||||
bound: float | None = None
|
||||
state: AccelControllerState = AccelControllerState.inactive
|
||||
relief_frames: int = 0
|
||||
bound_relief_frames: int = 0
|
||||
bound_relief_required_frames: int = 0
|
||||
departure_frames: int = 0
|
||||
creep_abort_frames: int = 0
|
||||
stale_frames: int = 0
|
||||
urgent: bool = False
|
||||
urgent_severe: bool = False
|
||||
@@ -102,15 +115,23 @@ class _ControllerPath:
|
||||
def robust_lead_decel(self) -> float:
|
||||
return float(np.median(self.lead_decel_samples)) if self.lead_decel_samples else 0.0
|
||||
|
||||
def robust_departure_separation(self, lead_index: int) -> float:
|
||||
samples = self.departure_samples[lead_index]
|
||||
return float(np.median(samples)) if samples else -math.inf
|
||||
|
||||
def reset(self) -> None:
|
||||
self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
|
||||
self.required_samples.clear()
|
||||
self.lead_decel_samples.clear()
|
||||
self.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
|
||||
self.departure_references = [None, None]
|
||||
self.bound = None
|
||||
self.state = AccelControllerState.inactive
|
||||
self.relief_frames = 0
|
||||
self.bound_relief_frames = 0
|
||||
self.bound_relief_required_frames = 0
|
||||
self.departure_frames = 0
|
||||
self.creep_abort_frames = 0
|
||||
self.stale_frames = 0
|
||||
self.urgent = False
|
||||
self.urgent_severe = False
|
||||
@@ -128,6 +149,7 @@ class AccelController:
|
||||
self.CP = CP
|
||||
self.dt = dt
|
||||
self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
|
||||
self.shallow_brake_relief_frames = max(RELIEF_CONFIRM_FRAMES, math.ceil(SHALLOW_BRAKE_RELIEF_TIME / dt))
|
||||
self.live = _ControllerPath()
|
||||
self.shadow = _ControllerPath()
|
||||
|
||||
@@ -209,7 +231,10 @@ class AccelController:
|
||||
x_ego, v_ego_delay = self._project_ego(v_ego, a_ego, delay)
|
||||
comfort_decel = PROFILE_CONFIGS[self._profile(profile)].comfort_decel
|
||||
candidates: list[EnergyEnvelope] = []
|
||||
departure_candidates: list[tuple[float, float]] = []
|
||||
departure_candidates: list[tuple[float, int]] = []
|
||||
departure_speeds = [math.inf, math.inf]
|
||||
departure_separations = [-math.inf, -math.inf]
|
||||
departure_caps = [math.inf, math.inf]
|
||||
for lead_index, lead in enumerate(leads):
|
||||
values = self._lead_values(lead)
|
||||
if values is None:
|
||||
@@ -219,27 +244,47 @@ class AccelController:
|
||||
lead_xv = LongitudinalMpc.extrapolate_lead(d_rel, v_lead, a_lead, a_lead_tau)
|
||||
x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
|
||||
v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
|
||||
usable_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_lead_delay, 0.0)
|
||||
safety_usable_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_lead_delay, 0.0)
|
||||
closing_speed = max(v_ego_delay - v_lead_delay, 0.0)
|
||||
required_decel = 0.0 if closing_speed == 0.0 else math.inf if usable_gap == 0.0 else closing_speed**2 / (2.0 * usable_gap)
|
||||
required_decel = (0.0 if closing_speed == 0.0 else math.inf if safety_usable_gap == 0.0
|
||||
else closing_speed**2 / (2.0 * safety_usable_gap))
|
||||
reserve_speed = float(np.interp(v_lead_delay, (0.0, STOP_GAP_RESERVE_LEAD_SPEED), (STOP_GAP_RESERVE, 0.0)))
|
||||
reserve_scale = float(np.interp(required_decel, STOP_GAP_RESERVE_DECEL_BP, (1.0, 0.0)))
|
||||
usable_gap = max(safety_usable_gap - reserve_speed * reserve_scale, 0.0)
|
||||
cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap)
|
||||
departure_cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * safety_usable_gap)
|
||||
projected_separation = x_lead - x_ego
|
||||
departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead_delay))
|
||||
except (FloatingPointError, OverflowError, TypeError, ValueError):
|
||||
continue
|
||||
finite_values = (x_lead, v_lead_delay, usable_gap, closing_speed, cap, departure_distance)
|
||||
finite_values = (x_lead, v_lead_delay, usable_gap, safety_usable_gap, closing_speed, cap, departure_cap, departure_distance)
|
||||
if not all(math.isfinite(value) and value >= 0.0 for value in finite_values) or math.isnan(required_decel) or required_decel < 0.0:
|
||||
continue
|
||||
candidates.append(EnergyEnvelope(cap, lead_index, v_lead_delay, max(-a_lead, 0.0), v_lead_delay, usable_gap, closing_speed,
|
||||
required_decel, lead_status=lead_status))
|
||||
departure_candidates.append((departure_distance, v_lead_delay))
|
||||
if not math.isfinite(projected_separation):
|
||||
continue
|
||||
candidates.append(EnergyEnvelope(cap=cap, selected_lead=lead_index, selected_lead_speed=v_lead_delay,
|
||||
selected_lead_decel=max(-a_lead, 0.0), usable_gap=usable_gap,
|
||||
safety_usable_gap=safety_usable_gap, closing_speed=closing_speed,
|
||||
required_decel=required_decel, lead_status=lead_status))
|
||||
departure_candidates.append((departure_distance, lead_index))
|
||||
departure_speeds[lead_index] = v_lead_delay
|
||||
departure_separations[lead_index] = projected_separation
|
||||
departure_caps[lead_index] = departure_cap
|
||||
|
||||
if not candidates:
|
||||
return EnergyEnvelope(lead_status=lead_status)
|
||||
selected = min(candidates, key=lambda candidate: candidate.cap)
|
||||
departure_lead_speed = min(departure_candidates, key=lambda candidate: candidate[0])[1]
|
||||
return EnergyEnvelope(selected.cap, selected.selected_lead, selected.selected_lead_speed, selected.selected_lead_decel,
|
||||
departure_lead_speed, selected.usable_gap, selected.closing_speed, selected.required_decel,
|
||||
departure_lead_speed < STOPPED_LEAD_SPEED, lead_status)
|
||||
departure_lead_index = min(departure_candidates, key=lambda candidate: candidate[0])[1]
|
||||
departure_lead_speed = departure_speeds[departure_lead_index]
|
||||
return EnergyEnvelope(
|
||||
cap=selected.cap, selected_lead=selected.selected_lead, selected_lead_speed=selected.selected_lead_speed,
|
||||
selected_lead_decel=selected.selected_lead_decel, departure_lead_index=departure_lead_index,
|
||||
departure_lead_speed=departure_lead_speed, departure_cap=departure_caps[departure_lead_index],
|
||||
departure_lead_speeds=tuple(departure_speeds), departure_lead_separations=tuple(departure_separations),
|
||||
usable_gap=selected.usable_gap, safety_usable_gap=selected.safety_usable_gap, closing_speed=selected.closing_speed,
|
||||
required_decel=selected.required_decel, has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED,
|
||||
lead_status=lead_status,
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _move(value: float, target: float, rate: float, dt: float) -> float:
|
||||
@@ -247,7 +292,7 @@ class AccelController:
|
||||
|
||||
@staticmethod
|
||||
def _ttc(envelope: EnergyEnvelope) -> float:
|
||||
return envelope.usable_gap / envelope.closing_speed if envelope.closing_speed > 0.0 else math.inf
|
||||
return envelope.safety_usable_gap / envelope.closing_speed if envelope.closing_speed > 0.0 else math.inf
|
||||
|
||||
def _update_samples(self, path: _ControllerPath, envelope: EnergyEnvelope) -> None:
|
||||
has_lead = envelope.selected_lead >= 0
|
||||
@@ -255,6 +300,9 @@ class AccelController:
|
||||
and envelope.selected_lead_speed > path.previous_lead_speed + RELIEF_LEAD_SPEED_STEP)
|
||||
path.previous_lead_speed = envelope.selected_lead_speed if has_lead else None
|
||||
path.cap_samples.append(envelope.cap if has_lead else math.inf)
|
||||
for lead_index, separation in enumerate(envelope.departure_lead_separations):
|
||||
if math.isfinite(separation):
|
||||
path.departure_samples[lead_index].append(separation)
|
||||
if has_lead:
|
||||
if math.isfinite(envelope.required_decel):
|
||||
path.required_samples.append(envelope.required_decel)
|
||||
@@ -264,6 +312,32 @@ class AccelController:
|
||||
path.required_samples.append(0.0)
|
||||
path.lead_decel_samples.append(0.0)
|
||||
|
||||
@staticmethod
|
||||
def _reset_departure_tracking(path: _ControllerPath, envelope: EnergyEnvelope) -> None:
|
||||
path.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
|
||||
path.departure_references = [None, None]
|
||||
for lead_index, separation in enumerate(envelope.departure_lead_separations):
|
||||
if math.isfinite(separation):
|
||||
path.departure_samples[lead_index].append(separation)
|
||||
path.departure_references[lead_index] = separation
|
||||
path.departure_frames = 0
|
||||
path.creep_abort_frames = 0
|
||||
|
||||
@staticmethod
|
||||
def _clear_bound_relief(path: _ControllerPath) -> None:
|
||||
path.bound_relief_frames = 0
|
||||
path.bound_relief_required_frames = 0
|
||||
|
||||
@staticmethod
|
||||
def _creep_departure(path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
|
||||
lead_index = envelope.departure_lead_index
|
||||
if lead_index < 0 or envelope.departure_lead_speed <= STOP_HOLD_CREEP_SPEED:
|
||||
return False
|
||||
|
||||
separation = path.robust_departure_separation(lead_index)
|
||||
reference = path.departure_references[lead_index]
|
||||
return reference is not None and separation - reference >= STOP_HOLD_CREEP_DISTANCE
|
||||
|
||||
def _update_path(self, path: _ControllerPath, envelope: EnergyEnvelope, base_speed: float, v_ego: float, action_accel: float,
|
||||
positive_accel_max: float, profile: AccelProfile, previous_should_stop: bool) -> bool:
|
||||
self._update_samples(path, envelope)
|
||||
@@ -275,14 +349,33 @@ class AccelController:
|
||||
moving_away = (has_lead and not envelope.has_nearly_stopped_lead
|
||||
and envelope.selected_lead_speed > v_ego + APPROACH_CLOSING_SPEED
|
||||
and envelope.cap > v_ego + RELIEF_CAP_MARGIN)
|
||||
stop_hold = (v_ego < STOP_HOLD_EGO_SPEED
|
||||
and ((previous_should_stop and not path.departing_from_stop)
|
||||
|
||||
if path.departing_from_stop:
|
||||
if v_ego >= STOP_HOLD_EGO_SPEED:
|
||||
path.departing_from_stop = False
|
||||
path.creep_abort_frames = 0
|
||||
elif envelope.lead_status and (not has_lead or envelope.departure_lead_speed <= STOP_HOLD_CREEP_SPEED):
|
||||
path.creep_abort_frames += 1
|
||||
if path.creep_abort_frames >= STOP_HOLD_CREEP_ABORT_FRAMES:
|
||||
path.departing_from_stop = False
|
||||
path.creep_abort_frames = 0
|
||||
else:
|
||||
path.creep_abort_frames = 0
|
||||
|
||||
stop_hold = (v_ego < STOP_HOLD_EGO_SPEED and not path.departing_from_stop
|
||||
and (previous_should_stop or (envelope.lead_status and not has_lead)
|
||||
or (has_lead and (envelope.has_nearly_stopped_lead or envelope.cap < 0.50))))
|
||||
|
||||
if path.state == AccelControllerState.stopHold:
|
||||
path.bound_relief_frames = 0
|
||||
departed = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_EXIT_SPEED and envelope.cap > STOP_HOLD_EXIT_SPEED)
|
||||
or not has_lead)
|
||||
self._clear_bound_relief(path)
|
||||
for lead_index in range(len(path.departure_references)):
|
||||
separation = path.robust_departure_separation(lead_index)
|
||||
if math.isfinite(separation) and path.departure_references[lead_index] is None:
|
||||
path.departure_references[lead_index] = separation
|
||||
departed = (not envelope.lead_status
|
||||
or (has_lead and envelope.departure_lead_speed > STOP_HOLD_EXIT_SPEED
|
||||
and envelope.departure_cap > STOP_HOLD_EXIT_SPEED)
|
||||
or self._creep_departure(path, envelope))
|
||||
path.departure_frames = path.departure_frames + 1 if departed else 0
|
||||
path.bound = 0.0
|
||||
if path.departure_frames < STOP_HOLD_EXIT_FRAMES:
|
||||
@@ -297,17 +390,15 @@ class AccelController:
|
||||
path.state = AccelControllerState.stopHold
|
||||
path.bound = 0.0
|
||||
path.relief_frames = 0
|
||||
path.bound_relief_frames = 0
|
||||
self._clear_bound_relief(path)
|
||||
path.departure_frames = 0
|
||||
path.urgent = False
|
||||
path.urgent_severe = False
|
||||
path.urgent_safe_frames = 0
|
||||
path.departing_from_stop = False
|
||||
self._reset_departure_tracking(path, envelope)
|
||||
return False
|
||||
|
||||
if path.departing_from_stop and v_ego >= STOP_HOLD_EGO_SPEED:
|
||||
path.departing_from_stop = False
|
||||
|
||||
urgent_closing = envelope.closing_speed > URGENT_TTC_MIN_CLOSING
|
||||
raw_urgent = (has_lead and v_ego >= STOP_HOLD_EGO_SPEED
|
||||
and (envelope.closing_speed >= URGENT_CLOSING_SPEED
|
||||
@@ -320,7 +411,7 @@ class AccelController:
|
||||
path.bound = None
|
||||
path.state = AccelControllerState.hold
|
||||
path.relief_frames = 0
|
||||
path.bound_relief_frames = 0
|
||||
self._clear_bound_relief(path)
|
||||
return True
|
||||
|
||||
if path.urgent:
|
||||
@@ -330,7 +421,7 @@ class AccelController:
|
||||
if path.urgent_safe_frames < RELIEF_CONFIRM_FRAMES:
|
||||
path.bound = None
|
||||
path.state = AccelControllerState.hold
|
||||
path.bound_relief_frames = 0
|
||||
self._clear_bound_relief(path)
|
||||
return True
|
||||
path.urgent = False
|
||||
path.urgent_severe = False
|
||||
@@ -344,7 +435,7 @@ class AccelController:
|
||||
|
||||
if path.state == AccelControllerState.inactive and has_lead and not math.isfinite(filtered_cap):
|
||||
path.bound = min(action_accel, 0.0)
|
||||
path.bound_relief_frames = 0
|
||||
self._clear_bound_relief(path)
|
||||
return False
|
||||
|
||||
dropout_guard = (not has_lead and math.isfinite(filtered_cap)
|
||||
@@ -377,8 +468,13 @@ class AccelController:
|
||||
target_decel = profile_config.glide_decel
|
||||
target = -target_decel
|
||||
bound_relief = has_lead and path.bound < 0.0 and target > path.bound + 1e-9
|
||||
if bound_relief and path.bound_relief_frames == 0:
|
||||
path.bound_relief_required_frames = (self.shallow_brake_relief_frames
|
||||
if path.bound >= SHALLOW_BRAKE_BOUND else RELIEF_CONFIRM_FRAMES)
|
||||
path.bound_relief_frames = path.bound_relief_frames + 1 if bound_relief else 0
|
||||
if bound_relief and path.bound_relief_frames < RELIEF_CONFIRM_FRAMES:
|
||||
if not bound_relief:
|
||||
self._clear_bound_relief(path)
|
||||
if bound_relief and path.bound_relief_frames < path.bound_relief_required_frames:
|
||||
target = path.bound
|
||||
path.bound = self._move(path.bound, target, CAP_RELAX_JERK if target > path.bound else CAP_TIGHTEN_JERK, self.dt)
|
||||
path.state = AccelControllerState.hold if matched or coast_to_match else AccelControllerState.restrict
|
||||
@@ -386,7 +482,7 @@ class AccelController:
|
||||
return False
|
||||
|
||||
if path.state in (AccelControllerState.restrict, AccelControllerState.hold):
|
||||
path.bound_relief_frames = 0
|
||||
self._clear_bound_relief(path)
|
||||
relief = not has_lead or moving_away
|
||||
path.relief_frames = path.relief_frames + 1 if relief else 0
|
||||
path.bound = min(path.bound if path.bound is not None else action_accel, 0.0)
|
||||
@@ -401,7 +497,7 @@ class AccelController:
|
||||
path.bound = positive_accel_max
|
||||
else:
|
||||
path.bound = self._move(path.bound, positive_accel_max, PROFILE_TRANSITION_JERK, self.dt)
|
||||
path.bound_relief_frames = 0
|
||||
self._clear_bound_relief(path)
|
||||
path.state = AccelControllerState.free
|
||||
return False
|
||||
|
||||
@@ -440,7 +536,7 @@ class AccelController:
|
||||
path.stale_frames = 0
|
||||
return True
|
||||
path.stale_frames += 1
|
||||
if path.stale_frames < self.radar_stale_frames and path.bound is not None:
|
||||
if path.stale_frames < self.radar_stale_frames and (path.bound is not None or path.urgent):
|
||||
return False
|
||||
path.reset()
|
||||
return False
|
||||
@@ -472,7 +568,7 @@ class AccelController:
|
||||
self._update_path(self.shadow, envelope, base_speed, sanitized_v_ego, action_accel, positive_accel_max,
|
||||
selected_profile, previous_should_stop)
|
||||
shadow_active = True
|
||||
elif valid_context and not shadow_fresh and self.shadow.bound is not None:
|
||||
elif valid_context and not shadow_fresh and (self.shadow.bound is not None or self.shadow.urgent):
|
||||
shadow_active = True
|
||||
else:
|
||||
self.shadow.reset()
|
||||
@@ -484,8 +580,8 @@ class AccelController:
|
||||
stock_mode = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, action_accel,
|
||||
positive_accel_max, selected_profile, previous_should_stop)
|
||||
live_active = True
|
||||
elif live_context and not live_fresh and self.live.bound is not None:
|
||||
stock_mode = False
|
||||
elif live_context and not live_fresh and (self.live.bound is not None or self.live.urgent):
|
||||
stock_mode = self.live.urgent
|
||||
live_active = True
|
||||
else:
|
||||
self.live.reset()
|
||||
|
||||
@@ -34,6 +34,12 @@ STOP_HOLD_EXIT_FRAMES = 4
|
||||
STOP_HOLD_EGO_SPEED = 0.30
|
||||
STOPPED_LEAD_SPEED = 0.30
|
||||
STOP_HOLD_EXIT_SPEED = 0.80
|
||||
STOP_HOLD_CREEP_SPEED = 0.15
|
||||
STOP_HOLD_CREEP_DISTANCE = 0.30
|
||||
STOP_HOLD_CREEP_ABORT_FRAMES = 4
|
||||
STOP_GAP_RESERVE = 0.75
|
||||
STOP_GAP_RESERVE_LEAD_SPEED = 2.0
|
||||
STOP_GAP_RESERVE_DECEL_BP = (0.30, 0.80)
|
||||
MPC_SEED_RISE_RATE = 6.0
|
||||
APPROACH_MIN_SPEED = 2.0
|
||||
APPROACH_CLOSING_SPEED = 0.15
|
||||
@@ -47,6 +53,8 @@ REQUIRED_DECEL_MARGIN = 0.03
|
||||
ROUTINE_DECEL_MAX = 1.0
|
||||
CAP_TIGHTEN_JERK = 0.60
|
||||
CAP_RELAX_JERK = 0.80
|
||||
SHALLOW_BRAKE_BOUND = -0.25
|
||||
SHALLOW_BRAKE_RELIEF_TIME = 1.75
|
||||
RELIEF_MPC_JERK = 3.20
|
||||
RELIEF_LEAD_SPEED_STEP = 0.05
|
||||
DROPOUT_ACTION_ACCEL_MARGIN = 0.08
|
||||
|
||||
@@ -18,6 +18,9 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control
|
||||
PROFILE_TRANSITION_JERK,
|
||||
RADAR_STALE_TIMEOUT,
|
||||
RELIEF_CONFIRM_FRAMES,
|
||||
SHALLOW_BRAKE_BOUND,
|
||||
STOP_GAP_RESERVE,
|
||||
STOP_HOLD_CREEP_ABORT_FRAMES,
|
||||
STOP_HOLD_EXIT_FRAMES,
|
||||
AccelController,
|
||||
AccelControllerState,
|
||||
@@ -134,6 +137,27 @@ class TestEnergyEnvelope:
|
||||
caps = [controller.calculate_energy_envelope(radar, 10.0, 0.0, profile).cap for profile in AccelProfile]
|
||||
assert caps[0] < caps[1] < caps[2]
|
||||
|
||||
def test_stopped_lead_reserve_only_reduces_comfort_gap(self):
|
||||
controller = make_controller()
|
||||
lead = make_lead(status=True, d_rel=20.0, v_lead_k=0.0)
|
||||
envelope = controller.calculate_energy_envelope(make_radar(lead), 2.0, 0.0, AccelProfile.normal)
|
||||
expected = math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * envelope.usable_gap)
|
||||
|
||||
assert envelope.required_decel < 0.30
|
||||
assert envelope.safety_usable_gap - envelope.usable_gap == pytest.approx(STOP_GAP_RESERVE)
|
||||
assert envelope.cap == pytest.approx(expected)
|
||||
assert envelope.departure_cap > envelope.cap
|
||||
|
||||
def test_stop_reserve_fades_out_of_urgent_braking(self):
|
||||
controller = make_controller()
|
||||
lead = make_lead(status=True, d_rel=20.0, v_lead_k=0.0)
|
||||
envelope = controller.calculate_energy_envelope(make_radar(lead), 20.0, 0.0, AccelProfile.normal)
|
||||
|
||||
assert envelope.required_decel > 0.80
|
||||
assert envelope.usable_gap == envelope.safety_usable_gap
|
||||
assert envelope.cap == envelope.departure_cap
|
||||
assert controller._ttc(envelope) == pytest.approx(envelope.safety_usable_gap / envelope.closing_speed)
|
||||
|
||||
def test_more_restrictive_lead_is_selected(self):
|
||||
radar = make_radar(make_lead(status=True, d_rel=70.0, v_lead_k=12.0), make_lead(status=True, d_rel=25.0, v_lead_k=8.0))
|
||||
envelope = make_controller().calculate_energy_envelope(radar, 10.0, 0.0, AccelProfile.normal)
|
||||
@@ -211,6 +235,35 @@ class TestAccelControllerState:
|
||||
accelerating = update(controller, moving_away)
|
||||
assert released.effective_accel_max < accelerating.effective_accel_max <= accelerating.positive_accel_max
|
||||
|
||||
def test_shallow_brake_relief_uses_long_confirmation_without_delaying_tightening(self):
|
||||
controller = make_controller()
|
||||
controller.live.state = AccelControllerState.restrict
|
||||
controller.live.bound = SHALLOW_BRAKE_BOUND + 0.05
|
||||
matched = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=10.0))
|
||||
|
||||
held = [update(controller, matched) for _ in range(controller.shallow_brake_relief_frames - 1)]
|
||||
assert all(result.effective_accel_max == pytest.approx(SHALLOW_BRAKE_BOUND + 0.05) for result in held)
|
||||
relaxed = update(controller, matched)
|
||||
assert relaxed.effective_accel_max > held[-1].effective_accel_max
|
||||
|
||||
for _ in range(CAP_FILTER_FRAMES):
|
||||
tightened = update(controller, restrictive_radar())
|
||||
assert tightened.effective_accel_max < relaxed.effective_accel_max
|
||||
|
||||
def test_strong_brake_relief_keeps_existing_confirmation(self):
|
||||
controller = make_controller()
|
||||
controller.live.state = AccelControllerState.restrict
|
||||
controller.live.bound = SHALLOW_BRAKE_BOUND - 0.25
|
||||
matched = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=10.0))
|
||||
|
||||
held = [update(controller, matched) for _ in range(RELIEF_CONFIRM_FRAMES - 1)]
|
||||
assert all(result.effective_accel_max == pytest.approx(SHALLOW_BRAKE_BOUND - 0.25) for result in held)
|
||||
relaxed = update(controller, matched)
|
||||
assert relaxed.effective_accel_max > held[-1].effective_accel_max
|
||||
continuing = [update(controller, matched) for _ in range(8)]
|
||||
assert all(current.effective_accel_max > previous.effective_accel_max
|
||||
for previous, current in zip([relaxed, *continuing[:-1]], continuing, strict=True))
|
||||
|
||||
def test_urgent_frame_uses_exact_stock_path(self):
|
||||
urgent = make_radar(make_lead(status=True, d_rel=18.0, v_lead_k=0.0))
|
||||
result = update(make_controller(), urgent, v_ego=20.0)
|
||||
@@ -243,6 +296,93 @@ class TestAccelControllerState:
|
||||
assert launched.launching and launched.state == AccelControllerState.free
|
||||
assert launched.effective_accel_max == launched.positive_accel_max
|
||||
|
||||
def test_false_creep_speed_without_range_gain_stays_held(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
|
||||
|
||||
false_creep = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.30))
|
||||
held = [update(controller, false_creep, base_speed=8.0, v_ego=0.1) for _ in range(20)]
|
||||
assert all(result.state == AccelControllerState.stopHold and not result.launching for result in held)
|
||||
|
||||
def test_invalid_lead_geometry_cannot_confirm_departure(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
|
||||
|
||||
invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=0.30))
|
||||
held = [update(controller, invalid, base_speed=8.0, v_ego=0.1) for _ in range(2 * STOP_HOLD_EXIT_FRAMES)]
|
||||
assert all(result.state == AccelControllerState.stopHold and not result.launching for result in held)
|
||||
|
||||
def test_short_range_drop_and_restore_cannot_fake_creep_departure(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
|
||||
|
||||
low_range = make_radar(make_lead(status=True, d_rel=5.0, v_lead_k=0.0))
|
||||
for _ in range(2):
|
||||
update(controller, low_range, base_speed=8.0, v_ego=0.1)
|
||||
restored = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.30))
|
||||
results = [update(controller, restored, base_speed=8.0, v_ego=0.1) for _ in range(8)]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results)
|
||||
|
||||
def test_slow_creep_with_confirmed_range_gain_releases(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
|
||||
|
||||
results = []
|
||||
for frame in range(20):
|
||||
creep = make_radar(make_lead(status=True, d_rel=6.0 + 0.05 * frame, v_lead_k=0.30))
|
||||
results.append(update(controller, creep, base_speed=8.0, v_ego=0.1))
|
||||
|
||||
launched = [frame for frame, result in enumerate(results) if result.launching]
|
||||
assert launched and launched[0] >= STOP_HOLD_EXIT_FRAMES
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results[:launched[0]])
|
||||
|
||||
def test_slow_creep_survives_lead_slot_switching(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0), make_lead(status=True, d_rel=6.1))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
|
||||
|
||||
results = []
|
||||
for frame in range(24):
|
||||
distance = 6.0 + 0.04 * frame
|
||||
offset = 0.05 if frame % 2 else 0.0
|
||||
lead_one = make_lead(status=True, d_rel=distance + offset, v_lead_k=0.30)
|
||||
lead_two = make_lead(status=True, d_rel=distance + 0.05 - offset, v_lead_k=0.30)
|
||||
results.append(update(controller, make_radar(lead_one, lead_two), base_speed=8.0, v_ego=0.1))
|
||||
|
||||
assert any(result.launching for result in results)
|
||||
|
||||
def test_departure_that_stops_again_returns_to_hold(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
|
||||
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
|
||||
for _ in range(STOP_HOLD_EXIT_FRAMES):
|
||||
launched = update(controller, departing, base_speed=8.0, v_ego=0.1)
|
||||
assert launched.launching
|
||||
|
||||
stalled = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.11))
|
||||
settling = [update(controller, stalled, base_speed=8.0, v_ego=0.1) for _ in range(STOP_HOLD_CREEP_ABORT_FRAMES)]
|
||||
assert all(result.launching for result in settling[:-1])
|
||||
assert settling[-1].state == AccelControllerState.stopHold and not settling[-1].launching
|
||||
|
||||
def test_invalid_geometry_after_departure_returns_to_hold(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.1, previous_should_stop=True)
|
||||
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
|
||||
for _ in range(STOP_HOLD_EXIT_FRAMES):
|
||||
launched = update(controller, departing, base_speed=8.0, v_ego=0.1)
|
||||
assert launched.launching
|
||||
|
||||
invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=0.30))
|
||||
settling = [update(controller, invalid, base_speed=8.0, v_ego=0.1) for _ in range(STOP_HOLD_CREEP_ABORT_FRAMES)]
|
||||
assert settling[-1].state == AccelControllerState.stopHold and not settling[-1].launching
|
||||
|
||||
def test_stale_radar_freezes_then_discards_live_state(self):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES):
|
||||
@@ -253,6 +393,20 @@ class TestAccelControllerState:
|
||||
timed_out = update(controller, radar_fresh=False)
|
||||
assert not timed_out.active and timed_out.mpc_accel_max is None
|
||||
|
||||
def test_stale_radar_preserves_urgent_stock_passthrough_until_timeout(self):
|
||||
controller = make_controller()
|
||||
urgent = make_radar(make_lead(status=True, d_rel=18.0, v_lead_k=0.0))
|
||||
result = update(controller, urgent, v_ego=20.0)
|
||||
assert result.active and result.shadow_active and result.stock_mode
|
||||
|
||||
hold_frames = math.ceil(RADAR_STALE_TIMEOUT / DT_MDL) - 1
|
||||
frozen = [update(controller, radar_fresh=False, v_ego=20.0) for _ in range(hold_frames)]
|
||||
assert all(sample.active and sample.shadow_active and sample.stock_mode for sample in frozen)
|
||||
assert all(sample.state == AccelControllerState.hold and sample.mpc_accel_max is None for sample in frozen)
|
||||
|
||||
timed_out = update(controller, radar_fresh=False, v_ego=20.0)
|
||||
assert not timed_out.active and not timed_out.shadow_active and not timed_out.stock_mode
|
||||
|
||||
@pytest.mark.parametrize("override", [
|
||||
{"enabled": False}, {"acc_selected": False}, {"engaged": False}, {"cruise_initialized": False}, {"controller_fault": True},
|
||||
])
|
||||
|
||||
+100
@@ -113,6 +113,106 @@ def test_last_solve_failure_survives_internal_reset():
|
||||
assert mpc.last_solution_status == 3
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("controller_active", "expected_accel"), [(True, -1.2), (False, None)])
|
||||
def test_e2e_to_acc_handoff_preserves_braking_only_when_controller_is_active(controller_active, expected_accel):
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner._previous_is_e2e = True
|
||||
planner.accel_personality_enabled = controller_active
|
||||
planner.accel_controller_fault_latched = False
|
||||
planner.is_e2e = lambda _sm: False
|
||||
planner.accel_controller = SimpleNamespace(reset=lambda: None)
|
||||
planner.mpc = SimpleNamespace(
|
||||
a_prev=np.zeros(N + 1), crash_cnt=0, v_solution=np.zeros(N + 1), a_solution=np.zeros(N + 1),
|
||||
j_solution=np.zeros(N), last_solution_status=0,
|
||||
)
|
||||
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
|
||||
planner, "accel_controller_result",
|
||||
SimpleNamespace(enabled=controller_active, active=controller_active, shadow_active=controller_active,
|
||||
stock_mode=not controller_active, target_speed=20.0, mpc_accel_max=None,
|
||||
state=AccelControllerState.free, effective_accel_max=0.8 if controller_active else np.inf),
|
||||
)
|
||||
calls = []
|
||||
planner._run_mpc = lambda *_args, **kwargs: calls.append(kwargs)
|
||||
|
||||
is_e2e = planner.update_accel_controller_mpc(
|
||||
{}, 20.0, 20.0, True, reset_state=False, cruise_initialized=True, planner_accel=0.1,
|
||||
previous_output_accel=-1.2, available_accel_max=ACCEL_MAX, previous_should_stop=False, force_decel=False,
|
||||
)
|
||||
|
||||
assert not is_e2e
|
||||
assert calls[0]["current_accel"] == expected_accel
|
||||
assert calls[0]["seed_target"] is None
|
||||
|
||||
|
||||
def test_failed_e2e_to_acc_handoff_retries_without_custom_state():
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner._previous_is_e2e = True
|
||||
planner.accel_personality_enabled = True
|
||||
planner.accel_controller_fault_latched = False
|
||||
planner.is_e2e = lambda _sm: False
|
||||
reset_calls = []
|
||||
planner.accel_controller = SimpleNamespace(reset=lambda: reset_calls.append(True))
|
||||
planner.mpc = SimpleNamespace(
|
||||
a_prev=np.zeros(N + 1), crash_cnt=0, v_solution=np.zeros(N + 1), a_solution=np.zeros(N + 1),
|
||||
j_solution=np.zeros(N), last_solution_status=0,
|
||||
)
|
||||
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
|
||||
planner, "accel_controller_result",
|
||||
SimpleNamespace(enabled=True, active=True, shadow_active=True, stock_mode=True, target_speed=15.0, mpc_accel_max=None,
|
||||
state=AccelControllerState.hold, effective_accel_max=np.inf),
|
||||
)
|
||||
calls = []
|
||||
positional_calls = []
|
||||
|
||||
def run_mpc(*args, **kwargs):
|
||||
positional_calls.append(args)
|
||||
calls.append(kwargs)
|
||||
planner.mpc.last_solution_status = 1 if len(calls) == 1 else 0
|
||||
|
||||
planner._run_mpc = run_mpc
|
||||
planner.update_accel_controller_mpc(
|
||||
{}, 20.0, 20.0, True, reset_state=False, cruise_initialized=True, planner_accel=0.1,
|
||||
previous_output_accel=-1.2, available_accel_max=ACCEL_MAX, previous_should_stop=False, force_decel=False,
|
||||
)
|
||||
|
||||
assert [call.get("current_accel") for call in calls] == [-1.2, None]
|
||||
assert [args[1] for args in positional_calls] == [15.0, 20.0]
|
||||
assert len(positional_calls[1]) == 3
|
||||
assert calls[0]["seed_target"] is None
|
||||
assert calls[1]["seed"]
|
||||
assert "current_accel" not in calls[1]
|
||||
assert "retry_state" in calls[1]
|
||||
assert reset_calls == [True]
|
||||
assert planner.accel_controller_fault_latched
|
||||
|
||||
|
||||
def test_e2e_to_acc_handoff_never_turns_braking_into_acceleration():
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner._previous_is_e2e = True
|
||||
planner.accel_personality_enabled = True
|
||||
planner.accel_controller_fault_latched = False
|
||||
planner.is_e2e = lambda _sm: False
|
||||
planner.accel_controller = SimpleNamespace(reset=lambda: None)
|
||||
planner.mpc = SimpleNamespace(
|
||||
a_prev=np.zeros(N + 1), crash_cnt=0, v_solution=np.zeros(N + 1), a_solution=np.zeros(N + 1),
|
||||
j_solution=np.zeros(N), last_solution_status=0,
|
||||
)
|
||||
planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
|
||||
planner, "accel_controller_result",
|
||||
SimpleNamespace(enabled=True, active=True, shadow_active=True, stock_mode=True, target_speed=20.0, mpc_accel_max=None,
|
||||
state=AccelControllerState.free, effective_accel_max=np.inf),
|
||||
)
|
||||
calls = []
|
||||
planner._run_mpc = lambda *_args, **kwargs: calls.append(kwargs)
|
||||
|
||||
planner.update_accel_controller_mpc(
|
||||
{}, 20.0, 20.0, True, reset_state=False, cruise_initialized=True, planner_accel=-0.7,
|
||||
previous_output_accel=0.3, available_accel_max=ACCEL_MAX, previous_should_stop=False, force_decel=False,
|
||||
)
|
||||
|
||||
assert calls[0]["current_accel"] == -0.7
|
||||
|
||||
|
||||
def test_shadow_telemetry_publishes_controller_fields():
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.source = LongitudinalPlanSource.cruise
|
||||
|
||||
@@ -45,6 +45,7 @@ class LongitudinalPlannerSP:
|
||||
self.accel_controller = AccelController(CP, dt=dt)
|
||||
self.accel_controller_result = None
|
||||
self.accel_controller_fault_latched = False
|
||||
self._previous_is_e2e = False
|
||||
|
||||
self._param_read_frames = max(1, int(round(0.25 / dt)))
|
||||
self._param_frame = 0
|
||||
@@ -120,12 +121,13 @@ class LongitudinalPlannerSP:
|
||||
return self.accel_controller_result.target_speed
|
||||
|
||||
def _run_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, accel_max=None, *, seed=False,
|
||||
seed_target=None, seed_rise_rate=MPC_SEED_RISE_RATE, retry_state=None) -> None:
|
||||
seed_target=None, seed_rise_rate=MPC_SEED_RISE_RATE, retry_state=None, current_accel=None) -> None:
|
||||
if retry_state is not None:
|
||||
self.mpc.a_prev = retry_state[0].copy()
|
||||
self.mpc.crash_cnt = retry_state[1]
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
mpc_accel = self.a_desired if current_accel is None else float(np.clip(current_accel, ACCEL_MIN, ACCEL_MAX))
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, mpc_accel)
|
||||
if seed or seed_target is not None:
|
||||
self._seed_mpc_current_state(seed_target, seed_rise_rate)
|
||||
self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality, accel_max=accel_max)
|
||||
@@ -158,6 +160,7 @@ class LongitudinalPlannerSP:
|
||||
planner_accel: float, previous_output_accel: float, available_accel_max: float,
|
||||
previous_should_stop: bool, force_decel: bool):
|
||||
is_e2e = self.is_e2e(sm)
|
||||
was_e2e = self._previous_is_e2e
|
||||
if reset_state or not self.accel_personality_enabled:
|
||||
self.accel_controller_fault_latched = False
|
||||
|
||||
@@ -168,14 +171,19 @@ class LongitudinalPlannerSP:
|
||||
controller_fault=self.accel_controller_fault_latched,
|
||||
)
|
||||
result = self.accel_controller_result
|
||||
handoff_context = result.enabled and result.shadow_active and not force_decel and not self.accel_controller_fault_latched
|
||||
transition_from_e2e = handoff_context and was_e2e and not is_e2e and result.active
|
||||
handoff_accel = (min(planner_accel, previous_output_accel)
|
||||
if transition_from_e2e and result.active and np.isfinite(previous_output_accel) else None)
|
||||
self._previous_is_e2e = is_e2e and handoff_context
|
||||
controller_actuating = result.active and not result.stock_mode and not force_decel
|
||||
accel_max = result.mpc_accel_max if controller_actuating else None
|
||||
free_profile_limit = controller_actuating and result.state == AccelControllerState.free and result.effective_accel_max > 0.0
|
||||
seed_target = result.effective_accel_max if free_profile_limit else None
|
||||
custom_mpc = controller_actuating and (accel_max is not None or seed_target is not None)
|
||||
seed_target = result.effective_accel_max if free_profile_limit and handoff_accel is None else None
|
||||
custom_mpc = handoff_accel is not None or (controller_actuating and (accel_max is not None or seed_target is not None))
|
||||
retry_state = (self.mpc.a_prev.copy(), self.mpc.crash_cnt)
|
||||
controller_v_cruise = min(mpc_v_cruise, result.target_speed)
|
||||
self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max, seed_target=seed_target)
|
||||
self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max, seed_target=seed_target, current_accel=handoff_accel)
|
||||
|
||||
finite_solution = all(np.all(np.isfinite(solution)) for solution in (self.mpc.v_solution, self.mpc.a_solution, self.mpc.j_solution))
|
||||
custom_failed = custom_mpc and (self.mpc.last_solution_status != 0 or not finite_solution)
|
||||
|
||||
@@ -261,6 +261,27 @@ def test_force_decel_matches_controller_off(lead_relevancy):
|
||||
assert trace.source == baseline.source
|
||||
|
||||
|
||||
def test_e2e_to_radar_acc_handoff_keeps_braking_continuous():
|
||||
plant = Plant(
|
||||
lead_relevancy=True, speed=10.0, distance_lead=30.0, actuator_delay=0.15, actuator_lag=0.20,
|
||||
model_action_fn=lambda current_time, _v_ego, _a_ego: (-1.0 if current_time < 2.0 else 0.0, False),
|
||||
)
|
||||
_configure_plant(plant, enabled=True)
|
||||
rows = []
|
||||
while plant.current_time < 2.4:
|
||||
plant.e2e = plant.current_time < 2.0
|
||||
result = plant.step(v_lead=8.0, v_cruise=20.0)
|
||||
rows.append((plant.current_time, result["a_target"], plant.planner.mpc.last_solution_status,
|
||||
plant.planner.accel_controller_result.active))
|
||||
|
||||
time_values, acceleration, solver_status, active = np.asarray(rows, dtype=float).T
|
||||
transition = np.flatnonzero(time_values > 2.0)[0]
|
||||
assert acceleration[transition] - acceleration[transition - 1] < 0.15
|
||||
assert np.max(np.diff(acceleration[transition - 1:]) / DT_MDL) < 3.0
|
||||
assert not solver_status[transition:].any()
|
||||
assert active[transition]
|
||||
|
||||
|
||||
def test_active_controller_is_pre_mpc_and_preserves_stock_lead_authority():
|
||||
plant = Plant(lead_relevancy=False, speed=0.0, actuator_delay=0.15, actuator_lag=0.20)
|
||||
_configure_plant(plant, enabled=True, profile=0)
|
||||
|
||||
Reference in New Issue
Block a user