oleoleole

cereal/log.capnp

@@ -1300,6 +1300,11 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
 
   solverExecutionTime @35 :Float32;
 
+  leadTrajectoryX0 @40 :List(Float32);
+  leadTrajectoryV0 @41 :List(Float32);
+  leadTrajectoryX1 @42 :List(Float32);
+  leadTrajectoryV1 @43 :List(Float32);
+
   enum LongitudinalPlanSource {
     cruise @0;
     lead0 @1;

common/params_keys.h

@@ -276,6 +276,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DeveloperSidebarMetric7", {PERSISTENT, INT, "7", "0", 3}},
     {"DeveloperUI", {PERSISTENT, BOOL, "0", "0", 3}},
     {"GalaxyDeveloperMode", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
+    {"TestModelLeadTrajectory", {PERSISTENT | DONT_LOG, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
     {"DeveloperWidgets", {PERSISTENT, BOOL, "1", "0", 3}},
     {"DeviceManagement", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
     {"DeviceShutdown", {PERSISTENT, INT, "6", "6", 1, SETTINGS_SIMPLE}},

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -15,7 +15,7 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.controls.lib.lead_behavior import get_tracked_lead_catchup_bias, is_radarless_matched_follow_window
 # WARNING: imports outside of constants will not trigger a rebuild
-from openpilot.selfdrive.modeld.constants import index_function
+from openpilot.selfdrive.modeld.constants import index_function, ModelConstants
 
 if __name__ == '__main__':  # generating code
   from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -149,10 +149,51 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1
 T_IDXS = np.array(T_IDXS_LST)
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
+LEAD_T_IDXS_MODEL = np.asarray(ModelConstants.LEAD_T_IDXS, dtype=np.float64)
 COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 
 
+def build_model_lead_trajectory(model_lead, radar_lead, v_ego):
+  """Build a model-predicted lead path while preserving the raw h=0 anchor."""
+  if model_lead is None or radar_lead is None or not bool(getattr(radar_lead, "status", False)):
+    return None
+
+  try:
+    if float(model_lead.prob) <= 0.5:
+      return None
+    model_x = np.asarray(model_lead.x, dtype=np.float64)
+    model_v = np.asarray(model_lead.v, dtype=np.float64)
+  except (AttributeError, TypeError, ValueError):
+    return None
+
+  expected_len = len(LEAD_T_IDXS_MODEL)
+  if model_x.shape != (expected_len,) or model_v.shape != (expected_len,):
+    return None
+  if not np.all(np.isfinite(model_x)) or not np.all(np.isfinite(model_v)):
+    return None
+
+  raw_d_rel = float(getattr(radar_lead, "dRel", float("nan")))
+  raw_v_lead = float(getattr(radar_lead, "vLead", float("nan")))
+  if not np.isfinite(raw_d_rel) or not np.isfinite(raw_v_lead):
+    return None
+
+  # The model contributes future deltas only. This preserves raw lead source
+  # selection and keeps the current lead distance/speed safety anchor intact.
+  x_lead_traj = raw_d_rel + (model_x - model_x[0])
+  v_lead_traj = raw_v_lead + (model_v - model_v[0])
+  v_lead_traj = np.clip(v_lead_traj, 0.0, 1e8)
+
+  # Match the existing MPC convergence guard using the physical brake limit.
+  v_ego = float(v_ego)
+  min_x_lead = ((v_ego + v_lead_traj[0]) / 2.0) * (v_ego - v_lead_traj[0]) / (-ACCEL_MIN * 2.0)
+  x_lead_traj[0] = max(x_lead_traj[0], min_x_lead)
+
+  x_lead_mpc = np.maximum.accumulate(np.interp(T_IDXS, LEAD_T_IDXS_MODEL, x_lead_traj))
+  v_lead_mpc = np.interp(T_IDXS, LEAD_T_IDXS_MODEL, v_lead_traj)
+  return np.column_stack((x_lead_mpc, v_lead_mpc))
+
+
 def should_trigger_planner_fcw(lead, v_ego: float) -> bool:
   if lead is None or not lead.status or float(getattr(lead, "modelProb", 0.0)) <= FCW_MIN_MODEL_PROB:
     return False
@@ -414,6 +455,8 @@ class LongitudinalMpc:
     self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
     self.x_sol = np.zeros((N+1, X_DIM))
     self.u_sol = np.zeros((N,1))
+    self.lead_xv_0 = np.zeros((N+1, 2))
+    self.lead_xv_1 = np.zeros((N+1, 2))
     self.params = np.zeros((N+1, PARAM_DIM))
     for i in range(N+1):
       self.solver.set(i, 'x', np.zeros(X_DIM))
@@ -571,9 +614,15 @@ class LongitudinalMpc:
     return lead_xv
 
   def process_lead(self, lead, tracking_lead=True, t_follow=None, *, lead_index=0,
-                   smooth_duplicate_vision=False):
+                   smooth_duplicate_vision=False, model_lead=None,
+                   use_model_lead_trajectory=False):
     v_ego = self.x0[1]
     lead_active = lead is not None and lead.status and tracking_lead
+    if lead_active and use_model_lead_trajectory:
+      model_lead_xv = build_model_lead_trajectory(model_lead, lead, v_ego)
+      if model_lead_xv is not None:
+        return model_lead_xv
+
     if lead_active:
       x_lead = lead.dRel
       v_lead = lead.vLead
@@ -862,15 +911,25 @@ class LongitudinalMpc:
   def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow,
              personality=log.LongitudinalPersonality.standard, tracking_lead=True,
              optional_far_lead_comfort=True, smooth_duplicate_vision=False,
-             stop_x=None, silverado_early_follow=False):
+             stop_x=None, silverado_early_follow=False, modelV2=None,
+             use_model_lead_trajectory=False):
     v_ego = self.x0[1]
     lead_one = radarstate.leadOne
     lead_two = radarstate.leadTwo
     self.status = tracking_lead and (lead_one.status or lead_two.status)
+    model_leads = ()
+    if use_model_lead_trajectory and modelV2 is not None:
+      model_leads = getattr(modelV2, "leadsV3", ())
     lead_xv_0 = self.process_lead(lead_one, tracking_lead, t_follow=t_follow, lead_index=0,
-                                  smooth_duplicate_vision=smooth_duplicate_vision)
+                                  smooth_duplicate_vision=smooth_duplicate_vision,
+                                  model_lead=model_leads[0] if len(model_leads) > 0 else None,
+                                  use_model_lead_trajectory=use_model_lead_trajectory)
     lead_xv_1 = self.process_lead(lead_two, tracking_lead, t_follow=t_follow, lead_index=1,
-                                  smooth_duplicate_vision=smooth_duplicate_vision)
+                                  smooth_duplicate_vision=smooth_duplicate_vision,
+                                  model_lead=model_leads[1] if len(model_leads) > 1 else None,
+                                  use_model_lead_trajectory=use_model_lead_trajectory)
+    self.lead_xv_0 = lead_xv_0
+    self.lead_xv_1 = lead_xv_1
 
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance

