diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index 27d2027bd8..ee472cc172 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -1,4 +1,3 @@ -from collections import deque from dataclasses import dataclass, fields import math @@ -13,7 +12,9 @@ DBC_CURVATURE = (-0.02, 0.02) DBC_CURVATURE_RATE = (-0.001024, 0.001023) _PATH_HORIZON = 7.0 -_C2_DELAY = 0.05 +_CURVATURE_RATE_HORIZONS = (3.5, 5.0, 7.0) +_FAST_POSE_CURVATURE_BAND = (0.009, 0.012) +_CURVATURE_NOISE_FLOOR = 0.0005 _PATH_RATES = (4.0, 1.0, math.inf, math.inf) @@ -30,6 +31,10 @@ def _finite(value: float) -> float: return float(value) if math.isfinite(value) else 0.0 +def _sample(distance: float, distances: list[float], values: list[float]) -> float: + return float(np.interp(distance, distances, values)) + + def _model_path(model) -> tuple[list[float], list[float], list[float]] | None: try: x = [float(value) for value in model.position.x] @@ -55,11 +60,38 @@ def _model_path(model) -> tuple[list[float], list[float], list[float]] | None: return distance, y, unwrapped_heading -def _encode_path(desired_curvature: float, current_curvature: float | None, centering_curvature: float) -> FordPath: +def _curvature_rate(path: tuple[list[float], list[float], list[float]]) -> float: + distance, _, heading = path + rates = [] + for requested_horizon in _CURVATURE_RATE_HORIZONS: + horizon = min(requested_horizon, distance[-1]) + start = _sample(0.0, distance, heading) + midpoint = _sample(0.5 * horizon, distance, heading) + end = _sample(horizon, distance, heading) + rates.append(4.0 * (start - 2.0 * midpoint + end) / horizon ** 2) + + magnitude = sum(abs(rate) for rate in rates) + if magnitude == 0.0: + return 0.0 + return sorted(rates)[1] * abs(sum(rates)) / magnitude + + +def _encode_path(model, desired_curvature: float, v_ego: float, current_curvature: float | None) -> FordPath: + path = _model_path(model) + if path is None: + return FordPath() + + model_curvature_rate = _curvature_rate(path) action_curvature = _finite(desired_curvature) measured_curvature = float(np.clip(_finite(current_curvature), -MAX_CURVATURE, MAX_CURVATURE)) \ if current_curvature is not None else 0.0 + future_curvature = action_curvature + model_curvature_rate * max(_finite(v_ego), _PATH_HORIZON) + sustained_curvature = 0.0 + if action_curvature * future_curvature > 0.0 and abs(future_curvature) > _CURVATURE_NOISE_FLOOR: + sustained_curvature = math.copysign(min(abs(action_curvature), abs(future_curvature)), action_curvature) + maneuver_share = float(np.interp(abs(action_curvature), _FAST_POSE_CURVATURE_BAND, (0.0, 1.0))) + centering_curvature = sustained_curvature * (1.0 - maneuver_share) fast_curvature = (action_curvature - centering_curvature) + (action_curvature - measured_curvature) path_offset = 0.5 * fast_curvature * _PATH_HORIZON ** 2 path_angle = fast_curvature * _PATH_HORIZON @@ -74,20 +106,11 @@ def _encode_path(desired_curvature: float, current_curvature: float | None, cent class FordPathController: - """Split each Ford path command into one-model-frame fast and steady components.""" + """Convert the model path directly into one vehicle-independent Ford path command.""" def __init__(self, dt: float = 0.01): self.dt = dt self._last_path = FordPath(valid=True) - delay_steps = max(round(_C2_DELAY / dt), 1) - self._desired_history: deque[float] = deque(maxlen=delay_steps + 1) - - def _delayed_curvature(self, desired_curvature: float) -> float: - curvature = float(np.clip(_finite(desired_curvature), *DBC_CURVATURE)) - if not self._desired_history: - self._desired_history.extend([curvature] * self._desired_history.maxlen) - self._desired_history.append(curvature) - return self._desired_history[0] def _limit(self, target: FordPath) -> FordPath: values = [] @@ -102,10 +125,7 @@ class FordPathController: current_curvature: float | None = None) -> FordPath: if not active: self._last_path = FordPath(valid=True) - self._desired_history.clear() return FordPath() - if model is None or _model_path(model) is None: - self._desired_history.clear() + if model is None: return self._limit(FordPath(valid=True)) - centering_curvature = self._delayed_curvature(desired_curvature) - return self._limit(_encode_path(desired_curvature, current_curvature, centering_curvature)) + return self._limit(_encode_path(model, desired_curvature, v_ego, current_curvature)) diff --git a/openpilot/selfdrive/controls/tests/test_ford_path.py b/openpilot/selfdrive/controls/tests/test_ford_path.py index 5ab5215773..bfafbf45a6 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path.py @@ -49,47 +49,30 @@ def test_steady_gentle_arc_uses_full_c2_without_fast_pose(): assert abs(path.curvature_rate) < 1e-5 -def test_upstream_c2_remains_primary_for_tight_curve_when_tracking(): +def test_tight_curve_uses_fast_pose_instead_of_sticky_c2(): path = _command(_path(0.015), 0.015, v_ego=20.0, current_curvature=0.015) - assert np.isclose(path.curvature, 0.015) - assert np.allclose(_fast_curvatures(path), (0.0, 0.0)) + assert path.curvature == 0.0 + assert np.allclose(_fast_curvatures(path), (0.015, 0.015)) -def test_fast_fields_only_encode_c2_excess_and_tracking_error(): +def test_fast_fields_encode_turn_demand_and_tracking_error_without_c2(): tracking = _command(_path(0.04), 0.04, v_ego=8.0, current_curvature=0.04) behind = _command(_path(0.04), 0.04, v_ego=8.0, current_curvature=0.03) - assert np.isclose(tracking.curvature, DBC_CURVATURE[1]) - assert np.allclose(_fast_curvatures(tracking), (0.04 - DBC_CURVATURE[1],) * 2) - assert np.allclose(_fast_curvatures(behind), (0.05 - DBC_CURVATURE[1],) * 2) + assert tracking.curvature == 0.0 + assert np.allclose(_fast_curvatures(tracking), (0.04, 0.04)) + assert np.allclose(_fast_curvatures(behind), (0.05, 0.05)) -def test_new_request_uses_fast_fields_for_one_model_frame_before_c2(): +def test_turn_entry_immediately_removes_gentle_centering_c2(): controller = FordPathController() for _ in range(10): controller.update(_path(0.004), 0.004, current_curvature=0.004) - changing = [controller.update(_path(0.0045), 0.0045, current_curvature=0.0045) for _ in range(6)] + turning = controller.update(_path(0.04), 0.04, current_curvature=0.004) - assert all(np.isclose(command.curvature, 0.004) for command in changing[:5]) - assert all(np.allclose(_fast_curvatures(command), (0.0005, 0.0005)) for command in changing[:5]) - assert all(np.isclose(command.curvature + _fast_curvatures(command)[0], 0.0045) for command in changing) - assert all(np.isclose(command.curvature + _fast_curvatures(command)[1], 0.0045) for command in changing) - assert np.isclose(changing[5].curvature, 0.0045) - assert np.allclose(_fast_curvatures(changing[5]), (0.0, 0.0)) - - -def test_one_frame_c2_history_clears_when_control_inactive(): - controller = FordPathController() - for _ in range(10): - controller.update(_path(0.01), 0.01, current_curvature=0.01) - - assert not controller.update(_path(0.0), 0.0, active=False).valid - resumed = controller.update(_path(-0.002), -0.002, current_curvature=-0.002) - - assert np.isclose(resumed.curvature, -0.002) - assert np.allclose(_fast_curvatures(resumed), (0.0, 0.0)) + assert turning.curvature == 0.0 def test_gentle_changing_curve_stays_on_full_c2_when_tracking(): @@ -100,21 +83,21 @@ def test_gentle_changing_curve_stays_on_full_c2_when_tracking(): assert np.isclose(path.curvature, 0.004) -def test_c2_follows_action_instead_of_independent_model_unwind(): +def test_c2_unloads_before_near_horizon_curve_exit(): path = _command(_path(0.004, -0.0005), 0.004, v_ego=8.0, current_curvature=0.004) - assert np.isclose(path.curvature, 0.004) + assert path.curvature == 0.0 assert path.curvature_rate == 0.0 - assert path.path_angle == 0.0 + assert path.path_angle < 0.03 -def test_model_unwind_does_not_remove_upstream_c2_baseline(): +def test_tight_curve_unwind_keeps_fast_pose_without_loading_c2(): steady = _command(_path(0.015), 0.015, v_ego=10.0, current_curvature=0.012) unwinding = _command(_path(0.015, -0.0005), 0.015, v_ego=10.0, current_curvature=0.012) - assert np.isclose(steady.curvature, 0.015) - assert np.isclose(unwinding.curvature, 0.015) - assert np.isclose(_equivalent_curvature(unwinding), _equivalent_curvature(steady)) + assert steady.curvature == 0.0 + assert unwinding.curvature == 0.0 + assert abs(_equivalent_curvature(unwinding)) > 0.9 * abs(_equivalent_curvature(steady)) def test_action_curvature_wins_over_opposing_model_geometry(): @@ -150,12 +133,12 @@ def test_sunnypilot_path_message_round_trip(): assert path.valid -def test_tight_arc_uses_c2_limit_and_signed_fast_residual(): +def test_tight_arc_uses_signed_forward_pose_without_slow_c2(): left = _command(_path(0.04), 0.04, v_ego=8.0) right = _command(_path(-0.04), -0.04, v_ego=8.0) - assert left.curvature == DBC_CURVATURE[1] - assert right.curvature == DBC_CURVATURE[0] + assert left.curvature == 0.0 + assert right.curvature == 0.0 assert abs(left.curvature_rate) < 1e-4 assert abs(right.curvature_rate) < 1e-4 assert left.path_angle > 0.06 @@ -173,17 +156,13 @@ def test_c2_does_not_increase_when_model_shows_tight_curve_unwind(): assert np.all(np.diff(commands) <= 1e-9) -def test_fresh_model_change_uses_fast_fields_until_one_frame_c2_catches_up(): +def test_fresh_model_replaces_previous_path_without_hidden_state(): controller = FordPathController(dt=1.0) initial = controller.update(_path(0.04), 0.04, v_ego=8.0) - replanned = controller.update(_path(0.0), 0.0, v_ego=8.0, current_curvature=0.0) - caught_up = controller.update(_path(0.0), 0.0, v_ego=8.0, current_curvature=0.0) + replanned = controller.update(_path(0.0), 0.0, v_ego=8.0) assert initial.path_offset > 0.5 - assert replanned.curvature > 0.0 - assert replanned.path_offset < 0.0 - assert replanned.path_angle < 0.0 - assert caught_up == FordPathController().update(_path(0.0), 0.0, v_ego=8.0) + assert replanned == FordPathController().update(_path(0.0), 0.0, v_ego=8.0) def test_s_turn_reverses_fast_fields_while_c2_is_bounded(): @@ -200,28 +179,18 @@ def test_s_turn_reverses_fast_fields_while_c2_is_bounded(): assert all(path.valid for path in outputs) assert all(DBC_CURVATURE[0] <= path.curvature <= DBC_CURVATURE[1] for path in outputs) - assert outputs[0].curvature > 0.0 - assert all(path.curvature <= 0.0 for path in outputs[1:]) - assert np.all(np.diff([path.path_angle for path in outputs]) < 0.0) - assert np.all(np.diff([path.path_offset for path in outputs]) < 0.0) - assert outputs[2].path_angle < 0.0 - assert outputs[2].path_offset < 0.0 + assert all(path.curvature <= 0.0 for path in outputs) assert outputs[-1].path_angle < -0.03 assert outputs[-1].path_offset < 0.0 -def test_reversal_countersteers_during_bounded_one_frame_c2_persistence(): +def test_reversal_does_not_add_software_persistence_to_centering_c2(): controller = FordPathController() - for _ in range(10): - assert controller.update(_path(0.002), 0.002, v_ego=8.0).curvature > 0.0 + assert controller.update(_path(0.002), 0.002, v_ego=8.0).curvature > 0.0 - reversing = [controller.update(_path(-0.02), -0.02, v_ego=8.0, current_curvature=0.002) for _ in range(6)] + reversing = controller.update(_path(-0.02), -0.02, v_ego=8.0) - assert all(command.curvature > 0.0 for command in reversing[:5]) - assert np.all(np.diff([command.path_offset for command in reversing]) < 0.0) - assert np.all(np.diff([command.path_angle for command in reversing]) < 0.0) - assert all(command.path_offset < 0.0 and command.path_angle < 0.0 for command in reversing[1:]) - assert reversing[5].curvature <= 0.0 + assert reversing.curvature <= 0.0 def test_requested_turn_is_not_cancelled_by_previous_path(): @@ -238,9 +207,7 @@ def test_requested_turn_is_not_cancelled_by_previous_path(): assert command.path_offset >= 0.0 assert command.path_angle >= 0.0 - assert command.curvature < 0.0 - assert np.isclose(command.curvature + _fast_curvatures(command)[0], 0.033) - assert np.isclose(command.curvature + _fast_curvatures(command)[1], 0.033) + assert command.curvature >= 0.0 assert _equivalent_curvature(command) >= 0.02 @@ -277,7 +244,7 @@ def test_fast_fields_encode_one_virtual_curvature(): def test_action_turn_exposes_fast_authority_without_large_model_arc(): command = FordPathController(dt=1.0).update(_path(0.002), 0.04, v_ego=8.0, current_curvature=0.002) - assert command.curvature == DBC_CURVATURE[1] + assert command.curvature == 0.0 assert command.path_offset > 0.5 assert command.path_angle > 0.2 @@ -372,8 +339,8 @@ def test_curvature_error_increases_forward_pose_command_while_behind(): def test_major_turn_undertracking_uses_bounded_fast_feedback_authority(): command = FordPathController(dt=1.0).update(_path(0.02), 0.02, v_ego=8.0, current_curvature=0.01) - assert command.curvature == DBC_CURVATURE[1] - assert np.isclose(command.path_angle, 0.07) + assert command.curvature == 0.0 + assert command.path_angle >= 0.2 def test_driver_well_beyond_requested_curvature_gets_countersteer(): @@ -400,7 +367,7 @@ def test_tight_turn_from_stop_builds_bounded_forward_pose_authority(): outputs = [controller.update(_path(0.04), 0.04, v_ego=0.0, current_curvature=0.0) for _ in range(20)] path = outputs[-1] - assert path.curvature == DBC_CURVATURE[1] + assert path.curvature == 0.0 assert path.path_offset > 0.7 assert path.path_angle > 0.15 assert np.max(np.abs(np.diff([output.path_offset for output in outputs]))) <= 0.04 + 1e-9 @@ -428,23 +395,23 @@ def test_invalid_model_ramps_pose_to_zero_while_remaining_in_extended_mode(): assert not controller.update(_path(0.0), 0.0, v_ego=12.0, active=False).valid -def test_fast_pose_holds_only_demand_beyond_c2_at_measured_target(): +def test_fast_pose_holds_requested_arc_at_measured_target(): command = _command(_path(0.03), 0.03, v_ego=20.0, current_curvature=0.03) offset_curvature, angle_curvature = _fast_curvatures(command) - assert np.isclose(command.curvature, DBC_CURVATURE[1]) - assert np.isclose(offset_curvature, 0.01) - assert np.isclose(angle_curvature, 0.01) + assert command.curvature == 0.0 + assert np.isclose(offset_curvature, 0.03) + assert np.isclose(angle_curvature, 0.03) -def test_fast_pose_adds_tracking_error_to_demand_beyond_c2(): +def test_fast_pose_adds_remaining_error_to_arc_feedforward(): entering = _command(_path(0.03), 0.03, current_curvature=0.0) halfway = _command(_path(0.03), 0.03, current_curvature=0.015) tracking = _command(_path(0.03), 0.03, current_curvature=0.03) - assert np.allclose(_fast_curvatures(entering), (0.04, 0.04)) - assert np.allclose(_fast_curvatures(halfway), (0.025, 0.025)) - assert np.allclose(_fast_curvatures(tracking), (0.01, 0.01)) + assert np.allclose(_fast_curvatures(entering), (0.06, 0.06)) + assert np.allclose(_fast_curvatures(halfway), (0.045, 0.045)) + assert np.allclose(_fast_curvatures(tracking), (0.03, 0.03)) def test_small_overshoot_reduces_hold_without_reversing_active_arc(): @@ -453,7 +420,7 @@ def test_small_overshoot_reduces_hold_without_reversing_active_arc(): assert 0.0 < overshoot.path_offset < tracking.path_offset assert 0.0 < overshoot.path_angle < tracking.path_angle - assert np.allclose(_fast_curvatures(overshoot), (0.009, 0.009)) + assert np.allclose(_fast_curvatures(overshoot), (0.029, 0.029)) def test_path_exit_uses_measured_curvature_to_unwind():