Compare commits

..

3 Commits

4 changed files with 443 additions and 5 deletions
+10 -3
View File
@@ -3,7 +3,7 @@ from collections.abc import Callable
import base64
import ctypes
import gc
from functools import cached_property
from functools import cached_property, partial
import os
os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
os.environ.setdefault('AM_POWER_LIMIT', '100')
@@ -43,6 +43,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob, wait_for_chestnut
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -54,7 +55,8 @@ BIG_MODEL_TIMEOUT = 60
def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
lat_action_t: float, long_action_t: float, v_ego: float,
turn_shaper: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
if 'action' not in model_output:
plan = model_output['plan'][0]
desired_accel = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
@@ -69,6 +71,8 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
else:
desired_accel = model_output['action'][0,1]
desired_curvature = model_output['action'][0,0] / (max(1.0, v_ego))**2
if turn_shaper is not None:
desired_curvature = turn_shaper(desired_curvature)
stop = should_stop(v_ego, desired_accel)
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
if v_ego > MIN_LAT_CONTROL_SPEED:
@@ -340,6 +344,7 @@ def main(demo=False):
DH = DesireHelper()
RELC = RoadEdgeLaneChangeController()
turn_planner = TurnPlanner(CP.wheelbase)
while True:
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -448,7 +453,9 @@ def main(demo=False):
if not sm['carControl'].latActive:
prev_action.desiredCurvature = sm['controlsState'].curvature
action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
partial(turn_planner.update, model_output, v_ego, sm['controlsState'].curvature,
sm['carControl'].latActive, is_rhd))
prev_action = action
fill_model_msg(modelv2_send, model_output, action,
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
+9 -2
View File
@@ -42,6 +42,7 @@ from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
from openpilot.sunnypilot.modeld_v2.blendful import Blendful
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
@@ -235,7 +236,8 @@ class ModelState(ModelStateBase):
def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
lat_action_t: float, long_action_t: float, v_ego: float,
blendful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
blendful: Callable[[float], float] | None = None,
turn_shaper: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
if 'action' not in model_output:
plan = model_output['plan'][0]
desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
@@ -249,6 +251,8 @@ class ModelState(ModelStateBase):
desired_curvature = model_output['action'][0, 0] / (max(2.5, v_ego))**2
if blendful is not None:
desired_curvature += blendful(desired_curvature)
if turn_shaper is not None:
desired_curvature = turn_shaper(desired_curvature)
stop = v_ego < 0.3 and desired_accel < 0.1
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
@@ -381,6 +385,7 @@ def main(demo=False):
RELC = RoadEdgeLaneChangeController()
blendful = Blendful()
blendful_enabled = False
turn_planner = TurnPlanner(CP.wheelbase)
while True:
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -506,7 +511,9 @@ def main(demo=False):
prev_action.desiredCurvature = sm['controlsState'].curvature
blendful_active = desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off
action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled))
partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled),
partial(turn_planner.update, model_output, v_ego, sm['controlsState'].curvature,
sm['carControl'].latActive, is_rhd))
prev_action = action
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
@@ -0,0 +1,210 @@
"""
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.
"""
import math
import numpy as np
from openpilot.common.realtime import DT_MDL
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
from openpilot.sunnypilot.modeld_v2.turn_planner import TurnPlanner, compute_plan_peak_curv
def make_plan(curv: float, speed: float = 4.0) -> dict[str, np.ndarray]:
"""Build a synthetic model_output dict with a constant-curvature arc plan."""
t = np.array(ModelConstants.T_IDXS, dtype=np.float32)
s = speed * t
plan = np.zeros((1, len(t), ModelConstants.PLAN_WIDTH), dtype=np.float32)
if abs(curv) < 1e-6:
plan[0, :, Plan.POSITION][:, 0] = s
plan[0, :, Plan.POSITION][:, 1] = 0.0
plan[0, :, Plan.T_FROM_CURRENT_EULER][:, 2] = 0.0
else:
r = 1.0 / curv
theta = s * curv
plan[0, :, Plan.POSITION][:, 0] = r * np.sin(theta)
plan[0, :, Plan.POSITION][:, 1] = r * (1.0 - np.cos(theta))
plan[0, :, Plan.T_FROM_CURRENT_EULER][:, 2] = theta
return {'plan': plan}
class TestTurnPlanner(OpenpilotTestCase):
def test_plan_peak_curv_extraction(self):
assert abs(compute_plan_peak_curv(make_plan(0.0, 5.0))) < 1e-5
pk = compute_plan_peak_curv(make_plan(0.12, 4.0))
assert abs(pk - 0.12) < 0.01
pk_l = compute_plan_peak_curv(make_plan(-0.15, 4.0))
assert abs(pk_l + 0.15) < 0.01
assert compute_plan_peak_curv({}) == 0.0
def test_untouched_at_highway_speed(self):
tp = TurnPlanner(wheelbase=2.89)
for i in range(60):
k = 0.01 * math.sin(i * 0.1)
out = tp.update(make_plan(k, 25.0), v_ego=25.0, actual_curv=k, active=True, is_rhd=False, model_curv=k)
assert out == k
def test_untouched_on_gentle_low_speed_curve(self):
tp = TurnPlanner(wheelbase=2.89)
for i in range(80):
k = 0.025 * min(1.0, i / 20.0)
out = tp.update(make_plan(k, 6.0), v_ego=6.0, actual_curv=k, active=True, is_rhd=False, model_curv=k)
assert out == k
def test_sign_preservation_and_magnitude_bound_under_jitter(self):
"""Output must never oppose model_curv or exceed APEX_PATH_BOOST_CAP * |model_curv|."""
tp = TurnPlanner(wheelbase=2.89)
# Reproduces a5 seg31 Turn 1: brief negative jitter then sharp positive right turn
ks = [-0.015] * 15 + [0.016, 0.05, 0.11, 0.14, 0.17, 0.18, 0.14, 0.08, 0.02, -0.01]
plan_right = make_plan(0.15, 3.0)
for k in ks:
out = tp.update(plan_right, v_ego=3.0, actual_curv=k * 0.7, active=True, is_rhd=False, model_curv=k)
assert abs(out) <= abs(k) * 1.35 + 1e-9
if k > 0:
assert out >= 0.0
elif k < 0:
assert out <= 0.0
def test_zero_lag_on_turn_exit_unwinding(self):
tp = TurnPlanner(wheelbase=2.89)
plan_tight = make_plan(0.14, 4.0)
# Ramp up entry and hold through apex so buffer is at peak curvature
for i in range(25):
k = 0.14 * (i / 25.0)
tp.update(plan_tight, v_ego=4.0, actual_curv=k * 0.8, active=True, is_rhd=False, model_curv=k)
for _ in range(15):
tp.update(plan_tight, v_ego=4.0, actual_curv=0.14, active=True, is_rhd=False, model_curv=0.14)
# Now unwind monotonically: output must equal model_curv with zero lag
for i in range(1, 20):
k = max(0.0, 0.14 * (1.0 - i / 15.0))
out = tp.update(make_plan(k, 4.0), v_ego=4.0, actual_curv=k + 0.01, active=True, is_rhd=False, model_curv=k)
assert abs(out - k) < 1e-3
def test_standstill_launch_and_mid_turn_pause(self):
tp = TurnPlanner(wheelbase=2.89)
# Stopped straight at stop bar, then launches into a sharp right turn
for _ in range(10):
tp.update(make_plan(0.12, 0.0), v_ego=0.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.002)
# First few steps of launch should hold back early curvature ramp
outs = []
for i in range(1, 20):
k = 0.006 * i
outs.append(tp.update(make_plan(0.12, 2.5), v_ego=2.5, actual_curv=0.004 * i, active=True, is_rhd=False, model_curv=k))
assert outs[10] < 0.006 * 11 - 0.01
# Pause mid-turn (actual_curv = 0.05) should preserve turn_psi and not dip below 0.05 on resume
tp.turn_psi = math.radians(30.0)
tp.update(make_plan(0.08, 0.0), v_ego=0.0, actual_curv=0.05, active=True, is_rhd=False, model_curv=0.05)
assert tp.turn_psi >= math.radians(29.0)
resumed = tp.update(make_plan(0.08, 2.0), v_ego=2.0, actual_curv=0.05, active=True, is_rhd=False, model_curv=0.06)
assert resumed >= 0.05 - 1e-6
def test_wheelbase_and_rhd_scaling(self):
tp_short = TurnPlanner(wheelbase=2.4)
tp_long = TurnPlanner(wheelbase=3.4)
plan_r = make_plan(0.14, 4.2)
for _ in range(25):
tp_short.update(plan_r, 4.2, 0.0, True, False, 0.02)
tp_long.update(plan_r, 4.2, 0.0, True, False, 0.02)
assert tp_long.shift_dist > tp_short.shift_dist * 1.3
# RHD gives larger shift on left turns (k < 0) than right turns (k > 0)
tp_rhd_l = TurnPlanner(wheelbase=2.89)
tp_rhd_r = TurnPlanner(wheelbase=2.89)
for _ in range(25):
tp_rhd_l.update(make_plan(-0.14, 3.5), 3.5, 0.0, True, True, -0.02)
tp_rhd_r.update(make_plan(0.14, 3.5), 3.5, 0.0, True, True, 0.02)
assert tp_rhd_l.shift_dist > tp_rhd_r.shift_dist * 1.2
def test_closed_loop_90deg_turn_clears_curb_without_oncoming_overshoot(self):
"""Closed-loop Tesla Model Y 90-deg right turn around R=2.5 m curb."""
L, W_half, x_cam, lane_w, curb_r = 2.89, 0.96, 1.50, 3.60, 2.5
y_curb = lane_w / 2.0
x_dest = curb_r + lane_w / 2.0
x_yellow = x_dest + lane_w / 2.0
pts = [(x, 0.0) for x in np.linspace(-20, x_dest - 2.0, 200)]
pts += [(x_dest - 2.0 + 2.0 * math.cos(th), 2.0 + 2.0 * math.sin(th)) for th in np.linspace(-math.pi / 2, 0, 50)]
pts += [(x_dest, y) for y in np.linspace(2.0, 25.0, 200)]
ref = np.array(pts)
def run(use_planner: bool) -> tuple[float, float]:
X, Y, psi, k_act = -15.0, 0.0, 0.0, 0.0
v = 3.5
tp = TurnPlanner(wheelbase=L, dt=DT_MDL) if use_planner else None
min_clr, max_ovr = 99.0, -99.0
for _ in range(400):
xc, yc = X + x_cam * math.cos(psi), Y + x_cam * math.sin(psi)
i_near = int(np.argmin(np.hypot(ref[:, 0] - xc, ref[:, 1] - yc)))
i_tgt = i_near
while i_tgt < len(ref) - 1 and np.hypot(ref[i_tgt, 0] - xc, ref[i_tgt, 1] - yc) < 6.5:
i_tgt += 1
dx, dy = ref[i_tgt, 0] - xc, ref[i_tgt, 1] - yc
lx = dx * math.cos(psi) + dy * math.sin(psi)
ly = -dx * math.sin(psi) + dy * math.cos(psi)
k_mod = float(np.clip(2.0 * ly / max(lx**2 + ly**2, 1.0), -0.19, 0.19))
plan = make_plan(0.14 if (max(0.0, x_dest - xc) < 15.0 and psi < math.radians(75)) else 0.0, v)
k_cmd = tp.update(plan, v, k_act, True, False, k_mod) if tp is not None else k_mod
k_act += (float(np.clip(k_cmd, -0.19, 0.19)) - k_act) * (DT_MDL / (0.22 + DT_MDL))
ds = v * DT_MDL
psi += k_act * ds
X += ds * math.cos(psi)
Y += ds * math.sin(psi)
x_ir, y_ir = X - W_half * math.sin(psi), Y + W_half * math.cos(psi)
x_of, y_of = X + L * math.cos(psi) + W_half * math.sin(psi), Y + L * math.sin(psi) - W_half * math.cos(psi)
if x_ir <= 0.0:
clr = y_curb - y_ir
elif y_ir >= y_curb + curb_r:
clr = x_ir - curb_r
else:
clr = math.hypot(x_ir, y_ir - (y_curb + curb_r)) - curb_r
min_clr = min(min_clr, clr)
if y_of > 2.0:
max_ovr = max(max_ovr, x_of - x_yellow)
if Y > 15.0:
break
return min_clr, max_ovr
base_clr, _ = run(False)
plan_clr, plan_ovr = run(True)
assert base_clr < -0.5 # baseline curbs the rear wheel badly
assert plan_clr > 0.20 # TurnPlanner clears the curb by > 20 cm
assert plan_ovr < 0.0 # stays inside the oncoming yellow line
def test_rolling_speed_taper(self):
"""Shift is strong at low speed, light at rolling speed, 0 at 19+ mph, and time-capped at crawl."""
tp_crawl = TurnPlanner(wheelbase=2.89)
tp_low = TurnPlanner(wheelbase=2.89)
tp_roll = TurnPlanner(wheelbase=2.89)
tp_fast = TurnPlanner(wheelbase=2.89)
for _ in range(30):
tp_crawl.update(make_plan(0.14, 1.8), v_ego=1.8, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
tp_low.update(make_plan(0.14, 3.5), v_ego=3.5, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
tp_roll.update(make_plan(0.14, 6.0), v_ego=6.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
tp_fast.update(make_plan(0.14, 9.0), v_ego=9.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02)
assert tp_crawl.shift_dist <= 1.8 * 0.48 + 0.05 # time-capped so crawl starts don't stall
assert tp_low.shift_dist > 1.3
assert 0.3 < tp_roll.shift_dist < 0.7
assert tp_fast.shift_dist == 0.0
def test_apex_1s_path_boost_and_exit_unwind(self):
"""Once past entry, blends with 1s path to boost under-steering apex and unwind hanging exit."""
tp = TurnPlanner(wheelbase=2.89)
# Drive past entry (psi > 30 deg)
for _ in range(35):
tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10)
# Apex (psi ~ 48 deg): action under-steers at 0.10 while 1s path says 0.14 -> boosted cleanly above 0.10
apex_out = tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10)
assert apex_out > 0.115
# Advance past 60 deg into late-turn exit
for _ in range(20):
tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.14)
# Exit hang: action still stuck at 0.20 while 1s path already unwound to 0.10 -> pulls down cleanly
exit_out = tp.update(make_plan(0.10, 4.0), v_ego=4.0, actual_curv=0.15, active=True, is_rhd=False, model_curv=0.20)
assert exit_out < 0.15
@@ -0,0 +1,214 @@
"""
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.
"""
import math
from collections import deque
import numpy as np
from openpilot.common.realtime import DT_MDL
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
# Two-stage rear-axle protected turn planner:
# Stage 1 (Entry): wheelbase-scaled spatial delay on curvature buildup so the
# inside rear tire clears the corner curb instead of off-tracking over it.
# Capped in time (MAX_SHIFT_TIME) so crawl-speed starts never feel stalled.
# Stage 2 (Apex & Exit): once past initial corner entry, smoothly blends
# action curvature with the model's own 1-second path curvature so tight
# apexes don't under-steer/go wide and exits unwind cleanly.
SHIFT_WHEELBASE_RATIO = 0.58 # fraction of wheelbase to delay turn-in (~1.68 m for 2.89 m WB)
WHEELBASE_CLIP = (2.0, 4.0) # m
OPPOSITE_SIDE_WEIGHT = 0.70 # lighter shift for non-curb-side turns
MAX_SHIFT_TIME = 0.48 # s, caps entry delay at crawl speeds (2-5 mph)
TURN_SPEED_TAPER = (7.5, 10.5) # m/s (~17-23.5 mph): overall active window
SHIFT_SPEED_BP = (3.5, 6.0, 8.5) # m/s (~7.8, 13.4, 19.0 mph)
SHIFT_SPEED_V = (1.0, 0.35, 0.0) # full from stop, light on rolling turns, 0 above 19 mph
CURV_GATE = (0.04, 0.09) # 1/m (R=25 m -> R=11 m): ignores gentle curves
ENTRY_PHASE_RELEASE_DEG = (8.0, 22.0) # deg: release entry shift crisply once rotating
APEX_BLEND_PHASE_DEG = (10.0, 25.0) # deg: ramp in 1s-path blend through apex & exit
APEX_PATH_BLEND_MAX = 0.65 # max weight on 1s path curvature during apex/exit
APEX_PATH_BOOST_CAP = 1.35 # max ratio vs |model_curv| for safety
PATH_AVG_HORIZON_S = 1.0 # s, horizon for average path curvature
SHIFT_BUILD_TAU = 0.15 # s
SHIFT_RELEASE_TAU = 0.07 # s
APEX_BLEND_TAU = 0.12 # s
BUFFER_MAX_DIST = 4.5 # m
MIN_MOVE_SPEED = 0.3 # m/s
MID_TURN_CURV_THRESH = 0.02 # 1/m
PLAN_S_RANGE = (1.0, 18.0) # m
PLAN_MIN_SEG_DS = 0.8 # m
T_IDXS = np.array(ModelConstants.T_IDXS, dtype=np.float32)
def compute_plan_metrics(model_output: dict[str, np.ndarray]) -> tuple[float, float]:
"""Return (peak_upcoming_curv, avg_1s_path_curv) from model_output['plan']."""
plan_arr = model_output.get('plan')
if plan_arr is None or plan_arr.ndim < 3 or plan_arr.shape[1] < 3:
return 0.0, 0.0
plan = plan_arr[0]
pos = plan[:, Plan.POSITION]
yaws = plan[:, Plan.T_FROM_CURRENT_EULER][:, 2]
ds = np.hypot(np.diff(pos[:, 0]), np.diff(pos[:, 1]))
s = np.concatenate(([0.0], np.cumsum(ds)))
peak_k = 0.0
n = len(yaws)
for i in range(n - 2):
if s[i] < PLAN_S_RANGE[0]:
continue
if s[i] > PLAN_S_RANGE[1]:
break
seg_ds = s[i + 2] - s[i]
if seg_ds < PLAN_MIN_SEG_DS:
continue
k = float((yaws[i + 2] - yaws[i]) / seg_ds)
if abs(k) > abs(peak_k):
peak_k = k
peak_k = float(np.clip(peak_k, -0.25, 0.25))
if len(yaws) == len(T_IDXS):
s_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, s))
yaw_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, yaws))
path_k_1s = float(np.clip(yaw_1s / s_1s, -0.25, 0.25)) if s_1s >= PLAN_MIN_SEG_DS else 0.0
else:
path_k_1s = 0.0
return peak_k, path_k_1s
def compute_plan_peak_curv(model_output: dict[str, np.ndarray]) -> float:
return compute_plan_metrics(model_output)[0]
class TurnPlanner:
"""
Two-stage rear-axle protected turn planner for tight low-speed turns.
Stage 1 (Entry): delays curvature buildup by a wheelbase-proportional distance
(time-capped at MAX_SHIFT_TIME) so the rear axle clears the inside curb.
Stage 2 (Apex & Exit): blends action curvature with the model's 1-second path
curvature once past entry, preventing wide apexes and exit hang-ups.
"""
def __init__(self, wheelbase: float = 2.7, dt: float = DT_MDL):
self.wheelbase = float(np.clip(wheelbase, *WHEELBASE_CLIP))
self.dt = dt
self.s = 0.0
self.buf_s: deque[float] = deque()
self.buf_k: deque[float] = deque()
self.turn_sign = 0.0
self.turn_psi = 0.0
self.shift_dist = 0.0
self.apex_blend = 0.0
self.plan_peak_curv = 0.0
self.path_curv_1s = 0.0
def reset(self) -> None:
self.s = 0.0
self.buf_s.clear()
self.buf_k.clear()
self.turn_sign = 0.0
self.turn_psi = 0.0
self.shift_dist = 0.0
self.apex_blend = 0.0
self.plan_peak_curv = 0.0
self.path_curv_1s = 0.0
def update(self, model_output: dict[str, np.ndarray], v_ego: float, actual_curv: float,
active: bool, is_rhd: bool, model_curv: float, enabled: bool = True) -> float:
if not enabled or not active:
self.reset()
return model_curv
if v_ego < MIN_MOVE_SPEED:
# Stopped straight (e.g., at a stop bar): reset turn phase so launch gets full protection.
# Stopped mid-turn (e.g., yielding to a pedestrian): preserve turn_psi so resume doesn't re-delay.
if abs(actual_curv) < MID_TURN_CURV_THRESH:
self.turn_psi = 0.0
self.turn_sign = 0.0
self.shift_dist = 0.0
self.apex_blend = 0.0
self.buf_s.clear()
self.buf_k.clear()
self.buf_s.append(self.s)
self.buf_k.append(float(actual_curv))
return model_curv
ds = v_ego * self.dt
self.s += ds
self.buf_s.append(self.s)
self.buf_k.append(float(model_curv))
while len(self.buf_s) > 2 and (self.s - self.buf_s[0]) > BUFFER_MAX_DIST:
self.buf_s.popleft()
self.buf_k.popleft()
turn_speed_w = float(np.interp(v_ego, TURN_SPEED_TAPER, [1.0, 0.0]))
if turn_speed_w <= 0.0 and self.shift_dist < 1e-3 and self.apex_blend < 1e-3:
self.turn_psi = 0.0
self.turn_sign = 0.0
self.plan_peak_curv = 0.0
self.path_curv_1s = 0.0
return model_curv
plan_peak, path_k_1s = compute_plan_metrics(model_output)
self.plan_peak_curv = plan_peak
self.path_curv_1s = path_k_1s
tightness = max(abs(model_curv), abs(plan_peak))
curv_w = float(np.interp(tightness, CURV_GATE, [0.0, 1.0]))
ref_k = model_curv if abs(model_curv) > 0.015 else plan_peak
if abs(ref_k) > 0.035 and turn_speed_w > 0.05:
new_sign = 1.0 if ref_k > 0.0 else -1.0
if new_sign != self.turn_sign:
self.turn_sign = new_sign
self.turn_psi = 0.0
self.turn_psi = max(0.0, self.turn_psi + actual_curv * self.turn_sign * ds)
else:
self.turn_psi = max(0.0, self.turn_psi - 0.5 * self.dt)
if self.turn_psi < 0.02:
self.turn_sign = 0.0
psi_deg = math.degrees(self.turn_psi)
# Stage 1: Entry rear-axle spatial shift (time-capped so crawl-speed starts stay responsive)
shift_speed_w = float(np.interp(v_ego, SHIFT_SPEED_BP, SHIFT_SPEED_V))
entry_phase_w = float(np.interp(psi_deg, ENTRY_PHASE_RELEASE_DEG, [1.0, 0.0]))
is_curb_side = (ref_k < 0.0) if is_rhd else (ref_k > 0.0)
side_w = 1.0 if is_curb_side else OPPOSITE_SIDE_WEIGHT
raw_shift = SHIFT_WHEELBASE_RATIO * self.wheelbase * shift_speed_w * curv_w * entry_phase_w * side_w
target_shift = min(raw_shift, v_ego * MAX_SHIFT_TIME)
tau = SHIFT_RELEASE_TAU if target_shift < self.shift_dist else SHIFT_BUILD_TAU
self.shift_dist += (target_shift - self.shift_dist) * (self.dt / (tau + self.dt))
out_k = model_curv
if self.shift_dist >= 0.05 and len(self.buf_s) >= 2:
k_delayed = float(np.interp(self.s - self.shift_dist, self.buf_s, self.buf_k))
# Same sign as model_curv (or 0), never exceeds |model_curv|, zero lag on unwind.
if model_curv >= 0.0:
out_k = float(np.clip(k_delayed, 0.0, model_curv))
else:
out_k = float(np.clip(k_delayed, model_curv, 0.0))
# Stage 2: Apex & Exit 1s-path blend (prevents wide apexes and exit hang-ups)
apex_phase_w = float(np.interp(psi_deg, APEX_BLEND_PHASE_DEG, [0.0, 1.0]))
target_blend = APEX_PATH_BLEND_MAX * turn_speed_w * curv_w * apex_phase_w
self.apex_blend += (target_blend - self.apex_blend) * (self.dt / (APEX_BLEND_TAU + self.dt))
if self.apex_blend > 0.01 and out_k * path_k_1s > 0.0:
# Through the apex, boost when 1s path asks for tighter curvature than action (prevents wide turns).
# Only pull curvature down toward 1s path late in the turn (psi > 60 deg) to prevent exit hang-ups.
if abs(path_k_1s) > abs(out_k) or psi_deg > 60.0:
max_boosted = abs(model_curv) * APEX_PATH_BOOST_CAP
target_k = float(np.clip(path_k_1s, -max_boosted, max_boosted))
out_k = (1.0 - self.apex_blend) * out_k + self.apex_blend * target_k
return out_k