diff --git a/CLAUDE.md b/CLAUDE.md index 58c5331..9325a4a 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -21,6 +21,7 @@ - `docs/03_TECH/TECH_02_システム構成仕様.txt` — Pi/PC の役割分担、通信フロー、設計方針 - `docs/03_TECH/TECH_03_デバッグオーバーレイ仕様.txt` — オーバーレイ表示項目、描画色、GUI 操作 - `docs/03_TECH/TECH_04_線検出精度向上方針.txt` — 線検出が最重要ファクターである理由、照明・影の課題、改善の方向性 +- `docs/03_TECH/TECH_05_コースアウト復帰仕様.txt` — 復帰判定ロジック、復帰動作、パラメータ一覧、GUI 仕様 ### 環境（セットアップ時に参照） - `docs/04_ENV/ENV_01_技術スタック選定.txt` — ZMQ, PySide6, OpenCV, Picamera2, RPi.GPIO, python-dotenv diff --git "a/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" "b/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" index 0b09d33..b1730a3 100644 --- "a/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" +++ "b/docs/03_TECH/TECH_01_\346\223\215\350\210\265\351\207\217\350\250\210\347\256\227\344\273\225\346\247\230.txt" @@ -338,3 +338,27 @@ ・src/pc/steering/ts_pd_control.py: TsPdControl クラス ・src/pc/gui/main_window.py: 制御手法の切替 UI + + +9. コースアウト復帰 (Course-Out Recovery) +------------------------------------------------------------------------ + + 9-1. 概要 + + 自動操縦中に黒線を一定時間検出できなかった場合に， + 最後に検出した方向へ旋回しながら走行して復帰を試みる機能． + 全制御手法に共通で適用される． + 詳細は `TECH_05_コースアウト復帰仕様.txt` を参照する． + + 9-2. パラメータ一覧（GUI で調整可能） + + ・enabled（有効/無効）: True + ・timeout_sec（判定時間）: 0.5 秒 + ・steer_amount（操舵量）: 0.5 + ・throttle（速度）: -0.3（負で後退，正で前進） + + 9-3. 実装ファイル + + ・src/pc/steering/recovery.py: RecoveryParams，RecoveryController + ・src/pc/gui/panels/recovery_panel.py: RecoveryPanel + ・src/pc/gui/main_window.py: 復帰ロジックの統合 diff --git "a/docs/03_TECH/TECH_05_\343\202\263\343\203\274\343\202\271\343\202\242\343\202\246\343\203\210\345\276\251\345\270\260\344\273\225\346\247\230.txt" "b/docs/03_TECH/TECH_05_\343\202\263\343\203\274\343\202\271\343\202\242\343\202\246\343\203\210\345\276\251\345\270\260\344\273\225\346\247\230.txt" new file mode 100644 index 0000000..b7c3e89 --- /dev/null +++ "b/docs/03_TECH/TECH_05_\343\202\263\343\203\274\343\202\271\343\202\242\343\202\246\343\203\210\345\276\251\345\270\260\344\273\225\346\247\230.txt" @@ -0,0 +1,122 @@ +======================================================================== +コースアウト復帰仕様 (Course-Out Recovery Specification) +======================================================================== + + +1. 概要 (Overview) +------------------------------------------------------------------------ + + 1-0. 目的 + + 自動操縦中に黒線を見失った場合（コースアウト）， + 一定時間経過後に最後に検出した方向へ旋回しながら走行し， + コースへの復帰を試みる機能を定義する． + + 1-1. 基本方針 + + ・全制御手法（PD / 2点パシュート / Theil-Sen PD）に共通で適用する + ・制御手法の外側（MainWindow）で復帰判定と操舵量の上書きを行う + ・復帰パラメータは GUI の折りたたみパネルでリアルタイムに調整可能 + ・パラメータは JSON ファイルに自動保存・復元される + + +2. 復帰判定 (Recovery Trigger) +------------------------------------------------------------------------ + + 2-1. 判定条件 + + 以下の条件がすべて満たされたとき，復帰モードに遷移する． + + 1. 自動操縦中である + 2. 復帰機能が有効（enabled = True）である + 3. 線検出結果が detected = False である + 4. 最後に線を検出した時刻から timeout_sec 秒以上経過している + + 2-2. 復帰モードの解除 + + 線が再び検出された時点で即座に復帰モードを解除し， + 通常の制御手法による操舵に戻る． + + +3. 復帰動作 (Recovery Behavior) +------------------------------------------------------------------------ + + 3-1. 操舵方向の決定 + + 最後に線を検出したときの位置偏差（position_error）の符号から， + 線がどちら側にあったかを記録する． + + ・position_error > 0（線が画像の左側）: 左へ旋回（steer < 0） + ・position_error < 0（線が画像の右側）: 右へ旋回（steer > 0） + + 復帰操舵量の計算: + + direction = -sign(last_position_error) + steer = direction × steer_amount + + 3-2. 走行速度 + + throttle パラメータで復帰時の走行速度を指定する． + + ・負の値: 後退しながら旋回（コースアウト地点に戻る方向） + ・正の値: 前進しながら旋回（コース先回りで復帰を試みる） + ・ゼロ: 停止状態で旋回のみ + + 用途に応じて GUI で調整する． + + 3-3. 処理フロー + + 1. 各制御手法の compute() を通常通り呼び出す + 2. 検出結果を RecoveryController.update() に渡す + 3. update() が SteeringOutput を返した場合， + 制御手法の出力を復帰用の出力で上書きする + 4. 上書きされた操舵量を Pi に送信する + + +4. パラメータ一覧 (Parameters) +------------------------------------------------------------------------ + + ■ コースアウト復帰パラメータ（GUI で調整可能） + + ・enabled（有効/無効）: True + - 復帰機能全体のオン/オフ + ・timeout_sec（判定時間）: 0.5 秒 + - 線を見失ってから復帰動作を開始するまでの時間 + - 短すぎると一時的な検出失敗で誤動作する + - 長すぎるとコースアウト後の復帰が遅れる + ・steer_amount（操舵量）: 0.5 + - 復帰時の旋回の強さ（0.0 ~ 1.0） + - 大きいほど急旋回する + ・throttle（速度）: -0.3 + - 復帰時の走行速度（-1.0 ~ +1.0） + - 負の値で後退，正の値で前進 + + +5. GUI 仕様 (GUI Specification) +------------------------------------------------------------------------ + + 5-1. パネル配置 + + 「コースアウト復帰」という名前の折りたたみパネルを， + 制御パラメータパネルとデバッグ表示パネルの間に配置する． + + 5-2. UI 構成 + + ・チェックボックス: 「復帰機能を有効にする」 + ・判定時間: QDoubleSpinBox（0.1 ~ 10.0 秒，0.1 刻み） + ・操舵量: QDoubleSpinBox（0.0 ~ 1.0，0.05 刻み） + ・速度: QDoubleSpinBox（-1.0 ~ +1.0，0.05 刻み） + + 5-3. 自動保存 + + パラメータの変更は即座に params/recovery.json に保存され， + 次回起動時に復元される． + + +6. 実装ファイル (Implementation Files) +------------------------------------------------------------------------ + + ・src/pc/steering/recovery.py: RecoveryParams，RecoveryController + ・src/pc/gui/panels/recovery_panel.py: RecoveryPanel + ・src/pc/gui/main_window.py: 復帰ロジックの統合 + ・src/pc/steering/auto_params.py: save_recovery / load_recovery diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index 6525e9a..ec7c5cd 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -24,16 +24,19 @@ ControlParamPanel, ImageParamPanel, OverlayPanel, + RecoveryPanel, ) from pc.steering.auto_params import ( load_control, load_detect_params, load_overlay, load_pursuit, + load_recovery, load_ts_pd, save_control, save_overlay, save_pursuit, + save_recovery, save_ts_pd, ) from pc.steering.base import SteeringBase @@ -42,6 +45,10 @@ PursuitControl, PursuitParams, ) +from pc.steering.recovery import ( + RecoveryController, + RecoveryParams, +) from pc.steering.ts_pd_control import ( TsPdControl, TsPdParams, @@ -107,6 +114,12 @@ # 現在の制御手法（"pd", "pursuit", "ts_pd"） self._steering_method: str = last_steering + # コースアウト復帰 + recovery_params = load_recovery() + self._recovery = RecoveryController( + params=recovery_params, + ) + # 最新フレームの保持（自動操縦で使用） self._latest_frame: np.ndarray | None = None @@ -233,6 +246,15 @@ ) control_layout.addWidget(self._control_panel) + # コースアウト復帰パネル + self._recovery_panel = RecoveryPanel( + self._recovery.params, + ) + self._recovery_panel.recovery_params_changed.connect( + self._on_recovery_params_changed, + ) + control_layout.addWidget(self._recovery_panel) + # デバッグ表示パネル overlay_flags = load_overlay() self._overlay_panel = OverlayPanel(overlay_flags) @@ -313,6 +335,13 @@ self._ts_pd_control.params = p save_ts_pd(p) + def _on_recovery_params_changed( + self, p: RecoveryParams, + ) -> None: + """復帰パラメータの変更を反映して保存する""" + self._recovery.params = p + save_recovery(p) + def _on_overlay_flags_changed(self) -> None: """オーバーレイフラグの変更を保存する""" save_overlay(self._overlay_panel.get_flags()) @@ -381,6 +410,7 @@ """自動操縦を開始する""" self._is_auto = True self._active_control.reset() + self._recovery.reset() self._pressed_keys.clear() self._auto_btn.setText("自動操縦 OFF") self._status_label.setText("接続中 (自動操縦)") @@ -407,12 +437,27 @@ if self._is_auto: ctrl = self._active_control output = ctrl.compute(frame) - self._throttle = output.throttle - self._steer = output.steer - self._update_control_label() self._last_detect_result = ( ctrl.last_detect_result ) + + # コースアウト復帰判定 + det = self._last_detect_result + detected = det is not None and det.detected + pos_err = ( + det.position_error + if detected and det is not None + else 0.0 + ) + recovery_output = self._recovery.update( + detected, pos_err, + ) + if recovery_output is not None: + output = recovery_output + + self._throttle = output.throttle + self._steer = output.steer + self._update_control_label() else: self._last_detect_result = detect_line( frame, @@ -638,10 +683,13 @@ def _update_control_label(self) -> None: """操舵量の表示を更新する""" - self._control_label.setText( + text = ( f"throttle: {self._throttle:+.2f}\n" f"steer: {self._steer:+.2f}" ) + if self._is_auto and self._recovery.is_recovering: + text += "\n[復帰中]" + self._control_label.setText(text) def _send_control(self) -> None: """操舵量を Pi に送信する""" diff --git a/src/pc/gui/panels/__init__.py b/src/pc/gui/panels/__init__.py index 7b0df04..9814afe 100644 --- a/src/pc/gui/panels/__init__.py +++ b/src/pc/gui/panels/__init__.py @@ -9,10 +9,12 @@ from pc.gui.panels.control_param_panel import ControlParamPanel from pc.gui.panels.image_param_panel import ImageParamPanel from pc.gui.panels.overlay_panel import OverlayPanel +from pc.gui.panels.recovery_panel import RecoveryPanel __all__ = [ "CollapsibleGroupBox", "ControlParamPanel", "ImageParamPanel", "OverlayPanel", + "RecoveryPanel", ] diff --git a/src/pc/gui/panels/recovery_panel.py b/src/pc/gui/panels/recovery_panel.py new file mode 100644 index 0000000..a56bb25 --- /dev/null +++ b/src/pc/gui/panels/recovery_panel.py @@ -0,0 +1,110 @@ +""" +recovery_panel +コースアウト復帰パラメータ調整 UI パネル +""" + +from PySide6.QtCore import Signal +from PySide6.QtWidgets import ( + QCheckBox, + QDoubleSpinBox, + QFormLayout, + QVBoxLayout, +) + +from pc.gui.panels.collapsible_group_box import ( + CollapsibleGroupBox, +) +from pc.steering.recovery import RecoveryParams + + +class RecoveryPanel(CollapsibleGroupBox): + """コースアウト復帰パラメータ調整 UI""" + + recovery_params_changed = Signal(object) + + def __init__( + self, + params: RecoveryParams | None = None, + ) -> None: + super().__init__("コースアウト復帰") + self._params = params or RecoveryParams() + self._auto_save_enabled = False + self._setup_ui() + self._auto_save_enabled = True + + def get_params(self) -> RecoveryParams: + """現在の復帰パラメータを返す""" + return self._params + + def _setup_ui(self) -> None: + """UI を構築する""" + layout = QVBoxLayout() + self.setLayout(layout) + + # 有効/無効チェックボックス + self._enabled_cb = QCheckBox("復帰機能を有効にする") + self._enabled_cb.setChecked(self._params.enabled) + self._enabled_cb.toggled.connect(self._on_changed) + layout.addWidget(self._enabled_cb) + + # パラメータフォーム + form = QFormLayout() + layout.addLayout(form) + + self._spin_timeout = QDoubleSpinBox() + self._spin_timeout.setRange(0.1, 10.0) + self._spin_timeout.setSingleStep(0.1) + self._spin_timeout.setDecimals(1) + self._spin_timeout.setSuffix(" 秒") + self._spin_timeout.setValue( + self._params.timeout_sec, + ) + self._spin_timeout.valueChanged.connect( + self._on_changed, + ) + form.addRow( + "判定時間:", self._spin_timeout, + ) + + self._spin_steer = QDoubleSpinBox() + self._spin_steer.setRange(0.0, 1.0) + self._spin_steer.setSingleStep(0.05) + self._spin_steer.setDecimals(2) + self._spin_steer.setValue( + self._params.steer_amount, + ) + self._spin_steer.valueChanged.connect( + self._on_changed, + ) + form.addRow("操舵量:", self._spin_steer) + + self._spin_throttle = QDoubleSpinBox() + self._spin_throttle.setRange(-1.0, 1.0) + self._spin_throttle.setSingleStep(0.05) + self._spin_throttle.setDecimals(2) + self._spin_throttle.setValue( + self._params.throttle, + ) + self._spin_throttle.valueChanged.connect( + self._on_changed, + ) + form.addRow("速度:", self._spin_throttle) + + def _on_changed(self) -> None: + """パラメータ変更時に反映してシグナルを発火する""" + self._params.enabled = ( + self._enabled_cb.isChecked() + ) + self._params.timeout_sec = ( + self._spin_timeout.value() + ) + self._params.steer_amount = ( + self._spin_steer.value() + ) + self._params.throttle = ( + self._spin_throttle.value() + ) + if self._auto_save_enabled: + self.recovery_params_changed.emit( + self._params, + ) diff --git a/src/pc/steering/auto_params.py b/src/pc/steering/auto_params.py index dc62da5..9b10c4c 100644 --- a/src/pc/steering/auto_params.py +++ b/src/pc/steering/auto_params.py @@ -8,6 +8,7 @@ ├── control.json PD 制御 + 最後に使用した手法 ├── pursuit.json パシュート制御パラメータ ├── ts_pd.json Theil-Sen PD 制御パラメータ + ├── recovery.json コースアウト復帰パラメータ ├── overlay.json オーバーレイ表示フラグ ├── detect_current.json 現行手法の二値化パラメータ ├── detect_blackhat.json 案A の二値化パラメータ @@ -20,6 +21,7 @@ from common.json_utils import PARAMS_DIR, read_json, write_json from pc.steering.pd_control import PdParams from pc.steering.pursuit_control import PursuitParams +from pc.steering.recovery import RecoveryParams from pc.steering.ts_pd_control import TsPdParams from pc.vision.line_detector import ImageParams from pc.vision.overlay import OverlayFlags @@ -33,6 +35,9 @@ # Theil-Sen PD 制御パラメータファイル _TS_PD_FILE = PARAMS_DIR / "ts_pd.json" +# コースアウト復帰パラメータファイル +_RECOVERY_FILE = PARAMS_DIR / "recovery.json" + # オーバーレイ表示フラグファイル _OVERLAY_FILE = PARAMS_DIR / "overlay.json" @@ -140,6 +145,33 @@ return TsPdParams(**filtered) +def save_recovery(params: RecoveryParams) -> None: + """コースアウト復帰パラメータを保存する + + Args: + params: コースアウト復帰パラメータ + """ + write_json(_RECOVERY_FILE, asdict(params)) + + +def load_recovery() -> RecoveryParams: + """コースアウト復帰パラメータを読み込む + + Returns: + 復帰パラメータ（ファイルがない場合はデフォルト） + """ + if not _RECOVERY_FILE.exists(): + return RecoveryParams() + + data = read_json(_RECOVERY_FILE) + known = RecoveryParams.__dataclass_fields__ + filtered = { + k: v for k, v in data.items() + if k in known + } + return RecoveryParams(**filtered) + + def save_overlay(flags: OverlayFlags) -> None: """オーバーレイ表示フラグを保存する diff --git a/src/pc/steering/recovery.py b/src/pc/steering/recovery.py new file mode 100644 index 0000000..dfa88d9 --- /dev/null +++ b/src/pc/steering/recovery.py @@ -0,0 +1,110 @@ +""" +recovery +コースアウト復帰のパラメータと判定ロジックを定義するモジュール +黒線を一定時間検出できなかった場合に， +最後に検出した方向へ旋回しながら走行して復帰する +""" + +import time +from dataclasses import dataclass + +from pc.steering.base import SteeringOutput + + +@dataclass +class RecoveryParams: + """コースアウト復帰のパラメータ + + Attributes: + enabled: 復帰機能の有効/無効 + timeout_sec: 線を見失ってから復帰動作を開始するまでの時間 + steer_amount: 復帰時の操舵量（0.0 ~ 1.0） + throttle: 復帰時の速度（負: 後退，正: 前進） + """ + enabled: bool = True + timeout_sec: float = 0.5 + steer_amount: float = 0.5 + throttle: float = -0.3 + + +class RecoveryController: + """コースアウト復帰の判定と操舵量算出を行うクラス + + 自動操縦中にフレームごとに呼び出し， + 線検出の成否を記録する．一定時間検出できなかった場合に + 復帰用の操舵量を返す + """ + + def __init__( + self, + params: RecoveryParams | None = None, + ) -> None: + self.params: RecoveryParams = ( + params or RecoveryParams() + ) + self._last_detected_time: float = 0.0 + self._last_error_sign: float = 0.0 + self._is_recovering: bool = False + + def reset(self) -> None: + """内部状態をリセットする + + 自動操縦の開始時に呼び出す + """ + self._last_detected_time = time.time() + self._last_error_sign = 0.0 + self._is_recovering = False + + def update( + self, + detected: bool, + position_error: float = 0.0, + ) -> SteeringOutput | None: + """検出結果を記録し，復帰が必要なら操舵量を返す + + 毎フレーム呼び出す．線が検出できている間は内部状態を + 更新して None を返す．検出できない時間が timeout_sec を + 超えたら復帰用の SteeringOutput を返す + + Args: + detected: 線が検出できたか + position_error: 検出時の位置偏差（正: 線が左） + + Returns: + 復帰操舵量，または None（通常走行を継続） + """ + if not self.params.enabled: + return None + + now = time.time() + + if detected: + self._last_detected_time = now + if position_error != 0.0: + self._last_error_sign = ( + 1.0 if position_error > 0 else -1.0 + ) + self._is_recovering = False + return None + + # 線を見失ってからの経過時間を判定 + elapsed = now - self._last_detected_time + if elapsed < self.params.timeout_sec: + return None + + # 復帰モード: 最後に検出した方向へ旋回 + # position_error > 0（線が左）→ 左へ旋回（steer < 0） + self._is_recovering = True + steer = ( + -self._last_error_sign + * self.params.steer_amount + ) + return SteeringOutput( + throttle=self.params.throttle, + steer=steer, + ) + + @property + def is_recovering(self) -> bool: + """現在復帰動作中かどうかを返す""" + return self._is_recovering