ford: make path allocation demand driven

Assisted-by: Codex

openpilot/selfdrive/controls/lib/ford_path.py

@@ -92,38 +92,51 @@ def _encode_path(model, desired_curvature: float | None, v_ego: float, current_c
   path_offset = _sample(_PATH_OFFSET_DISTANCE, distance, offset)
   path_angle = _sample(lookahead, distance, heading)
   model_curvature = _curvature(path)
+  model_curvature_rate = _curvature_rate(path)
   action_curvature = model_curvature if desired_curvature is None else _finite(desired_curvature)
   requested_curvature = max((model_curvature, action_curvature), key=abs)
   maneuver_residual = requested_curvature - model_curvature
   path_offset += 0.5 * maneuver_residual * _PATH_OFFSET_DISTANCE ** 2
   path_angle += maneuver_residual * lookahead
-  offset_curvature = 2.0 * path_offset / _PATH_OFFSET_DISTANCE ** 2
-  angle_curvature = path_angle / lookahead
-  geometry_demand = max(abs(offset_curvature), abs(angle_curvature))
   correction = 0.0
+  tracking_demand = 0.0
+  wheel_beyond_target = False
   if current_curvature is not None:
     target_curvature = requested_curvature
-    tracking_error = target_curvature - _finite(current_curvature)
+    measured_curvature = _finite(current_curvature)
+    tracking_error = target_curvature - measured_curvature
     correction = math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error)
+    tracking_demand = abs(correction)
+    wheel_beyond_target = target_curvature * measured_curvature > 0.0 and \
+      abs(target_curvature) + _TRACKING_ERROR_DEADZONE < abs(measured_curvature)
     correction_limit = _TRACKING_ERROR_LIMIT
     if correction * target_curvature < 0.0:
       correction_limit = 0.5 * abs(target_curvature)
     correction = float(np.clip(correction, -correction_limit, correction_limit))
-    path_offset += 0.5 * correction * _PATH_OFFSET_DISTANCE ** 2
-    path_angle += correction * lookahead
+    correction_offset = 0.5 * correction * _PATH_OFFSET_DISTANCE ** 2
+    correction_angle = correction * lookahead
+    if wheel_beyond_target:
+      path_offset = correction_offset
+      path_angle = correction_angle
+    else:
+      path_offset += correction_offset
+      path_angle += correction_angle
 
-  control_demand = max(geometry_demand, abs(requested_curvature), abs(correction))
-  maneuver_share = float(np.interp(control_demand, _CENTERING_CURVATURE_BASEBAND, (0.0, 1.0)))
+  spatial_demand = abs(model_curvature_rate) * lookahead / 3.0
+  overflow_demand = max(abs(requested_curvature) - DBC_CURVATURE[1], 0.0)
+  maneuver_demand = max(spatial_demand, overflow_demand, tracking_demand)
+  maneuver_share = 1.0 if wheel_beyond_target else \
+    float(np.interp(maneuver_demand, _CENTERING_CURVATURE_BASEBAND, (0.0, 1.0)))
   path_offset *= maneuver_share
   path_angle *= maneuver_share
-  centering_curvature = model_curvature if model_curvature * requested_curvature >= 0.0 else 0.0
+  centering_curvature = requested_curvature
 
   return FordPath(
     valid=True,
     path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
     path_angle=float(np.clip(path_angle, *DBC_ANGLE)),
     curvature=float(np.clip(centering_curvature * (1.0 - maneuver_share), *DBC_CURVATURE)),
-    curvature_rate=float(np.clip(_curvature_rate(path), *DBC_CURVATURE_RATE)),
+    curvature_rate=float(np.clip(model_curvature_rate, *DBC_CURVATURE_RATE)),
   )
 
 

openpilot/selfdrive/controls/tests/test_ford_path.py

@@ -40,14 +40,13 @@ def _equivalent_curvature(path, distance: float = 7.0) -> float:
   return 2.0 * offset / distance ** 2
 
 
-def test_gentle_arc_uses_forward_pose_fields():
+def test_steady_arc_uses_c2_without_fast_pose_fields():
   path = encode_ford_path(_path(0.008), 0.0, v_ego=8.0)
 
   assert path.valid
