IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/controls/lib/curvature_lookahead.py

@@ -0,0 +1,21 @@
+import math
+
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_curvature_from_plan
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+
+LOOKAHEAD_SECONDS = 0.20
+
+
+def get_lookahead_curvature(model_v2, v_ego: float, lat_delay: float) -> float | None:
+  try:
+    yaws = model_v2.orientation.z
+    yaw_rates = model_v2.orientationRate.z
+    if len(yaws) < CONTROL_N or len(yaw_rates) < CONTROL_N:
+      return None
+    if not all(math.isfinite(value) for value in yaws) or not all(math.isfinite(value) for value in yaw_rates):
+      return None
+    horizon = max(0.0, lat_delay) + LOOKAHEAD_SECONDS
+    return get_curvature_from_plan(yaws, yaw_rates, ModelConstants.T_IDXS, v_ego, horizon)
+  except (AttributeError, TypeError, ValueError):
+    return None