diff --git a/cereal/log.capnp b/cereal/log.capnp index 38b20bb1b..0322ecae3 100644 --- a/cereal/log.capnp +++ b/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; diff --git a/common/params_keys.h b/common/params_keys.h index 03dfb85d4..49ba5309d 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -276,6 +276,7 @@ inline static std::unordered_map 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}}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index cbbce37e5..4167390b0 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 32b58cbde..f37ab1e5c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 ( diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 6e17b74a4..b77e5ef63 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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) diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 9ba615ff8..564ad6f15 100644 --- a/starpilot/common/starpilot_variables.py +++ b/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", diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index 0602c166a..83b530543 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/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)", diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index 6db921654..00bfca4e6 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/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"