From a91b43e4ef8f7693649de1f860bf627982b6edae Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 17 Oct 2025 20:41:53 -0500 Subject: [PATCH] Scene Complexity --- .../lib/longitudinal_mpc_lib/long_mpc.py | 19 +++++++++++++- .../controls/lib/longitudinal_planner.py | 26 ++++++++++++++++++- 2 files changed, 43 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 5b08f403a..20db0f0f8 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -357,7 +357,7 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0): + def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0, uncertainty=0.0): # Update parameters based on current speed with interpolation for smooth scaling speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph @@ -389,6 +389,14 @@ class LongitudinalMpc: danger_jerk *= dist_factor speed_jerk *= dist_factor + # Scene complexity adjustment based on model uncertainty + complexity_factor = 1.0 + filter_time_factor = 1.0 + + if uncertainty > 1.0: # High uncertainty indicates complex scene + complexity_factor = 1.5 # Boost responsiveness + filter_time_factor = 0.0 # Disable smoothing for immediate response + if self.mode == 'acc': a_change_cost = acceleration_jerk if prev_accel_constraint else 0 cost_weights = [self.current_x_ego_cost, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk] @@ -401,6 +409,15 @@ class LongitudinalMpc: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') self.set_cost_weights(cost_weights, constraint_cost_weights) + # Adjust filter time constants for complex scenes + if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.1: + current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0 + current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0 + new_filter_time = self.current_filter_time * filter_time_factor + self.lead_a_filter = FirstOrderFilter(current_a, new_filter_time, self.dt) + self.lead_v_filter = FirstOrderFilter(current_v, new_filter_time, self.dt) + self.prev_filter_time_factor = filter_time_factor + def set_cur_state(self, v, a): v_prev = self.x0[1] self.x0[1] = v diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 77eca62b2..cf161471c 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -215,8 +215,32 @@ class LongitudinalPlanner: self.lead_two = sm['radarState'].leadTwo lead_dist = self.lead_one.dRel if self.lead_one.status else 50.0 + + # Calculate scene uncertainty from model desire prediction entropy and disengage predictions + uncertainty = 0.0 + if hasattr(sm['modelV2'], 'meta'): + # Desire prediction entropy (maneuver uncertainty) + desire_entropy = 0.0 + if hasattr(sm['modelV2'].meta, 'desirePrediction'): + desire_probs = sm['modelV2'].meta.desirePrediction + if len(desire_probs) > 1: + desire_probs = np.array(desire_probs) + desire_probs = desire_probs / np.sum(desire_probs) # Normalize + desire_entropy = -np.sum(desire_probs * np.log(desire_probs + 1e-10)) + + # Disengage prediction risk (intervention likelihood) + disengage_risk = 0.0 + if hasattr(sm['modelV2'].meta, 'disengagePredictions'): + # Use brake press probabilities as primary risk indicator + brake_probs = sm['modelV2'].meta.disengagePredictions.brakePressProbs + if len(brake_probs) > 0: + disengage_risk = np.max(brake_probs) # Peak risk over time horizon + + # Combined uncertainty metric + uncertainty = desire_entropy + disengage_risk + self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, - personality=sm['controlsState'].personality, v_ego=v_ego, lead_dist=lead_dist) + personality=sm['controlsState'].personality, v_ego=v_ego, lead_dist=lead_dist, uncertainty=uncertainty) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) # After deciding the MPC mode via get_mpc_mode(), ensure MPC uses that mode when not mlsim