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 439af03..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" @@ -171,7 +171,7 @@ 6. パラメータ一覧 (Parameters) ------------------------------------------------------------------------ - ■ 画像処理パラメータ（GUI で調整可能） + ■ 二値化パラメータ（GUI で調整可能） GUI のコンボボックスで検出手法を切り替えられる． 手法ごとに使用するパラメータが異なり， @@ -280,3 +280,85 @@ ・src/pc/steering/pursuit_control.py: PursuitControl クラス ・src/pc/gui/main_window.py: 制御手法の切替 UI + + +8. Theil-Sen PD 制御 (Theil-Sen PD Control) +------------------------------------------------------------------------ + + 8-1. 概要 + + 行中心点に Theil-Sen 直線近似を適用し，画像下端での位置偏差と + 直線の傾きから PD 制御で操舵量を算出する方式． + 2点パシュートの Theil-Sen フィッティングと PD 制御の微分項を + 組み合わせたハイブリッド手法である． + GUI で PD 制御・2点パシュートと切り替えて使用可能． + + 8-2. 特徴 + + ・Theil-Sen 推定による外れ値耐性: 行中心点の外れ値に強い + ・傾き（slope）を直接利用: 多項式の微分計算が不要 + ・D 項による振動抑制: 偏差の変化率を見て急な操舵変化を抑制 + ・傾きベースの速度制御: 直線の傾きでカーブ度合いを判定 + + 8-3. アルゴリズム + + (1) 二値画像の最大連結領域を抽出する + (2) 各行の左右端から中心 x 座標（row_centers）を求める + (3) 有効な行の (y, x) 座標に Theil-Sen 直線近似を適用する + x = slope × y + intercept + (4) 画像下端での位置偏差を算出する + bottom_x = slope × (画像高さ - 1) + intercept + position_error = (画像中心x - bottom_x) / 画像中心x + (5) P 項 + Heading 項で操舵量の基礎値を算出する + error = Kp × position_error + Kh × slope + (6) D 項（微分項）を加算する + derivative = (error - error_prev) / dt + steer = error + Kd × derivative + (7) レートリミッターで急な操舵変化を制限する + (8) 速度を算出する + throttle = max_throttle - speed_k × |slope| + + 8-4. 2点パシュートとの関係 + + 2点パシュートの操舵式 K_near × near_err + K_far × far_err は， + Theil-Sen 直線上の 2 点から計算されるため， + Kp × position_error + Kh × slope と数学的に等価である． + Theil-Sen PD はこれに D 項（微分項）を追加した拡張である． + + 8-5. パラメータ一覧（GUI で調整可能） + + ・kp（デフォルト: 0.5）: 位置偏差ゲイン + ・kh（デフォルト: 0.3）: 傾き（Theil-Sen slope）ゲイン + ・kd（デフォルト: 0.1）: 微分ゲイン + ・max_steer_rate（デフォルト: 0.1）: 1フレームあたりの最大操舵変化量 + ・max_throttle（デフォルト: 0.4）: 直線での最大速度 + ・speed_k（デフォルト: 2.0）: 傾きベースの減速係数 + + 8-6. 実装ファイル + + ・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_02_\343\202\267\343\202\271\343\203\206\343\203\240\346\247\213\346\210\220\344\273\225\346\247\230.txt" "b/docs/03_TECH/TECH_02_\343\202\267\343\202\271\343\203\206\343\203\240\346\247\213\346\210\220\344\273\225\346\247\230.txt" index 93307ae..e00c748 100644 --- "a/docs/03_TECH/TECH_02_\343\202\267\343\202\271\343\203\206\343\203\240\346\247\213\346\210\220\344\273\225\346\247\230.txt" +++ "b/docs/03_TECH/TECH_02_\343\202\267\343\202\271\343\203\206\343\203\240\346\247\213\346\210\220\344\273\225\346\247\230.txt" @@ -108,7 +108,7 @@ ■ パラメータ調整 ・PD 制御パラメータ（Kp，Kh，Kd 等）をリアルタイムに変更できる UI を設ける． - ・画像処理パラメータ（二値化閾値，CLAHE 強度等）も + ・二値化パラメータ（二値化閾値，CLAHE 強度等）も リアルタイムに変更できる． ・変更したパラメータは即座に処理に反映される． diff --git "a/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" "b/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" index 4117323..23d8b3e 100644 --- "a/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" +++ "b/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" @@ -27,7 +27,12 @@ ・二値化画像: 二値化結果を赤色の半透明で重ねる（不透明度 0.4） ・検出領域: 検出対象領域の枠を青色で表示 ・フィッティング曲線: 多項式の曲線を緑色で描画 + ・行中心点: 各行の線中心 x 座標をオレンジ色の点で描画 + ・Theil-Sen 直線: 行中心点の Theil-Sen 近似直線をマゼンタで描画 ・中心線: 画像の中心 x に垂直線を描画（黄色） + ・パシュート目標点: 2点パシュートの near/far 目標点をシアンの + 円（半径 2px）で描画．制御手法がパシュートのとき有効 + （手動操作中は検出結果からプレビュー表示） 2-2. 検出情報ラベル（常時表示） @@ -39,6 +44,9 @@ ・フィッティング曲線: (0, 255, 0) 緑 ・中心線: (0, 255, 255) 黄 ・検出領域: (255, 0, 0) 青 + ・行中心点: (0, 165, 255) オレンジ + ・Theil-Sen 直線: (255, 0, 255) マゼンタ + ・パシュート目標点: (255, 255, 0) シアン ・二値化オーバーレイ: 赤チャンネルに二値化画像を割り当て 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/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" "b/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" index 9f72519..178535d 100644 --- "a/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" +++ "b/docs/04_ENV/ENV_04_\343\203\207\343\202\243\343\203\254\343\202\257\343\203\210\343\203\252\346\247\213\346\210\220.txt" @@ -58,6 +58,7 @@ │ ├── base.py 共通インターフェース │ ├── pd_control.py PD 制御の実装 │ ├── pursuit_control.py 2点パシュート制御の実装 + │ ├── ts_pd_control.py Theil-Sen PD 制御の実装 │ ├── param_store.py プリセット保存・読み込み │ └── auto_params.py パラメータ自動保存・復元 └── vision/ 画像処理 diff --git a/src/common/config.py b/src/common/config.py index 4a9e106..8596713 100644 --- a/src/common/config.py +++ b/src/common/config.py @@ -74,7 +74,7 @@ MOTOR_RIGHT_REVERSED: bool = True # ステアリング方向反転フラグ -STEER_REVERSED: bool = True +STEER_REVERSED: bool = False # ── アドレス生成ヘルパー ────────────────────────────────── diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index 08de403..ec7c5cd 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -24,11 +24,20 @@ 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 from pc.steering.pd_control import PdControl, PdParams @@ -36,6 +45,15 @@ PursuitControl, PursuitParams, ) +from pc.steering.recovery import ( + RecoveryController, + RecoveryParams, +) +from pc.steering.ts_pd_control import ( + TsPdControl, + TsPdParams, +) +from pc.vision.fitting import theil_sen_fit from pc.vision.line_detector import ( ImageParams, LineDetectResult, @@ -74,18 +92,33 @@ self._steer: float = 0.0 # 前回のパラメータを復元 - pd_params, last_method = load_control() + pd_params, last_method, last_steering = ( + load_control() + ) image_params = load_detect_params(last_method) self._pd_control = PdControl( params=pd_params, image_params=image_params, ) + pursuit_params = load_pursuit() self._pursuit_control = PursuitControl( + params=pursuit_params, + image_params=image_params, + ) + ts_pd_params = load_ts_pd() + self._ts_pd_control = TsPdControl( + params=ts_pd_params, image_params=image_params, ) - # 現在の制御手法（"pd" or "pursuit"） - self._steering_method: str = "pd" + # 現在の制御手法（"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 @@ -180,7 +213,7 @@ ) control_layout.addWidget(self._control_label) - # 画像処理パラメータパネル + # 二値化パラメータパネル self._image_panel = ImageParamPanel( self._pd_control.image_params, ) @@ -196,6 +229,8 @@ self._control_panel = ControlParamPanel( self._pd_control.params, self._pursuit_control.params, + self._ts_pd_control.params, + self._steering_method, ) self._control_panel.pd_params_changed.connect( self._on_pd_params_changed, @@ -203,13 +238,29 @@ self._control_panel.pursuit_params_changed.connect( self._on_pursuit_params_changed, ) + self._control_panel.ts_pd_params_changed.connect( + self._on_ts_pd_params_changed, + ) self._control_panel.steering_method_changed.connect( self._on_steering_method_changed, ) 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) + # デバッグ表示パネル - self._overlay_panel = OverlayPanel() + overlay_flags = load_overlay() + self._overlay_panel = OverlayPanel(overlay_flags) + self._overlay_panel.overlay_flags_changed.connect( + self._on_overlay_flags_changed, + ) control_layout.addWidget(self._overlay_panel) # 操作ガイド @@ -231,6 +282,8 @@ """現在選択中の制御クラスを返す""" if self._steering_method == "pursuit": return self._pursuit_control + if self._steering_method == "ts_pd": + return self._ts_pd_control return self._pd_control def _setup_timers(self) -> None: @@ -248,32 +301,61 @@ def _on_image_params_changed( self, ip: ImageParams, ) -> None: - """画像処理パラメータの変更を両制御クラスに反映する""" + """二値化パラメータの変更を全制御クラスに反映する""" self._pd_control.image_params = ip self._pursuit_control.image_params = ip + self._ts_pd_control.image_params = ip def _on_method_changed(self, method: str) -> None: """検出手法の変更に合わせて制御設定を保存する""" - save_control(self._pd_control.params, method) + save_control( + self._pd_control.params, method, + self._steering_method, + ) def _on_pd_params_changed(self, p: PdParams) -> None: """PD パラメータの変更を制御クラスに反映して保存する""" self._pd_control.params = p save_control( p, self._pd_control.image_params.method, + self._steering_method, ) def _on_pursuit_params_changed( self, p: PursuitParams, ) -> None: - """Pursuit パラメータの変更を制御クラスに反映する""" + """Pursuit パラメータの変更を制御クラスに反映して保存する""" self._pursuit_control.params = p + save_pursuit(p) + + def _on_ts_pd_params_changed( + self, p: TsPdParams, + ) -> None: + """Theil-Sen PD パラメータの変更を反映して保存する""" + 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()) def _on_steering_method_changed( self, method: str, ) -> None: - """制御手法の切替を反映する""" + """制御手法の切替を反映して保存する""" self._steering_method = method + save_control( + self._pd_control.params, + self._pd_control.image_params.method, + method, + ) # ── 接続 ────────────────────────────────────────────── @@ -328,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("接続中 (自動操縦)") @@ -354,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, @@ -382,6 +480,82 @@ f" curv: {r.curvature:+.6f}" ) + def _calc_pursuit_points_preview( + self, + ) -> ( + tuple[tuple[float, float], tuple[float, float]] + | None + ): + """手動操作中にパシュート目標点を算出する + + Returns: + ((near_x, near_y), (far_x, far_y)) または None + """ + r = self._last_detect_result + if r is None or not r.detected: + return None + if r.row_centers is None: + return None + + centers = r.row_centers + valid = ~np.isnan(centers) + ys = np.where(valid)[0].astype(float) + xs = centers[valid] + if len(ys) < 2: + return None + + slope, intercept = theil_sen_fit(ys, xs) + h = len(centers) + p = self._pursuit_control.params + near_y = h * p.near_ratio + far_y = h * p.far_ratio + near_x = slope * near_y + intercept + far_x = slope * far_y + intercept + return ((near_x, near_y), (far_x, far_y)) + + def _calc_ts_pd_line_preview( + self, + ) -> ( + tuple[tuple[float, float], tuple[float, float]] + | None + ): + """Theil-Sen PD の近似直線を表示用に算出する + + 直線の上端と下端の 2 点を返す + + Returns: + ((bottom_x, bottom_y), (top_x, top_y)) または None + """ + if self._is_auto: + fit = self._ts_pd_control.last_fit_line + if fit is None: + return None + slope, intercept = fit + r = self._last_detect_result + if r is None or r.row_centers is None: + return None + h = len(r.row_centers) + else: + r = self._last_detect_result + if r is None or not r.detected: + return None + if r.row_centers is None: + return None + centers = r.row_centers + valid = ~np.isnan(centers) + ys = np.where(valid)[0].astype(float) + xs = centers[valid] + if len(ys) < 2: + return None + slope, intercept = theil_sen_fit(ys, xs) + h = len(centers) + + bottom_y = float(h - 1) + top_y = 0.0 + bottom_x = slope * bottom_y + intercept + top_x = slope * top_y + intercept + return ((bottom_x, bottom_y), (top_x, top_y)) + def _display_frame(self, frame: np.ndarray) -> None: """NumPy 配列の画像を QLabel に表示する @@ -392,9 +566,25 @@ bgr = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) # オーバーレイ描画 + pursuit_pts = None + if self._steering_method == "pursuit": + if self._is_auto: + pursuit_pts = ( + self._pursuit_control + .last_pursuit_points + ) + else: + pursuit_pts = ( + self._calc_pursuit_points_preview() + ) + elif self._steering_method == "ts_pd": + pursuit_pts = ( + self._calc_ts_pd_line_preview() + ) bgr = draw_overlay( bgr, self._last_detect_result, self._overlay_panel.get_flags(), + pursuit_points=pursuit_pts, ) # 検出情報をラベルに表示 @@ -493,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 7988084..9814afe 100644 --- a/src/pc/gui/panels/__init__.py +++ b/src/pc/gui/panels/__init__.py @@ -3,12 +3,18 @@ GUI パネルウィジェット群 """ +from pc.gui.panels.collapsible_group_box import ( + CollapsibleGroupBox, +) 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/collapsible_group_box.py b/src/pc/gui/panels/collapsible_group_box.py new file mode 100644 index 0000000..2b9e026 --- /dev/null +++ b/src/pc/gui/panels/collapsible_group_box.py @@ -0,0 +1,66 @@ +""" +collapsible_group_box +チェックボックスで折りたたみ可能な QGroupBox +""" + +from PySide6.QtGui import QShowEvent +from PySide6.QtWidgets import QGroupBox, QLayout, QWidget + + +class CollapsibleGroupBox(QGroupBox): + """タイトルのチェックで中身を折りたたみできる QGroupBox""" + + def __init__(self, title: str) -> None: + super().__init__(title) + self.setCheckable(True) + self.setChecked(False) + self._first_show: bool = True + self.toggled.connect(self._toggle_content) + + def showEvent(self, event: QShowEvent) -> None: + """初回表示時に折りたたみ状態を反映する""" + super().showEvent(event) + if self._first_show: + self._first_show = False + if not self.isChecked(): + self._toggle_content(False) + + def _toggle_content(self, on: bool) -> None: + """チェック状態に応じてレイアウト内の要素を表示/非表示する + + Args: + on: True で展開，False で折りたたみ + """ + layout = self.layout() + if layout is None: + return + _set_layout_visible(layout, on) + if on: + self._on_expanded() + + + def _on_expanded(self) -> None: + """展開後のフックポイント + + サブクラスでオーバーライドし，展開後に + ウィジェットの表示状態を再適用する + """ + + +def _set_layout_visible( + layout: QLayout, visible: bool, +) -> None: + """レイアウト内の全ウィジェットを再帰的に表示/非表示する + + Args: + layout: 対象レイアウト + visible: True で表示，False で非表示 + """ + for i in range(layout.count()): + item = layout.itemAt(i) + widget = item.widget() + if isinstance(widget, QWidget): + widget.setVisible(visible) + sub = item.layout() + if sub is not None: + _set_layout_visible(sub, visible) diff --git a/src/pc/gui/panels/control_param_panel.py b/src/pc/gui/panels/control_param_panel.py index 864288d..bddedfe 100644 --- a/src/pc/gui/panels/control_param_panel.py +++ b/src/pc/gui/panels/control_param_panel.py @@ -1,6 +1,6 @@ """ control_param_panel -PD / 2点パシュート制御パラメータ調整 UI パネル +PD / 2点パシュート / Theil-Sen PD 制御パラメータ調整 UI パネル """ from PySide6.QtCore import Signal @@ -8,7 +8,6 @@ QComboBox, QDoubleSpinBox, QFormLayout, - QGroupBox, QHBoxLayout, QInputDialog, QLabel, @@ -18,37 +17,53 @@ QWidget, ) +from pc.gui.panels.collapsible_group_box import ( + CollapsibleGroupBox, +) + from pc.gui.panels.image_param_panel import _create_preset_ui from pc.steering.param_store import ( PdPreset, + TsPdPreset, add_pd_preset, + add_ts_pd_preset, delete_pd_preset, + delete_ts_pd_preset, load_pd_presets, + load_ts_pd_presets, ) from pc.steering.pd_control import PdParams from pc.steering.pursuit_control import PursuitParams +from pc.steering.ts_pd_control import TsPdParams -class ControlParamPanel(QGroupBox): - """PD / 2点パシュート制御パラメータ調整 UI""" +class ControlParamPanel(CollapsibleGroupBox): + """PD / 2点パシュート / Theil-Sen PD 制御パラメータ調整 UI""" # PD パラメータが変更されたときに emit する pd_params_changed = Signal(object) # Pursuit パラメータが変更されたときに emit する pursuit_params_changed = Signal(object) - # 制御手法が変更されたときに emit する（"pd" or "pursuit"） + # Theil-Sen PD パラメータが変更されたときに emit する + ts_pd_params_changed = Signal(object) + # 制御手法が変更されたときに emit する steering_method_changed = Signal(str) def __init__( self, pd_params: PdParams, pursuit_params: PursuitParams, + ts_pd_params: TsPdParams, + steering_method: str = "pd", ) -> None: super().__init__("制御パラメータ") self._pd_params = pd_params self._pursuit_params = pursuit_params + self._ts_pd_params = ts_pd_params + self._initial_steering_method = steering_method self._auto_save_enabled = False self._pd_presets: list[PdPreset] = [] + self._ts_pd_presets: list[TsPdPreset] = [] self._setup_ui() self._auto_save_enabled = True @@ -61,6 +76,14 @@ """現在の Pursuit パラメータを返す""" return self._pursuit_params + def get_ts_pd_params(self) -> TsPdParams: + """現在の Theil-Sen PD パラメータを返す""" + return self._ts_pd_params + + def _on_expanded(self) -> None: + """展開後に制御手法に応じた表示切替を再適用する""" + self._on_steering_method_changed() + def _setup_ui(self) -> None: """UI を構築する""" layout = QVBoxLayout() @@ -72,6 +95,14 @@ self._steering_combo.addItem( "2点パシュート", "pursuit", ) + self._steering_combo.addItem( + "Theil-Sen PD", "ts_pd", + ) + idx = self._steering_combo.findData( + self._initial_steering_method, + ) + if idx >= 0: + self._steering_combo.setCurrentIndex(idx) layout.addWidget(self._steering_combo) # --- PD パラメータ --- @@ -183,16 +214,100 @@ self._spin_pursuit_speed_k, ] - # --- プリセット管理 --- + # --- Theil-Sen PD パラメータ --- + self._ts_pd_param_form = QFormLayout() + layout.addLayout(self._ts_pd_param_form) + + tp = self._ts_pd_params + + self._spin_ts_kp = _create_spin( + tp.kp, 0.0, 0.05, + ) + self._ts_pd_param_form.addRow( + "Kp (位置):", self._spin_ts_kp, + ) + + self._spin_ts_kh = _create_spin( + tp.kh, 0.0, 0.05, + ) + self._ts_pd_param_form.addRow( + "Kh (傾き):", self._spin_ts_kh, + ) + + self._spin_ts_kd = _create_spin( + tp.kd, 0.0, 0.05, + ) + self._ts_pd_param_form.addRow( + "Kd (微分):", self._spin_ts_kd, + ) + + self._spin_ts_steer_rate = _create_spin( + tp.max_steer_rate, 0.01, 0.01, + ) + self._ts_pd_param_form.addRow( + "操舵制限:", self._spin_ts_steer_rate, + ) + + self._spin_ts_throttle = _create_spin( + tp.max_throttle, 0.0, 0.05, + ) + self._ts_pd_param_form.addRow( + "最大速度:", self._spin_ts_throttle, + ) + + self._spin_ts_speed_k = _create_spin( + tp.speed_k, 0.0, 0.1, + ) + self._ts_pd_param_form.addRow( + "減速係数:", self._spin_ts_speed_k, + ) + + # Theil-Sen PD 固有ウィジェットリスト（表示切替用） + self._ts_pd_widgets: list[QWidget] = [ + self._spin_ts_kp, + self._spin_ts_kh, + self._spin_ts_kd, + self._spin_ts_steer_rate, + self._spin_ts_throttle, + self._spin_ts_speed_k, + ] + + # --- プリセット管理（PD 制御）--- + self._pd_preset_container = QWidget() + pd_preset_layout = QVBoxLayout( + self._pd_preset_container, + ) + pd_preset_layout.setContentsMargins(0, 0, 0, 0) self._pd_preset_combo, self._pd_preset_memo = ( _create_preset_ui( - layout, + pd_preset_layout, self._on_load_pd_preset, self._on_save_pd_preset, self._on_delete_pd_preset, self._on_pd_preset_selected, ) ) + layout.addWidget(self._pd_preset_container) + + # --- プリセット管理（Theil-Sen PD）--- + self._ts_pd_preset_container = QWidget() + ts_pd_preset_layout = QVBoxLayout( + self._ts_pd_preset_container, + ) + ts_pd_preset_layout.setContentsMargins( + 0, 0, 0, 0, + ) + ( + self._ts_pd_preset_combo, + self._ts_pd_preset_memo, + ) = _create_preset_ui( + ts_pd_preset_layout, + self._on_load_ts_pd_preset, + self._on_save_ts_pd_preset, + self._on_delete_ts_pd_preset, + self._on_ts_pd_preset_selected, + ) + layout.addWidget(self._ts_pd_preset_container) # コールバック接続 self._steering_combo.currentIndexChanged.connect( @@ -208,8 +323,13 @@ spin.valueChanged.connect( self._on_pursuit_changed, ) + for spin in self._ts_pd_widgets: + spin.valueChanged.connect( + self._on_ts_pd_changed, + ) self._refresh_pd_presets() + self._refresh_ts_pd_presets() # 初期表示の更新 self._on_steering_method_changed() @@ -245,10 +365,27 @@ if self._auto_save_enabled: self.pursuit_params_changed.emit(p) + def _on_ts_pd_changed(self) -> None: + """Theil-Sen PD パラメータの変更を反映する""" + p = self._ts_pd_params + p.kp = self._spin_ts_kp.value() + p.kh = self._spin_ts_kh.value() + p.kd = self._spin_ts_kd.value() + p.max_steer_rate = ( + self._spin_ts_steer_rate.value() + ) + p.max_throttle = self._spin_ts_throttle.value() + p.speed_k = self._spin_ts_speed_k.value() + + if self._auto_save_enabled: + self.ts_pd_params_changed.emit(p) + def _on_steering_method_changed(self) -> None: """制御手法の変更を反映する""" method = self._steering_combo.currentData() is_pd = method == "pd" + is_pursuit = method == "pursuit" + is_ts_pd = method == "ts_pd" # PD 固有ウィジェットの表示切替 for w in self._pd_widgets: @@ -257,7 +394,7 @@ if label: label.setVisible(is_pd) - # 共通ウィジェット（操舵制限/最大速度/減速係数） + # PD 共通ウィジェット（操舵制限/最大速度/減速係数） for w in [ self._spin_max_steer_rate, self._spin_max_throttle, @@ -270,12 +407,25 @@ # Pursuit ウィジェットの表示切替 for w in self._pursuit_widgets: - w.setVisible(not is_pd) + w.setVisible(is_pursuit) label = ( self._pursuit_param_form.labelForField(w) ) if label: - label.setVisible(not is_pd) + label.setVisible(is_pursuit) + + # Theil-Sen PD ウィジェットの表示切替 + for w in self._ts_pd_widgets: + w.setVisible(is_ts_pd) + label = ( + self._ts_pd_param_form.labelForField(w) + ) + if label: + label.setVisible(is_ts_pd) + + # プリセット UI の表示切替 + self._pd_preset_container.setVisible(is_pd) + self._ts_pd_preset_container.setVisible(is_ts_pd) if self._auto_save_enabled: self.steering_method_changed.emit(method) @@ -366,6 +516,94 @@ delete_pd_preset(idx) self._refresh_pd_presets() + # ── Theil-Sen PD プリセット管理 ───────────────────── + + def _refresh_ts_pd_presets(self) -> None: + """Theil-Sen PD プリセット一覧を更新する""" + self._ts_pd_presets = load_ts_pd_presets() + self._ts_pd_preset_combo.clear() + for p in self._ts_pd_presets: + self._ts_pd_preset_combo.addItem(p.title) + self._ts_pd_preset_memo.setText("") + + def _on_ts_pd_preset_selected(self) -> None: + """Theil-Sen PD プリセット選択時にメモを表示する""" + idx = self._ts_pd_preset_combo.currentIndex() + if 0 <= idx < len(self._ts_pd_presets): + self._ts_pd_preset_memo.setText( + self._ts_pd_presets[idx].memo, + ) + else: + self._ts_pd_preset_memo.setText("") + + def _on_load_ts_pd_preset(self) -> None: + """Theil-Sen PD プリセットを読み込む""" + idx = self._ts_pd_preset_combo.currentIndex() + if idx < 0 or idx >= len(self._ts_pd_presets): + return + self._auto_save_enabled = False + try: + p = self._ts_pd_presets[idx].params + self._spin_ts_kp.setValue(p.kp) + self._spin_ts_kh.setValue(p.kh) + self._spin_ts_kd.setValue(p.kd) + self._spin_ts_steer_rate.setValue( + p.max_steer_rate, + ) + self._spin_ts_throttle.setValue( + p.max_throttle, + ) + self._spin_ts_speed_k.setValue(p.speed_k) + self._ts_pd_params = p + finally: + self._auto_save_enabled = True + self.ts_pd_params_changed.emit(p) + + def _on_save_ts_pd_preset(self) -> None: + """Theil-Sen PD プリセットを保存する""" + title, ok = QInputDialog.getText( + self, "TS-PD プリセット保存", "タイトル:", + ) + if not ok or not title.strip(): + return + memo, ok = QInputDialog.getText( + self, "TS-PD プリセット保存", "メモ:", + ) + if not ok: + return + p = self._ts_pd_params + add_ts_pd_preset(TsPdPreset( + title=title.strip(), + memo=memo.strip(), + params=TsPdParams( + kp=p.kp, kh=p.kh, kd=p.kd, + max_steer_rate=p.max_steer_rate, + max_throttle=p.max_throttle, + speed_k=p.speed_k, + ), + )) + self._refresh_ts_pd_presets() + self._ts_pd_preset_combo.setCurrentIndex( + self._ts_pd_preset_combo.count() - 1, + ) + + def _on_delete_ts_pd_preset(self) -> None: + """Theil-Sen PD プリセットを削除する""" + idx = self._ts_pd_preset_combo.currentIndex() + if idx < 0 or idx >= len(self._ts_pd_presets): + return + title = self._ts_pd_presets[idx].title + reply = QMessageBox.question( + self, "削除確認", + f"「{title}」を削除しますか？", + QMessageBox.StandardButton.Yes + | QMessageBox.StandardButton.No, + QMessageBox.StandardButton.No, + ) + if reply == QMessageBox.StandardButton.Yes: + delete_ts_pd_preset(idx) + self._refresh_ts_pd_presets() + def _create_spin( value: float, min_val: float, step: float, diff --git a/src/pc/gui/panels/image_param_panel.py b/src/pc/gui/panels/image_param_panel.py index 1e7a1b7..570daa3 100644 --- a/src/pc/gui/panels/image_param_panel.py +++ b/src/pc/gui/panels/image_param_panel.py @@ -1,6 +1,6 @@ """ image_param_panel -画像処理パラメータ調整 UI パネル +二値化パラメータ調整 UI パネル """ from PySide6.QtCore import Signal @@ -8,7 +8,6 @@ QComboBox, QDoubleSpinBox, QFormLayout, - QGroupBox, QHBoxLayout, QInputDialog, QLabel, @@ -19,6 +18,10 @@ QWidget, ) +from pc.gui.panels.collapsible_group_box import ( + CollapsibleGroupBox, +) + from pc.steering.auto_params import ( load_detect_params, save_detect_params, @@ -36,16 +39,16 @@ ) -class ImageParamPanel(QGroupBox): - """画像処理パラメータ調整 UI""" +class ImageParamPanel(CollapsibleGroupBox): + """二値化パラメータ調整 UI""" - # 画像処理パラメータが変更されたときに emit する + # 二値化パラメータが変更されたときに emit する image_params_changed = Signal(object) # 検出手法が変更されたときに emit する（新手法キー） method_changed = Signal(str) def __init__(self, image_params: ImageParams) -> None: - super().__init__("画像処理パラメータ") + super().__init__("二値化パラメータ") self._image_params = image_params self._auto_save_enabled = False self._image_presets: list[ImagePreset] = [] @@ -58,7 +61,7 @@ self._auto_save_enabled = True def get_image_params(self) -> ImageParams: - """現在の画像処理パラメータを返す""" + """現在の二値化パラメータを返す""" return self._image_params def _setup_ui(self) -> None: @@ -380,7 +383,7 @@ methods: set[str], field: str, ) -> None: - """画像処理パラメータの行を追加する + """二値化パラメータの行を追加する Args: label: フォームラベル diff --git a/src/pc/gui/panels/overlay_panel.py b/src/pc/gui/panels/overlay_panel.py index 1a86ffa..6be26d8 100644 --- a/src/pc/gui/panels/overlay_panel.py +++ b/src/pc/gui/panels/overlay_panel.py @@ -3,17 +3,25 @@ デバッグ表示の切替チェックボックスを提供するパネル """ -from PySide6.QtWidgets import QCheckBox, QGroupBox, QVBoxLayout +from PySide6.QtCore import Signal +from PySide6.QtWidgets import QCheckBox, QVBoxLayout +from pc.gui.panels.collapsible_group_box import ( + CollapsibleGroupBox, +) from pc.vision.overlay import OverlayFlags -class OverlayPanel(QGroupBox): +class OverlayPanel(CollapsibleGroupBox): """デバッグ表示の切替チェックボックス群""" - def __init__(self) -> None: + overlay_flags_changed = Signal() + + def __init__( + self, flags: OverlayFlags | None = None, + ) -> None: super().__init__("デバッグ表示") - self._flags = OverlayFlags() + self._flags = flags or OverlayFlags() self._setup_ui() def _setup_ui(self) -> None: @@ -28,16 +36,25 @@ ("行中心点", "row_centers"), ("Theil-Sen直線", "theil_sen"), ("中心線", "center_line"), + ("パシュート目標点", "pursuit_points"), ] for label, attr in items: cb = QCheckBox(label) + cb.setChecked(getattr(self._flags, attr)) cb.toggled.connect( - lambda checked, a=attr: setattr( - self._flags, a, checked, + lambda checked, a=attr: self._on_toggled( + a, checked, ), ) layout.addWidget(cb) + def _on_toggled( + self, attr: str, checked: bool, + ) -> None: + """チェックボックスの切替を反映してシグナルを発火する""" + setattr(self._flags, attr, checked) + self.overlay_flags_changed.emit() + def get_flags(self) -> OverlayFlags: """現在のオーバーレイフラグを返す""" return self._flags 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 24be64e..9b10c4c 100644 --- a/src/pc/steering/auto_params.py +++ b/src/pc/steering/auto_params.py @@ -6,21 +6,41 @@ ファイル構成: params/ ├── control.json PD 制御 + 最後に使用した手法 - ├── detect_current.json 現行手法の画像処理パラメータ - ├── detect_blackhat.json 案A の画像処理パラメータ - ├── detect_dual_norm.json 案B の画像処理パラメータ - └── detect_robust.json 案C の画像処理パラメータ + ├── pursuit.json パシュート制御パラメータ + ├── ts_pd.json Theil-Sen PD 制御パラメータ + ├── recovery.json コースアウト復帰パラメータ + ├── overlay.json オーバーレイ表示フラグ + ├── detect_current.json 現行手法の二値化パラメータ + ├── detect_blackhat.json 案A の二値化パラメータ + ├── detect_dual_norm.json 案B の二値化パラメータ + └── detect_robust.json 案C の二値化パラメータ """ from dataclasses import asdict 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 # PD 制御パラメータファイル _CONTROL_FILE = PARAMS_DIR / "control.json" +# パシュート制御パラメータファイル +_PURSUIT_FILE = PARAMS_DIR / "pursuit.json" + +# Theil-Sen PD 制御パラメータファイル +_TS_PD_FILE = PARAMS_DIR / "ts_pd.json" + +# コースアウト復帰パラメータファイル +_RECOVERY_FILE = PARAMS_DIR / "recovery.json" + +# オーバーレイ表示フラグファイル +_OVERLAY_FILE = PARAMS_DIR / "overlay.json" + # 検出手法ごとのファイル名 _DETECT_FILES: dict[str, str] = { "current": "detect_current.json", @@ -32,36 +52,151 @@ def save_control( - params: PdParams, method: str, + params: PdParams, + method: str, + steering_method: str = "pd", ) -> None: """PD 制御パラメータと最後に使用した手法を保存する Args: params: PD 制御パラメータ method: 最後に使用した検出手法の識別子 + steering_method: 最後に使用した制御手法（"pd" or "pursuit"） """ data = asdict(params) data["last_method"] = method + data["last_steering_method"] = steering_method write_json(_CONTROL_FILE, data) -def load_control() -> tuple[PdParams, str]: +def load_control() -> tuple[PdParams, str, str]: """PD 制御パラメータと最後に使用した手法を読み込む Returns: - (PD 制御パラメータ, 最後に使用した手法の識別子) + (PD 制御パラメータ, 検出手法の識別子, 制御手法の識別子) """ if not _CONTROL_FILE.exists(): - return PdParams(), "current" + return PdParams(), "current", "pd" data = read_json(_CONTROL_FILE) method = data.pop("last_method", "current") + steering_method = data.pop( + "last_steering_method", "pd", + ) known = PdParams.__dataclass_fields__ filtered = { k: v for k, v in data.items() if k in known } - return PdParams(**filtered), method + return PdParams(**filtered), method, steering_method + + +def save_pursuit(params: PursuitParams) -> None: + """パシュート制御パラメータを保存する + + Args: + params: パシュート制御パラメータ + """ + write_json(_PURSUIT_FILE, asdict(params)) + + +def load_pursuit() -> PursuitParams: + """パシュート制御パラメータを読み込む + + Returns: + パシュート制御パラメータ（ファイルがない場合はデフォルト） + """ + if not _PURSUIT_FILE.exists(): + return PursuitParams() + + data = read_json(_PURSUIT_FILE) + known = PursuitParams.__dataclass_fields__ + filtered = { + k: v for k, v in data.items() + if k in known + } + return PursuitParams(**filtered) + + +def save_ts_pd(params: TsPdParams) -> None: + """Theil-Sen PD 制御パラメータを保存する + + Args: + params: Theil-Sen PD 制御パラメータ + """ + write_json(_TS_PD_FILE, asdict(params)) + + +def load_ts_pd() -> TsPdParams: + """Theil-Sen PD 制御パラメータを読み込む + + Returns: + Theil-Sen PD 制御パラメータ（ファイルがない場合はデフォルト） + """ + if not _TS_PD_FILE.exists(): + return TsPdParams() + + data = read_json(_TS_PD_FILE) + known = TsPdParams.__dataclass_fields__ + filtered = { + k: v for k, v in data.items() + if k in known + } + 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: + """オーバーレイ表示フラグを保存する + + Args: + flags: オーバーレイ表示フラグ + """ + write_json(_OVERLAY_FILE, asdict(flags)) + + +def load_overlay() -> OverlayFlags: + """オーバーレイ表示フラグを読み込む + + Returns: + オーバーレイ表示フラグ（ファイルがない場合はデフォルト） + """ + if not _OVERLAY_FILE.exists(): + return OverlayFlags() + + data = read_json(_OVERLAY_FILE) + known = OverlayFlags.__dataclass_fields__ + filtered = { + k: v for k, v in data.items() + if k in known + } + return OverlayFlags(**filtered) def save_detect_params( @@ -71,7 +206,7 @@ Args: method: 検出手法の識別子 - params: 画像処理パラメータ + params: 二値化パラメータ """ filename = _DETECT_FILES.get(method) if filename is None: @@ -88,7 +223,7 @@ method: 検出手法の識別子 Returns: - 画像処理パラメータ（ファイルがない場合はデフォルト） + 二値化パラメータ（ファイルがない場合はデフォルト） """ filename = _DETECT_FILES.get(method) if filename is None: diff --git a/src/pc/steering/param_store.py b/src/pc/steering/param_store.py index 5a9cc58..503bf7c 100644 --- a/src/pc/steering/param_store.py +++ b/src/pc/steering/param_store.py @@ -1,16 +1,18 @@ """ param_store パラメータプリセットの保存・読み込みを管理するモジュール -画像処理パラメータと PD 制御パラメータを独立して管理する +画像処理・PD 制御・Theil-Sen PD 制御パラメータを独立して管理する """ from dataclasses import asdict, dataclass from common.json_utils import PARAMS_DIR, read_json, write_json from pc.steering.pd_control import PdParams +from pc.steering.ts_pd_control import TsPdParams from pc.vision.line_detector import ImageParams _PD_FILE = PARAMS_DIR / "presets_pd.json" +_TS_PD_FILE = PARAMS_DIR / "presets_ts_pd.json" _IMAGE_FILE = PARAMS_DIR / "presets_image.json" @@ -58,17 +60,62 @@ ) +# ── Theil-Sen PD 制御プリセット ──────────────── + + +@dataclass +class TsPdPreset: + """Theil-Sen PD 制御パラメータのプリセット + + Attributes: + title: プリセットのタイトル + memo: メモ + params: Theil-Sen PD 制御パラメータ + """ + + title: str + memo: str + params: TsPdParams + + +def load_ts_pd_presets() -> list[TsPdPreset]: + """Theil-Sen PD 制御プリセット一覧を読み込む""" + return _load_presets( + _TS_PD_FILE, TsPdPreset, + "params", TsPdParams, + ) + + +def add_ts_pd_preset(preset: TsPdPreset) -> None: + """Theil-Sen PD 制御プリセットを追加する""" + presets = load_ts_pd_presets() + presets.append(preset) + _save_presets( + _TS_PD_FILE, presets, "params", + ) + + +def delete_ts_pd_preset(index: int) -> None: + """Theil-Sen PD 制御プリセットを削除する""" + presets = load_ts_pd_presets() + if 0 <= index < len(presets): + presets.pop(index) + _save_presets( + _TS_PD_FILE, presets, "params", + ) + + # ── 画像処理プリセット ──────────────────────── @dataclass class ImagePreset: - """画像処理パラメータのプリセット + """二値化パラメータのプリセット Attributes: title: プリセットのタイトル memo: メモ - image_params: 画像処理パラメータ + image_params: 二値化パラメータ """ title: str diff --git a/src/pc/steering/pursuit_control.py b/src/pc/steering/pursuit_control.py index 833edd4..db0b793 100644 --- a/src/pc/steering/pursuit_control.py +++ b/src/pc/steering/pursuit_control.py @@ -60,6 +60,10 @@ ) self._prev_steer: float = 0.0 self._last_result = None + self._last_pursuit_points: ( + tuple[tuple[float, float], tuple[float, float]] + | None + ) = None def compute( self, frame: np.ndarray, @@ -79,6 +83,7 @@ self._last_result = result if not result.detected or result.row_centers is None: + self._last_pursuit_points = None return SteeringOutput( throttle=0.0, steer=0.0, ) @@ -91,6 +96,7 @@ xs = centers[valid] if len(ys) < 2: + self._last_pursuit_points = None return SteeringOutput( throttle=0.0, steer=0.0, ) @@ -107,6 +113,12 @@ near_x = slope * near_y + intercept far_x = slope * far_y + intercept + # 目標点を保持（デバッグ表示用） + self._last_pursuit_points = ( + (near_x, near_y), + (far_x, far_y), + ) + # 各点の偏差（正: 線が左にある → 右に曲がる） near_err = (center_x - near_x) / center_x far_err = (center_x - far_x) / center_x @@ -136,9 +148,24 @@ """内部状態をリセットする""" self._prev_steer = 0.0 self._last_result = None + self._last_pursuit_points = None reset_valley_tracker() @property def last_detect_result(self): """直近の線検出結果を取得する""" return self._last_result + + @property + def last_pursuit_points( + self, + ) -> ( + tuple[tuple[float, float], tuple[float, float]] + | None + ): + """直近の2点パシュート目標点を取得する + + Returns: + ((near_x, near_y), (far_x, far_y)) または None + """ + return self._last_pursuit_points 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 diff --git a/src/pc/steering/ts_pd_control.py b/src/pc/steering/ts_pd_control.py new file mode 100644 index 0000000..67f3bc5 --- /dev/null +++ b/src/pc/steering/ts_pd_control.py @@ -0,0 +1,178 @@ +""" +ts_pd_control +Theil-Sen 直線近似による PD 制御モジュール +行中心点に Theil-Sen 直線をフィッティングし， +位置偏差・傾き・微分項から操舵量を算出する +""" + +import time +from dataclasses import dataclass + +import numpy as np + +from common import config +from pc.steering.base import SteeringBase, SteeringOutput +from pc.vision.fitting import theil_sen_fit +from pc.vision.line_detector import ( + ImageParams, + detect_line, + reset_valley_tracker, +) + + +@dataclass +class TsPdParams: + """Theil-Sen PD 制御のパラメータ + + Attributes: + kp: 位置偏差ゲイン + kh: 傾き（Theil-Sen slope）ゲイン + kd: 微分ゲイン + max_steer_rate: 1フレームあたりの最大操舵変化量 + max_throttle: 直線での最大速度 + speed_k: 傾きベースの減速係数 + """ + kp: float = 0.5 + kh: float = 0.3 + kd: float = 0.1 + max_steer_rate: float = 0.1 + max_throttle: float = 0.4 + speed_k: float = 2.0 + + +class TsPdControl(SteeringBase): + """Theil-Sen 直線近似による PD 制御クラス + + 行中心点から Theil-Sen 直線近似を行い， + 画像下端での位置偏差と直線の傾きから PD 制御で操舵量を計算する + """ + + def __init__( + self, + params: TsPdParams | None = None, + image_params: ImageParams | None = None, + ) -> None: + self.params: TsPdParams = ( + params or TsPdParams() + ) + self.image_params: ImageParams = ( + image_params or ImageParams() + ) + self._prev_error: float = 0.0 + self._prev_time: float = 0.0 + self._prev_steer: float = 0.0 + self._last_result = None + self._last_fit_line: ( + tuple[float, float] | None + ) = None + + def compute( + self, frame: np.ndarray, + ) -> SteeringOutput: + """カメラ画像から Theil-Sen PD 制御で操舵量を計算する + + Args: + frame: グレースケールのカメラ画像 + + Returns: + 計算された操舵量 + """ + p = self.params + + # 線検出 + result = detect_line(frame, self.image_params) + self._last_result = result + + if not result.detected or result.row_centers is None: + self._last_fit_line = None + return SteeringOutput( + throttle=0.0, steer=0.0, + ) + + centers = result.row_centers + + # 有効な点（NaN でない行）を抽出 + valid = ~np.isnan(centers) + ys = np.where(valid)[0].astype(float) + xs = centers[valid] + + if len(ys) < 2: + self._last_fit_line = None + return SteeringOutput( + throttle=0.0, steer=0.0, + ) + + # Theil-Sen 直線近似: x = slope * y + intercept + slope, intercept = theil_sen_fit(ys, xs) + self._last_fit_line = (slope, intercept) + + center_x = config.FRAME_WIDTH / 2.0 + h = len(centers) + + # 画像下端での位置偏差 + bottom_x = slope * (h - 1) + intercept + position_error = (center_x - bottom_x) / center_x + + # 操舵量: P 項（位置偏差）+ Heading 項（傾き） + # 符号反転: 偏差正（線が左）→ steer 負（左へ曲がる） + error = -(p.kp * position_error + p.kh * slope) + + # 時間差分の計算 + now = time.time() + dt = ( + now - self._prev_time + if self._prev_time > 0 + else 0.033 + ) + dt = max(dt, 0.001) + + # D 項（微分項） + derivative = (error - self._prev_error) / dt + steer = error + p.kd * derivative + + # 操舵量のクランプ + steer = max(-1.0, min(1.0, steer)) + + # レートリミッター + delta = steer - self._prev_steer + max_delta = p.max_steer_rate + delta = max(-max_delta, min(max_delta, delta)) + steer = self._prev_steer + delta + + # 速度制御（傾きベース: 傾きが大きい → カーブ → 減速） + throttle = p.max_throttle - p.speed_k * abs(slope) + throttle = max(0.0, throttle) + + # 状態の更新 + self._prev_error = error + self._prev_time = now + self._prev_steer = steer + + return SteeringOutput( + throttle=throttle, steer=steer, + ) + + def reset(self) -> None: + """内部状態をリセットする""" + self._prev_error = 0.0 + self._prev_time = 0.0 + self._prev_steer = 0.0 + self._last_result = None + self._last_fit_line = None + reset_valley_tracker() + + @property + def last_detect_result(self): + """直近の線検出結果を取得する""" + return self._last_result + + @property + def last_fit_line( + self, + ) -> tuple[float, float] | None: + """直近の Theil-Sen 直線近似結果を取得する + + Returns: + (slope, intercept) または None + """ + return self._last_fit_line diff --git a/src/pc/vision/line_detector.py b/src/pc/vision/line_detector.py index 09115bf..7155d16 100644 --- a/src/pc/vision/line_detector.py +++ b/src/pc/vision/line_detector.py @@ -36,7 +36,7 @@ @dataclass class ImageParams: - """画像処理パラメータ + """二値化パラメータ Attributes: method: 検出手法の識別子 @@ -165,7 +165,7 @@ Args: frame: グレースケールのカメラ画像 - params: 画像処理パラメータ（None でデフォルト） + params: 二値化パラメータ（None でデフォルト） Returns: 線検出の結果 diff --git a/src/pc/vision/overlay.py b/src/pc/vision/overlay.py index 27436f2..f5bb0a5 100644 --- a/src/pc/vision/overlay.py +++ b/src/pc/vision/overlay.py @@ -18,6 +18,10 @@ COLOR_REGION: tuple = (255, 0, 0) COLOR_ROW_CENTER: tuple = (0, 165, 255) COLOR_THEIL_SEN: tuple = (255, 0, 255) +COLOR_PURSUIT: tuple = (255, 255, 0) + +# パシュート目標点の描画半径 +PURSUIT_POINT_RADIUS: int = 2 # 二値化オーバーレイの不透明度 BINARY_OPACITY: float = 0.4 @@ -34,6 +38,7 @@ row_centers: 各行の線中心点 theil_sen: Theil-Sen 近似直線 center_line: 画像中心線 + pursuit_points: 2点パシュートの目標点 """ binary: bool = False detect_region: bool = False @@ -41,12 +46,17 @@ row_centers: bool = False theil_sen: bool = False center_line: bool = False + pursuit_points: bool = False def draw_overlay( frame: np.ndarray, result: LineDetectResult | None, flags: OverlayFlags, + pursuit_points: ( + tuple[tuple[float, float], tuple[float, float]] + | None + ) = None, ) -> np.ndarray: """カメラ映像にオーバーレイを描画する @@ -54,6 +64,8 @@ frame: 元の BGR カメラ画像 result: 線検出の結果（None の場合はオーバーレイなし） flags: 表示項目のフラグ + pursuit_points: 2点パシュートの目標点 + ((near_x, near_y), (far_x, far_y)) Returns: オーバーレイ描画済みの画像 @@ -103,6 +115,10 @@ display, result.row_centers, ) + # 2点パシュートの目標点 + if flags.pursuit_points and pursuit_points is not None: + _draw_pursuit_points(display, pursuit_points) + return display @@ -207,3 +223,29 @@ frame, [pts], False, COLOR_LINE, 1, ) + + +def _draw_pursuit_points( + frame: np.ndarray, + points: tuple[ + tuple[float, float], tuple[float, float] + ], +) -> None: + """2点パシュートの目標点を描画する + + Args: + frame: 描画先の画像 + points: ((near_x, near_y), (far_x, far_y)) + """ + w = frame.shape[1] + for x, y in points: + ix = int(round(x)) + iy = int(round(y)) + if 0 <= ix < w: + cv2.circle( + frame, + (ix, iy), + PURSUIT_POINT_RADIUS, + COLOR_PURSUIT, + -1, + )