diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e1ed872e0..094199728 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -169,12 +169,27 @@ CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01 # 1/m CURVATURE_HOLD_CONFIRM_MIN = 0.003 # 1/m (~7 deg) of wound curvature before capture CURVATURE_HOLD_CONFIRM_SWEPT = 0.6 # rad of heading swept this blinker cycle; past this the push is exit-shaping, not initiation +# Suppress low-speed action spikes while the model's spatial path remains straight. +TWITCH_GUARD_MAX_SPEED = 4.0 +TWITCH_GUARD_FADE_SPEED = 3.0 +TWITCH_GUARD_DURATION = 1.5 +TWITCH_GUARD_PLAN_RATIO = 4.0 +TWITCH_GUARD_FLOOR = 0.002 +TWITCH_GUARD_STRAIGHT_LO = 0.005 +TWITCH_GUARD_STRAIGHT_HI = 0.014 +TWITCH_GUARD_MIN_REACH = 12.0 + def _plan_circle_curvature(xs, ys, lookahead: float) -> float: - # curvature of the circle through the origin, tangent to the car's heading, passing - # through the plan point ~lookahead meters ahead: kappa = 2y / (x^2 + y^2) + # Fit curvature through the plan point at the requested lookahead. px, py = 0.0, 0.0 - for x, y in zip(xs, ys): + for x, y in zip(xs, ys, strict=False): + try: + x, y = float(x), float(y) + except (TypeError, ValueError, OverflowError): + return 0.0 + if not (math.isfinite(x) and math.isfinite(y)): + return 0.0 px, py = x, y if math.hypot(x, y) >= lookahead: break @@ -185,11 +200,7 @@ def _plan_circle_curvature(xs, ys, lookahead: float) -> float: def _plan_dual_probe(model_v2, d_near: float, d_far: float) -> float: - # Min-magnitude of a near and a far circle fit. The far probe alone assumes the turn - # starts immediately, which over-winds wide turns whose arc begins several meters out - # (wide multi-lane lefts): the near probe reads ~straight there and only grows as the - # car approaches the arc, so the readout self-scales to the turn geometry. Sign - # disagreement means no coherent turn ahead: contribute nothing. + # Use the smaller magnitude of near and far probes to avoid early turn bias. xs, ys = model_v2.position.x, model_v2.position.y near = _plan_circle_curvature(xs, ys, d_near) far = _plan_circle_curvature(xs, ys, d_far) @@ -222,8 +233,52 @@ def get_plan_turn_onset_dist(model_v2) -> float: def get_plan_reach(model_v2) -> float: - xs = model_v2.position.x - return xs[-1] if len(xs) else 0.0 + try: + xs = model_v2.position.x + return float(xs[-1]) if len(xs) else 0.0 + except (AttributeError, IndexError, TypeError, ValueError, OverflowError): + return 0.0 + + +def _plan_positions_are_finite(model_v2) -> bool: + try: + xs, ys = model_v2.position.x, model_v2.position.y + return len(xs) == len(ys) and all( + math.isfinite(float(x)) and math.isfinite(float(y)) for x, y in zip(xs, ys, strict=True) + ) + except (AttributeError, TypeError, ValueError, OverflowError): + return False + + +def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float: + if not (math.isfinite(curvature) and math.isfinite(v_ego)): + return curvature + if v_ego >= TWITCH_GUARD_MAX_SPEED or curvature == 0.0: + return curvature + if not _plan_positions_are_finite(model_v2): + return curvature + reach = get_plan_reach(model_v2) + if not math.isfinite(reach) or reach < TWITCH_GUARD_MIN_REACH: + return curvature + plan = abs(_plan_circle_curvature(model_v2.position.x, model_v2.position.y, + CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR)) + if not math.isfinite(plan): + return curvature + straightness = (plan - TWITCH_GUARD_STRAIGHT_LO) / (TWITCH_GUARD_STRAIGHT_HI - TWITCH_GUARD_STRAIGHT_LO) + limit = max(TWITCH_GUARD_PLAN_RATIO * plan * min(max(straightness, 0.0), 1.0), TWITCH_GUARD_FLOOR) + if abs(curvature) <= limit: + return curvature + fade = (TWITCH_GUARD_MAX_SPEED - v_ego) / (TWITCH_GUARD_MAX_SPEED - TWITCH_GUARD_FADE_SPEED) + fade = min(max(fade, 0.0), 1.0) + return curvature + (math.copysign(limit, curvature) - curvature) * fade + + +def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> float: + if not (math.isfinite(remaining) and math.isfinite(v_ego)): + return 0.0 + if standstill or abs(v_ego) <= 0.3: + return TWITCH_GUARD_DURATION + return max(remaining - DT_CTRL, 0.0) def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float: @@ -346,6 +401,7 @@ class Controls: self.turn_hold_handoff_t = 0.0 self.turn_hold_done = False self.turn_blinker_swept = 0.0 + self.twitch_guard_remaining = 0.0 self.kona_non_scc_lateral_active = False self.pose_calibrator = PoseCalibrator() @@ -401,6 +457,7 @@ class Controls: def state_control(self): CS = self.sm['carState'] + self.twitch_guard_remaining = update_twitch_guard(self.twitch_guard_remaining, CS.vEgo, CS.standstill) # Update VehicleModel lp = self.sm['liveParameters'] @@ -460,7 +517,11 @@ class Controls: # EcuDisableFailed is set when car started in READY mode (ECU disable was rejected) # Disable longitudinal so stock ACC works instead self.update_ecu_disable_failed() - CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed + CC.longActive = ( + CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and + not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and + not self.ecu_disable_failed + ) actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state @@ -499,6 +560,9 @@ class Controls: # here is positive for RIGHT turns (pauseturn log: left turn at +148 deg steering # angle logs desiredCurvature -0.07), so the blinker maps right=+1, left=-1. blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker) + if (CC.latActive and self.twitch_guard_remaining > 0.0 and + blinker_dir == 0.0 and self.turn_hold_curvature == 0.0): + new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo) # heading swept in the blinker's direction over the whole blinker cycle (any speed): # discriminates a turn not yet made from one being exited (see the re-arm below) if blinker_dir == 0.0: diff --git a/selfdrive/controls/tests/test_turn_lead.py b/selfdrive/controls/tests/test_turn_lead.py index 765e3507e..b4cfd4d53 100644 --- a/selfdrive/controls/tests/test_turn_lead.py +++ b/selfdrive/controls/tests/test_turn_lead.py @@ -1,13 +1,40 @@ +import math +import types + from cereal import car import pytest -from openpilot.selfdrive.controls.controlsd import get_control_lateral_smooth_seconds, turn_lead_allowed +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.controlsd import ( + TWITCH_GUARD_DURATION, + TWITCH_GUARD_FLOOR, + TWITCH_GUARD_MAX_SPEED, + get_control_lateral_smooth_seconds, + limit_curvature_to_plan, + turn_lead_allowed, + update_twitch_guard, +) LateralControlMode = car.CarControl.Actuators.LateralControlMode +def _plan(xs, ys): + return types.SimpleNamespace(position=types.SimpleNamespace(x=xs, y=ys)) + + +def _arc_plan(radius, n=200): + return _plan([radius * math.sin(i / n) for i in range(n)], + [radius * (1.0 - math.cos(i / n)) for i in range(n)]) + + +STRAIGHT_PLAN = _plan([i * 0.5 for i in range(200)], [0.0] * 200) +STANDSTILL_STUB_PLAN = _plan([0.0, 0.3], [0.0, 0.0]) +TURN_PLAN = _arc_plan(30.0) +GENTLE_BEND_PLAN = _arc_plan(143.0) + + def test_turn_lead_is_suppressed_only_during_applied_angle_control(): assert not turn_lead_allowed("rivian", LateralControlMode.angle) assert turn_lead_allowed("rivian", LateralControlMode.torque) @@ -37,3 +64,79 @@ def test_subaru_control_smoothing_uses_vehicle_schedule(v_ego, expected): ]) def test_rivian_control_smoothing_remains_speed_scheduled(v_ego, expected): assert get_control_lateral_smooth_seconds("rivian", v_ego, 0.4) == pytest.approx(expected) + + +@pytest.mark.parametrize("curvature", [0.0155, -0.0155]) +def test_twitch_against_a_straight_plan_is_clamped_to_the_floor(curvature): + guarded = limit_curvature_to_plan(STRAIGHT_PLAN, curvature, 1.2) + assert abs(guarded) == pytest.approx(TWITCH_GUARD_FLOOR) + assert math.copysign(1.0, guarded) == math.copysign(1.0, curvature) + + +def test_command_already_below_the_floor_is_untouched(): + assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0015, 1.2) == pytest.approx(0.0015) + + +@pytest.mark.parametrize("v_ego", [TWITCH_GUARD_MAX_SPEED, 6.0, 30.0]) +def test_guard_is_inactive_above_its_speed_band(v_ego): + assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, v_ego) == pytest.approx(0.0155) + + +def test_guard_fades_out_across_the_speed_band(): + full = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 1.2) + half = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 3.5) + assert full < half < 0.0155 + + +@pytest.mark.parametrize("ratio", [0.8, 1.0, 2.0, 3.0]) +def test_real_turns_tracking_their_own_plan_are_untouched(ratio): + action = (1.0 / 30.0) * ratio + assert limit_curvature_to_plan(TURN_PLAN, action, 1.2) == pytest.approx(action) + + +def test_a_barely_bending_plan_does_not_license_a_large_command(): + guarded = limit_curvature_to_plan(GENTLE_BEND_PLAN, 0.0155, 1.2) + assert TWITCH_GUARD_FLOOR < guarded < 0.008 + + +@pytest.mark.parametrize("plan", [STANDSTILL_STUB_PLAN, _plan([], [])]) +def test_guard_stands_down_when_the_plan_is_too_short_to_judge(plan): + assert limit_curvature_to_plan(plan, 0.0155, 0.4) == pytest.approx(0.0155) + + +def test_zero_command_stays_zero(): + assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0, 1.2) == 0.0 + + +@pytest.mark.parametrize("bad_plan", [ + _plan([0.0, math.nan, 20.0], [0.0, 0.0, 0.0]), + _plan([0.0, math.inf, 20.0], [0.0, 0.0, 0.0]), + _plan([0.0, 20.0], [0.0]), +]) +def test_invalid_plan_data_leaves_curvature_untouched(bad_plan): + assert limit_curvature_to_plan(bad_plan, 0.0155, 1.2) == pytest.approx(0.0155) + + +@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf]) +def test_nonfinite_guard_inputs_disarm(value): + assert update_twitch_guard(value, 1.0, False) == 0.0 + assert update_twitch_guard(TWITCH_GUARD_DURATION, value, False) == 0.0 + + +def test_twitch_guard_arms_at_standstill_or_creep_speed(): + assert update_twitch_guard(0.0, 0.0, True) == TWITCH_GUARD_DURATION + assert update_twitch_guard(0.0, 0.3, False) == TWITCH_GUARD_DURATION + + +def test_twitch_guard_decays_after_pullaway_and_expires(): + remaining = update_twitch_guard(0.0, 0.0, True) + remaining = update_twitch_guard(remaining, 1.0, False) + assert remaining == pytest.approx(TWITCH_GUARD_DURATION - DT_CTRL) + + for _ in range(int(TWITCH_GUARD_DURATION / DT_CTRL) + 1): + remaining = update_twitch_guard(remaining, 1.0, False) + assert remaining == 0.0 + + +def test_twitch_guard_does_not_arm_while_moving(): + assert update_twitch_guard(0.0, 1.0, False) == 0.0