mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-06 17:35:44 +08:00
Prevent false stop departures without damping takeoff
This commit is contained in:
rav4kumar
@@ -20,7 +20,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
|
||||
PACE_RESTRICT_DEADBAND, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP,
|
||||
STOP_GAP_RESERVE_LEAD_SPEED,
|
||||
STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED,
|
||||
STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile,
|
||||
STOP_HOLD_FAST_DEPARTURE_DISTANCE, STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile,
|
||||
)
|
||||
|
||||
|
||||
@@ -43,6 +43,9 @@ class EnergyEnvelope:
|
||||
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_distances: tuple[float, float] = (-math.inf, -math.inf)
|
||||
departure_lead_track_ids: tuple[int, int] = (-1, -1)
|
||||
departure_lead_separations: tuple[float, float] = (-math.inf, -math.inf)
|
||||
usable_gap: float = math.inf
|
||||
closing_speed: float = 0.0
|
||||
@@ -85,7 +88,9 @@ class _ControllerPath:
|
||||
departure_samples: tuple[deque[float], deque[float]] = field(
|
||||
default_factory=lambda: (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES)),
|
||||
)
|
||||
departure_motion_samples: deque[float] = field(default_factory=lambda: deque(maxlen=CAP_FILTER_FRAMES))
|
||||
departure_references: list[float | None] = field(default_factory=lambda: [None, None])
|
||||
departure_track_ids: list[int] = field(default_factory=lambda: [-1, -1])
|
||||
pace: float | None = None
|
||||
state: AccelControllerState = AccelControllerState.inactive
|
||||
departure_frames: int = 0
|
||||
@@ -124,7 +129,9 @@ class _ControllerPath:
|
||||
self.lead_speed_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
|
||||
self.lead_accel_samples = deque([0.0] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
|
||||
self.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
|
||||
self.departure_motion_samples = deque(maxlen=CAP_FILTER_FRAMES)
|
||||
self.departure_references = [None, None]
|
||||
self.departure_track_ids = [-1, -1]
|
||||
self.pace = None
|
||||
self.state = AccelControllerState.inactive
|
||||
self.departure_frames = 0
|
||||
@@ -242,6 +249,8 @@ class AccelController:
|
||||
candidates: list[EnergyEnvelope] = []
|
||||
departure_candidates: list[tuple[float, int]] = []
|
||||
departure_speeds = [math.inf, math.inf]
|
||||
departure_distances = [-math.inf, -math.inf]
|
||||
departure_track_ids = [-1, -1]
|
||||
departure_separations = [-math.inf, -math.inf]
|
||||
departure_caps = [math.inf, math.inf]
|
||||
|
||||
@@ -280,6 +289,8 @@ class AccelController:
|
||||
))
|
||||
departure_candidates.append((departure_distance, lead_index))
|
||||
departure_speeds[lead_index] = v_lead_delay
|
||||
departure_distances[lead_index] = d_rel
|
||||
departure_track_ids[lead_index] = self._lead_track_id(lead)
|
||||
departure_separations[lead_index] = separation
|
||||
departure_caps[lead_index] = departure_cap
|
||||
|
||||
@@ -294,7 +305,9 @@ class AccelController:
|
||||
selected_lead_speed=selected.selected_lead_speed,
|
||||
selected_lead_accel=selected.selected_lead_accel,
|
||||
departure_lead_index=departure_lead_index, departure_lead_speed=departure_lead_speed,
|
||||
departure_cap=departure_caps[departure_lead_index], departure_lead_separations=tuple(departure_separations),
|
||||
departure_cap=departure_caps[departure_lead_index], departure_lead_speeds=tuple(departure_speeds),
|
||||
departure_lead_distances=tuple(departure_distances), departure_lead_track_ids=tuple(departure_track_ids),
|
||||
departure_lead_separations=tuple(departure_separations),
|
||||
usable_gap=selected.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,
|
||||
)
|
||||
@@ -307,37 +320,71 @@ class AccelController:
|
||||
def _lead_source(source) -> bool:
|
||||
return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
|
||||
|
||||
@staticmethod
|
||||
def _update_samples(path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
|
||||
def _update_samples(self, path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
|
||||
had_filtered_lead = math.isfinite(path.filtered_cap)
|
||||
has_lead = envelope.selected_lead >= 0
|
||||
path.cap_samples.append(envelope.cap if has_lead else math.inf)
|
||||
path.lead_speed_samples.append(envelope.selected_lead_speed if has_lead else math.inf)
|
||||
path.lead_accel_samples.append(envelope.selected_lead_accel if has_lead else 0.0)
|
||||
path.lead_loss_frames = 0 if has_lead else path.lead_loss_frames + 1
|
||||
for lead_index, separation in enumerate(envelope.departure_lead_separations):
|
||||
if math.isfinite(separation):
|
||||
path.departure_samples[lead_index].append(separation)
|
||||
for lead_index, distance in enumerate(envelope.departure_lead_distances):
|
||||
if not math.isfinite(distance):
|
||||
continue
|
||||
samples = path.departure_samples[lead_index]
|
||||
track_id = envelope.departure_lead_track_ids[lead_index]
|
||||
identity_changed = bool(samples) and track_id != path.departure_track_ids[lead_index] and (track_id >= 0 or path.departure_track_ids[lead_index] >= 0)
|
||||
max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * envelope.departure_lead_speeds[lead_index] * self.dt)
|
||||
geometry_jump = bool(samples) and abs(distance - samples[-1]) > max_distance_step
|
||||
if identity_changed or geometry_jump:
|
||||
samples.clear()
|
||||
path.departure_references[lead_index] = distance
|
||||
samples.append(distance)
|
||||
path.departure_track_ids[lead_index] = track_id
|
||||
lead_index = envelope.departure_lead_index
|
||||
if lead_index >= 0:
|
||||
distance = envelope.departure_lead_distances[lead_index]
|
||||
samples = path.departure_motion_samples
|
||||
max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * envelope.departure_lead_speed * self.dt)
|
||||
if samples and abs(distance - samples[-1]) > max_distance_step:
|
||||
samples.clear()
|
||||
samples.append(distance)
|
||||
return not had_filtered_lead and math.isfinite(path.filtered_cap)
|
||||
|
||||
@staticmethod
|
||||
def _seed_departure_tracking(path: _ControllerPath, envelope: EnergyEnvelope) -> None:
|
||||
path.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
|
||||
path.departure_motion_samples = 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_track_ids = list(envelope.departure_lead_track_ids)
|
||||
for lead_index, distance in enumerate(envelope.departure_lead_distances):
|
||||
if math.isfinite(distance):
|
||||
path.departure_samples[lead_index].append(distance)
|
||||
path.departure_references[lead_index] = distance
|
||||
if envelope.departure_lead_index >= 0:
|
||||
path.departure_motion_samples.append(envelope.departure_lead_distances[envelope.departure_lead_index])
|
||||
path.departure_frames = 0
|
||||
|
||||
@staticmethod
|
||||
def _creep_departure(path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
|
||||
def _departure_progress(path: _ControllerPath, envelope: EnergyEnvelope, minimum_distance: float, *, robust: bool = True) -> bool:
|
||||
lead_index = envelope.departure_lead_index
|
||||
if lead_index < 0 or envelope.departure_lead_speed <= STOP_HOLD_CREEP_SPEED:
|
||||
return False
|
||||
reference = path.departure_references[lead_index]
|
||||
separation = path.robust_departure_separation(lead_index)
|
||||
return reference is not None and separation - reference >= STOP_HOLD_CREEP_DISTANCE
|
||||
samples = path.departure_samples[lead_index]
|
||||
distance = path.robust_departure_separation(lead_index) if robust else samples[-1] if samples else -math.inf
|
||||
return reference is not None and distance - reference >= minimum_distance
|
||||
|
||||
@classmethod
|
||||
def _creep_departure(cls, path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
|
||||
return cls._departure_progress(path, envelope, STOP_HOLD_CREEP_DISTANCE)
|
||||
|
||||
@staticmethod
|
||||
def _recent_departure_motion(path: _ControllerPath) -> bool:
|
||||
samples = tuple(path.departure_motion_samples)[-STOP_HOLD_EXIT_FRAMES:]
|
||||
if len(samples) < STOP_HOLD_EXIT_FRAMES:
|
||||
return False
|
||||
deltas = np.diff(samples)
|
||||
return samples[-1] - samples[0] >= STOP_HOLD_FAST_DEPARTURE_DISTANCE and np.count_nonzero(deltas > 0.005) >= 2
|
||||
|
||||
def _enter_stop_hold(self, path: _ControllerPath, envelope: EnergyEnvelope) -> None:
|
||||
if path.state != AccelControllerState.stopHold:
|
||||
@@ -390,8 +437,8 @@ class AccelController:
|
||||
stop_evidence = (stopped_lead_hold or envelope.cap < 0.50 or filtered_cap < 0.50
|
||||
or (previous_stop and not path.launching) or invalid_lead)
|
||||
confirmed_creep_departure = (path.launching and path.departure_launch and has_lead
|
||||
and (envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED
|
||||
or self._creep_departure(path, envelope)))
|
||||
and (self._departure_progress(path, envelope, STOP_HOLD_FAST_DEPARTURE_DISTANCE)
|
||||
or self._recent_departure_motion(path)))
|
||||
if (path.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap)
|
||||
and has_lead and planner_accel <= BRAKING_ACCEL_LIMIT_THRESHOLD):
|
||||
path.braking_limited = True
|
||||
@@ -423,13 +470,17 @@ class AccelController:
|
||||
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
|
||||
raw_departure = ((has_lead and min(envelope.selected_lead_speed, envelope.departure_lead_speed) > STOP_HOLD_CREEP_SPEED
|
||||
and envelope.departure_cap > STOP_HOLD_CREEP_SPEED)
|
||||
fast_departure = (has_lead and min(envelope.selected_lead_speed, envelope.departure_lead_speed) > STOP_HOLD_EXIT_SPEED
|
||||
and envelope.departure_cap > STOP_HOLD_EXIT_SPEED)
|
||||
raw_departure = (fast_departure
|
||||
or (not envelope.lead_status and path.lead_loss_frames >= self.lead_loss_hold_frames))
|
||||
departed = self._creep_departure(path, envelope) or raw_departure
|
||||
if fast_departure and path.departure_frames == 0 and path.departure_motion_samples:
|
||||
path.departure_motion_samples = deque([path.departure_motion_samples[-1]], maxlen=CAP_FILTER_FRAMES)
|
||||
path.departure_frames = path.departure_frames + 1 if departed else 0
|
||||
path.pace = 0.0
|
||||
if path.departure_frames < STOP_HOLD_EXIT_FRAMES:
|
||||
fast_departure_confirmed = fast_departure and self._recent_departure_motion(path)
|
||||
if path.departure_frames < STOP_HOLD_EXIT_FRAMES or (fast_departure and not fast_departure_confirmed):
|
||||
return path.pace
|
||||
path.pace = base_speed
|
||||
path.state = AccelControllerState.release
|
||||
@@ -589,28 +640,29 @@ class AccelController:
|
||||
planner_speed = sanitized_v_ego if planner_speed is None else planner_speed
|
||||
valid_context = self._valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self._delay(),
|
||||
engaged, cruise_initialized)
|
||||
if valid_context and radar_fresh:
|
||||
feature_context = valid_context and bool(enabled)
|
||||
if feature_context and radar_fresh:
|
||||
envelope = self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, selected_profile, follow_personality)
|
||||
self._held_envelope = envelope
|
||||
elif valid_context and self._held_envelope is not None:
|
||||
elif feature_context and self._held_envelope is not None:
|
||||
envelope = self._held_envelope
|
||||
else:
|
||||
envelope = EnergyEnvelope(lead_status=self._radar_has_lead(radar_state))
|
||||
if not valid_context:
|
||||
if not feature_context:
|
||||
self._held_envelope = None
|
||||
|
||||
shadow_fresh = self._update_freshness(self.shadow, radar_fresh) if valid_context else False
|
||||
if valid_context and radar_fresh:
|
||||
shadow_fresh = self._update_freshness(self.shadow, radar_fresh) if feature_context else False
|
||||
if feature_context and radar_fresh:
|
||||
self._update_path(self.shadow, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max, previous_should_stop,
|
||||
previous_mpc_source, planner_speed, planner_accel)
|
||||
shadow_active = True
|
||||
elif valid_context and not shadow_fresh and self.shadow.pace is not None:
|
||||
elif feature_context and not shadow_fresh and self.shadow.pace is not None:
|
||||
shadow_active = True
|
||||
else:
|
||||
self.shadow.reset()
|
||||
shadow_active = False
|
||||
|
||||
live_context = valid_context and bool(enabled) and bool(acc_selected)
|
||||
live_context = feature_context and bool(acc_selected)
|
||||
live_fresh = self._update_freshness(self.live, radar_fresh) if live_context else False
|
||||
if live_context and radar_fresh:
|
||||
pace_target = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max,
|
||||
|
||||
@@ -53,6 +53,7 @@ STOP_HOLD_EXIT_SPEED = 0.80
|
||||
STOP_HOLD_EXIT_FRAMES = 4
|
||||
STOP_HOLD_CREEP_SPEED = 0.15
|
||||
STOP_HOLD_CREEP_DISTANCE = 0.30
|
||||
STOP_HOLD_FAST_DEPARTURE_DISTANCE = 0.03
|
||||
STOP_HOLD_MAX_LEAD_DISTANCE = 30.0
|
||||
STOP_GAP_RESERVE = 0.75
|
||||
STOP_GAP_RESERVE_LEAD_SPEED = 2.0
|
||||
|
||||
+123
-11
@@ -183,10 +183,13 @@ class TestMpcCeiling:
|
||||
assert np.all(ceiling >= 0.0)
|
||||
|
||||
def test_inactive_controller_has_no_custom_ceiling(self):
|
||||
result = update(make_controller(), enabled=False)
|
||||
controller = make_controller()
|
||||
result = update(controller, enabled=False)
|
||||
assert not result.active
|
||||
assert not result.shadow_active
|
||||
assert result.mpc_accel_max is None
|
||||
assert math.isinf(result.effective_accel_max)
|
||||
assert controller.live.pace is None and controller.shadow.pace is None
|
||||
|
||||
def test_profile_ceiling_does_not_interfere_while_planner_is_braking(self):
|
||||
controller = make_controller()
|
||||
@@ -513,8 +516,8 @@ class TestPaceAndLifecycle:
|
||||
controller = make_controller()
|
||||
held = enter_stop_hold(controller)
|
||||
assert controller.live.pace == 0.0
|
||||
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
|
||||
results = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)),
|
||||
base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
launch_index = next(index for index, result in enumerate(results) if result.launching)
|
||||
|
||||
assert held.state == AccelControllerState.stopHold
|
||||
@@ -539,19 +542,126 @@ class TestPaceAndLifecycle:
|
||||
assert held.state == AccelControllerState.stopHold
|
||||
assert held.target_speed == 0.0 and not held.launching
|
||||
|
||||
departing = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=2.0, radar_track_id=4887),
|
||||
make_lead(status=True, d_rel=6.48, v_lead_k=2.0, radar_track_id=4905))
|
||||
results = [update(controller, departing, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)]
|
||||
results = [
|
||||
update(controller, make_radar(make_lead(status=True, d_rel=6.04 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4887),
|
||||
make_lead(status=True, d_rel=6.12 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4905)),
|
||||
base_speed=8.0, v_ego=0.0)
|
||||
for frame in range(STOP_HOLD_EXIT_FRAMES)
|
||||
]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
|
||||
assert results[-1].launching and results[-1].departure_launching
|
||||
|
||||
def test_route_520_slow_lead_pulse_cannot_release_stop_hold_but_real_departure_can(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller, v_ego=0.0)
|
||||
speeds = (0.01, 0.03, 0.07, 0.10, 0.14, 0.20, 0.26, 0.32, 0.34, 0.33, 0.31, 0.28, 0.24, 0.20, 0.15, 0.09, 0.05, 0.01)
|
||||
offsets = (0.00, 0.00, 0.00, 0.01, 0.01, 0.02, 0.03, 0.04, 0.06, 0.07, 0.09, 0.11, 0.12, 0.13, 0.14, 0.15, 0.15, 0.16)
|
||||
|
||||
for offset, speed in zip(offsets, speeds, strict=True):
|
||||
pulse = make_radar(make_lead(status=True, d_rel=6.0 + offset, v_lead_k=speed, radar_track_id=2133))
|
||||
held = update(controller, pulse, base_speed=8.0, v_ego=0.0)
|
||||
assert held.state == AccelControllerState.stopHold
|
||||
assert held.target_speed == 0.0 and not held.launching
|
||||
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.2, v_lead_k=0.0, radar_track_id=2133))
|
||||
assert update(controller, stopped, base_speed=8.0, v_ego=0.0).state == AccelControllerState.stopHold
|
||||
results = [update(controller, make_radar(make_lead(status=True, d_rel=6.2 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=2133)),
|
||||
base_speed=8.0, v_ego=0.0) for frame in range(STOP_HOLD_EXIT_FRAMES)]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
|
||||
assert results[-1].launching and results[-1].departure_launching
|
||||
|
||||
def test_fast_speed_signal_that_slows_without_separating_never_releases_stop_hold(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller, v_ego=0.0)
|
||||
departing = make_radar(make_lead(status=True, d_rel=5.9, v_lead_k=2.0))
|
||||
results = [update(controller, departing, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)]
|
||||
slowed = update(controller, make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.2)), base_speed=8.0, v_ego=0.0)
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results)
|
||||
assert slowed.state == AccelControllerState.stopHold
|
||||
assert slowed.target_speed == 0.0 and not slowed.launching
|
||||
|
||||
def test_stop_hold_reseeds_departure_distance_when_radar_track_is_replaced(self):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
|
||||
update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
replacement = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=200))
|
||||
results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results)
|
||||
assert all(result.target_speed == 0.0 and not result.launching for result in results)
|
||||
|
||||
def test_stop_hold_rejects_persistent_same_track_distance_step(self):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
|
||||
update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
stepped = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=100))
|
||||
results = [update(controller, stepped, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results)
|
||||
assert all(result.target_speed == 0.0 and not result.launching for result in results)
|
||||
|
||||
def test_stop_hold_reseeds_non_selected_departure_lead_when_its_track_is_replaced(self):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.2, radar_track_id=100),
|
||||
make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200))
|
||||
update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
replacement = make_radar(make_lead(status=True, d_rel=3.4, v_lead_k=0.2, radar_track_id=101),
|
||||
make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200))
|
||||
envelope = controller.calculate_energy_envelope(replacement, 0.0, 0.0, AccelProfile.normal)
|
||||
results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
|
||||
assert envelope.selected_lead == 1 and envelope.departure_lead_index == 0
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results)
|
||||
assert all(result.target_speed == 0.0 and not result.launching for result in results)
|
||||
|
||||
def test_genuine_departure_survives_lead_slot_and_track_flicker(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller, v_ego=0.0)
|
||||
results = []
|
||||
for frame in range(STOP_HOLD_EXIT_FRAMES):
|
||||
moving = make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)
|
||||
secondary = make_lead(status=True, d_rel=7.0, v_lead_k=2.0, radar_track_id=200)
|
||||
results.append(update(controller, make_radar(moving, secondary) if frame % 2 == 0 else make_radar(secondary, moving),
|
||||
base_speed=8.0, v_ego=0.0))
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
|
||||
assert results[-1].launching and results[-1].departure_launching
|
||||
|
||||
def test_fast_speed_glitch_without_distance_progress_stays_in_stop_hold(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
glitch = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.9, radar_track_id=100))
|
||||
results = [update(controller, glitch, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)]
|
||||
results.append(update(controller, stopped, base_speed=8.0, v_ego=0.0))
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results)
|
||||
assert all(result.target_speed == 0.0 and not result.launching for result in results)
|
||||
|
||||
def test_moving_departure_does_not_reenter_stop_hold_when_speed_crosses_exit_threshold(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
distance = 6.0
|
||||
results = []
|
||||
for speed in (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72):
|
||||
distance += speed * DT_MDL
|
||||
radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=100))
|
||||
results.append(update(controller, radar, base_speed=8.0, v_ego=0.0))
|
||||
|
||||
launch_index = next(index for index, result in enumerate(results) if result.launching)
|
||||
assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:])
|
||||
assert all(result.target_speed > 0.0 and result.departure_launching for result in results[launch_index:])
|
||||
|
||||
def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller)
|
||||
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
|
||||
|
||||
for frame in range(STOP_HOLD_EXIT_FRAMES):
|
||||
departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0))
|
||||
fresh = update(controller, departing, base_speed=8.0, v_ego=0.1)
|
||||
held = update(controller, departing, base_speed=8.0, v_ego=0.1, radar_fresh=False,
|
||||
previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=0.01)
|
||||
@@ -623,14 +733,15 @@ class TestPaceAndLifecycle:
|
||||
results.append(update(controller, creeping, base_speed=8.0, v_ego=0.0))
|
||||
|
||||
launch_index = next(index for index, result in enumerate(results) if result.launching)
|
||||
assert launch_index * DT_MDL <= 2.0
|
||||
assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:])
|
||||
assert all(result.target_speed > 0.0 for result in results[launch_index:])
|
||||
|
||||
def test_departure_dropout_holds_without_resurrecting_stop_hold(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller)
|
||||
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
|
||||
results = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)),
|
||||
base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
launched = next(result for result in results if result.launching)
|
||||
before_dropout = results[-1]
|
||||
dropout = [update(controller, base_speed=8.0, v_ego=0.1) for _ in range(controller.lead_loss_hold_frames + 1)]
|
||||
@@ -644,8 +755,8 @@ class TestPaceAndLifecycle:
|
||||
def test_invalid_departure_geometry_returns_to_stop_hold(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller)
|
||||
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
|
||||
for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES):
|
||||
for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES):
|
||||
departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0))
|
||||
launched = update(controller, departing, base_speed=8.0, v_ego=0.1)
|
||||
invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0))
|
||||
guarded = update(controller, invalid, base_speed=8.0, v_ego=0.1)
|
||||
@@ -702,6 +813,7 @@ class TestPaceAndLifecycle:
|
||||
assert path.pace is None and path.matched_accel_limit is None
|
||||
assert path.state == AccelControllerState.inactive
|
||||
assert path.departure_frames == path.active_frames == path.lead_loss_frames == path.stale_frames == 0
|
||||
assert not path.departure_motion_samples
|
||||
assert not path.launching and not path.departure_launch and not path.matched_lead
|
||||
assert not path.braking_limited and not path.braking_handoff and not path.pace_reserve_armed
|
||||
assert math.isinf(path.filtered_cap) and math.isinf(path.filtered_lead_speed) and path.filtered_lead_accel == 0.0
|
||||
|
||||
+22
@@ -75,6 +75,7 @@ def test_profile_enum_keeps_toyota_importable():
|
||||
|
||||
assert AccelPersonality.schema.enumerants == expected
|
||||
assert CarState.__module__ == "opendbc.car.toyota.carstate"
|
||||
assert "Params()" not in inspect.getsource(CarState.update)
|
||||
|
||||
|
||||
def test_mpc_accepts_optional_acceleration_ceiling_without_changing_stock_bounds():
|
||||
@@ -356,6 +357,7 @@ def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.accel_personality = int(AccelProfile.normal)
|
||||
planner.accel_personality_enabled = True
|
||||
planner.accel_personality_available = True
|
||||
planner._radar_fresh_this_cycle = True
|
||||
planner.a_desired = -0.4
|
||||
planner.v_desired_filter = SimpleNamespace(x=9.5)
|
||||
@@ -377,6 +379,25 @@ def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
|
||||
assert received["radar_fresh"] is True
|
||||
|
||||
|
||||
def test_controller_is_disabled_when_openpilot_longitudinal_control_is_unavailable():
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.accel_personality = int(AccelProfile.normal)
|
||||
planner.accel_personality_enabled = True
|
||||
planner.accel_personality_available = False
|
||||
planner._radar_fresh_this_cycle = True
|
||||
planner.accel_controller = SimpleNamespace(
|
||||
update=lambda *_args, **kwargs: SimpleNamespace(target_speed=20.0, received_enabled=kwargs["enabled"]),
|
||||
)
|
||||
planner.a_desired = 0.0
|
||||
planner.v_desired_filter = SimpleNamespace(x=10.0)
|
||||
planner.mpc = SimpleNamespace(source=MpcLongitudinalPlanSource.cruise)
|
||||
sm = {"radarState": radar_state(), "carState": SimpleNamespace(vEgo=10.0, aEgo=0.0), "selfdriveState": SimpleNamespace(personality=0)}
|
||||
|
||||
planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False)
|
||||
|
||||
assert not planner.accel_controller_result.received_enabled
|
||||
|
||||
|
||||
def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner._radar_log_mono_time = None
|
||||
@@ -388,6 +409,7 @@ def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
|
||||
planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
|
||||
planner.accel_personality = int(AccelProfile.normal)
|
||||
planner.accel_personality_enabled = True
|
||||
planner.accel_personality_available = True
|
||||
planner.output_a_target = 0.0
|
||||
planner.a_desired = 0.0
|
||||
planner.v_desired_filter = SimpleNamespace(x=10.0)
|
||||
|
||||
+83
@@ -0,0 +1,83 @@
|
||||
import pytest
|
||||
|
||||
from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.ford.values import CAR as FORD
|
||||
from opendbc.car.gm.values import CAR as GM
|
||||
from opendbc.car.honda.values import CAR as HONDA
|
||||
from opendbc.car.hyundai.values import CAR as HYUNDAI
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans
|
||||
|
||||
|
||||
VEHICLES = [
|
||||
pytest.param(TOYOTA.TOYOTA_RAV4_TSS2, (True, False, 0.0, -2.0, 0.25, 0.25, 0.3), id="toyota-rav4-tss2"),
|
||||
pytest.param(HONDA.HONDA_ACCORD, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="honda-accord"),
|
||||
pytest.param(GM.CHEVROLET_BOLT_EUV, (True, False, 0.0, -2.0, 0.25, 0.25, 2.0), id="gm-bolt-euv"),
|
||||
pytest.param(HYUNDAI.HYUNDAI_SONATA, (True, True, 1.0, -2.0, 0.1, 0.5, 0.8), id="hyundai-sonata"),
|
||||
pytest.param(FORD.FORD_ESCAPE_MK4, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="ford-escape"),
|
||||
]
|
||||
|
||||
|
||||
def get_car_params(candidate):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
interface = interfaces[candidate]
|
||||
CP = interface.get_params(candidate, fingerprint, [], True, False, False)
|
||||
CP_SP = interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False)
|
||||
return CP, CP_SP
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("candidate", "expected"), VEHICLES)
|
||||
def test_real_vehicle_longcontrol_stop_and_start(candidate, expected):
|
||||
CP, CP_SP = get_car_params(candidate)
|
||||
expected_long, expected_starting, *expected_tuning = expected
|
||||
|
||||
assert CP.openpilotLongitudinalControl is expected_long
|
||||
assert CP.startingState is expected_starting
|
||||
assert (CP.startAccel, CP.stopAccel, CP.vEgoStarting, CP.vEgoStopping, CP.stoppingDecelRate) == pytest.approx(expected_tuning)
|
||||
|
||||
stop_speeds = [CP.vEgoStopping - 0.01] * 2
|
||||
drive_speeds = [CP.vEgoStopping + 0.01] * 2
|
||||
_, should_stop = get_accel_from_plan(stop_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping)
|
||||
_, should_drive = get_accel_from_plan(drive_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping)
|
||||
assert should_stop
|
||||
assert not should_drive
|
||||
|
||||
departure_state = long_control_state_trans(
|
||||
CP,
|
||||
CP_SP,
|
||||
True,
|
||||
LongCtrlState.stopping,
|
||||
CP.vEgoStarting - 0.01,
|
||||
should_drive,
|
||||
brake_pressed=False,
|
||||
cruise_standstill=False,
|
||||
)
|
||||
assert departure_state == (LongCtrlState.starting if CP.startingState else LongCtrlState.pid)
|
||||
assert (
|
||||
long_control_state_trans(
|
||||
CP,
|
||||
CP_SP,
|
||||
True,
|
||||
departure_state,
|
||||
CP.vEgoStarting + 0.01,
|
||||
should_drive,
|
||||
brake_pressed=False,
|
||||
cruise_standstill=False,
|
||||
)
|
||||
== LongCtrlState.pid
|
||||
)
|
||||
|
||||
CS = structs.CarState()
|
||||
CS.vEgo = 0.0
|
||||
CS.aEgo = 0.0
|
||||
control = LongControl(CP, CP_SP)
|
||||
|
||||
stopping_accel = control.update(True, CS, 0.0, should_stop, (-3.0, 2.0))
|
||||
assert control.long_control_state == LongCtrlState.stopping
|
||||
assert stopping_accel == pytest.approx(-CP.stoppingDecelRate * DT_CTRL)
|
||||
|
||||
departure_accel = control.update(True, CS, 0.0, should_drive, (-3.0, 2.0))
|
||||
assert control.long_control_state == departure_state
|
||||
assert departure_accel == pytest.approx(CP.startAccel)
|
||||
@@ -46,6 +46,7 @@ class LongitudinalPlannerSP:
|
||||
self.e2e_alerts_helper = E2EAlertsHelper()
|
||||
self.accel_controller = AccelController(CP, dt=dt)
|
||||
self.accel_controller_result = None
|
||||
self.accel_personality_available = bool(CP.openpilotLongitudinalControl)
|
||||
self._accel_jerk_smoothing_blocked = False
|
||||
self._accel_required_decel_samples = deque(maxlen=4)
|
||||
self._accel_required_decel_lead = -1
|
||||
@@ -125,7 +126,8 @@ class LongitudinalPlannerSP:
|
||||
self.accel_controller_result = self.accel_controller.update(
|
||||
sm['radarState'], base_speed=base_speed, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
|
||||
profile=self.accel_personality, follow_personality=sm['selfdriveState'].personality,
|
||||
enabled=self.accel_personality_enabled, acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized,
|
||||
enabled=self.accel_personality_enabled and self.accel_personality_available,
|
||||
acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized,
|
||||
stock_accel_max=stock_accel_max, previous_should_stop=previous_should_stop,
|
||||
radar_fresh=getattr(self, '_radar_fresh_this_cycle', True),
|
||||
previous_mpc_source=getattr(getattr(self, 'mpc', None), 'source', None),
|
||||
|
||||
@@ -563,6 +563,59 @@ def test_stop_hold_survives_short_full_field_dropout():
|
||||
_assert_no_new_solver_failures(trace, baseline)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("replacement_track_id", (100, 200), ids=("same-track", "replacement"))
|
||||
def test_stop_hold_rejects_persistent_same_slot_range_step(replacement_track_id):
|
||||
step_time = 1.0
|
||||
|
||||
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
|
||||
if lead_name == "leadTwo":
|
||||
return None
|
||||
stepped = current_time >= step_time
|
||||
speed = 0.2 if stepped else 0.0
|
||||
return truth | {"dRel": truth["dRel"] + 0.4 * stepped, "vLead": speed, "vLeadK": speed, "vRel": speed,
|
||||
"aLeadK": 0.0, "radarTrackId": replacement_track_id if stepped else 100, "radar": True}
|
||||
|
||||
trace = _run(
|
||||
duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
|
||||
v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
|
||||
)
|
||||
|
||||
assert np.all(trace.state == int(AccelControllerState.stopHold))
|
||||
assert np.all(trace.target_speed == 0.0)
|
||||
assert trace.should_stop.all()
|
||||
assert np.max(trace.speed) < 1e-3
|
||||
assert not trace.fcw.any()
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
def test_moving_departure_crossing_exit_speed_releases_once():
|
||||
departure_time = 1.0
|
||||
speeds = (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72)
|
||||
|
||||
def lead_speed(current_time: float) -> float:
|
||||
frame = round((current_time - departure_time) / DT_MDL)
|
||||
return 0.0 if frame < 0 else speeds[min(frame, len(speeds) - 1)]
|
||||
|
||||
def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
|
||||
return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 100, "radar": True}
|
||||
|
||||
trace = _run(
|
||||
duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
|
||||
v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
|
||||
)
|
||||
after_departure = trace.time >= departure_time
|
||||
stop_hold = int(AccelControllerState.stopHold)
|
||||
releases = np.flatnonzero((trace.state[:-1] == stop_hold) & (trace.state[1:] != stop_hold)) + 1
|
||||
|
||||
assert len(releases) == 1
|
||||
assert trace.launching[releases[0]]
|
||||
assert not np.any(trace.state[releases[0]:] == stop_hold)
|
||||
assert np.count_nonzero(np.diff(trace.should_stop[after_departure].astype(int))) == 1
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target[after_departure])
|
||||
assert not trace.fcw.any()
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
def test_route_51d_duplicate_lead_speed_pulse_cannot_release_stop_hold():
|
||||
pulse_start = 1.0
|
||||
departure_time = 2.0
|
||||
@@ -597,6 +650,45 @@ def test_route_51d_duplicate_lead_speed_pulse_cannot_release_stop_hold():
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
def test_route_520_slow_lead_pulse_cannot_release_stop_hold_or_dampen_real_departure():
|
||||
pulse_start = 1.0
|
||||
departure_time = 2.5
|
||||
pulse_speeds = (0.01, 0.03, 0.07, 0.10, 0.14, 0.20, 0.26, 0.32, 0.34, 0.33, 0.31, 0.28, 0.24, 0.20, 0.15, 0.09, 0.05, 0.01)
|
||||
pulse_offsets = (0.00, 0.00, 0.00, 0.01, 0.01, 0.02, 0.03, 0.04, 0.06, 0.07, 0.09, 0.11, 0.12, 0.13, 0.14, 0.15, 0.15, 0.16)
|
||||
|
||||
def lead_speed(current_time: float) -> float:
|
||||
return 0.0 if current_time < departure_time else 2.0
|
||||
|
||||
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
|
||||
if lead_name == "leadTwo":
|
||||
return None
|
||||
pulse_frame = round((current_time - pulse_start) / DT_MDL)
|
||||
if 0 <= pulse_frame < len(pulse_speeds):
|
||||
speed = pulse_speeds[pulse_frame]
|
||||
return truth | {"dRel": 6.0 + pulse_offsets[pulse_frame], "vLead": speed, "vLeadK": speed, "vRel": speed,
|
||||
"aLeadK": 0.0, "radarTrackId": 2133, "radar": True}
|
||||
return truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True}
|
||||
|
||||
common = dict(
|
||||
duration=4.0, lead_relevancy=True, speed=0.0, distance_lead=6.0, v_lead=lead_speed, v_cruise=8.0,
|
||||
lead_observation_fn=observe, actuator_model=PRIUS_TSS2_ROUTE_MODEL,
|
||||
)
|
||||
baseline = _run(controller_enabled=False, **common)
|
||||
trace = _run(controller_enabled=True, **common)
|
||||
pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL)
|
||||
release = np.flatnonzero((trace.time >= departure_time) & trace.launching)
|
||||
|
||||
assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold))
|
||||
assert np.all(trace.target_speed[pulse] == 0.0)
|
||||
assert np.max(trace.speed[trace.time < departure_time]) < 0.01
|
||||
assert len(release) and trace.time[release[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9
|
||||
assert trace.a_target[release[0]] > 0.05
|
||||
assert np.allclose(trace.a_target[release[0]:], baseline.a_target[release[0]:], atol=1e-5, rtol=0.0)
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time])
|
||||
assert not trace.fcw.any()
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
|
||||
def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_second(actuator_delay, actuator_lag):
|
||||
departure_time = 1.0
|
||||
@@ -723,6 +815,7 @@ def test_constant_creep_departure_does_not_pulse_between_launch_and_stop_hold():
|
||||
)
|
||||
launched = np.flatnonzero((trace.time >= departure_time) & trace.launching)
|
||||
assert len(launched)
|
||||
assert trace.time[launched[0]] <= departure_time + 2.0
|
||||
after_launch = slice(launched[0], None)
|
||||
|
||||
assert not np.any(trace.state[after_launch] == int(AccelControllerState.stopHold))
|
||||
|
||||
Reference in New Issue
Block a user