-  assert path.path_offset > 0.1
-  assert path.path_angle > 0.03
-  assert path.curvature == 0.0
-  assert _equivalent_curvature(path) > 0.008
+  assert abs(path.path_offset) < 1e-9
+  assert abs(path.path_angle) < 1e-9
+  assert np.isclose(path.curvature, 0.008, atol=5e-5)
   assert abs(path.curvature_rate) < 1e-5
 
 
@@ -83,7 +82,9 @@ def test_tight_arc_uses_signed_forward_pose_without_slow_c2():
 
 def test_c2_does_not_increase_while_tight_curve_unwinds():
   curvatures = (0.04, 0.018, 0.016, 0.014, 0.012, 0.010, 0.008, 0.006, 0.0)
-  commands = [encode_ford_path(_path(curvature), 0.0, v_ego=8.0).curvature for curvature in curvatures]
+  measured = (0.04,) + curvatures[:-1]
+  commands = [encode_ford_path(_path(curvature), 0.0, curvature, v_ego=8.0, current_curvature=actual).curvature
+              for curvature, actual in zip(curvatures, measured, strict=True)]
 
   assert np.all(np.diff(commands) <= 1e-9)
 
@@ -97,10 +98,10 @@ def test_lateral_delay_does_not_change_the_reference_polynomial():
 
 def test_fresh_model_replaces_previous_path_without_hidden_state():
   controller = FordPathController(dt=1.0)
-  initial = controller.update(_offset_path(0.4), v_ego=8.0)
+  initial = controller.update(_path(0.04), 0.04, v_ego=8.0)
   replanned = controller.update(_path(0.0), v_ego=8.0)
 
-  assert initial.path_offset > 0.39
+  assert initial.path_offset > 0.5
   assert replanned == FordPathController().update(_path(0.0), v_ego=8.0)
 
 
@@ -164,6 +165,14 @@ def test_action_demand_exposes_forward_path_authority():
   assert _equivalent_curvature(command) >= 0.004
 
 
+def test_minor_curve_uses_c2_when_tracking_is_close():
+  command = FordPathController(dt=1.0).update(_path(0.005), 0.005, v_ego=8.0, current_curvature=0.0048)
+
+  assert abs(command.path_offset) < 1e-9
+  assert abs(command.path_angle) < 1e-9
+  assert command.curvature > 0.0049
+
+
 def test_action_curvature_corrects_stale_opposing_model_at_low_speed():
   command = FordPathController().update(_path(-0.001, speed=1.0), 0.005, v_ego=1.0, current_curvature=0.001)
 
@@ -189,15 +198,15 @@ def test_measured_curvature_after_path_exit_commands_countersteer():
   assert command.path_angle < 0.0
 
 
-def test_measured_curvature_does_not_cancel_a_modeled_arc():
+def test_measured_curvature_countersteers_when_beyond_modeled_arc():
   controller = FordPathController(dt=1.0)
   command = controller.update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.012)
   tracking = FordPathController(dt=1.0).update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.004)
 
-  assert command.path_offset > 0.0
-  assert command.path_angle > 0.0
+  assert command.path_offset < 0.0
+  assert command.path_angle < 0.0
   assert command.path_angle < tracking.path_angle
-  assert _equivalent_curvature(command) > 0.0
+  assert command.curvature == 0.0
 
 
 def test_model_reversal_suppresses_old_c2_and_countersteers():
@@ -243,7 +252,8 @@ def test_curvature_error_increases_forward_pose_command_while_behind():
 
   assert behind.path_offset > tracking.path_offset + 0.01
   assert behind.path_angle > tracking.path_angle + 0.015
-  assert np.isclose(behind.curvature, tracking.curvature)
+  assert behind.curvature == 0.0
+  assert tracking.curvature > 0.007
   assert np.isclose(behind.curvature_rate, tracking.curvature_rate)
 
 
@@ -287,7 +297,7 @@ def test_curvature_feedback_is_bounded_for_bad_measurement():
 def test_invalid_model_ramps_pose_to_zero_while_remaining_in_extended_mode():
   controller = FordPathController()
   for _ in range(10):
-    active = controller.update(_offset_path(0.4), v_ego=12.0)
+    active = controller.update(_path(0.04), 0.04, v_ego=12.0)
   missing = controller.update(None, v_ego=12.0)
 
   assert active.path_offset > 0.0