selfdrive/controls/lib/longitudinal_planner.py

@@ -2207,7 +2207,9 @@ class LongitudinalPlanner:
                     optional_far_lead_comfort=True,
                     smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass,
                     stop_x=force_stop_x,
-                    silverado_early_follow=early_truck_follow)
+                    silverado_early_follow=early_truck_follow,
+                    modelV2=sm['modelV2'],
+                    use_model_lead_trajectory=bool(getattr(starpilot_toggles, "test_model_lead_trajectory", False)))
 
     self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@@ -2911,6 +2913,11 @@ class LongitudinalPlanner:
     longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
+    longitudinalPlan.leadTrajectoryX0 = self.mpc.lead_xv_0[:, 0].tolist()
+    longitudinalPlan.leadTrajectoryV0 = self.mpc.lead_xv_0[:, 1].tolist()
+    longitudinalPlan.leadTrajectoryX1 = self.mpc.lead_xv_1[:, 0].tolist()
+    longitudinalPlan.leadTrajectoryV1 = self.mpc.lead_xv_1[:, 1].tolist()
+
     longitudinalPlan.aTarget = float(self.output_a_target)
     force_stop_handoff = bool(
       sm['starpilotPlan'].forcingStop and (

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -16,7 +16,12 @@ import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_pla
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel, should_publish_planner_fcw
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, soften_far_radar_lead_accel, should_trigger_planner_fcw
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
+  LongitudinalMpc,
+  build_model_lead_trajectory,
+  soften_far_radar_lead_accel,
+  should_trigger_planner_fcw,
+)
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   allow_radar_standstill_gap_settle,
@@ -314,6 +319,63 @@ def set_model_lead(model, idx: int, *, prob: float, x0: float, y0: float, v0: fl
   lead.a = [float(a0)]
 
 
+def make_model_lead(*, prob: float = 0.99, x=None, v=None):
+  model = log.ModelDataV2.new_message()
+  model.init('leadsV3', 3)
+  lead = model.leadsV3[0]
+  lead.prob = float(prob)
+  lead.x = list(x if x is not None else [0.0, 20.0, 38.0, 54.0, 68.0, 80.0])
+  lead.v = list(v if v is not None else [18.0, 18.0, 17.0, 16.0, 15.0, 14.0])
+  return model, lead
+
+
+def test_model_lead_trajectory_is_opt_in_and_disabled_path_is_unchanged():
+  lead = make_lead(status=True, d_rel=42.0, v_lead=18.0, model_prob=0.99)
+  _, model_lead = make_model_lead()
+
+  legacy_mpc = LongitudinalMpc()
+  disabled_mpc = LongitudinalMpc()
+  legacy_mpc.set_cur_state(20.0, 0.0)
+  disabled_mpc.set_cur_state(20.0, 0.0)
+
+  legacy = legacy_mpc.process_lead(lead)
+  disabled = disabled_mpc.process_lead(
+    lead, model_lead=model_lead, use_model_lead_trajectory=False,
+  )
+  np.testing.assert_allclose(disabled, legacy)
+
+
+def test_model_lead_trajectory_uses_raw_current_anchor_and_future_deltas():
+  lead = make_lead(status=True, d_rel=42.0, v_lead=18.0, model_prob=0.99)
+  _, model_lead = make_model_lead()
+
+  trajectory = build_model_lead_trajectory(model_lead, lead, 20.0)
+
+  assert trajectory is not None
+  assert trajectory[0, 0] == pytest.approx(42.0)
+  assert trajectory[0, 1] == pytest.approx(18.0)
+  assert np.all(np.diff(trajectory[:, 0]) >= -1e-9)
+  assert trajectory[-1, 0] > trajectory[0, 0]
+  assert trajectory[-1, 1] < trajectory[0, 1]
+
+
+@pytest.mark.parametrize("prob", [0.0, 0.5])
+def test_model_lead_trajectory_falls_back_for_low_confidence(prob):
+  lead = make_lead(status=True, d_rel=42.0, v_lead=18.0, model_prob=prob)
+  _, model_lead = make_model_lead(prob=prob)
+  assert build_model_lead_trajectory(model_lead, lead, 20.0) is None
+
+
+def test_model_lead_trajectory_falls_back_without_raw_lead_or_valid_shape():
+  _, model_lead = make_model_lead()
+  no_raw_lead = make_lead(status=False, d_rel=42.0, v_lead=18.0)
+  assert build_model_lead_trajectory(model_lead, no_raw_lead, 20.0) is None
+
+  raw_lead = make_lead(status=True, d_rel=42.0, v_lead=18.0)
+  _, short_model_lead = make_model_lead(x=[0.0], v=[18.0])
+  assert build_model_lead_trajectory(short_model_lead, raw_lead, 20.0) is None
+
+
 def set_model_launch_trajectory(model, *, wait_time: float = 0.6, accel: float = 1.0):
   times = np.asarray(ModelConstants.T_IDXS, dtype=float)
   moving_time = np.maximum(times - wait_time, 0.0)

starpilot/common/starpilot_variables.py

@@ -953,6 +953,7 @@ class StarPilotVariables:
     )
 
     developer_feature_access = self.params.get_bool("DeveloperUI") or self.params.get_bool("GalaxyDeveloperMode")
+    toggle.test_model_lead_trajectory = self.get_value("TestModelLeadTrajectory", condition=developer_feature_access)
     toggle.pulse_and_glide_available = toggle.openpilot_longitudinal and developer_feature_access
     toggle.pulse_glide_speed_delta = self.get_value(
       "PulseGlideSpeedDelta",

starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json

@@ -4165,6 +4165,16 @@
         "ui_type": "toggle",
         "settings_tier": "simple"
       },
+      {
+        "key": "TestModelLeadTrajectory",
+        "label": "Test Model-Predicted Lead Trajectory",
+        "description": "Experimental: use the model's predicted lead path as the MPC lead trajectory. Raw lead safety and stop logic remain active.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "parent_key": "GalaxyDeveloperMode",
+        "requires_offroad": true,
+        "settings_tier": "advanced"
+      },
       {
         "key": "AlphaLongitudinalEnabled",
         "label": "openpilot Longitudinal Control (Alpha)",

starpilot/system/the_galaxy/tests/test_device_settings_layout.py

@@ -58,7 +58,7 @@ def test_galaxy_layout_contains_basic_mode_controls():
   } <= sections["Longitudinal (Speed & Following)"].keys()
   assert "RedneckCruise" not in sections["Longitudinal (Speed & Following)"].keys()
   assert sections["Developer"]["RedneckCruise"]["parent_key"] == "GalaxyDeveloperMode"
-  assert {"AlphaLongitudinalEnabled", "ForceOffroad", "GalaxyDeveloperMode"} <= sections["Developer"].keys()
+  assert {"AlphaLongitudinalEnabled", "ForceOffroad", "GalaxyDeveloperMode", "TestModelLeadTrajectory"} <= sections["Developer"].keys()
 
 
 def test_device_shutdown_uses_literal_hours():
@@ -140,6 +140,9 @@ def test_requested_simple_and_advanced_settings_tiers():
     assert longitudinal[key]["settings_tier"] == "advanced"
 
   assert developer["GalaxyDeveloperMode"]["settings_tier"] == "simple"
+  assert developer["TestModelLeadTrajectory"]["parent_key"] == "GalaxyDeveloperMode"
+  assert developer["TestModelLeadTrajectory"]["requires_offroad"] is True
+  assert developer["TestModelLeadTrajectory"]["settings_tier"] == "advanced"
   assert developer["AlphaLongitudinalEnabled"]["parent_key"] == "GalaxyDeveloperMode"
   assert developer["AlphaLongitudinalEnabled"]["requires_offroad"] is True
   assert developer["AlphaLongitudinalEnabled"]["settings_tier"] == "advanced"
@@ -153,6 +156,7 @@ def test_requested_simple_and_advanced_settings_tiers():
 
 def test_hidden_feature_defaults_remain_enabled():
   assert _declared_default("GalaxyDeveloperMode") == "0"
+  assert _declared_default("TestModelLeadTrajectory") == "0"
   assert _declared_default("NavDesiresAllowed") == "1"
   assert _declared_default("NavLanePositioningAllowed") == "0"
   assert _declared_default("NavLongitudinalAllowed") == "1"