diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 833845d36d..511dedc3c5 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -150,6 +150,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { aTarget @5 :Float32; events @6 :List(OnroadEventSP.Event); e2eAlerts @7 :E2eAlerts; + accelPersonality @8 :AccelerationPersonality; struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; @@ -252,6 +253,11 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { greenLightAlert @0 :Bool; leadDepartAlert @1 :Bool; } + enum AccelerationPersonality { + sport @0; + normal @1; + eco @2; + } } struct OnroadEventSP @0xda96579883444c35 { diff --git a/common/params_keys.h b/common/params_keys.h index 3b5d02a429..d845b33319 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -171,6 +171,9 @@ inline static std::unordered_map keys = { {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}}, {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}}, {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"VibePersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"VibeAccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"VibeFollowPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}}, // MADS params {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3f9d8245bd..e5bb835522 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -10,6 +10,8 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.modeld.constants import index_function from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU +from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController + if __name__ == '__main__': # generating code from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: @@ -228,6 +230,7 @@ class LongitudinalMpc: self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() self.source = SOURCES[2] + self.vibe_controller = VibePersonalityController() def reset(self): # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) @@ -328,10 +331,31 @@ class LongitudinalMpc: return lead_xv def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): - t_follow = get_T_FOLLOW(personality) v_ego = self.x0[1] + + # Get following distance + if self.vibe_controller.is_follow_enabled(): + t_follow = self.vibe_controller.get_follow_distance_multiplier(v_ego) + if t_follow is None: + # Fallback to stock behavior when vibe controller can't provide a value + t_follow = get_T_FOLLOW(personality) + else: + t_follow = get_T_FOLLOW(personality) + self.status = radarstate.leadOne.status or radarstate.leadTwo.status + # Get acceleration limits + if self.vibe_controller.is_accel_enabled(): + accel_limits = self.vibe_controller.get_accel_limits(v_ego) + if accel_limits is not None: + min_accel = accel_limits[0] + else: + min_accel = CRUISE_MIN_ACCEL + else: + min_accel = CRUISE_MIN_ACCEL + + a_cruise_min = min_accel + lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) @@ -350,7 +374,7 @@ class LongitudinalMpc: # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. - v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) + v_lower = v_ego + (T_IDXS * a_cruise_min * 1.05) # TODO does this make sense when max_a is negative? v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 53eca38356..87e05ad9ed 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -124,9 +124,22 @@ class LongitudinalPlanner(LongitudinalPlannerSP): prev_accel_constraint = not (reset_state or sm['carState'].standstill) if mode == 'acc': - accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] - steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) + if self.vibe_controller.is_accel_enabled(): + # Only get max acceleration from vibe controller, use default ACCEL_MIN for minimum + accel_limits = self.vibe_controller.get_accel_limits(v_ego) + if accel_limits is not None: + max_accel = accel_limits[1] + accel_clip = [ACCEL_MIN, max_accel] + else: + # Fallback to stock if vibe controller returns None + accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] + # Recalculate limit turn according to the new max limit + steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg + accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) + else: + accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] + steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg + accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) else: accel_clip = [ACCEL_MIN, ACCEL_MAX] diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 205a14624a..fc39993296 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -92,7 +92,15 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "your steering wheel distance button."), "../assets/icons/speed_limit.png", longi_button_texts); - + // accel controller + std::vector accel_personality_texts{tr("Sport"), tr("Normal"), tr("Eco")}; + accel_personality_setting = new ButtonParamControlSP("AccelPersonality", tr("Acceleration Personality"), + tr("Normal is recommended. In sport mode, sunnypilot will provide aggressive acceleration for a dynamic driving experience. " + "In eco mode, sunnypilot will apply smoother and more relaxed acceleration. On supported cars, you can cycle through these " + "acceleration personality within Onroad Settings on the driving screen."), + "", + accel_personality_texts); + accel_personality_setting->showDescription(); // set up uiState update for personality setting QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState); @@ -120,6 +128,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { // insert longitudinal personality after NDOG toggle if (param == "DisengageOnAccelerator") { addItem(long_personality_setting); + addItem(accel_personality_setting); } } @@ -140,6 +149,13 @@ void TogglesPanel::updateState(const UIState &s) { } uiState()->scene.personality = personality; } + if (sm.updated("longitudinalPlanSP")) { + auto accel_personality = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getAccelPersonality(); + if (accel_personality != s.scene.accel_personality && s.scene.started && isVisible()) { + accel_personality_setting->setCheckedButton(static_cast(accel_personality)); + } + uiState()->scene.accel_personality = accel_personality; + } } void TogglesPanel::expandToggleDescription(const QString ¶m) { @@ -186,10 +202,12 @@ void TogglesPanel::updateToggles() { experimental_mode_toggle->setEnabled(true); experimental_mode_toggle->setDescription(e2e_description); long_personality_setting->setEnabled(true); + accel_personality_setting->setEnabled(true); } else { // no long for now experimental_mode_toggle->setEnabled(false); long_personality_setting->setEnabled(false); + accel_personality_setting->setEnabled(true); params.remove("ExperimentalMode"); const QString unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control."); diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index a11a300128..02cd557669 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -88,6 +88,7 @@ protected: Params params; std::map toggles; ButtonParamControl *long_personality_setting; + ButtonParamControl *accel_personality_setting; virtual void updateToggles(); }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index 31124a3242..9f5fbdc511 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc @@ -75,6 +75,35 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { main_layout->setCurrentWidget(cruisePanelScreen); }); + + // Vibe Personality Controller + vibePersonalityControl = new ParamControlSP("VibePersonalityEnabled", + tr("Vibe Personality Controller"), + tr("Advanced driving personality system with separate controls for acceleration behavior (Eco/Normal/Sport) and following distance/braking (Relaxed/Standard/Aggressive). " + "Customize your driving experience with independent acceleration and distance personalities."), + "../assets/offroad/icon_shell.png"); + list->addItem(vibePersonalityControl); + + connect(vibePersonalityControl, &ParamControlSP::toggleFlipped, [=]() { + refresh(offroad); + }); + + // Vibe Acceleration Personality + vibeAccelPersonalityControl = new ParamControlSP("VibeAccelPersonalityEnabled", + tr("Acceleration Personality"), + tr("Controls acceleration behavior: Eco (efficient), Normal (balanced), Sport (responsive). " + "Adjust how aggressively the vehicle accelerates while maintaining smooth operation."), + "../assets/offroad/icon_shell.png"); + list->addItem(vibeAccelPersonalityControl); + + // Vibe Following Distance Personality + vibeFollowPersonalityControl = new ParamControlSP("VibeFollowPersonalityEnabled", + tr("Following Distance Personality"), + tr("Controls following distance and braking behavior: Relaxed (longer distance, gentler braking), Standard (balanced), Aggressive (shorter distance, firmer braking). " + "Fine-tune your comfort level in traffic situations."), + "../assets/offroad/icon_shell.png"); + list->addItem(vibeFollowPersonalityControl); + main_layout->addWidget(cruisePanelScreen); main_layout->addWidget(speedLimitScreen); main_layout->setCurrentWidget(cruisePanelScreen); @@ -135,6 +164,14 @@ void LongitudinalPanel::refresh(bool _offroad) { intelligentCruiseButtonManagement->toggleFlipped(false); } } + bool vibePersonalityEnabled = params.getBool("VibePersonalityEnabled"); + if (vibePersonalityEnabled) { + vibeAccelPersonalityControl->setVisible(true); + vibeFollowPersonalityControl->setVisible(true); + } else { + vibeAccelPersonalityControl->setVisible(false); + vibeFollowPersonalityControl->setVisible(false); + } bool icbm_allowed = intelligent_cruise_button_management_available && !has_longitudinal_control; intelligentCruiseButtonManagement->setEnabled(icbm_allowed && offroad); @@ -146,6 +183,13 @@ void LongitudinalPanel::refresh(bool _offroad) { SmartCruiseControlVision->setEnabled(has_longitudinal_control || icbm_allowed); SmartCruiseControlMap->setEnabled(has_longitudinal_control || icbm_allowed); + // Vibe Personality controls - always enabled for toggling + vibePersonalityControl->setEnabled(true); + vibeAccelPersonalityControl->setEnabled(true); + vibeFollowPersonalityControl->setEnabled(true); + vibePersonalityControl->refresh(); + vibeAccelPersonalityControl->refresh(); + vibeFollowPersonalityControl->refresh(); offroad = _offroad; } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h index a94369a560..bb4bc132ad 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h @@ -37,4 +37,8 @@ private: ParamControl *intelligentCruiseButtonManagement = nullptr; SpeedLimitSettings *speedLimitScreen; PushButtonSP *speedLimitSettings; + + ParamControlSP *vibePersonalityControl; + ParamControlSP *vibeAccelPersonalityControl; + ParamControlSP *vibeFollowPersonalityControl; }; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 5b3872b3d4..c2dc68e98e 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -60,6 +60,7 @@ typedef struct UIScene { cereal::PandaState::PandaType pandaType; cereal::LongitudinalPersonality personality; + cereal::LongitudinalPlanSP::AccelerationPersonality accel_personality; float light_sensor = -1; bool started, ignition, is_metric, recording_audio; diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 4c2443f253..629f185cd9 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -17,6 +17,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolve from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.sunnypilot.models.helpers import get_active_bundle +from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource @@ -29,6 +30,7 @@ class LongitudinalPlannerSP: self.scc = SmartCruiseControl() self.resolver = SpeedLimitResolver() self.sla = SpeedLimitAssist(CP) + self.vibe_controller = VibePersonalityController() self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None self.source = LongitudinalPlanSource.cruise self.e2e_alerts_helper = E2EAlertsHelper() @@ -81,6 +83,7 @@ class LongitudinalPlannerSP: self.events_sp.clear() self.dec.update(sm) self.e2e_alerts_helper.update(sm, self.events_sp) + self.vibe_controller.update() def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') diff --git a/sunnypilot/selfdrive/controls/lib/vibe_personality/__init__.py b/sunnypilot/selfdrive/controls/lib/vibe_personality/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/vibe_personality/vibe_personality.py b/sunnypilot/selfdrive/controls/lib/vibe_personality/vibe_personality.py new file mode 100644 index 0000000000..b0154579eb --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/vibe_personality/vibe_personality.py @@ -0,0 +1,331 @@ +""" +Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from cereal import log, custom +import numpy as np +from openpilot.common.realtime import DT_MDL +from openpilot.common.params import Params + +LongPersonality = log.LongitudinalPersonality +AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality + +# Acceleration Profiles mapped to AccelPersonality (eco/normal/sport) +MAX_ACCEL_PROFILES = { + AccelPersonality.eco: [2.00, 2.00, 1.32, 0.85, .58, .46, .365, .317, .089], # eco + AccelPersonality.normal: [2.00, 2.00, 1.42, 1.10, .65, .56, .43, .36, .12], # normal + AccelPersonality.sport: [2.00, 2.00, 1.52, 1.40, .80, .70, .53, .46, .20], # sport +} +MAX_ACCEL_BREAKPOINTS = [0., 6., 9., 11., 16., 20., 25., 30., 55.] + +# Braking profiles mapped to LongPersonality (relaxed/standard/aggressive) +MIN_ACCEL_PROFILES = { + LongPersonality.relaxed: [-1.20, -1.20], # gentler braking + LongPersonality.standard: [-1.30, -1.30], # normal braking + LongPersonality.aggressive: [-1.40, -1.40], # more aggressive braking +} +MIN_ACCEL_BREAKPOINTS = [0., 50.] + +# Following Distance Profiles mapped to LongPersonality (relaxed/standard/aggressive) +FOLLOW_DISTANCE_PROFILES = { + LongPersonality.relaxed: { + 'x_vel': [0., 19.7, 22.2, 40.], + 'y_dist': [1.40, 1.40, 1.65, 1.65] # longer following distance + }, + LongPersonality.standard: { + 'x_vel': [0., 19.7, 22.2, 40.], + 'y_dist': [1.35, 1.35, 1.40, 1.40] # normal following distance + }, + LongPersonality.aggressive: { + 'x_vel': [0., 19.7, 22.2, 40.], + 'y_dist': [1.20, 1.20, 1.30, 1.30] # shorter following distance + } +} + +class VibePersonalityController: + """ + Controller for managing separated acceleration and distance controls: + - AccelPersonality controls acceleration behavior (eco, normal, sport) + - LongPersonality controls braking and following distance (relaxed, standard, aggressive) + """ + + def __init__(self): + self.params = Params() + self.frame = 0 + + # Separate personalities for acceleration and distance control + self.accel_personality = AccelPersonality.normal + self.long_personality = LongPersonality.standard + + # Parameter keys + self.param_keys = { + 'accel_personality': 'AccelPersonality', # eco=0, normal=1, sport=2 + 'long_personality': 'LongitudinalPersonality', # relaxed=0, standard=1, aggressive=2 + 'enabled': 'VibePersonalityEnabled', + 'accel_enabled': 'VibeAccelPersonalityEnabled', + 'follow_enabled': 'VibeFollowPersonalityEnabled' + } + + print(f"[VIBE_DEBUG] Initializing VibePersonalityController - accel_personality: {self.accel_personality}, long_personality: {self.long_personality}") + + # Precompute slopes for all personalities + self._precompute_slopes() + + def _precompute_slopes(self): + """Precompute all interpolation slopes for efficiency""" + self.max_accel_slopes = {} + self.min_accel_slopes = {} + self.follow_distance_slopes = {} + + # Precompute for AccelPersonality (acceleration) + for personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]: + if personality in MAX_ACCEL_PROFILES: + self.max_accel_slopes[personality] = self._compute_slopes(MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[personality]) + + # Precompute for LongPersonality (braking and following) + for personality in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]: + if personality in MIN_ACCEL_PROFILES: + self.min_accel_slopes[personality] = self._compute_slopes(MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[personality]) + + if personality in FOLLOW_DISTANCE_PROFILES: + profile = FOLLOW_DISTANCE_PROFILES[personality] + self.follow_distance_slopes[personality] = self._compute_slopes(profile['x_vel'], profile['y_dist']) + + def _update_from_params(self): + """Update personalities from params (rate limited)""" + if self.frame % int(1. / DT_MDL) != 0: + return + + # Update AccelPersonality + try: + accel_personality_str = self.params.get(self.param_keys['accel_personality'], encoding='utf-8') + if accel_personality_str: + accel_personality_int = int(accel_personality_str) + if accel_personality_int in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]: + if accel_personality_int != self.accel_personality: + print(f"[VIBE_DEBUG] AccelPersonality changed from {self.accel_personality} to {accel_personality_int}") + self.accel_personality = accel_personality_int + except (ValueError, TypeError): + pass + + # Update LongPersonality + try: + long_personality_str = self.params.get(self.param_keys['long_personality'], encoding='utf-8') + if long_personality_str: + long_personality_int = int(long_personality_str) + if long_personality_int in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]: + if long_personality_int != self.long_personality: + print(f"[VIBE_DEBUG] LongPersonality changed from {self.long_personality} to {long_personality_int}") + self.long_personality = long_personality_int + except (ValueError, TypeError): + pass + + def _get_toggle_state(self, key: str, default: bool = True) -> bool: + """Get toggle state with default fallback""" + return self.params.get_bool(self.param_keys.get(key, key)) if key in self.param_keys else default + + def _set_toggle_state(self, key: str, value: bool): + """Set toggle state in params""" + if key in self.param_keys: + self.params.put_bool(self.param_keys[key], value) + + # AccelPersonality Management (for acceleration) + def set_accel_personality(self, personality: int) -> bool: + """Set AccelPersonality (eco=0, normal=1, sport=2)""" + if personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]: + old_personality = self.accel_personality + self.accel_personality = personality + self.params.put(self.param_keys['accel_personality'], str(personality)) + print(f"[VIBE_DEBUG] AccelPersonality set: {old_personality} -> {personality}") + return True + return False + + def cycle_accel_personality(self) -> int: + """Cycle through AccelPersonality: eco -> normal -> sport -> eco""" + personalities = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport] + current_idx = personalities.index(self.accel_personality) + next_personality = personalities[(current_idx + 1) % len(personalities)] + self.set_accel_personality(next_personality) + return next_personality + + def get_accel_personality(self) -> int: + """Get current AccelPersonality""" + self._update_from_params() + return self.accel_personality + + # LongPersonality Management (for braking and following distance) + def set_long_personality(self, personality: int) -> bool: + """Set LongPersonality (relaxed=0, standard=1, aggressive=2)""" + if personality in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]: + old_personality = self.long_personality + self.long_personality = personality + self.params.put(self.param_keys['long_personality'], str(personality)) + print(f"[VIBE_DEBUG] LongPersonality set: {old_personality} -> {personality}") + return True + return False + + def cycle_long_personality(self) -> int: + """Cycle through LongPersonality: relaxed -> standard -> aggressive -> relaxed""" + personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive] + current_idx = personalities.index(self.long_personality) + next_personality = personalities[(current_idx + 1) % len(personalities)] + self.set_long_personality(next_personality) + return next_personality + + def get_long_personality(self) -> int: + """Get current LongPersonality""" + self._update_from_params() + return self.long_personality + + # Toggle Functions + def toggle_personality(self): return self._toggle_flag('enabled') + def toggle_accel_personality(self): return self._toggle_flag('accel_enabled') + def toggle_follow_distance_personality(self): return self._toggle_flag('follow_enabled') + + def _toggle_flag(self, key): + current = self._get_toggle_state(key) + self._set_toggle_state(key, not current) + return not current + + def set_personality_enabled(self, enabled: bool): self._set_toggle_state('enabled', enabled) + + # Feature-specific enable checks + def is_accel_enabled(self) -> bool: + self._update_from_params() + enabled = self._get_toggle_state('enabled') and self._get_toggle_state('accel_enabled') + if enabled: + print(f"[VIBE_DEBUG] is_accel_enabled: TRUE using AccelPersonality {self.accel_personality}") + return enabled + + def is_follow_enabled(self) -> bool: + self._update_from_params() + enabled = self._get_toggle_state('enabled') and self._get_toggle_state('follow_enabled') + if enabled: + print(f"[VIBE_DEBUG] is_follow_enabled: TRUE using LongPersonality {self.long_personality}") + return enabled + + def is_enabled(self) -> bool: + self._update_from_params() + return (self._get_toggle_state('enabled') and + (self._get_toggle_state('accel_enabled') or self._get_toggle_state('follow_enabled'))) + + def get_accel_limits(self, v_ego: float) -> tuple[float, float] | None: + """ + Get acceleration limits based on current personalities. + - Max acceleration from AccelPersonality (eco/normal/sport) + - Min acceleration (braking) from LongPersonality (relaxed/standard/aggressive) + Returns None if controller is disabled. + """ + self._update_from_params() + if not self.is_accel_enabled(): + return None + + try: + print(f"[VIBE_DEBUG] get_accel_limits: AccelPersonality={self.accel_personality}, LongPersonality={self.long_personality}, speed={v_ego:.2f}") + + # Max acceleration from AccelPersonality + max_a = self._interpolate(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality], + self.max_accel_slopes[self.accel_personality]) + + # Min acceleration (braking) from LongPersonality + min_a = self._interpolate(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.long_personality], + self.min_accel_slopes[self.long_personality]) + + print(f"[VIBE_DEBUG] get_accel_limits: min_a={min_a:.3f} (from LongPersonality), max_a={max_a:.3f} (from AccelPersonality)") + return float(min_a), float(max_a) + except (KeyError, IndexError): + print(f"[VIBE_DEBUG] get_accel_limits: ERROR - KeyError/IndexError") + return None + + def get_follow_distance_multiplier(self, v_ego: float) -> float | None: + """Get following distance multiplier based on LongPersonality only""" + self._update_from_params() + if not self.is_follow_enabled(): + return None + + try: + print(f"[VIBE_DEBUG] get_follow_distance_multiplier: LongPersonality={self.long_personality}, speed={v_ego:.2f}") + profile = FOLLOW_DISTANCE_PROFILES[self.long_personality] + multiplier = float(self._interpolate(v_ego, profile['x_vel'], profile['y_dist'], + self.follow_distance_slopes[self.long_personality])) + print(f"[VIBE_DEBUG] get_follow_distance_multiplier: multiplier={multiplier:.3f}") + return multiplier + except (KeyError, IndexError): + print(f"[VIBE_DEBUG] get_follow_distance_multiplier: ERROR - KeyError/IndexError") + return None + + def get_personality_info(self) -> dict: + """Get comprehensive info about current personalities and settings""" + self._update_from_params() + + accel_names = {AccelPersonality.eco: "Eco", AccelPersonality.normal: "Normal", AccelPersonality.sport: "Sport"} + long_names = {LongPersonality.relaxed: "Relaxed", LongPersonality.standard: "Standard", LongPersonality.aggressive: "Aggressive"} + + info = { + "accel_personality": accel_names.get(self.accel_personality, "Unknown"), + "accel_personality_int": self.accel_personality, + "long_personality": long_names.get(self.long_personality, "Unknown"), + "long_personality_int": self.long_personality, + "enabled": self._get_toggle_state('enabled'), + "accel_enabled": self._get_toggle_state('accel_enabled'), + "follow_enabled": self._get_toggle_state('follow_enabled'), + "accel_description": f"Acceleration: {accel_names.get(self.accel_personality, 'Unknown')}", + "long_description": f"Following/Braking: {long_names.get(self.long_personality, 'Unknown')}", + } + + print(f"[VIBE_DEBUG] get_personality_info: {info}") + return info + + def get_min_accel(self, v_ego: float) -> float | None: + """Get minimum acceleration (braking) from distance mode""" + limits = self.get_accel_limits(v_ego) + return limits[0] if limits else None + + def get_max_accel(self, v_ego: float) -> float | None: + """Get maximum acceleration from drive mode""" + limits = self.get_accel_limits(v_ego) + return limits[1] if limits else None + + def reset(self): + """Reset to default modes""" + print(f"[VIBE_DEBUG] Reset to default personalities - accel: {self.accel_personality}, long: {self.long_personality}") + self.accel_personality = AccelPersonality.normal + self.long_personality = LongPersonality.standard + self.frame = 0 + + def update(self): + """Update frame counter""" + self.frame = (self.frame + 1) % 1000000 + + def _compute_slopes(self, x, y): + """Compute slopes for Hermite interpolation using symmetric difference method.""" + n = len(x) + if n < 2: + raise ValueError("At least two points required") + + m = np.zeros(n) + for i in range(n): + if i == 0: + m[i] = (y[1] - y[0]) / (x[1] - x[0]) + elif i == n-1: + m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1]) + else: + m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2 + return m + + def _interpolate(self, x, xp, yp, slopes): + """Perform cubic Hermite interpolation.""" + x = np.clip(x, xp[0], xp[-1]) + idx = np.clip(np.searchsorted(xp, x) - 1, 0, len(slopes) - 2) + + x0, x1 = xp[idx], xp[idx+1] + y0, y1 = yp[idx], yp[idx+1] + m0, m1 = slopes[idx], slopes[idx+1] + + t = (x - x0) / (x1 - x0) + h = [2*t**3 - 3*t**2 + 1, t**3 - 2*t**2 + t, -2*t**3 + 3*t**2, t**3 - t**2] + + return h[0]*y0 + h[1]*(x1 - x0)*m0 + h[2]*y1 + h[3]*(x1 - x0)*m1