mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-06 02:25:45 +08:00
vibe
This commit is contained in:
@@ -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 {
|
||||
|
||||
@@ -171,6 +171,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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]
|
||||
|
||||
|
||||
@@ -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<QString> 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<int>(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.");
|
||||
|
||||
@@ -88,6 +88,7 @@ protected:
|
||||
Params params;
|
||||
std::map<std::string, ParamControl*> toggles;
|
||||
ButtonParamControl *long_personality_setting;
|
||||
ButtonParamControl *accel_personality_setting;
|
||||
|
||||
virtual void updateToggles();
|
||||
};
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -37,4 +37,8 @@ private:
|
||||
ParamControl *intelligentCruiseButtonManagement = nullptr;
|
||||
SpeedLimitSettings *speedLimitScreen;
|
||||
PushButtonSP *speedLimitSettings;
|
||||
|
||||
ParamControlSP *vibePersonalityControl;
|
||||
ParamControlSP *vibeAccelPersonalityControl;
|
||||
ParamControlSP *vibeFollowPersonalityControl;
|
||||
};
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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')
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user