From c37e9a2fc423964b38e0f40744c8756f577477bf Mon Sep 17 00:00:00 2001 From: Prabhaav Pillai Date: Wed, 26 Aug 2026 16:33:23 -0400 Subject: [PATCH] Refine HybridExperimentalMode: improve departure signal identification and adjust low-speed acceleration lockout conditions --- .../controls/lib/hybrid_experimental_mode.py | 45 +++++++++++++------ 1 file changed, 32 insertions(+), 13 deletions(-) diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py index 193cc440f..93b4d7424 100644 --- a/starpilot/controls/lib/hybrid_experimental_mode.py +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -48,6 +48,7 @@ class HybridExperimentalMode: self.prev_a_target = 0.0 self.exp_authority = 0.5 self.w_vision_filtered = 0.0 + self.tracked_stop_dist = None # Last-frame diagnostics surfaced to live logs self.last_w_vision = 0.0 @@ -73,6 +74,7 @@ class HybridExperimentalMode: self.prev_a_target = float(a_ego) if np.isfinite(a_ego) else 0.0 self.exp_authority = 0.5 self.w_vision_filtered = 0.0 + self.tracked_stop_dist = None self.last_w_vision = 0.0 self.last_regime = "throttle" self.last_standstill = False @@ -149,24 +151,28 @@ class HybridExperimentalMode: speed_drop_ratio = max(0.0, (v_ego - v_min) / v_ref) model_decel_strength = max(0.0, -a_exp / 2.0) - # Fix 3: Use a shorter planning horizon (~4 seconds out, index 23) for stop sign detection - # This prevents the model's proceed-after-stop trajectory predictions from blinding the stop confidence + # Fix 3 (Refined): Use a shorter planning horizon (~4 seconds out, index 23) for stop sign detection. + # Sigmoids adjusted to engage earlier at higher approach speeds, preventing the vehicle from eating up stop distance. v_horizon_short = float(traj_v[min(len(traj_v) - 1, 23)]) if len(traj_v) > 0 else v_ego - stop_target_active = sigmoid(1.2 - v_horizon_short, k=4.0) + stop_target_active = sigmoid(1.8 - v_horizon_short, k=3.0) raw_vision_metric = max(speed_drop_ratio, stop_target_active, model_decel_strength) w_vision_raw = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0)) # Identify departure signals lead_departing = lead_status and (getattr(lead_one, "vLead", 0.0) > 0.5) - driver_departing = (a_chill > 0.4) and (not lead_status or lead_d_rel > 10.0) + + # Core Bug Fix: Suppress false driver_departing latch resets if we are actively stopping, + # unless a true heavy driver override (>1.2) occurs. + is_actively_stopping = self.w_vision_filtered > 0.25 and v_ego > 0.5 + driver_override = (a_chill > 1.2) + + driver_departing = (a_chill > 0.4) and (not lead_status or lead_d_rel > 10.0) and (not is_actively_stopping or driver_override) model_stop_predicted = len(traj_v) > 1 and v_horizon < 0.5 vision_departing = (v_horizon > 0.5) and (a_exp > 0.1) departing = (lead_departing or vision_departing or driver_departing) and not model_stop_predicted - # Refined Fix 2: Protect against rolling approach glitches. - # We uniquely trust driver override or lead vehicle departure to clear the latch at any speed. - # However, vision model departure is only trusted to clear the latch at a standstill (v_ego < 0.15 m/s). + # Latch holds during approach, decay only on verified departure or drivers override should_reset_latch = driver_departing or lead_departing or (vision_departing and v_ego < 0.15 and not model_stop_predicted) if should_reset_latch: @@ -183,10 +189,8 @@ class HybridExperimentalMode: # Kinematic stopping calculation d_min = float(traj_x[min_idx]) if len(traj_x) > min_idx else float("inf") - d_stop_effective = max(d_min - 1.5, 2.0) - a_kinematic_stop = 0.0 - slow_horizon = sigmoid(3.0 - v_horizon_short, k=2.0) + slow_horizon = sigmoid(4.0 - v_horizon_short, k=1.5) stop_confidence = max(stop_target_active, slow_horizon * speed_drop_ratio) # Bug A & B Fix: check if model plans a stop/slowdown anywhere in near-to-mid distance @@ -199,7 +203,23 @@ class HybridExperimentalMode: if near_stop_planned: stop_confidence = max(stop_confidence, 0.8) - if v_ego > 0.1 and 0.2 < d_min < float("inf") and stop_confidence > 0.15: + # Stable odometry-based stop line tracking to bypass distance latency: + # Trigger tracking when filtered w_vision is high and model thinks there's a stop line. + if w_vision > 0.25 and d_min < 100.0: + if self.tracked_stop_dist is None: + self.tracked_stop_dist = d_min # Initialize once on stop latching + else: + self.tracked_stop_dist -= v_ego * self.DT # Track strictly using physical odometry + d_stop_calc = self.tracked_stop_dist + else: + self.tracked_stop_dist = None + d_stop_calc = d_min + + d_stop_effective = max(d_stop_calc - 1.5, 2.0) + a_kinematic_stop = 0.0 + + # Kinematic deceleration is calculated from the stable odometer-tracked distance + if v_ego > 0.1 and 0.2 < d_stop_calc < float("inf") and w_vision > 0.25: a_kinematic_stop = float(np.clip(- (v_ego ** 2) / (2.0 * d_stop_effective), -3.5, 0.0)) a_kinematic_stop *= self.KINEMATIC_STOP_GAIN if d_stop_effective < 6.0 and v_ego < 3.0: @@ -228,8 +248,7 @@ class HybridExperimentalMode: a_throttle_conservative = smooth_min(a_chill, a_exp, k=4.0) a_throttle_fused = lerp(a_throttle_optimal, a_throttle_conservative, w_vision) - # Fix 1: Eliminate target dilution on the brake path. - # If Experimental Mode wants to brake, strictly respect the minimum of the targets. + # Braking Regime: Never dilute Exp stop braking with Chill's 0.0 m/s^2 a_chill_brake = 0.0 if a_exp_effective < 0.0: a_chill_brake = min(a_chill, 0.0)