mirror of
https://github.com/herizon1054/openpilot.git
synced 2026-06-08 09:54:13 +08:00
Update human_turn_detection.py
This commit is contained in:
herizon1054
@@ -5,6 +5,7 @@ from enum import Enum, auto
|
||||
from openpilot.common.params import Params
|
||||
|
||||
|
||||
# --- 系統與安全常數 ---
|
||||
LOG_PATH = "/data/media/0/realdata/debug.log"
|
||||
PARAM_REFRESH_SEC = 2.0
|
||||
MIN_SPEED_MS = 0.1
|
||||
@@ -13,182 +14,188 @@ MAX_SPEED_MS = 9.72
|
||||
|
||||
|
||||
def _log(message: str) -> None:
|
||||
try:
|
||||
os.makedirs(os.path.dirname(LOG_PATH), exist_ok=True)
|
||||
with open(LOG_PATH, "a", encoding="utf-8") as f:
|
||||
f.write(f"{time.time():.3f} {message}\n")
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
os.makedirs(os.path.dirname(LOG_PATH), exist_ok=True)
|
||||
with open(LOG_PATH, "a", encoding="utf-8") as f:
|
||||
f.write(f"{time.time():.3f} {message}\n")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class HTDState(Enum):
|
||||
INACTIVE = auto()
|
||||
MANUAL_TURN = auto()
|
||||
RAMPING = auto()
|
||||
INACTIVE = auto()
|
||||
MANUAL_TURN = auto()
|
||||
RAMPING = auto()
|
||||
|
||||
|
||||
class HumanTurnDetection:
|
||||
def __init__(self) -> None:
|
||||
self._params = Params()
|
||||
self._last_params_read = 0.0
|
||||
def __init__(self) -> None:
|
||||
self._params = Params()
|
||||
self._last_params_read = 0.0
|
||||
|
||||
# --- 由 UI 控制的變數 ---
|
||||
self._enabled = False # [修正1] 預設 False，等第一次 _read_params() 從 Params 讀取
|
||||
self._angle_threshold_deg = 90.0
|
||||
# --- 1. UI 控制參數 (從 Params 讀取) ---
|
||||
self._enabled = False # [修正1] 預設 False，等第一次 _read_params() 從 Params 讀取
|
||||
self._angle_threshold_deg = 90.0
|
||||
|
||||
# --- 以下為寫死的系統參數，不再從 Params 讀取 ---
|
||||
self._angle_release_deg = 20.0
|
||||
self._torque_start_nm = 2.0
|
||||
self._torque_release_nm = 0.6
|
||||
# 安全接管角度鎖：角度大於此值時，即使倒數完畢也拒絕恢復自動駕駛
|
||||
self._resume_angle_lock_deg = 40.0
|
||||
# --- 2. 系統內建參數 (不從 Params 讀取) ---
|
||||
self._angle_release_deg = 20.0
|
||||
self._torque_start_nm = 2.0
|
||||
self._torque_release_nm = 0.6
|
||||
self._resume_angle_lock_deg = 40.0 # 安全接管角度鎖：大於此值拒絕恢復自駕
|
||||
|
||||
self._state: HTDState = HTDState.INACTIVE
|
||||
self._state_change_time = 0.0
|
||||
# --- 3. 狀態與計時器 ---
|
||||
self._state: HTDState = HTDState.INACTIVE
|
||||
self._state_change_time = 0.0
|
||||
self._trigger_start_time = 0.0
|
||||
|
||||
# 帶正負號的原始數值，用於方向一致性判斷
|
||||
self._last_angle_raw = 0.0
|
||||
self._last_torque_raw = 0.0
|
||||
# --- 4. 車輛動態資料快取 ---
|
||||
self._last_angle_raw = 0.0
|
||||
self._last_torque_raw = 0.0
|
||||
self._last_angle = 0.0
|
||||
self._last_torque = 0.0
|
||||
self._last_pressed = False
|
||||
|
||||
self._last_angle = 0.0
|
||||
self._last_torque = 0.0
|
||||
self._last_pressed = False
|
||||
self._trigger_start_time = 0.0
|
||||
# --- 5. 運行過程暫存變數 ---
|
||||
self._max_turn_angle = 0.0 # 本次彎道最大角度
|
||||
self._dynamic_delay = 0.5 # 準備接管時的動態等待秒數
|
||||
|
||||
# 本次彎道最大角度
|
||||
self._max_turn_angle = 0.0
|
||||
# 準備接管時的動態等待秒數
|
||||
self._dynamic_delay = 0.5
|
||||
def _read_params(self) -> None:
|
||||
"""定期更新外部設定參數"""
|
||||
now = time.monotonic()
|
||||
if now - self._last_params_read < PARAM_REFRESH_SEC:
|
||||
return
|
||||
|
||||
self._last_params_read = now
|
||||
self._enabled = self._params.get_bool("dp_htd_enabled")
|
||||
self._angle_threshold_deg = self._get_float("dp_htd_turn_angle_threshold", 90.0)
|
||||
|
||||
def _read_params(self) -> None:
|
||||
now = time.monotonic()
|
||||
if now - self._last_params_read < PARAM_REFRESH_SEC:
|
||||
return
|
||||
self._last_params_read = now
|
||||
def _transition(self, new_state: HTDState, reason: str) -> None:
|
||||
"""處理狀態切換並記錄 Log"""
|
||||
if new_state == self._state:
|
||||
return
|
||||
|
||||
self._state = new_state
|
||||
self._state_change_time = time.monotonic()
|
||||
_log(
|
||||
f"HTD {new_state.name} reason={reason} angle={self._last_angle:.1f} "
|
||||
f"torque={self._last_torque:.2f} pressed={self._last_pressed} delay={self._dynamic_delay:.2f}"
|
||||
)
|
||||
|
||||
self._enabled = self._params.get_bool("dp_htd_enabled")
|
||||
self._angle_threshold_deg = self._get_float("dp_htd_turn_angle_threshold", 90.0)
|
||||
def update(
|
||||
self,
|
||||
lat_active: bool,
|
||||
cruise_enabled: bool,
|
||||
steering_angle_deg: float,
|
||||
steering_torque_nm: float,
|
||||
v_ego: float,
|
||||
steering_pressed: bool = False,
|
||||
) -> tuple[bool, HTDState]:
|
||||
|
||||
self._read_params()
|
||||
|
||||
def _transition(self, new_state: HTDState, reason: str) -> None:
|
||||
if new_state == self._state:
|
||||
return
|
||||
self._state = new_state
|
||||
self._state_change_time = time.monotonic()
|
||||
_log(
|
||||
f"HTD {new_state.name} reason={reason} angle={self._last_angle:.1f} "
|
||||
f"torque={self._last_torque:.2f} pressed={self._last_pressed} delay={self._dynamic_delay:.2f}"
|
||||
)
|
||||
# --- 更新車輛動態資料 ---
|
||||
self._last_angle_raw = steering_angle_deg
|
||||
self._last_torque_raw = steering_torque_nm
|
||||
self._last_angle = abs(steering_angle_deg)
|
||||
self._last_torque = abs(steering_torque_nm)
|
||||
self._last_pressed = steering_pressed
|
||||
|
||||
def update(
|
||||
self,
|
||||
lat_active: bool,
|
||||
cruise_enabled: bool,
|
||||
steering_angle_deg: float,
|
||||
steering_torque_nm: float,
|
||||
v_ego: float,
|
||||
steering_pressed: bool = False,
|
||||
) -> tuple[bool, HTDState]:
|
||||
self._read_params()
|
||||
# --- 守衛條件 (Guard Clauses) ---
|
||||
# [修正2] cruise_enabled 時 HTD 不應介入（巡航中不是「人工轉向」場景）
|
||||
# 同時保留速度範圍與啟用開關的守衛
|
||||
is_invalid_condition = (
|
||||
not self._enabled or
|
||||
cruise_enabled or
|
||||
not lat_active or
|
||||
not (MIN_SPEED_MS <= v_ego <= MAX_SPEED_MS)
|
||||
)
|
||||
|
||||
if is_invalid_condition:
|
||||
if self._state != HTDState.INACTIVE:
|
||||
self._transition(HTDState.INACTIVE, "disabled_or_invalid_condition")
|
||||
self._trigger_start_time = 0.0
|
||||
return True, self._state
|
||||
|
||||
# 儲存帶正負號的原始數值
|
||||
self._last_angle_raw = steering_angle_deg
|
||||
self._last_torque_raw = steering_torque_nm
|
||||
# --- 狀態機邏輯 (State Machine) ---
|
||||
if self._state == HTDState.INACTIVE:
|
||||
if self._should_trigger():
|
||||
self._max_turn_angle = self._last_angle
|
||||
self._transition(HTDState.MANUAL_TURN, "trigger")
|
||||
return False, self._state
|
||||
|
||||
elif self._state == HTDState.MANUAL_TURN:
|
||||
self._max_turn_angle = max(self._max_turn_angle, self._last_angle)
|
||||
if self._should_release():
|
||||
# 計算動態延遲時間 (0.5 ~ 1.0 秒)
|
||||
calculated_delay = self._max_turn_angle / 270.0
|
||||
self._dynamic_delay = max(0.5, min(calculated_delay, 1.0))
|
||||
self._transition(HTDState.RAMPING, "release")
|
||||
return False, self._state
|
||||
|
||||
# 儲存絕對值用於門檻比對
|
||||
self._last_angle = abs(steering_angle_deg)
|
||||
self._last_torque = abs(steering_torque_nm)
|
||||
self._last_pressed = steering_pressed
|
||||
elif self._state == HTDState.RAMPING:
|
||||
# [修正3] retrigger 判斷移至 RAMPING 最頂端，確保每個 tick 都能重新進入 MANUAL_TURN
|
||||
if self._should_trigger():
|
||||
self._trigger_start_time = 0.0 # [修正4] retrigger 時重置防抖計時器，避免立刻再觸發
|
||||
self._transition(HTDState.MANUAL_TURN, "retrigger")
|
||||
return False, self._state
|
||||
|
||||
# [修正2] cruise_enabled 時 HTD 不應介入（巡航中不是「人工轉向」場景）
|
||||
# 同時保留速度範圍與啟用開關的守衛
|
||||
if (not self._enabled
|
||||
or cruise_enabled
|
||||
or not lat_active
|
||||
or v_ego < MIN_SPEED_MS
|
||||
or v_ego > MAX_SPEED_MS):
|
||||
if self._state != HTDState.INACTIVE:
|
||||
self._transition(HTDState.INACTIVE, "disabled_or_invalid_condition")
|
||||
self._trigger_start_time = 0.0
|
||||
return True, self._state
|
||||
elapsed = time.monotonic() - self._state_change_time
|
||||
if elapsed >= self._dynamic_delay:
|
||||
# 角度尚未回正，繼續等待
|
||||
if self._last_angle > self._resume_angle_lock_deg:
|
||||
return False, self._state
|
||||
|
||||
# ── INACTIVE ──────────────────────────────────────────────
|
||||
if self._state == HTDState.INACTIVE:
|
||||
if self._should_trigger():
|
||||
self._max_turn_angle = self._last_angle
|
||||
self._transition(HTDState.MANUAL_TURN, "trigger")
|
||||
return False, self._state
|
||||
return True, self._state
|
||||
# 滿足恢復條件
|
||||
self._max_turn_angle = 0.0
|
||||
self._transition(HTDState.INACTIVE, "resume")
|
||||
return True, self._state
|
||||
|
||||
return False, self._state
|
||||
|
||||
# ── MANUAL_TURN ───────────────────────────────────────────
|
||||
if self._state == HTDState.MANUAL_TURN:
|
||||
self._max_turn_angle = max(self._max_turn_angle, self._last_angle)
|
||||
# 預設行為 (理論上不會走到這裡)
|
||||
return True, self._state
|
||||
|
||||
if self._should_release():
|
||||
calculated_delay = self._max_turn_angle / 270.0
|
||||
self._dynamic_delay = max(0.5, min(calculated_delay, 1.0))
|
||||
self._transition(HTDState.RAMPING, "release")
|
||||
return False, self._state
|
||||
def _should_trigger(self) -> bool:
|
||||
"""判斷是否應該觸發手動轉向狀態"""
|
||||
direction_match = (self._last_angle_raw * self._last_torque_raw) > 0
|
||||
condition_met = (
|
||||
self._last_pressed
|
||||
and direction_match
|
||||
and self._last_torque >= self._torque_start_nm
|
||||
and self._last_angle >= self._angle_threshold_deg
|
||||
)
|
||||
|
||||
# ── RAMPING ───────────────────────────────────────────────
|
||||
# [修正3] retrigger 判斷移至 RAMPING 最頂端，確保每個 tick 都能重新進入 MANUAL_TURN
|
||||
if self._should_trigger():
|
||||
self._trigger_start_time = 0.0 # [修正4] retrigger 時重置防抖計時器，避免立刻再觸發
|
||||
self._transition(HTDState.MANUAL_TURN, "retrigger")
|
||||
return False, self._state
|
||||
if condition_met:
|
||||
if self._trigger_start_time == 0.0:
|
||||
self._trigger_start_time = time.monotonic()
|
||||
elif time.monotonic() - self._trigger_start_time >= 0.1:
|
||||
return True
|
||||
else:
|
||||
self._trigger_start_time = 0.0
|
||||
|
||||
elapsed = time.monotonic() - self._state_change_time
|
||||
if elapsed >= self._dynamic_delay:
|
||||
if self._last_angle > self._resume_angle_lock_deg:
|
||||
# 角度尚未回正，繼續等待
|
||||
return False, self._state
|
||||
return False
|
||||
|
||||
self._max_turn_angle = 0.0
|
||||
self._transition(HTDState.INACTIVE, "resume")
|
||||
return True, self._state
|
||||
def _should_release(self) -> bool:
|
||||
"""判斷是否應該解除手動轉向狀態"""
|
||||
# 軌道 A：方向盤回正且扭力歸零
|
||||
perfect_return = (
|
||||
self._last_torque <= self._torque_release_nm
|
||||
and self._last_angle <= self._angle_release_deg
|
||||
)
|
||||
# 軌道 B：駕駛放開方向盤
|
||||
hands_off = not self._last_pressed
|
||||
|
||||
return False, self._state
|
||||
release_condition = perfect_return or hands_off
|
||||
|
||||
def _should_trigger(self) -> bool:
|
||||
direction_match = (self._last_angle_raw * self._last_torque_raw) > 0
|
||||
if release_condition:
|
||||
self._trigger_start_time = 0.0 # 釋放時同步清除防抖計時
|
||||
|
||||
condition_met = (
|
||||
self._last_pressed
|
||||
and direction_match
|
||||
and self._last_torque >= self._torque_start_nm
|
||||
and self._last_angle >= self._angle_threshold_deg
|
||||
)
|
||||
return release_condition
|
||||
|
||||
if condition_met:
|
||||
if self._trigger_start_time == 0.0:
|
||||
self._trigger_start_time = time.monotonic()
|
||||
elif time.monotonic() - self._trigger_start_time >= 0.1:
|
||||
return True
|
||||
else:
|
||||
self._trigger_start_time = 0.0
|
||||
|
||||
return False
|
||||
|
||||
def _should_release(self) -> bool:
|
||||
# 軌道 A：方向盤回正且扭力歸零
|
||||
perfect_return = (
|
||||
self._last_torque <= self._torque_release_nm
|
||||
and self._last_angle <= self._angle_release_deg
|
||||
)
|
||||
# 軌道 B：駕駛放開方向盤
|
||||
hands_off = not self._last_pressed
|
||||
|
||||
release_condition = perfect_return or hands_off
|
||||
|
||||
if release_condition:
|
||||
self._trigger_start_time = 0.0 # 釋放時同步清除防抖計時
|
||||
|
||||
return release_condition
|
||||
|
||||
def _get_float(self, key: str, default: float) -> float:
|
||||
try:
|
||||
val = self._params.get(key)
|
||||
if val is None:
|
||||
return default
|
||||
return float(val)
|
||||
except Exception:
|
||||
return default
|
||||
def _get_float(self, key: str, default: float) -> float:
|
||||
"""安全地從 Params 取得浮點數"""
|
||||
try:
|
||||
val = self._params.get(key)
|
||||
return float(val) if val is not None else default
|
||||
except Exception:
|
||||
return default
|
||||
|
||||
Reference in New Issue
Block a user