From 632db8bfd3ed281ce8e364eedcc41947b235a9df Mon Sep 17 00:00:00 2001 From: Prabhaav Pillai Date: Tue, 25 Aug 2026 04:16:10 -0400 Subject: [PATCH] Add HybridExperimental mode --- common/params_keys.h | 3 + .../controls/lib/longitudinal_planner.py | 41 +- .../tests/test_longitudinal_planner.py | 40 + .../controls/tests/test_starpilot_planner.py | 17 + selfdrive/ui/lib/starpilot_status.py | 12 + selfdrive/ui/tests/test_starpilot_status.py | 18 +- .../common/assets/device_settings_layout.json | 38 + starpilot/common/safe_mode.py | 1 + starpilot/common/starpilot_variables.py | 25 +- .../common/tests/test_starpilot_variables.py | 12 + .../controls/lib/hybrid_experimental_mode.py | 152 ++++ starpilot/controls/starpilot_planner.py | 61 +- .../tests/test_hybrid_experimental_mode.py | 125 +++ starpilot/system/the_galaxy/the_galaxy.py | 10 +- tools/replay/mode_sim.py | 837 ++++++++++++++++++ 15 files changed, 1373 insertions(+), 19 deletions(-) create mode 100644 starpilot/controls/lib/hybrid_experimental_mode.py create mode 100644 starpilot/controls/tests/test_hybrid_experimental_mode.py create mode 100644 tools/replay/mode_sim.py diff --git a/common/params_keys.h b/common/params_keys.h index ac8a23f3d..5d79d5772 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -238,6 +238,9 @@ inline static std::unordered_map keys = { {"CommunityFavorites", {PERSISTENT, STRING, "", "", 1}}, {"ConditionalChill", {PERSISTENT, BOOL, "0", "0", 1}}, {"ConditionalExperimental", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}}, + {"HybridExpBias", {PERSISTENT, FLOAT, "0", "0", 1}}, + {"HybridExperimental", {PERSISTENT, BOOL, "0", "0", 1}}, + {"HybridVisionBrakeSensitivity", {PERSISTENT, FLOAT, "1", "1", 1}}, {"CurvatureData", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}}, {"CurveSpeedController", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}}, {"CurveSpeedControllerNoLead", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 991f38778..bd5d639f4 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -9,6 +9,7 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.starpilot.common.model_versions import is_tinygrad_model_version +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance @@ -596,6 +597,7 @@ class LongitudinalPlanner: self.duplicate_vision_comfort_lead_source = None self.prev_experimental_mode = None self.experimental_release_accel_until = 0.0 + self.hybrid_controller = HybridExperimentalMode() if self.is_preap: try: @@ -1936,6 +1938,7 @@ class LongitudinalPlanner: self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) self.model_allow_throttle = True self.model_allow_throttle_transition_t = 0.0 + self.hybrid_controller.reset(float(self.a_desired)) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) @@ -2284,26 +2287,42 @@ class LongitudinalPlanner: model_launch_accel = self.get_model_launch_accel(model_launch_v, model_launch_a, action_t, scene_v_ego) if classic_model: - output_a_target, output_should_stop = get_accel_from_plan_classic( + output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan_classic( self.CP, self.v_desired_trajectory, self.a_desired_trajectory, starpilot_toggles.vEgoStopping) elif tinygrad_model: output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan( self.v_desired_trajectory, self.a_desired_trajectory, action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping) - output_a_target_e2e = sm['modelV2'].action.desiredAcceleration - output_should_stop_e2e = sm['modelV2'].action.shouldStop - - if self.mode == 'acc' or self.generation == 'v9': - output_a_target = output_a_target_mpc - output_should_stop = output_should_stop_mpc - else: - output_a_target = min(output_a_target_mpc, output_a_target_e2e) - output_should_stop = output_should_stop_e2e or output_should_stop_mpc else: - output_a_target, output_should_stop = get_accel_from_plan( + output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan( self.v_desired_trajectory, self.a_desired_trajectory, action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping) + if bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)): + self.hybrid_controller.set_tuning( + getattr(starpilot_toggles, "hybrid_exp_bias", 0.0), + getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0), + ) + output_a_target_e2e = sm['modelV2'].action.desiredAcceleration + output_should_stop_e2e = sm['modelV2'].action.shouldStop + output_a_target = self.hybrid_controller.update( + v_ego=scene_v_ego, + v_cruise=v_cruise, + lead_one=self.lead_one, + model_v2=sm['modelV2'], + a_chill=output_a_target_mpc, + a_exp=output_a_target_e2e, + ) + output_should_stop = output_should_stop_mpc or output_should_stop_e2e + elif tinygrad_model and self.mode != 'acc' and self.generation != 'v9': + output_a_target_e2e = sm['modelV2'].action.desiredAcceleration + output_should_stop_e2e = sm['modelV2'].action.shouldStop + output_a_target = min(output_a_target_mpc, output_a_target_e2e) + output_should_stop = output_should_stop_e2e or output_should_stop_mpc + else: + output_a_target = output_a_target_mpc + output_should_stop = output_should_stop_mpc + comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0] vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego)) output_accel_min = comfort_output_accel_min diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 5f9147c94..de77517c1 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -514,6 +514,46 @@ def test_experimental_mlsim_uses_vehicle_min_accel_floor(model_version): assert planner.output_a_target < comfort_min_accel +def test_hybrid_mode_shields_unconfirmed_vision_braking(): + v_ego = 20.0 + desired_accel = -2.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + + # Hybrid mode clamps unconfirmed vision braking to a gentle coast (no lead, + # model not predicting a stop), instead of passing the raw E2E target. + hybrid_toggles = SimpleNamespace(**vars(make_toggles("v11")), hybrid_experimental_mode=True) + planner_hybrid = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm(v_ego, desired_accel, -2.0, experimental_mode=False) + planner_hybrid.update(sm, hybrid_toggles) + assert planner_hybrid.mode == "acc" + assert planner_hybrid.output_a_target >= -0.6 + + # Without the hybrid shield, experimental mode lets the raw E2E target through. + planner_exp = LongitudinalPlanner(CP, init_v=v_ego) + sm_exp = make_sm(v_ego, desired_accel, -2.0, experimental_mode=True) + planner_exp.update(sm_exp, make_toggles("v11")) + assert planner_exp.output_a_target <= -1.5 + + +def test_hybrid_mode_arbitrates_vision_braking_with_lead(): + v_ego = 20.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + hybrid_toggles = SimpleNamespace(**vars(make_toggles("v11")), hybrid_experimental_mode=True) + + # With a tracked lead, the hybrid grants full E2E braking authority. + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=-2.0, + min_accel=-2.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=30.0, v_lead=10.0), + ) + planner.update(sm, hybrid_toggles) + assert planner.output_a_target < 0.0 + + def test_gm_pedal_vehicle_min_accel_uses_brand_when_car_name_is_missing(): CP = SimpleNamespace( carName=None, diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index deea9de2c..1735ec943 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -175,6 +175,23 @@ def test_lateral_resume_delay_ignores_signal_cycles_that_never_slow_enough(monke planner.shutdown() +def test_hybrid_mode_runs_continuous_controller(monkeypatch): + planner = make_planner(monkeypatch) + monkeypatch.setattr(planner.starpilot_cem, "update", lambda *args, **kwargs: None) + + try: + toggles = make_toggles(hybrid_experimental_mode=True) + + planner.update(0.0, False, make_sm(planner, frame=1, v_ego=20.0, left_blinker=False), toggles) + + assert planner.hybrid_controller is not None + assert planner.hybrid_acceleration != 0.0 + assert planner.starpilot_ccm.experimental_mode is False + assert planner.starpilot_cem.experimental_mode is False + finally: + planner.shutdown() + + def test_radarless_follow_hold_applies_to_tracked_vision_lead(monkeypatch): planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) diff --git a/selfdrive/ui/lib/starpilot_status.py b/selfdrive/ui/lib/starpilot_status.py index 161c23457..4ad04e89f 100644 --- a/selfdrive/ui/lib/starpilot_status.py +++ b/selfdrive/ui/lib/starpilot_status.py @@ -11,6 +11,7 @@ AOL_COLOR = rl.Color(10, 186, 181, 255) ENGAGED_COLOR = rl.Color(22, 127, 64, 255) OVERRIDE_COLOR = rl.Color(137, 146, 141, 255) EXPERIMENTAL_COLOR = rl.Color(218, 111, 37, 255) +HYBRID_EXPERIMENTAL_COLOR = rl.Color(0, 153, 255, 255) CEM_OVERRIDE_COLOR = rl.Color(255, 214, 0, 255) SWITCHBACK_COLOR = rl.Color(139, 108, 197, 255) TRAFFIC_COLOR = rl.Color(201, 34, 49, 255) @@ -27,6 +28,11 @@ def is_longitudinal_only_active(state: UIState) -> bool: return bool(state.sm["selfdriveState"].enabled and not car_control.latActive) +def _is_hybrid_experimental_mode(state: UIState) -> bool: + """True when HEM (hybrid experimental) is the active longitudinal mode.""" + return bool(state.starpilot_toggles.get("hybrid_experimental_mode", False)) + + def _override_color_applies(state: UIState) -> bool: """Only gray the status when the active control mode is being overridden.""" if state.status != UIStatus.OVERRIDE: @@ -57,6 +63,8 @@ def get_border_color(state: UIState): if state.always_on_lateral_active: return AOL_COLOR # Only color the border for CEM/experimental while actually enabled. + if enabled and _is_hybrid_experimental_mode(state): + return HYBRID_EXPERIMENTAL_COLOR if enabled and state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES: return CEM_OVERRIDE_COLOR if enabled and state.sm["selfdriveState"].experimentalMode: @@ -67,6 +75,8 @@ def get_border_color(state: UIState): def get_path_edge_color(state: UIState): + if state.sm["selfdriveState"].enabled and _is_hybrid_experimental_mode(state): + return HYBRID_EXPERIMENTAL_COLOR if state.conditional_status in CEM_ACTIVE_STATUSES: return EXPERIMENTAL_COLOR return get_border_color(state) @@ -85,6 +95,8 @@ def get_screen_edge_color(state: UIState): return AOL_COLOR # Keep the screen edge disengaged-blue when experimental mode is only the # requested longitudinal mode, not the active driving state. + if enabled and _is_hybrid_experimental_mode(state): + return HYBRID_EXPERIMENTAL_COLOR if enabled and state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES: return CEM_OVERRIDE_COLOR if enabled and state.sm["selfdriveState"].experimentalMode: diff --git a/selfdrive/ui/tests/test_starpilot_status.py b/selfdrive/ui/tests/test_starpilot_status.py index e2c4dc3c9..28efd7ece 100644 --- a/selfdrive/ui/tests/test_starpilot_status.py +++ b/selfdrive/ui/tests/test_starpilot_status.py @@ -3,16 +3,18 @@ from types import SimpleNamespace from openpilot.selfdrive.ui.lib.starpilot_status import ( DISENGAGED_COLOR, ENGAGED_COLOR, + HYBRID_EXPERIMENTAL_COLOR, LONGITUDINAL_ONLY_COLOR, AOL_COLOR, OVERRIDE_COLOR, get_border_color, + get_path_edge_color, get_screen_edge_color, ) from openpilot.selfdrive.ui.ui_state import UIStatus -def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=()): +def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=(), hybrid=False): return SimpleNamespace( sm={ "selfdriveState": SimpleNamespace(enabled=enabled, experimentalMode=False), @@ -24,6 +26,7 @@ def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=() switchback_mode_enabled=False, traffic_mode_enabled=False, conditional_status=0, + starpilot_toggles={"hybrid_experimental_mode": hybrid}, ) @@ -44,6 +47,19 @@ def test_lateral_active_colors_remain_unchanged(): assert _rgb(get_border_color(_state())) == _rgb(DISENGAGED_COLOR) +def test_hybrid_experimental_mode_uses_purple_border(): + state = _state(enabled=True, hybrid=True) + + assert _rgb(get_border_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) + assert _rgb(get_screen_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) + assert _rgb(get_path_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) + + +def test_hybrid_experimental_mode_requires_enabled(): + assert _rgb(get_border_color(_state(hybrid=True))) == _rgb(DISENGAGED_COLOR) + assert _rgb(get_border_color(_state(enabled=True))) == _rgb(ENGAGED_COLOR) + + def test_override_color_matches_active_control_mode(): lateral_override = SimpleNamespace(overrideLateral=True, overrideLongitudinal=False) longitudinal_override = SimpleNamespace(overrideLateral=False, overrideLongitudinal=True) diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 7ee7b669c..e81439856 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -2029,6 +2029,44 @@ "parent_key": "ConditionalChill", "settings_tier": "advanced" }, + { + "key": "HybridExperimental", + "label": "Continuous Hybrid Control", + "description": "Fuse the crisp throttle response of classical ACC with the model's early, natural vision braking using a single continuous controller. Classical ACC always provides the hard safety distance, while the model's vision trajectory is blended in for early stops. Unconfirmed vision braking is capped to a gentle coast, and vision is never allowed to under-brake.", + "picker_description": "Continuously blends Chill ACC with the model's vision braking.", + "data_type": "bool", + "ui_type": "toggle", + "is_parent_toggle": true, + "settings_tier": "advanced" + }, + { + "key": "HybridExpBias", + "label": "E2E Authority Bias", + "description": "Biases the continuous hybrid between Chill and E2E. Negative favors Chill throttle and braking, positive favors E2E vision behavior. Range -1.0 to 1.0, default 0.0.", + "picker_description": "Biases authority between Chill and E2E vision.", + "data_type": "float", + "ui_type": "numeric", + "min": -1.0, + "max": 1.0, + "step": 0.05, + "precision": 2, + "parent_key": "HybridExperimental", + "settings_tier": "advanced" + }, + { + "key": "HybridVisionBrakeSensitivity", + "label": "Vision Brake Sensitivity", + "description": "Scales how strongly the model's predicted stop horizon contributes to early braking. Below 1.0 reduces unprompted slowing; above 1.0 starts red-light and queue braking earlier. Range 0.0 to 2.0, default 1.0.", + "picker_description": "Scales early vision braking authority.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 2.0, + "step": 0.05, + "precision": 2, + "parent_key": "HybridExperimental", + "settings_tier": "advanced" + }, { "key": "SLCAbbreviatedSources", "label": "Show Abbreviated Icon Sources", diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 8b2cdb3d1..b178452b3 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -112,6 +112,7 @@ SAFE_MODE_MANAGED_KEYS = ( "ReduceLateralAccelerationSnow", "ConditionalExperimental", "ConditionalChill", + "HybridExperimental", "CECurves", "CECurvesLead", "CELead", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 033e5b7be..c7a857061 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -377,6 +377,19 @@ def speed_limit_controller_available(openpilot_longitudinal: bool, redneck_cruis return openpilot_longitudinal or redneck_cruise +def get_longitudinal_modes(openpilot_longitudinal: bool, cem: bool, ccm: bool, hybrid: bool) -> tuple[bool, bool, bool]: + """Resolve the mutually exclusive longitudinal control modes (CEM/CCM/Hybrid).""" + conditional_experimental_mode = bool(openpilot_longitudinal) and bool(cem) + conditional_chill_mode = bool(openpilot_longitudinal) and not conditional_experimental_mode and bool(ccm) + hybrid_experimental_mode = ( + bool(openpilot_longitudinal) and + not conditional_experimental_mode and + not conditional_chill_mode and + bool(hybrid) + ) + return conditional_experimental_mode, conditional_chill_mode, hybrid_experimental_mode + + def migrate_cancel_button_controls(params: Params | None = None) -> bool: params = params or Params(return_defaults=True) if params.get_bool(CANCEL_BUTTON_MIGRATION_KEY) or not params.get_bool("RemapCancelToDistance"): @@ -812,8 +825,16 @@ class StarPilotVariables: self.migrate_prius_cluster_offset(str(toggle.car_model)) toggle.cluster_offset = self.get_value("ClusterOffset", cast=float, condition=toggle.car_make == "toyota") - toggle.conditional_experimental_mode = toggle.openpilot_longitudinal and self.get_value("ConditionalExperimental") - toggle.conditional_chill_mode = toggle.openpilot_longitudinal and not toggle.conditional_experimental_mode and self.get_value("ConditionalChill") + toggle.conditional_experimental_mode, toggle.conditional_chill_mode, toggle.hybrid_experimental_mode = get_longitudinal_modes( + toggle.openpilot_longitudinal, + self.get_value("ConditionalExperimental"), + self.get_value("ConditionalChill"), + self.get_value("HybridExperimental"), + ) + toggle.hybrid_exp_bias = self.get_value( + "HybridExpBias", cast=float, condition=toggle.hybrid_experimental_mode, default=0.0, min=-1.0, max=1.0) + toggle.hybrid_vision_brake_sensitivity = self.get_value( + "HybridVisionBrakeSensitivity", cast=float, condition=toggle.hybrid_experimental_mode, default=1.0, min=0.0, max=2.0) toggle.conditional_curves = self.get_value("CECurves", condition=toggle.conditional_experimental_mode) toggle.conditional_curves_lead = self.get_value("CECurvesLead", condition=toggle.conditional_curves) toggle.conditional_lead = self.get_value("CELead", condition=toggle.conditional_experimental_mode) diff --git a/starpilot/common/tests/test_starpilot_variables.py b/starpilot/common/tests/test_starpilot_variables.py index a52852911..626ff2ae8 100644 --- a/starpilot/common/tests/test_starpilot_variables.py +++ b/starpilot/common/tests/test_starpilot_variables.py @@ -227,6 +227,18 @@ def test_missing_bounded_value_uses_explicit_default(): assert value == 1.0 +def test_get_longitudinal_modes_is_mutually_exclusive(): + # Hybrid wins when selected and CEM/CCM params are off. + assert spv.get_longitudinal_modes(True, False, False, True) == (False, False, True) + # CEM takes priority over the other modes when enabled. + assert spv.get_longitudinal_modes(True, True, True, True) == (True, False, False) + # CCM engages only when CEM is off. + assert spv.get_longitudinal_modes(True, False, True, False) == (False, True, False) + assert spv.get_longitudinal_modes(True, False, True, True) == (False, True, False) + # Nothing engages without openpilot longitudinal. + assert spv.get_longitudinal_modes(False, True, True, True) == (False, False, False) + + def test_disabled_conditional_experimental_toggles_are_off(monkeypatch, tmp_path): params_cls = spv.Params diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py new file mode 100644 index 000000000..350c0c0a5 --- /dev/null +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 +import numpy as np +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE + +def lerp(a: float, b: float, t: float) -> float: + """Linear interpolation / blend between a and b by weight t (0.0 to 1.0).""" + return float((1.0 - t) * a + t * b) + + +def sigmoid(x: float, k: float = 4.0, x0: float = 0.0) -> float: + """Smooth 0-to-1 activation curve.""" + z = np.clip(-k * (x - x0), -30.0, 30.0) + return float(1.0 / (1.0 + np.exp(z))) + + +def smooth_min(a: float, b: float, k: float = 6.0) -> float: + return lerp(b, a, sigmoid(b - a, k=k)) + + +def smooth_max(a: float, b: float, k: float = 6.0) -> float: + return lerp(b, a, sigmoid(a - b, k=k)) + +class HybridExperimentalMode: + """ + Fuses Chill Mode (radar/lead tracking) and Experimental Mode (vision/stop signs/lights): + 1. Detects vision braking intent from the E2E model trajectory. + 2. Blends Chill and Exp acceleration smoothly based on intent. + 3. Holds 0 m/s at standstills to prevent creep. + 4. Falls back safely to Chill if lead vehicle distance is compromised. + 5. Slew-rate limits acceleration to respect vehicle jerk limits. + """ + + # Base physical actuator jerk limits (m/s^3) + BASE_MAX_JERK_BRAKE = 3.5 + BASE_MAX_JERK_ACCEL = 5.5 + + # Base safety floor parameters + BASE_T_FOLLOW = 1.45 + BASE_STOP_DISTANCE = STOP_DISTANCE # 6.0 m + + def __init__(self): + self.DT = DT_MDL + self.prev_a_target = 0.0 + self.exp_authority = 0.5 + + # User tuning + self.HYBRID_EXP_BIAS = 0.0 # [-1.0, 1.0] + self.VISION_BRAKE_SENSITIVITY = 1.0 # [0.0, 2.0] + + # Active profile parameters + self.t_follow = self.BASE_T_FOLLOW + self.jerk_factor = 1.0 + self._update_profile_limits(self.t_follow, self.jerk_factor) + + def reset(self, a: float = 0.0): + self.prev_a_target = float(a) + self.exp_authority = 0.5 + + def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float, t_follow=None, jerk_factor=None): + self.HYBRID_EXP_BIAS = float(np.clip(exp_bias, -1.0, 1.0)) + self.VISION_BRAKE_SENSITIVITY = float(np.clip(vision_brake_sensitivity, 0.0, 2.0)) + if t_follow is not None or jerk_factor is not None: + self._update_profile_limits(t_follow, jerk_factor) + + def _update_profile_limits(self, t_follow, jerk_factor=1.0): + """Updates safety headway floor and jerk limits based on active driving profile.""" + self.t_follow = float(t_follow) if t_follow is not None else self.BASE_T_FOLLOW + self.jerk_factor = float(np.clip(jerk_factor if jerk_factor is not None else 1.0, 0.25, 2.0)) + + # Safety buffer floors + self.T_FOLLOW_SAFE = float(np.clip(self.t_follow * 0.80, 1.00, 1.60)) + self.D_STATIC_SAFE = float(np.clip(self.BASE_STOP_DISTANCE * 0.75, 3.5, 6.0)) + + # Jerk rate limits + self.MAX_JERK_ACCEL = float(np.clip(self.BASE_MAX_JERK_ACCEL * self.jerk_factor, 2.5, 8.0)) + self.MAX_JERK_BRAKE = float(np.clip(self.BASE_MAX_JERK_BRAKE * self.jerk_factor, 1.8, 6.0)) + + @staticmethod + def _get_model_trajectory_v(model_v2, v_ego: float) -> np.ndarray: + velocity = getattr(model_v2, "velocity", None) + traj_v = getattr(velocity, "x", None) if velocity is not None else None + if traj_v is None or len(traj_v) == 0: + return np.array([v_ego], dtype=float) + return np.asarray(traj_v, dtype=float) + + def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp, + t_follow=None, jerk_factor=None): + # 0. Sync profile parameters if passed per-frame + if (t_follow is not None and abs(t_follow - self.t_follow) > 1e-4) or \ + (jerk_factor is not None and abs(jerk_factor - self.jerk_factor) > 1e-4): + self._update_profile_limits(t_follow, jerk_factor) + + lead_status = bool(getattr(lead_one, "status", False)) + lead_d_rel = float(getattr(lead_one, "dRel", 150.0)) + + # 1. VISION INTENT DETECTION (How urgently does the vision model want to slow?) + traj_v = self._get_model_trajectory_v(model_v2, v_ego) + v_terminal = float(traj_v[-1]) + v_min = float(np.min(traj_v)) + v_ref = max(v_ego, 2.0) + + speed_drop_ratio = max(0.0, (v_ego - v_min) / v_ref) + stop_ahead_intent = max(0.0, (v_ego - v_terminal) / v_ref) * sigmoid(v_ego, k=3.0, x0=1.0) + model_decel_strength = max(0.0, -a_exp / 3.0) + + raw_vision_metric = max(speed_drop_ratio, stop_ahead_intent, model_decel_strength) + w_vision = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0)) + + # Experimental authority weight (bias + vision confidence) + base_auth = 0.5 + (0.35 * self.HYBRID_EXP_BIAS) + alpha_exp = float(np.clip(base_auth + (0.5 * w_vision), 0.0, 1.0)) + self.exp_authority = alpha_exp + + # 2. ACCELERATION FUSION (Throttle vs Braking Regimes) + # Throttle: Be responsive (smooth_max), but prioritize stopping if vision sees a stop + a_throttle_optimal = smooth_max(a_chill, a_exp, k=4.0) + a_throttle_conservative = smooth_min(a_chill, a_exp, k=4.0) + a_throttle_fused = lerp(a_throttle_optimal, a_throttle_conservative, w_vision) + + # Braking: Smoothly hand control to Exp based on vision confidence + a_brake_fused = lerp(a_chill, a_exp, alpha_exp) + + # Pick between Accel and Brake regimes + phase_metric = smooth_min(a_chill, a_exp, k=4.0) + w_accel = sigmoid(phase_metric, k=3.0, x0=-0.1) + a_fused = lerp(a_brake_fused, a_throttle_fused, w_accel) + + # 3. STANDSTILL ANCHOR (Prevent creeping at 0 mph) + is_stopped = sigmoid(0.3 - v_ego, k=8.0, x0=0.0) + is_terminal_stopped = sigmoid(0.8 - v_terminal, k=4.0, x0=0.0) + standstill_weight = is_stopped * is_terminal_stopped + a_anchored = lerp(a_fused, smooth_min(a_fused, 0.0, k=8.0), standstill_weight) + + # 4. SAFETY BARRIER (Lead Vehicle Proximity Check) + d_static_effective = self.D_STATIC_SAFE + max(0.0, 1.5 * (1.0 - (v_ego / 4.0))) + d_safe = (v_ego * self.T_FOLLOW_SAFE) + d_static_effective + + # Compute safety risk if lead is within minimum buffer + distance_ratio = (lead_d_rel - d_static_effective) / max(d_safe - d_static_effective, 1.0) + lead_safety_risk = sigmoid(1.0 - distance_ratio, k=5.0, x0=0.0) * float(lead_status) + + # Fall back to Chill braking if Chill is more conservative + a_emergency_brake = smooth_min(a_anchored, a_chill, k=6.0) + a_safe = lerp(a_anchored, a_emergency_brake, lead_safety_risk) + + # 5. ASYMMETRIC SLEW FILTER (Limit Jerk) + jerk_limit = self.MAX_JERK_ACCEL if a_safe >= self.prev_a_target else self.MAX_JERK_BRAKE + max_delta = jerk_limit * self.DT + + self.prev_a_target = float(np.clip(a_safe, self.prev_a_target - max_delta, self.prev_a_target + max_delta)) + return self.prev_a_target \ No newline at end of file diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 650bdecb1..b9a4c0a4f 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -24,6 +24,7 @@ from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode from openpilot.starpilot.controls.lib.starpilot_acceleration import StarPilotAcceleration from openpilot.starpilot.controls.lib.starpilot_events import StarPilotEvents from openpilot.starpilot.controls.lib.starpilot_following import StarPilotFollowing @@ -68,6 +69,7 @@ class StarPilotPlanner: self.starpilot_acceleration = StarPilotAcceleration(self) self.starpilot_cem = ConditionalExperimentalMode(self) self.starpilot_ccm = ConditionalChillMode(self, self.starpilot_cem) + self.hybrid_controller = HybridExperimentalMode() self.starpilot_events = StarPilotEvents(self, error_log, ThemeManager) self.starpilot_following = StarPilotFollowing(self) self.starpilot_vcruise = StarPilotVCruise(self) @@ -100,6 +102,7 @@ class StarPilotPlanner: self.road_curvature = 0 self.time_to_curve = 0 self.v_cruise = 0 + self.hybrid_acceleration = 0.0 self.gps_position = None @@ -220,7 +223,29 @@ class StarPilotPlanner: self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles) conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled - if conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)): + if conditional_tracking_active and bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)): + # Continuous Hybrid Experimental Control: instead of a binary CEM/CCM mode + # switch, fuse the classical Chill ACC target with the model's E2E + # trajectory through the HybridExperimentalMode controller. CEM's detector + # stays warm so red-light/stop-sign scene state (redLight, forcing_stop) + # remains accurate. Experimental Mode stays off so the MPC keeps producing + # the crisp classical a_chill that the hybrid is built on. + self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise) + self.starpilot_ccm.experimental_mode = False + self.starpilot_cem.experimental_mode = False + self.hybrid_controller.set_tuning( + getattr(starpilot_toggles, "hybrid_exp_bias", 0.0), + getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0), + ) + self.hybrid_acceleration = self.hybrid_controller.update( + v_ego=v_ego, + v_cruise=v_cruise, + lead_one=self.lead_one, + model_v2=sm["modelV2"], + a_chill=self._get_chill_accel(v_ego, v_cruise), + a_exp=self._get_vision_exp_accel(sm), + ) + elif conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)): # Keep CEM's filters warm in AOL so engagement can inherit the current scene. self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise) self.starpilot_ccm.experimental_mode = True @@ -296,6 +321,40 @@ class StarPilotPlanner: self.tracking_lead_filter.update(following_lead) return self.tracking_lead_filter.x >= THRESHOLD + def _get_vision_exp_accel(self, sm): + """Model's raw predicted acceleration for the E2E (Experimental) channel.""" + try: + accel_x = sm["modelV2"].acceleration.x + if len(accel_x) > 0: + return float(accel_x[0]) + except (AttributeError, IndexError, TypeError): + pass + try: + return float(sm["modelV2"].action.desiredAcceleration) + except (AttributeError, TypeError, ValueError): + return 0.0 + + def _get_chill_accel(self, v_ego, v_cruise): + """Cruise/lead tracking acceleration for the Chill (ACC) channel.""" + max_accel = float(getattr(self.starpilot_acceleration, "max_accel", 0.0) or 0.0) + min_accel = float(getattr(self.starpilot_acceleration, "min_accel", 0.0) or 0.0) + if max_accel <= 0.0 and min_accel == 0.0: + max_accel, min_accel = 1.5, -2.0 + + a_chill = float(np.clip((v_cruise - v_ego) / 2.0, min_accel, max_accel)) + lead = getattr(self, "lead_one", None) + if lead is not None and bool(getattr(lead, "status", False)): + d_rel = float(getattr(lead, "dRel", float("inf"))) + v_lead = float(getattr(lead, "vLead", v_ego)) + t_follow = float(getattr(self.starpilot_following, "t_follow", 1.45) or 1.45) + desired_gap = max(v_ego * t_follow, 4.0) + if d_rel < desired_gap: + gap_deficit = max(desired_gap - d_rel, 0.0) + closing = max(0.0, v_ego - v_lead) + lead_decel = -min(closing / 2.0 + gap_deficit / 4.0, 3.0) + a_chill = min(a_chill, lead_decel) + return float(np.clip(a_chill, min_accel, max_accel)) + def publish(self, theme_updated, sm, pm, starpilot_toggles, serialized_toggles=""): starpilot_plan_send = messaging.new_message("starpilotPlan") starpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"]) diff --git a/starpilot/controls/tests/test_hybrid_experimental_mode.py b/starpilot/controls/tests/test_hybrid_experimental_mode.py new file mode 100644 index 000000000..18434a280 --- /dev/null +++ b/starpilot/controls/tests/test_hybrid_experimental_mode.py @@ -0,0 +1,125 @@ +import numpy as np +import pytest + +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import ( + HybridExperimentalMode, + sigmoid, + soft_max, + soft_min, +) + + +class FakeLead: + def __init__(self, status=False, d_rel=150.0, v_lead=0.0): + self.status = status + self.dRel = d_rel + self.vLead = v_lead + + +class FakeModel: + def __init__(self, velocity=None): + self.velocity = type("Velocity", (), {"x": velocity})() + + +def make_controller(prev=0.0): + controller = HybridExperimentalMode() + controller.reset(prev) + return controller + + +def run(controller, *, v_ego=20.0, v_cruise=30.0, lead=None, model=None, a_chill=0.5, a_exp=0.8, frames=80): + lead = lead if lead is not None else FakeLead() + model = model if model is not None else FakeModel(velocity=[v_ego] * 20) + result = 0.0 + for _ in range(frames): + result = controller.update(v_ego, v_cruise, lead, model, a_chill, a_exp) + return result + + +def test_soft_operators_are_continuous_and_bounded(): + for value in (-5.0, -0.1, 0.0, 0.1, 5.0): + assert 0.0 < sigmoid(value) < 1.0 + assert soft_max(1.0, 2.0) == pytest.approx(2.0, abs=1e-2) + assert soft_min(1.0, 2.0) == pytest.approx(1.0, abs=1e-2) + + +def test_throttle_fusion_leans_toward_snappier_target(): + controller = make_controller() + # No lead, no vision stop: the continuous fusion routes toward the more + # confident throttle (exp 0.8) without a mode switch. + a = run(controller, a_chill=0.5, a_exp=0.8) + assert a > 0.75 + assert a < 0.85 + + +def test_vision_stop_horizon_grants_full_braking(): + controller = make_controller() + model = FakeModel(velocity=np.linspace(20.0, 0.1, 20)) + # The model trajectory decays to a terminal stop, so w_vision -> 1 and the + # early vision braking curve takes over. + a = run(controller, lead=FakeLead(status=False), model=model, a_chill=-0.5, a_exp=-2.0) + assert a <= -1.5 + + +def test_phantom_brake_shield_caps_unconfirmed_vision_braking(): + controller = make_controller() + # No lead and a flat model horizon: unconfirmed vision braking is clamped to + # the gentle coast limit instead of passing the raw E2E target through. + a = run(controller, lead=FakeLead(status=False), a_chill=0.0, a_exp=-2.0) + assert -0.6 <= a <= -0.5 + + +def test_cbf_safety_floor_prevents_under_braking_near_close_lead(): + controller = make_controller() + # Vision wants throttle but a close lead means the Control Barrier Function + # smoothly forces the target toward the Chill safety floor. + lead = FakeLead(status=True, d_rel=5.0, v_lead=0.0) + a = run(controller, lead=lead, a_chill=-1.0, a_exp=0.5) + assert a == pytest.approx(-1.0, abs=1e-2) + + +def test_cbf_is_inactive_when_lead_is_far(): + controller = make_controller() + lead = FakeLead(status=True, d_rel=80.0, v_lead=25.0) + a = run(controller, lead=lead, a_chill=0.5, a_exp=0.8) + assert a > 0.7 + + +def test_jerk_slew_limits_single_frame_step(): + controller = make_controller(prev=0.0) + lead = FakeLead(status=False) + model = FakeModel(velocity=[20.0] * 20) + a = controller.update(20.0, 30.0, lead, model, 2.0, 2.0) + assert a == pytest.approx(controller.MAX_JERK * controller.DT, abs=1e-3) + + +def test_reset_clears_integrator_state(): + controller = make_controller(prev=0.0) + lead = FakeLead(status=True, d_rel=20.0, v_lead=15.0) + model = FakeModel(velocity=[20.0] * 20) + controller.update(20.0, 30.0, lead, model, -1.0, -2.0) + assert controller.prev_a_target != 0.0 + controller.reset(0.0) + assert controller.prev_a_target == 0.0 + + +def test_set_tuning_clamps_to_documented_ranges(): + controller = make_controller() + controller.set_tuning(5.0, 9.0) + assert controller.HYBRID_EXP_BIAS == 1.0 + assert controller.VISION_BRAKE_SENSITIVITY == 2.0 + controller.set_tuning(-5.0, -1.0) + assert controller.HYBRID_EXP_BIAS == -1.0 + assert controller.VISION_BRAKE_SENSITIVITY == 0.0 + controller.set_tuning(0.5, 1.5) + assert controller.HYBRID_EXP_BIAS == 0.5 + assert controller.VISION_BRAKE_SENSITIVITY == 1.5 + + +def test_missing_model_velocity_falls_back_to_ego_speed(): + controller = make_controller() + model = FakeModel(velocity=None) + lead = FakeLead(status=False) + # Falls back to a constant-velocity horizon, so no vision braking is injected. + a = run(controller, lead=lead, model=model, a_chill=0.5, a_exp=0.2) + assert a > 0.4 diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 15c6eaea6..ad064f6cf 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -1523,6 +1523,7 @@ _TROUBLESHOOT_PERSONALITY_KEYS = [ _TROUBLESHOOT_CEM_KEYS = [ "ConditionalExperimental", + "HybridExperimental", "CESpeed", "CESpeedLead", "CECurves", @@ -5201,15 +5202,16 @@ def setup(app): "updated": updated, }), 200 - if key in {"ConditionalExperimental", "ConditionalChill"}: + if key in {"ConditionalExperimental", "ConditionalChill", "HybridExperimental"}: enabled = str_val.strip() in ("1", "true", "True") params.put_bool(key, enabled) updated = {key: enabled} if enabled: - other_key = "ConditionalChill" if key == "ConditionalExperimental" else "ConditionalExperimental" - params.put_bool(other_key, False) - updated[other_key] = False + for other_key in ("ConditionalExperimental", "ConditionalChill", "HybridExperimental"): + if other_key != key: + params.put_bool(other_key, False) + updated[other_key] = False update_starpilot_toggles() return jsonify({ diff --git a/tools/replay/mode_sim.py b/tools/replay/mode_sim.py new file mode 100644 index 000000000..43e0b7f49 --- /dev/null +++ b/tools/replay/mode_sim.py @@ -0,0 +1,837 @@ +#!/usr/bin/env python3 +"""Offline mode-scenario estimator for CEM / Chill (CCM) / HEM / Exp. + +Replays a logged route segment through the real StarPilot mode state machines +(ConditionalExperimentalMode, ConditionalChillMode, HybridExperimentalMode) +and openpilot's LongitudinalPlanner, then plots and prints what each mode's +experimental intent and acceleration target would have been at every 20 Hz frame. + +Usage: + ./dev python tools/replay/mode_sim.py [--segment 0] [--start 30] [--end 120] + [--data_dir /path/to/routes] [--out graph.png] [--show] + [--set CESpeed=20 --set CEModelStopTime=3.0 --set CCMLead=true] [--csv out.csv] + +Examples: + ./dev python tools/replay/mode_sim.py afb7ef2ed593d651/00000095--9dcb90357c --segment 9 +""" +from __future__ import annotations + +import argparse +import bisect +import sys +import time +from pathlib import Path +from types import SimpleNamespace +from typing import Any + +import numpy as np + +ROOT_DIR = Path(__file__).resolve().parents[2] +if str(ROOT_DIR) not in sys.path: + sys.path.insert(0, str(ROOT_DIR)) + +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_accel_from_plan +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE +from openpilot.starpilot.common.experimental_state import CCStatus, CEStatus +from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature +from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME +from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode +from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode +from openpilot.tools.lib.logreader import LogReader, ReadMode, parse_direct, parse_indirect +from openpilot.tools.lib.route import SegmentRange + +SERVICES = { + "carState", "radarState", "starpilotRadarState", "modelV2", + "longitudinalPlan", "selfdriveState", "starpilotPlan", "starpilotCarState", + "controlsState", "carParams", "liveParameters", "carControl", +} + +CE_LABELS = { + CEStatus["CURVATURE"]: "CURVE", + CEStatus["LEAD"]: "LEAD", + CEStatus["SIGNAL"]: "SIGNAL", + CEStatus["SPEED"]: "SPEED", + CEStatus["SPEED_LIMIT"]: "SPEED_LIMIT", + CEStatus["STOP_LIGHT"]: "STOP_LIGHT", +} +CC_LABELS = { + CCStatus["LEAD"]: "LEAD", + CCStatus["SPEED"]: "SPEED", +} +CC_AUTO_STATUSES = {CCStatus["SPEED"], CCStatus["LEAD"]} + +# CEM/CCM toggle defaults (units in m/s where appropriate). +TOGGLE_DEFAULTS: dict[str, Any] = { + # CEM + "conditional_curves": True, + "conditional_curves_lead": True, + "conditional_lead": True, + "conditional_slower_lead": True, + "conditional_stopped_lead": True, + "conditional_open_road": False, + "conditional_limit": 0.0, # CESpeed, m/s (0 = disabled) + "conditional_limit_lead": 0.0, # CESpeedLead, m/s (0 = disabled) + "conditional_signal": 0.0, # CESignalSpeed, m/s (0 = disabled) + "conditional_model_stop_time": 0.0, # CEModelStopTime, s (0 = disabled) + "conditional_signal_lane_detection": False, + "lane_detection_width": 0.0, + # Chill (CCM) + "conditional_chill_speed": 30 * CV.MPH_TO_MS, # CCMSpeed + "conditional_chill_speed_lead": 25 * CV.MPH_TO_MS, # CCMSpeedLead + "conditional_chill_speed_margin": 3 * CV.MPH_TO_MS, # CCMSetSpeedMargin + "conditional_chill_lead": True, + "conditional_chill_launch_assist": False, + # HEM + "hybrid_exp_bias": 0.0, + "hybrid_vision_brake_sensitivity": 1.0, + # StarPilot / Planner toggles + "taco_tune": False, + "classic_model": False, + "tinygrad_model": False, + "vEgoStopping": 0.05, + "hybrid_experimental_mode": False, + "radar_takeoffs": False, + "lane_change_close_gap": False, + "minimum_lane_change_speed": 0.0, + "model_version": None, +} +BOOL_KEYS = { + "conditional_curves", "conditional_curves_lead", "conditional_lead", + "conditional_slower_lead", "conditional_stopped_lead", "conditional_open_road", + "conditional_signal_lane_detection", "conditional_chill_lead", + "conditional_chill_launch_assist", "taco_tune", "classic_model", "tinygrad_model", + "hybrid_experimental_mode", "radar_takeoffs", "lane_change_close_gap", +} + + +class FakeParams: + def __init__(self): + self.bools = {} + self.ints = {} + + def get_bool(self, key): + return bool(self.bools.get(key, False)) + + def put_bool(self, key, value): + self.bools[key] = bool(value) + + def get_int(self, key, default=0): + return int(self.ints.get(key, default)) + + def put_int(self, key, value): + self.ints[key] = int(value) + + +def default_lead(status=False): + return SimpleNamespace( + status=status, + dRel=float("inf"), + yRel=0.0, + vRel=0.0, + aRel=0.0, + vLead=0.0, + vLeadK=0.0, + dPath=0.0, + vLat=0.0, + aLeadK=0.0, + aLeadTau=1.5, + fcw=False, + modelProb=0.0, + radar=False, + ) + + +def as_lead(raw): + if raw is None: + return default_lead() + try: + status = bool(getattr(raw, "status", False)) + except Exception: + status = False + return SimpleNamespace( + status=status, + dRel=float(getattr(raw, "dRel", float("inf"))), + yRel=float(getattr(raw, "yRel", 0.0)), + vRel=float(getattr(raw, "vRel", 0.0)), + aRel=float(getattr(raw, "aRel", 0.0)), + vLead=float(getattr(raw, "vLead", 0.0)), + vLeadK=float(getattr(raw, "vLeadK", getattr(raw, "vLead", 0.0))), + dPath=float(getattr(raw, "dPath", 0.0)), + vLat=float(getattr(raw, "vLat", 0.0)), + aLeadK=float(getattr(raw, "aLeadK", 0.0)), + aLeadTau=float(getattr(raw, "aLeadTau", 1.5)), + fcw=bool(getattr(raw, "fcw", False)), + modelProb=float(getattr(raw, "modelProb", 0.0)), + radar=bool(getattr(raw, "radar", False)), + ) + + +def local_segment_files(data_dir, route_name, segment): + data_root = Path(data_dir) + segment_names = (f"{route_name}--{segment}", f"{route_name.replace('|', '/')}/{segment}") + filenames = ("rlog.zst", "rlog.bz2", "qlog.zst", "qlog.bz2") + identifiers = [] + for segment_name in segment_names: + for filename in filenames: + candidate = data_root / segment_name / filename + if candidate.exists(): + identifiers.append(str(candidate)) + for filename in filenames: + explorer = data_root / f"{route_name}--{segment}--{filename}" + if explorer.exists(): + identifiers.append(str(explorer)) + return identifiers + + +def resolve_segment_identifier(route, segment, data_dir): + direct = parse_direct(route) + if direct is not None: + return [str(direct)] + parsed = parse_indirect(route) + sr = SegmentRange(parsed) + route_name = sr.route_name.replace("/", "|") + if data_dir: + identifiers = local_segment_files(data_dir, route_name, segment) + if identifiers: + return identifiers + return [f"{route_name}--{segment}"] + + +def load_buffers(identifiers, services): + bufs = {s: [] for s in services} + for identifier in identifiers: + try: + logreader = LogReader(identifier, default_mode=ReadMode.RLOG) + except Exception as exc: + print(f"Unable to open {identifier}: {exc}", file=sys.stderr) + continue + for evt in logreader: + which = evt.which() + if which not in services: + continue + try: + t = evt.logMonoTime * 1e-9 + except Exception: + continue + bufs[which].append((t, getattr(evt, which))) + for service in services: + bufs[service].sort(key=lambda x: x[0]) + return bufs + + +def parse_toggle_overrides(args_set): + overrides = {} + for item in args_set or []: + key, sep, raw = item.partition("=") + key = key.strip() + if not sep or key not in TOGGLE_DEFAULTS: + print(f"warning: ignoring unknown toggle override '{item}'", file=sys.stderr) + continue + raw = raw.strip() + if key in BOOL_KEYS: + overrides[key] = raw.lower() in {"1", "true", "yes", "on"} + else: + try: + overrides[key] = float(raw) + except ValueError: + print(f"warning: ignoring non-numeric toggle override '{item}'", file=sys.stderr) + return overrides + + +def run_simulation(grid, bufs, toggles): + ts = {s: [t for t, _ in m] for s, m in bufs.items()} + ms = {s: [m for _, m in bufs[s]] for s in bufs} + + def latest(service, t): + times = ts.get(service) + if not times: + return None + idx = bisect.bisect_right(times, t) - 1 + return ms[service][idx] if idx >= 0 else None + + cp_candidate = None + for _, cp in bufs.get("carParams", []): + if cp is not None: + cp_candidate = cp + break + + if cp_candidate is None: + cp_candidate = SimpleNamespace( + brand="toyota", + carFingerprint="TOYOTA_RAV4", + openpilotLongitudinalControl=True, + pcmCruise=False, + steerRatio=15.0, + wheelbase=2.7, + longitudinalActuatorDelay=0.2, + flags=0, + ) + + planner_state = SimpleNamespace( + params=FakeParams(), + params_memory=FakeParams(), + starpilot_following=SimpleNamespace(following_lead=False, slower_lead=False), + starpilot_vcruise=SimpleNamespace( + slc=SimpleNamespace(experimental_mode=False), + stop_sign_confirmed=False, + forcing_stop=False, + ), + ) + + cem = ConditionalExperimentalMode(planner_state) + cem_ccm_detector = ConditionalExperimentalMode(planner_state) + ccm = ConditionalChillMode(planner_state, cem_ccm_detector) + hybrid = HybridExperimentalMode() + hybrid.set_tuning(toggles.hybrid_exp_bias, toggles.hybrid_vision_brake_sensitivity) + + # Full openpilot Longitudinal Planner for true Chill (ACC) MPC execution + chill_long_planner = LongitudinalPlanner(cp_candidate, dt=DT_MDL) + + n = len(grid) + out = { + "t": grid, + "v_ego": np.zeros(n), + "v_cruise": np.zeros(n), + "model_v0": np.zeros(n), + "lead_status": np.zeros(n, dtype=bool), + "lead_dRel": np.zeros(n), + "lead_vLead": np.zeros(n), + "cem_exp": np.zeros(n, dtype=bool), + "cem_status": np.zeros(n, dtype=int), + "ccm_exp": np.zeros(n, dtype=bool), + "ccm_status": np.zeros(n, dtype=int), + "chill_active": np.zeros(n, dtype=bool), + "logged_exp": np.zeros(n, dtype=bool), + "logged_aTarget": np.zeros(n), + "a_chill": np.zeros(n), + "a_exp": np.zeros(n), + "hem_a": np.zeros(n), + "hem_authority": np.zeros(n), + } + + real_monotonic = time.monotonic + fake_clock = [0.0] + time.monotonic = lambda: fake_clock[0] + + try: + for i, t in enumerate(grid): + fake_clock[0] = float(i) * DT_MDL + + car_raw = latest("carState", t) + v_ego_raw = max(float(getattr(car_raw, "vEgo", 0.0)), 0.0) + car = SimpleNamespace( + vEgo=v_ego_raw, + vEgoCluster=max(float(getattr(car_raw, "vEgoCluster", v_ego_raw)), 0.0), + vCruise=float(getattr(car_raw, "vCruise", 0.0)), + standstill=bool(getattr(car_raw, "standstill", False)), + leftBlinker=bool(getattr(car_raw, "leftBlinker", False)), + rightBlinker=bool(getattr(car_raw, "rightBlinker", False)), + gasPressed=bool(getattr(car_raw, "gasPressed", False)), + brakePressed=bool(getattr(car_raw, "brakePressed", False)), + steeringAngleDeg=float(getattr(car_raw, "steeringAngleDeg", 0.0)), + aEgo=float(getattr(car_raw, "aEgo", 0.0)), + leftBlindspot=bool(getattr(car_raw, "leftBlindspot", False)), + rightBlindspot=bool(getattr(car_raw, "rightBlindspot", False)), + ) + v_ego = car.vEgo + + radar = latest("radarState", t) + lead = as_lead(getattr(radar, "leadOne", None) if radar is not None else None) + lead2 = as_lead(getattr(radar, "leadTwo", None) if radar is not None else None) + + sradar = latest("starpilotRadarState", t) + lead_left = as_lead(getattr(sradar, "leadLeft", None) if sradar is not None else None) + lead_right = as_lead(getattr(sradar, "leadRight", None) if sradar is not None else None) + + lplan = latest("longitudinalPlan", t) + sds = latest("selfdriveState", t) + splan = latest("starpilotPlan", t) + scs = latest("starpilotCarState", t) + model_v2 = latest("modelV2", t) + live_params = latest("liveParameters", t) + car_control = latest("carControl", t) + controls_state = latest("controlsState", t) + car_params = latest("carParams", t) or cp_candidate + + controls_enabled = bool(getattr(sds, "enabled", False)) + conditional_tracking_active = controls_enabled or bool(getattr(scs, "alwaysOnLateralEnabled", False)) + + v_cruise = float(getattr(splan, "vCruise", 0.0) or 0.0) + if not (v_cruise > 0): + v_cruise_kph = float(getattr(car_raw, "vCruise", 0.0) or 0.0) + if 0 < v_cruise_kph < V_CRUISE_UNSET: + v_cruise = min(v_cruise_kph, V_CRUISE_MAX) * CV.KPH_TO_MS + else: + v_cruise = v_ego + + tracking_lead = bool(getattr(splan, "trackingLead", False)) + if not tracking_lead: + tracking_lead = bool(getattr(lplan, "hasLead", False)) + + model_length = 0.0 + if model_v2 is not None: + try: + pos_x = getattr(model_v2, "position", None) + if pos_x is not None and len(pos_x.x): + model_length = float(pos_x.x[-1]) + except Exception: + model_length = 0.0 + + forcing_stop = bool(getattr(splan, "forcingStop", False)) + raw_model_stopped = model_length < CRUISING_SPEED * PLANNER_TIME + model_stopped = raw_model_stopped or forcing_stop + + road_curvature_detected = False + if model_v2 is not None: + try: + road_curvature, _time_to_curve = calculate_road_curvature(model_v2, v_ego) + road_curvature_detected = ( + (1 / abs(road_curvature)) ** 0.5 < v_ego > CRUISING_SPEED + and not (car.leftBlinker or car.rightBlinker) + ) + except Exception: + road_curvature_detected = False + + try: + curvature = float(getattr(controls_state, "curvature", 0.0)) + except Exception: + curvature = 0.0 + lateral_accel = v_ego ** 2 * curvature + driving_in_curve = abs(lateral_accel) >= MINIMUM_LATERAL_ACCELERATION + + lane_width_left = 0.0 + lane_width_right = 0.0 + if model_v2 is not None: + try: + lane_lines = getattr(model_v2, "laneLines", None) + road_edges = getattr(model_v2, "roadEdges", None) + if lane_lines is not None and len(lane_lines) >= 4: + edge_left = road_edges[0] if road_edges is not None and len(road_edges) else None + edge_right = road_edges[1] if road_edges is not None and len(road_edges) > 1 else None + lane_width_left = calculate_lane_width(lane_lines[0], lane_lines[1], edge_left) + lane_width_right = calculate_lane_width(lane_lines[3], lane_lines[2], edge_right) + except Exception: + pass + + t_follow = float(getattr(splan, "tFollow", 1.45)) + following_lead = tracking_lead and lead.dRel < (t_follow * 2) * v_ego + slower_lead = False + if (conditional_tracking_active and tracking_lead and toggles.conditional_slower_lead + and lead.status and lead.vLead < v_ego): + distance_factor = max(lead.dRel - (lead.vLead * t_follow), 1) + braking_offset = float(np.clip(min(v_ego - lead.vLead, lead.vLead) - COMFORT_BRAKE, 1, distance_factor)) + slower_lead = braking_offset > 1 + + planner_state.lead_one = lead + planner_state.tracking_lead = tracking_lead + planner_state.model_length = model_length + planner_state.raw_model_stopped = raw_model_stopped + planner_state.model_stopped = model_stopped + planner_state.road_curvature_detected = road_curvature_detected + planner_state.driving_in_curve = driving_in_curve + planner_state.lane_width_left = lane_width_left + planner_state.lane_width_right = lane_width_right + planner_state.starpilot_following.following_lead = following_lead + planner_state.starpilot_following.slower_lead = slower_lead + planner_state.starpilot_vcruise.forcing_stop = forcing_stop + + raw_personality = getattr(sds, "personality", None) + if hasattr(raw_personality, "raw"): + personality_val = int(raw_personality.raw) + elif isinstance(raw_personality, (int, float)): + personality_val = int(raw_personality) + else: + personality_val = 0 + + long_ctrl_state = getattr(controls_state, "longControlState", LongCtrlState.pid) + + sm_dict = { + "carState": car, + "radarState": SimpleNamespace(leadOne=lead, leadTwo=lead2), + "starpilotRadarState": SimpleNamespace(leadLeft=lead_left, leadRight=lead_right), + "starpilotCarState": SimpleNamespace( + trafficModeEnabled=bool(getattr(scs, "trafficModeEnabled", False)), + alwaysOnLateralEnabled=bool(getattr(scs, "alwaysOnLateralEnabled", False)), + dashboardStopSign=int(getattr(scs, "dashboardStopSign", 0)), + accelPressed=bool(getattr(scs, "accelPressed", False)), + ), + "selfdriveState": SimpleNamespace( + enabled=controls_enabled, + experimentalMode=False, # Enforce ACC/Chill mode evaluation in MPC + personality=personality_val, + ), + "longitudinalPlan": SimpleNamespace( + hasLead=bool(getattr(lplan, "hasLead", False)), + allowThrottle=bool(getattr(lplan, "allowThrottle", True)), + shouldStop=bool(getattr(lplan, "shouldStop", False)), + aTarget=float(getattr(lplan, "aTarget", 0.0)), + ), + "starpilotPlan": SimpleNamespace( + vCruise=v_cruise, + tFollow=t_follow, + trackingLead=tracking_lead, + redLight=bool(getattr(splan, "redLight", False)), + forcingStop=forcing_stop, + forcingStopLength=float(getattr(splan, "forcingStopLength", 100.0)), + minAcceleration=float(getattr(splan, "minAcceleration", -3.5)), + maxAcceleration=float(getattr(splan, "maxAcceleration", 1.5)), + accelerationJerk=float(getattr(splan, "accelerationJerk", 1.0)), + dangerJerk=float(getattr(splan, "dangerJerk", 1.0)), + speedJerk=float(getattr(splan, "speedJerk", 1.0)), + dangerFactor=float(getattr(splan, "dangerFactor", 1.0)), + disableThrottle=bool(getattr(splan, "disableThrottle", False)), + ), + "controlsState": SimpleNamespace( + longControlState=long_ctrl_state, + forceDecel=bool(getattr(controls_state, "forceDecel", False)), + curvature=curvature, + ), + "liveParameters": SimpleNamespace( + angleOffsetDeg=float(getattr(live_params, "angleOffsetDeg", 0.0)) if live_params else 0.0, + ), + "carControl": SimpleNamespace( + orientationNED=getattr(car_control, "orientationNED", [0.0, 0.0, 0.0]) if car_control else [0.0, 0.0, 0.0], + ), + "modelV2": model_v2, + "carParams": car_params, + } + + # Run CEM / CCM State Machines + if conditional_tracking_active: + cem.update(v_ego, sm_dict, toggles, v_cruise) + ccm.update(v_ego, v_cruise, sm_dict, toggles) + else: + cem.experimental_mode = False + cem.status_value = CEStatus["OFF"] + ccm.experimental_mode = True + ccm.status_value = CCStatus["OFF"] + + chill_active = (not ccm.experimental_mode) and ccm.status_value in CC_AUTO_STATUSES + + # Compute accurate MPC Chill (ACC) Acceleration + chill_long_planner.update(sm_dict, toggles) + a_chill = float(chill_long_planner.output_a_target) + + # Determine realistic Exp acceleration target + a_exp = 0.0 + if model_v2 is not None: + try: + action_obj = getattr(model_v2, "action", None) + if action_obj is not None and hasattr(action_obj, "desiredAcceleration"): + a_exp = float(action_obj.desiredAcceleration) + else: + accel_x = getattr(model_v2, "acceleration", None) + if accel_x is not None and len(accel_x.x): + a_exp = float(accel_x.x[0]) + except Exception: + a_exp = 0.0 + + # Hybrid Experimental Mode continuous fusion + a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp) + authority = hybrid.exp_authority + + model_v0 = v_ego + if model_v2 is not None: + try: + vel_x = getattr(model_v2, "velocity", None) + if vel_x is not None and len(vel_x.x): + model_v0 = float(vel_x.x[0]) + except Exception: + model_v0 = v_ego + + out["v_ego"][i] = v_ego + out["v_cruise"][i] = v_cruise + out["model_v0"][i] = model_v0 + out["lead_status"][i] = lead.status + out["lead_dRel"][i] = lead.dRel + out["lead_vLead"][i] = lead.vLead + out["cem_exp"][i] = bool(cem.experimental_mode) + out["cem_status"][i] = int(cem.status_value) + out["ccm_exp"][i] = bool(ccm.experimental_mode) + out["ccm_status"][i] = int(ccm.status_value) + out["chill_active"][i] = chill_active + out["logged_exp"][i] = bool(getattr(sds, "experimentalMode", False)) + out["logged_aTarget"][i] = float(getattr(lplan, "aTarget", 0.0)) + out["a_chill"][i] = a_chill + out["a_exp"][i] = a_exp + out["hem_a"][i] = a_hem + out["hem_authority"][i] = authority + finally: + time.monotonic = real_monotonic + + return out + + +def intervals_for(t, keys): + """Group consecutive identical non-zero status keys into [start, end, key] spans.""" + spans = [] + cur = 0 + for i, k in enumerate(keys): + if k and k == cur: + spans[-1][1] = t[i] + elif k: + spans.append([t[i], t[i], k]) + cur = k + else: + cur = 0 + return spans + + +def print_summary(out): + t = out["t"] + n = len(t) + if n == 0: + print("No simulation frames to summarize.") + return + + v_ego_mph = out["v_ego"] * CV.MS_TO_MPH + v_cruise_mph = out["v_cruise"] * CV.MS_TO_MPH + standstill_mask = out["v_ego"] < 0.2 + standstill_pct = 100.0 * np.mean(standstill_mask) + + stops = 0 + for i in range(1, n): + if standstill_mask[i] and not standstill_mask[i - 1]: + stops += 1 + + cem_switches = int(np.sum(np.diff(out["cem_exp"].astype(int)) != 0)) + ccm_switches = int(np.sum(np.diff(out["chill_active"].astype(int)) != 0)) + + print("\n" + "=" * 78) + print(f" MODE SIMULATION SUMMARY & TELEMETRY LOG ({t[0]:.1f}s -> {t[-1]:.1f}s | {n} frames)") + print("=" * 78) + + print("\n[1. DRIVE PROFILE]") + print(f" • Speed (v_ego) : Min {v_ego_mph.min():.1f} mph | Avg {v_ego_mph.mean():.1f} mph | Max {v_ego_mph.max():.1f} mph") + print(f" • Cruise Set Speed : Min {v_cruise_mph.min():.1f} mph | Avg {v_cruise_mph.mean():.1f} mph | Max {v_cruise_mph.max():.1f} mph") + print(f" • Standstill Time : {standstill_pct:.1f}% ({stops} full stop events)") + print(f" • Lead Presence : {100.0 * np.mean(out['lead_status']):.1f}% of route") + + print("\n[2. MODE INTENT & TRIGGER BREAKDOWN]") + print(f" • Logged (Actual) : Exp {100.0 * out['logged_exp'].mean():.1f}%") + print(f" • CEM (Conditional) : Exp {100.0 * out['cem_exp'].mean():.1f}% | {cem_switches} mode toggles") + cem_reasons = [] + for status_code, label in CE_LABELS.items(): + pct = 100.0 * np.mean((out["cem_status"] == status_code) & out["cem_exp"]) + if pct > 0.1: + cem_reasons.append(f"{label}: {pct:.1f}%") + if cem_reasons: + print(f" └─ CEM Active Triggers -> {', '.join(cem_reasons)}") + + print(f" • Chill Mode (CCM) : Chill {100.0 * out['chill_active'].mean():.1f}% | Exp {100.0 * out['ccm_exp'].mean():.1f}% | {ccm_switches} toggles") + ccm_reasons = [] + for status_code, label in CC_LABELS.items(): + pct = 100.0 * np.mean((out["ccm_status"] == status_code) & out["chill_active"]) + if pct > 0.1: + ccm_reasons.append(f"{label}: {pct:.1f}%") + if ccm_reasons: + print(f" └─ Chill Active Reasons -> {', '.join(ccm_reasons)}") + + auth = out["hem_authority"] + chill_dom = 100.0 * np.mean(auth < 0.35) + blended = 100.0 * np.mean((auth >= 0.35) & (auth <= 0.65)) + exp_dom = 100.0 * np.mean(auth > 0.65) + print(f" • HEM (Hybrid) : Mean Exp Authority {auth.mean():.2f}") + print(f" └─ Distribution -> Chill-dominant (<0.35): {chill_dom:.1f}% | Blended (0.35-0.65): {blended:.1f}% | Exp-dominant (>0.65): {exp_dom:.1f}%") + + print("\n[3. ACCELERATION ENVELOPE (m/s²)]") + print(f" {'Signal':<16} | {'Min (Max Brake)':<15} | {'Mean':<10} | {'Max (Max Throttle)':<18}") + print(" " + "-" * 66) + print(f" {'Logged aTarget':<16} | {out['logged_aTarget'].min():<15.2f} | {out['logged_aTarget'].mean():<10.2f} | {out['logged_aTarget'].max():<18.2f}") + print(f" {'Chill a':<16} | {out['a_chill'].min():<15.2f} | {out['a_chill'].mean():<10.2f} | {out['a_chill'].max():<18.2f}") + print(f" {'Exp a':<16} | {out['a_exp'].min():<15.2f} | {out['a_exp'].mean():<10.2f} | {out['a_exp'].max():<18.2f}") + print(f" {'HEM Fused a':<16} | {out['hem_a'].min():<15.2f} | {out['hem_a'].mean():<10.2f} | {out['hem_a'].max():<18.2f}") + + print("\n[4. KEY EVENT CHRONOLOGY (Sampled Significant Transitions)]") + print(f" {'Time (s)':<9} | {'vEgo':<8} | {'CEM Status':<12} | {'CCM':<7} | {'a_chill':<8} | {'a_exp':<8} | {'HEM a':<8} | {'Auth':<5} | Event Context") + print(" " + "-" * 95) + + step = max(1, n // 18) + sample_indices = set(range(0, n, step)) + + hard_brakes = np.where(out["hem_a"] < -1.2)[0] + if len(hard_brakes) > 0: + sample_indices.update(hard_brakes[::max(1, len(hard_brakes)//4)]) + + standstills = np.where(standstill_mask)[0] + if len(standstills) > 0: + sample_indices.update(standstills[::max(1, len(standstills)//3)]) + + sorted_indices = sorted(list(sample_indices))[:22] + + for idx in sorted_indices: + cur_t = t[idx] - t[0] + ego_mph = v_ego_mph[idx] + cem_st = CE_LABELS.get(out["cem_status"][idx], "OFF") if out["cem_exp"][idx] else "CHILL" + ccm_st = "CHILL" if out["chill_active"][idx] else "EXP" + ac = out["a_chill"][idx] + ae = out["a_exp"][idx] + ah = out["hem_a"][idx] + au = out["hem_authority"][idx] + + ctx = [] + if out["lead_status"][idx]: + ctx.append(f"Lead {out['lead_dRel'][idx]:.0f}m") + if ego_mph < 1.0: + ctx.append("Standstill") + elif ah < -1.0: + ctx.append("Braking") + elif ah > 0.8: + ctx.append("Accelerating") + else: + ctx.append("Cruising") + + if abs(ac - ae) > 1.0: + ctx.append("Disagreement") + + context_str = ", ".join(ctx) + print(f" {cur_t:<9.1f} | {ego_mph:<5.1f}mph | {cem_st:<12} | {ccm_st:<7} | {ac:<8.2f} | {ae:<8.2f} | {ah:<8.2f} | {au:<5.2f} | {context_str}") + + print("=" * 78 + "\n") + + +def plot_results(out, toggles, args): + import matplotlib + if not args.show: + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + t = out["t"] + n = len(t) + + fig, (ax_speed, ax_mode, ax_accel) = plt.subplots( + 3, 1, figsize=(14, 10), sharex=True, + gridspec_kw={"height_ratios": [2, 3, 2]}, + ) + fig.suptitle(f"Mode scenario estimate {args.route} seg {args.segment}", fontsize=11) + + ax_speed.plot(t, out["v_ego"] * CV.MS_TO_MPH, color="black", lw=1.4, label="v_ego") + ax_speed.plot(t, out["v_cruise"] * CV.MS_TO_MPH, color="tab:blue", lw=1.0, ls="--", label="v_cruise") + ax_speed.plot(t, out["model_v0"] * CV.MS_TO_MPH, color="tab:green", lw=1.0, alpha=0.7, label="model v[0]") + ax_speed.set_ylabel("mph") + ax_speed.legend(loc="upper right", fontsize=8, ncol=3) + ax_speed.grid(alpha=0.3) + + rows = [ + ("Exp (always)", out["t"], np.ones(n, dtype=bool), "tab:blue", 3.0), + ("CEM", out["t"], out["cem_exp"], "tab:orange", 2.0), + ("Chill", out["t"], out["chill_active"], "tab:green", 1.0), + ("Logged", out["t"], out["logged_exp"], "tab:gray", 0.0), + ] + for label, tt, active, color, ycenter in rows: + ax_mode.fill_between(tt, ycenter - 0.28, ycenter + 0.28, where=active, + step="post", color=color, alpha=0.55, edgecolor="none") + + cem_spans = intervals_for(t, [out["cem_status"][i] if out["cem_exp"][i] else 0 for i in range(n)]) + for a, b, k in cem_spans: + if b - a > 0.8 and k in CE_LABELS: + ax_mode.text((a + b) / 2, 2.0 + 0.34, CE_LABELS[k], ha="center", fontsize=7, color="tab:orange") + cc_spans = intervals_for(t, [out["ccm_status"][i] if out["chill_active"][i] else 0 for i in range(n)]) + for a, b, k in cc_spans: + if b - a > 0.8 and k in CC_LABELS: + ax_mode.text((a + b) / 2, 1.0 + 0.34, CC_LABELS[k], ha="center", fontsize=7, color="tab:green") + + ax_hem = ax_mode.twinx() + ax_hem.plot(t, out["hem_authority"], color="tab:cyan", lw=1.2, label="HEM exp-authority") + ax_hem.set_ylim(0, 1) + ax_hem.set_ylabel("HEM exp authority", color="tab:cyan", fontsize=8) + ax_hem.tick_params(axis="y", labelcolor="tab:cyan", labelsize=7) + + ax_mode.set_yticks([3.0, 2.0, 1.0, 0.0]) + ax_mode.set_yticklabels(["Exp", "CEM", "Chill", "Logged"]) + ax_mode.set_ylim(-0.6, 3.6) + ax_mode.grid(axis="y", alpha=0.3) + + ax_accel.plot(t, out["logged_aTarget"], color="tab:gray", lw=1.2, label="logged aTarget") + ax_accel.plot(t, out["a_chill"], color="tab:blue", lw=1.0, ls="--", label="chill (ACC) a") + ax_accel.plot(t, out["a_exp"], color="tab:orange", lw=1.0, ls=":", label="exp (model) a") + ax_accel.plot(t, out["hem_a"], color="tab:cyan", lw=1.4, label="HEM fused a") + ax_accel.set_ylabel("accel (m/s²)") + ax_accel.set_xlabel("time (s, segment-relative)") + ax_accel.legend(loc="upper right", fontsize=8, ncol=2) + ax_accel.grid(alpha=0.3) + + fig.tight_layout() + if args.out: + out_path = Path(args.out) + out_path.parent.mkdir(parents=True, exist_ok=True) + fig.savefig(out_path, dpi=150) + print(f"Saved graph to {out_path}") + if args.show: + plt.show() + plt.close(fig) + + +def main(argv=None): + parser = argparse.ArgumentParser( + description="Estimate CEM/Chill/HEM/Exp mode intent from a logged route and plot a comparison graph.") + parser.add_argument("route", help="route (e.g. dongle|2023-07-27--13-01-19 or dongle/2023-07-27--13-01-19/0)") + parser.add_argument("--segment", type=int, default=0, help="segment index (default 0)") + parser.add_argument("--data_dir", help="local directory containing route files") + parser.add_argument("--start", type=float, default=None, help="start seconds (segment-relative)") + parser.add_argument("--end", type=float, default=None, help="end seconds (segment-relative)") + parser.add_argument("--out", default="mode_sim.png", help="output PNG path") + parser.add_argument("--show", action="store_true", help="show the plot window instead of saving only") + parser.add_argument("--csv", default=None, help="optional CSV output of the time series") + parser.add_argument("--set", action="append", default=[], metavar="KEY=value", + help="override a mode toggle, e.g. --set CESpeed=20 --set CEModelStopTime=3.0 --set CCMLead=false") + args = parser.parse_args(argv) + + overrides = parse_toggle_overrides(args.set) + defaults = dict(TOGGLE_DEFAULTS) + defaults.update(overrides) + toggles = SimpleNamespace(**defaults) + for key, value in overrides.items(): + print(f"toggle override: {key} = {value}") + + identifiers = resolve_segment_identifier(args.route, args.segment, args.data_dir) + if not identifiers: + print(f"No segment data found for {args.route} seg {args.segment}", file=sys.stderr) + return 1 + print(f"Loading: {identifiers}") + bufs = load_buffers(identifiers, SERVICES) + if not any(bufs[s] for s in SERVICES): + print("No messages loaded.", file=sys.stderr) + return 1 + + all_min = min(t for s in SERVICES for t, _ in bufs[s]) + all_max = max(t for s in SERVICES for t, _ in bufs[s]) + start = all_min if args.start is None else all_min + args.start + end = all_max if args.end is None else min(all_max, all_min + args.end) + if end - start < DT_MDL: + print(f"Empty time window [{start:.1f}, {end:.1f}]. Check --start/--end.", file=sys.stderr) + return 1 + grid = np.arange(start, end, DT_MDL) + print(f"Window: {start - all_min:.1f}s -> {end - all_min:.1f}s ({len(grid)} frames at {1 / DT_MDL:.0f} Hz)") + + out = run_simulation(grid, bufs, toggles) + + if args.csv: + import csv + csv_path = Path(args.csv) + csv_path.parent.mkdir(parents=True, exist_ok=True) + with open(csv_path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow(["t", "v_ego", "v_cruise", "model_v0", "cem_exp", "cem_status", + "ccm_exp", "ccm_status", "chill_active", "logged_exp", "logged_aTarget", + "a_chill", "a_exp", "hem_a", "hem_authority"]) + for i in range(len(out["t"])): + writer.writerow([out["t"][i], out["v_ego"][i], out["v_cruise"][i], out["model_v0"][i], + int(out["cem_exp"][i]), out["cem_status"][i], + int(out["ccm_exp"][i]), out["ccm_status"][i], int(out["chill_active"][i]), + int(out["logged_exp"][i]), out["logged_aTarget"][i], + out["a_chill"][i], out["a_exp"][i], out["hem_a"][i], out["hem_authority"][i]]) + print(f"Saved CSV to {csv_path}") + + print_summary(out) + plot_results(out, toggles, args) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) \ No newline at end of file