diff --git a/.gitignore b/.gitignore index 050c8ab..b4966c7 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,10 @@ # 環境変数 .env +# パラメータ保存 +pd_params.json +params/ + # 旧コード（参照用，Git 管理外） src_old/ diff --git a/CLAUDE.md b/CLAUDE.md index 3d33c8c..58c5331 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -19,11 +19,13 @@ - `docs/02_PLAN/PLAN_01_プロジェクト概要.txt` — 目的・目標・ハードウェア構成 - `docs/03_TECH/TECH_01_操舵量計算仕様.txt` — PD 制御、2領域偏差、速度制御 - `docs/03_TECH/TECH_02_システム構成仕様.txt` — Pi/PC の役割分担、通信フロー、設計方針 +- `docs/03_TECH/TECH_03_デバッグオーバーレイ仕様.txt` — オーバーレイ表示項目、描画色、GUI 操作 +- `docs/03_TECH/TECH_04_線検出精度向上方針.txt` — 線検出が最重要ファクターである理由、照明・影の課題、改善の方向性 ### 環境（セットアップ時に参照） -- `docs/04_ENV/ENV_01_技術スタック選定.txt` — ZMQ, PySide6, OpenCV, Picamera2, RPi.GPIO +- `docs/04_ENV/ENV_01_技術スタック選定.txt` — ZMQ, PySide6, OpenCV, Picamera2, RPi.GPIO, python-dotenv - `docs/04_ENV/ENV_02_PC環境構築手順.txt` — venv 作成、ライブラリインストール -- `docs/04_ENV/ENV_03_RaspPi環境構築手順.txt` — SSH 接続、SCP 転送、Pi への転送対象 +- `docs/04_ENV/ENV_03_RaspPi環境構築手順.txt` — SSH 接続、deploy.sh による転送、venv 構築、動作確認 - `docs/04_ENV/ENV_04_ディレクトリ構成.txt` — src/ の構成と実装状態 ## コーディング規則（要点） @@ -46,3 +48,8 @@ - 書式: `[タグ] 内容`（日本語、末尾句点なし） - タグ: [add], [update], [fix], [remove], [clean] + +## 作業フロー + +- 実装やドキュメント作成の作業が一段落したタイミングで、関連するドキュメントに変更・追記すべき箇所がないかを確認すること +- 確認が済んだら、ユーザにコミットするかを尋ねること diff --git a/deploy.sh b/deploy.sh new file mode 100644 index 0000000..ba1ed4d --- /dev/null +++ b/deploy.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# deploy.sh +# Pi へのファイル転送スクリプト +# Pi 側の対象フォルダを削除してから最新のコードを転送する + +# ── 設定 ────────────────────────────────────────────────── +PI_HOST="user@192.168.23.224" +PI_DIR="/home/user/RobotCar" + +# ── 転送対象 ────────────────────────────────────────────── +# スクリプトの場所を基準にパスを解決する +SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)" +SRC_DIR="${SCRIPT_DIR}/src" + +# ── Pi 側の既存フォルダを削除 ───────────────────────────── +echo "Pi 側のフォルダを初期化中..." +ssh "${PI_HOST}" "rm -rf ${PI_DIR}/common ${PI_DIR}/pi" + +# ── ファイル転送 ────────────────────────────────────────── +echo "common/ を転送中..." +scp -r "${SRC_DIR}/common" "${PI_HOST}:${PI_DIR}/" + +echo "pi/ を転送中..." +scp -r "${SRC_DIR}/pi" "${PI_HOST}:${PI_DIR}/" + +# ── 設定ファイルの転送 ──────────────────────────────────── +echo ".env を転送中..." +scp "${SCRIPT_DIR}/.env" "${PI_HOST}:${PI_DIR}/.env" + +echo "requirements_pi.txt を転送中..." +scp "${SCRIPT_DIR}/requirements_pi.txt" "${PI_HOST}:${PI_DIR}/requirements_pi.txt" + +echo "転送完了" 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 94b66d6..d3d77e5 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" @@ -15,9 +15,11 @@ 1-1. 基本方針 ・制御方式: PD 制御（I 項なしで開始） - ・偏差の算出: 画像の近方・遠方2領域から黒線の重心位置を取得する． - ・操舵量: 偏差に基づく PD 制御で決定する． - ・速度: 操舵量に応じて動的に調整する（カーブ減速，直線加速）． + ・線検出: 二値化画像の白ピクセルに多項式フィッティングし， + 位置偏差・傾き・曲率を算出する． + ・操舵量: 位置偏差と傾きに基づく PD 制御で決定する． + ・速度: 曲率に応じて動的に調整する（カーブ減速，直線加速）． + ・操舵変化: レートリミッターで急激な操舵変化を抑制する． 2. 画像処理パイプライン (Image Processing Pipeline) @@ -27,50 +29,85 @@ 1. カメラからフレームを取得する． 2. グレースケールに変換する． - 3. ガウシアンブラーを適用する（軽いノイズ除去）． - 4. 二値化する（固定閾値 or 大津の方法）． - 5. 近方領域・遠方領域それぞれで黒ピクセルの重心 x 座標を算出する． + 3. CLAHE でコントラストを強調する（照明ムラの影響を低減）． + 4. ガウシアンブラーを適用する（軽いノイズ除去）． + 5. 固定閾値で二値化する（BINARY_INV）． + 6. オープニングで孤立ノイズを除去する． + 7. 横方向クロージングで反射による途切れを補間する． + 8. 検出領域内の白ピクセルに2次多項式をフィッティングする． - 2-2. 2領域方式 + ■ CLAHE（コントラスト制限付き適応ヒストグラム均等化） - カメラは真下を向いているため，画像内の位置が路面上の距離に対応する． - 画像を上下2つの領域に分割し，それぞれで線の位置を検出する． + 照明ムラや反射により黒線と灰色の床の差が小さくなる問題への + 対策として，グレースケール変換後に CLAHE を適用する． + 局所的にコントラストを強調し，黒線と背景の分離を改善する． - 画像上部 ─── 遠方領域（これから通る場所） - │ - 画像下部 ─── 近方領域（今いる場所） + ・clahe_clip: コントラスト増幅の上限（デフォルト: 2.0） + ・clahe_grid: 局所領域の分割数（デフォルト: 8） - ・近方領域: 画像下部の水平帯．現在の車体位置に対する線のずれを示す． - ・遠方領域: 画像上部の水平帯．この先のコース方向を示す． + ■ オープニング（収縮→膨張） - ※ 各領域の具体的な y 座標範囲は実走テストで調整する． + 影や汚れにより発生する孤立した小さなノイズピクセルを除去する． + 小さい楕円カーネルで収縮→膨張を行い，小さな塊を消す． + + ・open_size: カーネルサイズ（デフォルト: 5） + ・効果: 線とつながっていない孤立ピクセルを除去 + + ■ 横方向クロージング（膨張→収縮） + + 光の反射により線が途切れる問題への対策として， + 二値化後にモルフォロジーのクロージング処理を行う． + 横長の楕円カーネルを使用することで，縦方向への影響を抑えつつ + 線の途切れを左右から埋める． + + ・close_width: カーネルの横幅（デフォルト: 25） + ・close_height: カーネルの高さ（デフォルト: 3） + ・効果: 反射で欠けた部分を，左右の検出済みピクセルからつなぐ + + 2-2. 多項式フィッティング + + 検出領域（画像全体）内の白ピクセル座標に対して， + 2次多項式 x = f(y) = ay² + by + c をフィッティングする． + これにより線全体の形状を1つの式で表現し，以下の情報を得る． + + ・位置偏差: 画像下端での x 座標と画像中心の差 + ・傾き（ヘディング）: 画像下端での dx/dy + ・曲率: d²x/dy²（2次多項式では定数 = 2a） 3. 偏差の算出 (Error Calculation) ------------------------------------------------------------------------ - 3-1. 基本偏差 + 3-1. 位置偏差 - 各領域で検出した黒線の重心 x 座標と，画像中心 x 座標との差を - 偏差とする． + 多項式を画像下端で評価し，画像中心との差を正規化する． - e_near = image_center_x - near_line_x - e_far = image_center_x - far_line_x + position_error = (image_center_x - f(y_bottom)) + / image_center_x - ・e > 0: 線が画像の左側にある（右に寄っている） - ・e < 0: 線が画像の右側にある（左に寄っている） - ・e = 0: 線が画像の中心にある（理想状態） + ・position_error > 0: 線が画像の左側にある + ・position_error < 0: 線が画像の右側にある + ・position_error = 0: 線が画像の中心にある - 3-2. 制御用偏差 + 3-2. 傾き（ヘディング） - 近方偏差と遠方偏差を重み付きで合成し，制御に使用する偏差とする． + 画像下端での多項式の1次微分値を傾きとする． - e = α * e_near + β * e_far + heading = dx/dy |_(y=y_bottom) - ・α: 近方の重み（現在の位置補正） - ・β: 遠方の重み（先読み補正） - ・α + β = 1.0 とする． - ※ α，β の具体的な値は実走テストで調整する． + ・heading > 0: 先（画像上方）に向かって線が左に曲がっている + ・heading < 0: 先に向かって線が右に曲がっている + ・効果: カーブの方向を事前に把握し，先読み操舵できる + + 3-3. 曲率 + + 多項式の2次微分値を曲率とする． + + curvature = d²x/dy² + + ・曲率が大きい: 急カーブ + ・曲率が小さい: 緩やかなカーブまたは直線 + ・用途: 速度制御に使用（急カーブで減速） 4. PD 制御 (PD Control) @@ -78,22 +115,35 @@ 4-1. 操舵量の計算 - steer = Kp * e + Kd * (e - e_prev) / dt + error = Kp * position_error + Kh * heading + steer = error + Kd * (error - error_prev) / dt - ・Kp: 比例ゲイン．偏差に比例した操舵量を出力する． + ・Kp: 位置偏差ゲイン．偏差に比例した操舵量を出力する． - 大きいほど応答が速いが，振動しやすい． + ・Kh: 傾きゲイン．線の傾きに比例した先読み操舵を出力する． + - カーブの方向を検知し，事前にステアリングを切る． ・Kd: 微分ゲイン．偏差の変化率に比例した操舵量を出力する． - 振動を抑制し，カーブへの追従を滑らかにする． - ・e_prev: 前フレームの偏差． + ・error_prev: 前フレームの error． ・dt: 前フレームからの経過時間． - 4-2. 操舵量の制限 + 4-2. レートリミッター + + 1フレームあたりの操舵量の変化を max_steer_rate 以内に制限する． + 感度を高く設定しても急激な操舵変化を防ぎ， + コースアウトのリスクを低減する． + + delta = clamp(steer - steer_prev, + -max_steer_rate, max_steer_rate) + steer = steer_prev + delta + + 4-3. 操舵量の制限 計算結果を -1.0 ~ +1.0 の範囲にクランプする． steer = clamp(steer, -1.0, +1.0) - 4-3. I 項について + 4-4. I 項について 初期段階では I 項を使用しない． 理由: カーブ中に偏差が蓄積し，カーブ出口でオーバーシュート @@ -105,47 +155,78 @@ 5. 速度制御 (Speed Control) ------------------------------------------------------------------------ - 5-1. 基本方式（操舵量連動） + 5-1. 曲率連動方式 - throttle = max_throttle - k * |steer| + throttle = max_throttle - speed_k * |curvature| ・max_throttle: 直線での最大速度． - ・k: 減速係数．操舵量が大きいほど減速する． - ・|steer| が大きい → カーブ中 → 減速 - ・|steer| が小さい → 直線 → 加速 + ・speed_k: 減速係数．曲率が大きいほど減速する． + ・|curvature| が大きい → 急カーブ → 減速 + ・|curvature| が小さい → 直線 → 加速 - 5-2. 発展方式（曲率推定連動） - - 基本方式で速度の限界を感じた場合，近方・遠方の偏差差分から - 曲率を推定し，カーブの手前で事前に減速する方式へ移行する． - - curvature = |e_far - e_near| - throttle = max_throttle - k * curvature - - ※ 基本方式で完走を達成した後に検討する． + 多項式フィッティングにより曲率を直接算出できるため， + カーブの手前で事前に減速できる． 6. パラメータ一覧 (Parameters) ------------------------------------------------------------------------ - ■ 画像処理パラメータ + ■ 画像処理パラメータ（GUI で調整可能） - ・画像解像度: 未定（既存設定は 320x240） - ・二値化閾値: 実走テストで決定 - ・近方領域の y 範囲: 実走テストで決定 - ・遠方領域の y 範囲: 実走テストで決定 + GUI のコンボボックスで検出手法を切り替えられる． + 手法ごとに使用するパラメータが異なり， + GUI では選択中の手法に関連するパラメータのみ表示される． + 詳細は `TECH_04_線検出精度向上方針.txt` を参照する． - ■ 偏差合成パラメータ + 1. 現行手法（CLAHE + 固定閾値） + ・画像解像度: 320x240 + ・CLAHE 強度（clahe_clip）: 2.0 + ・CLAHE 分割数（clahe_grid）: 8 + ・ブラーカーネルサイズ（blur_size）: 5 + ・二値化閾値（binary_thresh）: 80 + ・オープニングサイズ（open_size）: 5 + ・クロージング横幅（close_width）: 25 + ・クロージング高さ（close_height）: 3 - ・α（近方の重み）: 実走テストで決定 - ・β（遠方の重み）: 実走テストで決定 + 2. 案A: Black-hat 中心型 + ・Black-hat カーネルサイズ（blackhat_ksize）: 45 + ・二値化閾値（binary_thresh）: 80 + ・等方クロージングサイズ（iso_close_size）: 15 + ・距離変換閾値（dist_thresh）: 3.0 + ・最小線幅（min_line_width）: 3 - ■ PD 制御パラメータ + 3. 案B: 二重正規化型 + ・背景ブラーカーネルサイズ（bg_blur_ksize）: 101 + ・適応的閾値ブロックサイズ（adaptive_block）: 51 + ・適応的閾値定数 C（adaptive_c）: 10 + ・等方クロージングサイズ（iso_close_size）: 15 + ・距離変換閾値（dist_thresh）: 3.0 + ・最小線幅（min_line_width）: 3 - ・Kp（比例ゲイン）: 実走テストで決定 - ・Kd（微分ゲイン）: 実走テストで決定 + 4. 案C: 最高ロバスト型 + ・Black-hat カーネルサイズ（blackhat_ksize）: 45 + ・適応的閾値ブロックサイズ（adaptive_block）: 51 + ・適応的閾値定数 C（adaptive_c）: 10 + ・等方クロージングサイズ（iso_close_size）: 15 + ・距離変換閾値（dist_thresh）: 3.0 + ・最小線幅（min_line_width）: 3 + ・RANSAC 閾値（ransac_thresh）: 5.0 - ■ 速度制御パラメータ + ■ PD 制御パラメータ（GUI で調整可能） - ・max_throttle（最大速度）: 実走テストで決定 - ・k（減速係数）: 実走テストで決定 + ・Kp（位置偏差ゲイン）: 0.5 + ・Kh（傾きゲイン）: 0.3 + ・Kd（微分ゲイン）: 0.1 + ・max_steer_rate（最大操舵変化量）: 0.1 + + ■ 速度制御パラメータ（GUI で調整可能） + + ・max_throttle（最大速度）: 0.4 + ・speed_k（曲率減速係数）: 0.3 + + ■ パラメータ管理 + + ・全パラメータ（画像処理 + PD 制御 + 速度制御）を + タイトル・メモ付きで JSON ファイルに保存できる． + ・GUI のコンボボックスで保存済みパラメータを選択・読み込み可能． + ・保存ファイル: pd_params.json（.gitignore に登録済み） 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 afdc5bf..14bd004 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" @@ -72,15 +72,21 @@ ・受信した画像から黒線の位置を検出する． ・処理手順: 1. グレースケール変換 - 2. ガウシアンブラー（ノイズ除去） - 3. 二値化 - 4. 近方・遠方2領域で黒ピクセルの重心 x 座標を算出 + 2. CLAHE によるコントラスト強調 + 3. ガウシアンブラー（ノイズ除去） + 4. 固定閾値で二値化 + 5. オープニングで孤立ノイズ除去 + 6. 横方向クロージングで途切れ補間 + 7. 白ピクセルに2次多項式フィッティング + ・位置偏差・傾き・曲率を算出する． ・詳細は `TECH_01_操舵量計算仕様.txt` を参照する． 3-3. 操舵量計算 - ・検出した線の位置から偏差を算出し，PD 制御で操舵量を計算する． - ・速度は操舵量に応じて動的に調整する． + ・多項式フィッティングから得た位置偏差と傾きで + PD 制御により操舵量を計算する． + ・速度は曲率に応じて動的に調整する． + ・レートリミッターで急激な操舵変化を抑制する． ・詳細は `TECH_01_操舵量計算仕様.txt` を参照する． 3-4. 操舵量の送信 @@ -91,7 +97,8 @@ ■ カメラ映像表示 ・受信した画像をリアルタイムで表示する． - ・画像処理の結果（検出した線の位置，偏差等）をオーバーレイ表示する． + ・デバッグオーバーレイを重ねて表示できる． + ・詳細は `TECH_03_デバッグオーバーレイ仕様.txt` を参照する． ■ 自動操縦の切り替え ・自動操縦の ON / OFF を切り替えるボタンを設ける． @@ -99,9 +106,16 @@ ・OFF: 手動操作モードに切り替わる． ■ パラメータ調整 - ・PD 制御パラメータ（Kp，Kd 等）をリアルタイムに変更できる + ・PD 制御パラメータ（Kp，Kh，Kd 等）をリアルタイムに変更できる UI を設ける． - ・変更したパラメータは即座に操舵量計算に反映される． + ・画像処理パラメータ（二値化閾値，CLAHE 強度等）も + リアルタイムに変更できる． + ・変更したパラメータは即座に処理に反映される． + + ■ パラメータ保存・読み込み + ・調整したパラメータをタイトル・メモ付きで JSON に保存できる． + ・保存済みパラメータをコンボボックスから選択・読み込みできる． + ・確認ダイアログ付きの削除機能を備える． ■ 手動操作 ・自動操縦 OFF 時に，ユーザーが手動で車体を操作できる． @@ -128,7 +142,7 @@ 5. 通信の流れ (Communication Flow) ------------------------------------------------------------------------ - 4-1. 全体のループ + 5-1. 全体のループ 以下のサイクルを毎フレーム繰り返す． @@ -139,7 +153,7 @@ 5. PC → Pi: 操舵量（throttle，steer）を送信する． 6. Pi: 受信した操舵量でモーターを制御する． - 4-2. 通信要件 + 5-2. 通信要件 ・双方向通信: Pi → PC（画像），PC → Pi（操舵量）． ・低遅延: 画像取得からモーター反映までの遅延を最小限にする． 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" new file mode 100644 index 0000000..3793ab1 --- /dev/null +++ "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" @@ -0,0 +1,55 @@ +======================================================================== +デバッグオーバーレイ仕様 (Debug Overlay Specification) +======================================================================== + + +1. 概要 (Overview) +------------------------------------------------------------------------ + + 1-0. 目的 + + 画像処理パイプラインの動作を視覚的に確認するためのオーバーレイ + 表示機能を定義する．GUI のチェックボックスで項目ごとに ON/OFF + できる． + + 1-1. 基本方針 + + ・オーバーレイはカメラ映像に重ねて描画する． + ・手動操作中でもオーバーレイが有効なら線検出を実行する． + ・自動操縦中は操舵量計算で実行済みの検出結果を再利用する． + + +2. 表示項目 (Overlay Items) +------------------------------------------------------------------------ + + 2-1. 一覧 + + ・二値化画像: 二値化結果を赤色の半透明で重ねる（不透明度 0.4） + ・検出領域: 検出対象領域の枠を青色で表示 + ・フィッティング曲線: 多項式の曲線を緑色で描画 + ・中心線: 画像の中心 x に垂直線を描画（黄色） + ・検出情報: 位置偏差・傾き・曲率の数値を画像左上に表示 + + 2-2. 描画色 (BGR) + + ・フィッティング曲線: (0, 255, 0) 緑 + ・中心線: (0, 255, 255) 黄 + ・検出領域: (255, 0, 0) 青 + ・テキスト: (255, 255, 255) 白 + ・二値化オーバーレイ: 赤チャンネルに二値化画像を割り当て + + +3. GUI 操作 (GUI Controls) +------------------------------------------------------------------------ + + 3-1. チェックボックス + + コントロールパネルに「デバッグ表示」グループを設け， + 各表示項目に対応するチェックボックスを配置する． + チェックの ON/OFF で即時に表示が切り替わる． + + 3-2. 動作モードとの関係 + + ・手動操作中: オーバーレイが 1 つでも ON なら線検出を実行する + ・自動操縦中: 操舵量計算の線検出結果をそのまま使用する + ・未接続時: オーバーレイは表示されない（映像がないため） diff --git "a/docs/03_TECH/TECH_04_\347\267\232\346\244\234\345\207\272\347\262\276\345\272\246\345\220\221\344\270\212\346\226\271\351\207\235.txt" "b/docs/03_TECH/TECH_04_\347\267\232\346\244\234\345\207\272\347\262\276\345\272\246\345\220\221\344\270\212\346\226\271\351\207\235.txt" new file mode 100644 index 0000000..afea42c --- /dev/null +++ "b/docs/03_TECH/TECH_04_\347\267\232\346\244\234\345\207\272\347\262\276\345\272\246\345\220\221\344\270\212\346\226\271\351\207\235.txt" @@ -0,0 +1,787 @@ +======================================================================== +線検出精度向上方針 (Line Detection Accuracy Improvement) +======================================================================== + + +1. 概要 (Overview) +------------------------------------------------------------------------ + + 1-0. 目的 + + 黒線の検出精度がコースアウトの有無を左右する最重要ファクターで + あることを示し，現在の手法の課題と改善手法の選択肢を定義する． + + 1-1. 線検出がコースアウトの最重要ファクターである理由 + + 自律走行の制御フローは以下の通りである． + + カメラ画像 → 線検出 → position_error/heading/curvature + → PD 制御 → 操舵量 + + PD 制御以降はいかにパラメータを調整しても，入力である + position_error が誤っていれば正しい操舵は得られない． + すなわち，線検出の精度がシステム全体の制御品質の上限を決める． + + ・線が正しく検出できる → 正確な偏差 → 正確な操舵 + ・線の検出が崩れる → 誤った偏差 → 反対方向への操舵 → コースアウト + + 速度を上げるほど 1 フレームあたりの移動距離が増えるため， + 検出の崩れが即コースアウトにつながる．高速化と精度向上は + 不可分の関係にある． + + 1-2. パイプライン構成 + + 線検出パイプラインは 4 段階で構成される． + 各段階ごとに手法を選択・組み合わせることで精度を向上させる． + + [Stage 1] 前処理（照明正規化） + [Stage 2] 二値化（線と背景の分離） + [Stage 3] 後処理（穴埋め・ノイズ除去・幅正規化） + [Stage 4] 特徴抽出（中心線 → 多項式フィッティング） + + +2. 検出対象の特性 (Target Characteristics) +------------------------------------------------------------------------ + + 2-1. 黒線の物理的特性 + + 本システムの走行コースは，道路の白線に近い性質を持つ黒線である． + + ・形状: 幅一定の単一直線（カーブも含むが幾何学的に連続） + ・色: 高輝度の床面に対して明確に暗い（理想条件下） + ・連続性: 途切れがなく，2次多項式で十分近似できる + ・幅: 画像上で数ピクセル〜数十ピクセル程度 + + 2-2. 理想条件と実環境の乖離 + + 理想条件（均一な拡散光，均質な床面）では固定閾値の2値化でも + 明確に検出できる．しかし実環境では以下の要因が存在する． + + ・光源の影響 + - 蛍光灯・LED 照明による局所的な輝度ムラ + - 窓からの外光による時間変動 + - カメラの自動露出による全体輝度の変動 + + ・影の影響 + - ロボット車体自体が床に影を落とす + - 床の継ぎ目・段差による輝度の乱れ + + ・床面の影響 + - 床材の光沢による反射スポット + - 黒線上の光沢（テープの反射） + + これらにより，黒線と背景の輝度差が局所的に縮小・逆転し， + 固定閾値の2値化では正しい線形状を得られないことがある． + + 2-3. カメラ高さ・線幅の固定による幾何学的制約 + + 本システムでは以下の2点が実走行中に変化しない． + + ・カメラの地面からの高さ（固定マウント） + ・黒線の実際の幅（コース仕様による固定値） + + 透視投影の関係から，行（y 座標）が変わると + カメラから黒線までの距離が変化し，画像上の線幅（ピクセル）も変わる． + + 近距離（画像下端，y = Y_END）: 線幅 = width_near [px] + 遠距離（画像上端，y = Y_START）: 線幅 = width_far [px] + + この関係は画像の y 座標を入力として線形補間で近似できる． + + expected_width(y) = width_near + + (width_far - width_near) * (Y_END - y) / (Y_END - Y_START) + + この既知の幾何学情報を利用することで，以下が可能になる． + + ・各行の「正常な線幅」の範囲を事前に計算できる + ・陰による線幅の広がりを行ごとに検出できる + ・モルフォロジーパラメータ（Black-hat カーネル，等方クロージングサイズ等） + を線幅から自動的に決定でき，手動調整の余地を減らせる + + ■ width_near / width_far の測定方法 + + 実際のカメラ映像の二値化画像を観察し， + 画像下端付近の行と上端付近の行それぞれで白ピクセルの幅を計測する． + 計測後はコードまたはパラメータファイルに固定値として記録する． + + 2-4. 実環境で発生する2つの典型的な劣化 + + ■ 穴（光による欠損） + + 黒線テープの表面が光を反射し，反射部分の輝度が背景と同程度に + なるため，二値化後に線の中央が白抜け（穴）になる現象． + 多項式フィッティングの点が欠損し，検出が不安定になる． + + ■ 広がり（陰による膨張） + + 車体の影や照明ムラにより黒線の周囲の床が暗くなり， + 二値化後に線の幅が実際より広がる現象． + 広がった行はピクセル数が多いためフィッティングを支配し， + 線の位置がずれる原因になる． + + +3. 現在の手法と限界 (Current Approach and Limitations) +------------------------------------------------------------------------ + + 3-1. 現在の処理パイプライン + + ・Stage 1（前処理）: グレースケール → CLAHE → ガウシアンブラー + ・Stage 2（二値化）: 固定閾値（BINARY_INV） + ・Stage 3（後処理）: オープニング → 横方向クロージング + ・Stage 4（特徴抽出）: 全白ピクセルに2次多項式フィッティング + + 詳細は `TECH_01_操舵量計算仕様.txt` の + 「2. 画像処理パイプライン」を参照する． + + 3-2. 現在の対策とその効果・限界 + + ■ CLAHE（局所コントラスト強調） + + ・効果: 局所的な輝度ムラを補正し，黒線と背景の差を拡大する + ・限界: 輝度差が完全になくなった領域では効果がない + + ■ 固定閾値二値化 + + ・効果: 実装がシンプルで処理が高速 + ・限界: 1つの閾値で画像全体を判定するため， + 照明ムラが大きい場合に不均一な分離が生じる + - 閾値を低くする → 反射スポットが黒線として誤検出 + - 閾値を高くする → 影の下の黒線が検出されない + + ■ モルフォロジー処理 + + ・効果: 孤立ノイズの除去（オープニング）， + 線の途切れの補間（横方向クロージング） + ・限界: 線の位置が大きくずれたノイズには対処できない． + 誤検出された大きな塊はフィッティングを大幅に歪める + + ■ 全白ピクセルフィッティング + + ・効果: 実装が単純 + ・限界: 陰で幅が広がった行はピクセル数が多く， + フィッティングへの寄与が大きいため線の位置がずれる + + 3-3. 残存する課題 + + ・強い照明ムラ環境では固定閾値が不安定になりやすい + ・カメラの自動露出でシーンが変わると適切な閾値が変動する + ・黒線上に光沢がある場合，線の中央が白抜けして線幅が細くなる + ・陰による幅の広がりが全白ピクセルフィッティングを歪める + + +4. Stage 1: 前処理の手法比較 (Pre-processing) +------------------------------------------------------------------------ + + 4-0. 目的 + + 照明ムラや輝度変動の影響を低減し，後段の二値化を安定させる． + + 4-1. CLAHE（現在の手法） + + 局所領域ごとにヒストグラム均等化を行い，コントラストを強調する． + + ・計算量: 低 + ・穴への効果: △（反射が強い場合は輝度差を復元できない） + ・陰への効果: ○（局所コントラスト向上で境界が明確になる） + ・実装: `cv2.createCLAHE(clipLimit, tileGridSize)` + ・備考: 現在の手法．照明ムラには有効だが根本的な正規化ではない + + 4-2. 背景除算正規化 + + 画像を大きなカーネルでぼかした画像で割り， + 照明の勾配（低周波成分）を除去する． + + ・計算量: 低（ガウシアンブラー1回 + 除算） + ・穴への効果: ○（局所的な高輝度を正規化できる） + ・陰への効果: ◎（照明勾配が除去されるため暗い領域も正規化） + ・実装: + + blur_bg = cv2.GaussianBlur(gray, (ksize, ksize), 0) + normalized = (gray.astype(np.float32) * 255.0 + / (blur_bg.astype(np.float32) + 1.0)) + normalized = np.clip(normalized, 0, 255).astype(np.uint8) + + ・パラメータ: ksize は線幅の 10 倍程度（大きいほど広い照明ムラに対応） + ・備考: 照明の勾配が緩やかな場合に最も効果的． + 急激な明暗境界（影の縁）には効きにくい + + 4-3. Black-hat 変換 + + モルフォロジーのクロージング結果から原画像を引くことで， + 「背景より暗い構造」だけを直接抽出する． + + ・計算量: 低（モルフォロジー演算1回） + ・穴への効果: ○（暗い構造として線全体を検出できる） + ・陰への効果: ◎（背景の輝度変動が除去される） + ・実装: + + kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (ksize, ksize)) + blackhat = cv2.morphologyEx( + gray, cv2.MORPH_BLACKHAT, kernel) + + ・パラメータ: ksize は線幅の 2〜3 倍（線より大きく， + 影より小さいサイズ） + ・備考: 黒線検出に原理的に最もフィットした手法． + 「背景に対してどれだけ暗いか」を直接出力するため， + 背景の絶対輝度に依存しない + + 4-4. ホモモルフィックフィルタ + + 画像の対数を取り，FFT で低周波（照明成分）と + 高周波（反射率成分）を分離し，照明成分を抑制する． + + ・計算量: 中（FFT + 逆 FFT） + ・穴への効果: ○（照明成分を分離するため反射の影響が減る） + ・陰への効果: ◎（照明成分の除去が原理的に正確） + ・実装: `np.fft.fft2` + ハイパスフィルタ + `np.fft.ifft2` + ・備考: 理論的には最も正確だが，FFT の計算量が + リアルタイム処理に影響する可能性がある． + 320x240 であれば実用範囲内 + + 4-5. LAB 色空間 L チャネル + + BGR → LAB 変換し，知覚均等な輝度チャネル L を使用する． + + ・計算量: 極低（色変換のみ） + ・穴への効果: △（グレースケールと大差ない） + ・陰への効果: △（輝度の表現が若干改善される程度） + ・実装: `cv2.cvtColor(frame, cv2.COLOR_BGR2LAB)[:,:,0]` + ・備考: 単体での効果は限定的．他の手法と組み合わせて使う + + +5. Stage 2: 二値化の手法比較 (Binarization) +------------------------------------------------------------------------ + + 5-0. 目的 + + 前処理後の画像から黒線と背景を分離する． + + 5-1. 固定閾値（現在の手法） + + 1つの閾値で画像全体を二値化する． + + ・計算量: 極低 + ・穴への効果: ✕（反射で輝度が上がった部分は検出できない） + ・陰への効果: ✕（影で暗くなった床を誤検出する） + ・実装: `cv2.threshold(src, thresh, 255, THRESH_BINARY_INV)` + ・備考: Stage 1 で照明が正規化されていれば十分機能する． + Stage 1 が弱い場合は照明ムラに脆弱 + + 5-2. 大津の方法（Otsu） + + ヒストグラムからクラス間分散を最大化する閾値を自動決定する． + + ・計算量: 極低（ヒストグラム計算のみ） + ・穴への効果: ✕（グローバル閾値のため局所問題に弱い） + ・陰への効果: △（全体の明暗変化には追従する） + ・実装: `cv2.threshold(src, 0, 255, + THRESH_BINARY_INV + THRESH_OTSU)` + ・備考: 閾値の手動調整が不要になる利点がある． + ただし局所的な照明ムラには固定閾値と同様に弱い + + 5-3. 適応的閾値（ガウシアン加重平均） + + 各ピクセルの周囲 blockSize x blockSize 領域の + ガウシアン加重平均から閾値を算出する． + + ・計算量: 低（積分画像ベース） + ・穴への効果: ○（局所的に閾値が変わるため反射領域にも対応） + ・陰への効果: ○（影のある領域で閾値が下がるため追従できる） + ・実装: + + cv2.adaptiveThreshold( + src, 255, + cv2.ADAPTIVE_THRESH_GAUSSIAN_C, + cv2.THRESH_BINARY_INV, + blockSize, C) + + ・パラメータ: blockSize（局所領域サイズ，奇数）， + C（閾値からの減算定数） + ・備考: 局所照明ムラへの耐性が固定閾値より大幅に向上する． + Stage 1 が弱い場合の有力な選択肢 + + 5-4. 適応的閾値（平均値） + + ガウシアン加重ではなく単純平均で閾値を算出する． + + ・計算量: 低 + ・穴への効果: ○ + ・陰への効果: △（ガウシアンより若干ノイズに弱い） + ・実装: `cv2.ADAPTIVE_THRESH_MEAN_C` を指定 + ・備考: ガウシアン版より高速だが精度はやや劣る + + 5-5. Sauvola 閾値 + + 局所の平均値と標準偏差から閾値を計算する． + コントラストが低い領域（= 線がない領域）を自動的に + 背景として扱える特性がある． + + ・計算量: 低〜中（積分画像 + 二乗積分画像） + ・穴への効果: ○ + ・陰への効果: ◎（低コントラスト領域を無視できる） + ・実装: OpenCV 標準にないため自前実装が必要 + + mean = cv2.blur(src, (k, k)) + sq_mean = cv2.blur( + (src.astype(np.float32)) ** 2, (k, k)) + std = np.sqrt(sq_mean - mean.astype(np.float32) ** 2) + thresh = mean * (1.0 + coeff * (std / 128.0 - 1.0)) + + ・備考: 文書画像の文字検出で実績が高い． + 黒線検出にも適している + + 5-6. 行ごと Otsu + + 画像を行（または行のブロック）単位で分割し， + 行ごとに Otsu の方法で閾値を決定する． + + ・計算量: 低（行数分の Otsu 計算） + ・穴への効果: ○（行単位で閾値が変わる） + ・陰への効果: ○（行ごとの照明変化に追従できる） + ・実装: for ループで行ブロックごとに `cv2.threshold` + Otsu + ・備考: 縦方向の照明勾配に強い．横方向のムラには弱い + + +6. Stage 3: 後処理の手法比較 (Post-processing) +------------------------------------------------------------------------ + + 6-0. 目的 + + 二値化後の画像から穴を埋め，ノイズを除去し， + 線の幅を正規化して特徴抽出の精度を向上させる． + + 6-1. オープニング（現在の手法） + + 収縮 → 膨張で孤立した小さなノイズピクセルを除去する． + + ・計算量: 極低 + ・穴への効果: -（穴の除去には寄与しない） + ・陰への効果: △（小さな誤検出は除去できる） + ・備考: 現在の手法．孤立ノイズ除去として引き続き有効 + + 6-2. 横方向クロージング（現在の手法） + + 膨張 → 収縮で線の横方向の途切れを補間する． + + ・計算量: 極低 + ・穴への効果: ○（横方向の途切れを埋める） + ・陰への効果: - + ・備考: 現在の手法．横長の穴には有効だが丸い穴には効きにくい + + 6-3. 等方クロージング + + 円形カーネルによるクロージングで， + 方向を問わず穴（光スポット等）を埋める． + + ・計算量: 極低 + ・穴への効果: ◎（丸い穴にも対応できる） + ・陰への効果: - + ・実装: + + hole_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (hole_size, hole_size)) + binary = cv2.morphologyEx( + binary, cv2.MORPH_CLOSE, hole_kernel) + + ・パラメータ: hole_size は想定される穴の最大径 + ・備考: 横方向クロージングの代替または補完として使用する + + 6-4. 距離変換 + 閾値マスク + + 二値画像の各白ピクセルについて，最も近い黒ピクセルまでの + 距離を算出し，閾値以上の距離（= 中心部）だけを残す． + + ・計算量: 低 + ・穴への効果: -（穴の除去には寄与しない．先にクロージングが必要） + ・陰への効果: ◎（陰で広がった外縁は距離が短いため除去される） + ・実装: + + dist = cv2.distanceTransform( + binary, cv2.DIST_L2, 5) + _, center_mask = cv2.threshold( + dist, half_width_px, 255, cv2.THRESH_BINARY) + + ・パラメータ: half_width_px は黒線の幅の半分（ピクセル単位） + ・備考: 陰による幅の広がりを削り取る最も直接的な手法． + クロージングで穴を埋めた後に適用すると効果的 + + 6-5. スケルトン化（細線化） + + 二値画像の塊を反復的に収縮し， + 1ピクセル幅の中心線（骨格）を抽出する． + + ・計算量: 中（反復処理のため塊の幅に比例） + ・穴への効果: -（穴があると骨格が分断される．先にクロージングが必要） + ・陰への効果: ◎（幅に関わらず中心線が得られる） + ・実装: `cv2.ximgproc.thinning(binary)` + ・依存: `opencv-contrib-python` の `ximgproc` モジュールが必要 + ・備考: 幅の正規化としては最も確実だが， + 追加依存と計算量を考慮する必要がある + + 6-6. 連結成分フィルタ + + 連結成分分析で各塊の面積・アスペクト比を算出し， + 線として不適切な塊（小さすぎる，横に広すぎる等）を除去する． + + ・計算量: 低 + ・穴への効果: - + ・陰への効果: ○（大きな誤検出塊を面積で除去できる） + ・実装: + + n, labels, stats, _ = cv2.connectedComponentsWithStats( + binary) + # stats で面積・幅・高さを確認し， + # 異常な塊のラベルを 0（背景）に置換 + + ・備考: 線から離れた場所の大きな誤検出に有効． + 線に隣接した陰は連結成分が線と結合するため効かない + + 6-7. 幅フィルタ（行ごと，固定閾値） + + 各行の白ピクセルの幅（右端 - 左端）を計算し， + 期待される線幅の範囲外の行を除外する． + + ・計算量: 極低 + ・穴への効果: - + ・陰への効果: ◎（幅が広すぎる行を直接除外できる） + ・実装: + + for y in range(height): + xs = np.where(binary[y] > 0)[0] + if len(xs) > 0: + width = xs[-1] - xs[0] + 1 + if width > max_line_width: + binary[y] = 0 # 幅が異常な行を除去 + + ・パラメータ: max_line_width は線幅の最大許容値（ピクセル） + ・備考: 実装が最も簡単で陰への効果が高い． + ただし単一の固定閾値では透視投影により近距離の行で + 上限が厳しすぎる，遠距離の行では甘すぎる問題がある． + 透視補正付き（6-8 節）を推奨する + + 6-8. 幅フィルタ（行ごと，透視補正付き） + + 「2-3. カメラ高さ・線幅の固定による幾何学的制約」で述べた + 線幅の透視変化を考慮し，各行の期待幅を線形補間で算出して + フィルタ閾値を行ごとに動的に決定する． + + ・計算量: 極低 + ・穴への効果: -（幅フィルタは穴の補間をしない） + ・陰への効果: ◎（行ごとに正確な上限を設定できる） + ・実装: + + def _apply_width_filter( + binary: np.ndarray, + width_near: int, # 画像下端での期待線幅 [px] + width_far: int, # 画像上端での期待線幅 [px] + tolerance: float, # 上限倍率（例: 1.8） + ) -> np.ndarray: + result = binary.copy() + h = binary.shape[0] + for y_local in range(h): + xs = np.where(binary[y_local] > 0)[0] + if len(xs) == 0: + continue + # 行ごとの期待幅を線形補間 + t = (h - 1 - y_local) / max(h - 1, 1) + expected = width_far + (width_near - width_far) * (1.0 - t) + max_w = expected * tolerance + actual_w = int(xs[-1]) - int(xs[0]) + 1 + if actual_w > max_w: + result[y_local] = 0 + return result + + ・パラメータ: + - width_near: 画像下端での期待線幅（実測値，px） + - width_far: 画像上端での期待線幅（実測値，px） + - width_tolerance: 上限倍率（推奨 1.8〜2.0） + + ・備考: width_near / width_far は実際の二値化画像から一度計測して + 固定値に設定する．以後はパラメータ調整が不要になる． + 陰の片側広がりは除外されずに中心がずれる弱点は残るが， + 距離変換マスク（6-4 節）と組み合わせることで軽減できる． + 6-7 よりも適用精度が高く，こちらを優先して使用すること + + +7. Stage 4: 特徴抽出の手法比較 (Feature Extraction) +------------------------------------------------------------------------ + + 7-0. 目的 + + 二値化・後処理後の画像から線の位置・傾き・曲率を算出する． + + 7-1. 全白ピクセルフィッティング（現在の手法） + + 検出された全白ピクセルの (y, x) 座標に + 2次多項式 x = f(y) をフィッティングする． + + ・計算量: 低 + ・穴への効果: △（点が欠損した行の影響は小さいが精度は下がる） + ・陰への効果: ✕（幅が広い行のピクセルがフィッティングを支配する） + ・実装: `np.polyfit(ys, xs, 2)` + ・備考: 現在の手法．Stage 3 で幅が正規化されていれば有効だが， + 陰の影響を受けやすい根本的な弱点がある + + 7-2. 行ごと中心抽出 + フィッティング + + 各行の白ピクセル群の中心（平均 x）を1点ずつ抽出し， + 中心点列に対してフィッティングする． + + ・計算量: 極低 + ・穴への効果: ○（穴のある行は中心が計算できず除外される） + ・陰への効果: ◎（幅が広がっても中心位置はほぼ変わらない． + 各行が等しく1票なので幅による支配が発生しない） + ・実装: + + centers_y, centers_x = [], [] + for y in range(height): + xs = np.where(binary[y] > 0)[0] + if len(xs) >= min_line_width: + centers_y.append(y) + centers_x.append(float(np.mean(xs))) + coeffs = np.polyfit(centers_y, centers_x, 2) + + ・備考: コード変更が最小で効果が最大の改善． + 幅による重み付けの偏りを根本的に解消する + + 7-3. 行ごと中央値抽出 + フィッティング + + 7-2 の亜種．平均値の代わりに中央値を使用する． + + ・計算量: 極低 + ・穴への効果: ○ + ・陰への効果: ◎（中央値は外れ値に更に強い） + ・実装: `np.mean(xs)` を `np.median(xs)` に変更 + ・備考: 陰が線の片側だけに広がった場合に平均値より頑健 + + 7-4. RANSAC フィッティング + + ランダムにサンプルした点から仮モデルを作り， + 外れ値を除去しながらフィッティングする． + + ・計算量: 中（反復回数 x サンプル数に比例） + ・穴への効果: ◎（欠損があっても外れ値として除外） + ・陰への効果: ◎（外れ値（= 陰のピクセル）を直接除外） + ・実装: `sklearn.linear_model.RANSACRegressor` または自前実装 + ・依存: scikit-learn を使う場合は追加依存が発生 + ・備考: 最もロバストだが計算量が最大． + 行ごと中心抽出で十分な場合はオーバースペック + + 7-5. 重み付きフィッティング + + 距離変換値（= 線の中心からの距離）を重みとして + フィッティングに使用する．中心に近いピクセルほど重みが大きい． + + ・計算量: 低 + ・穴への効果: ○ + ・陰への効果: ○（外縁のピクセルの重みが自動的に小さくなる） + ・実装: `np.polyfit(ys, xs, 2, w=weights)` + ・備考: 全ピクセルを使いつつ陰の影響を低減できる折衷案 + + 7-6. スプライン補間 + + 2次多項式の代わりにスプライン曲線を使用する． + + ・計算量: 低 + ・穴への効果: ○ + ・陰への効果: ○ + ・実装: `scipy.interpolate.UnivariateSpline` + ・依存: scipy（多くの場合すでにインストール済み） + ・備考: S 字カーブ等，2次多項式では表現できない + 複雑な形状に対応できる．ただし過学習のリスクがある + + +8. Stage 0: 撮影条件の最適化 (Camera Settings) +------------------------------------------------------------------------ + + 8-0. 目的 + + ソフトウェア処理の前段階として，撮影条件を制御することで + 入力画像の品質を安定させる． + + 8-1. カメラ露出の固定 + + Picamera2 の自動露出（AE）を無効化し，固定値で撮影する． + + ・コスト: なし（ソフトウェア設定のみ） + ・効果: ◎（フレーム間の輝度変動を排除し，閾値パラメータを安定させる） + ・注意: 環境ごとに適切な露出値を設定する必要がある + ・参照: `src/pi/camera/capture.py` の撮影パラメータ設定 + + 8-2. ホワイトバランスの固定 + + 自動ホワイトバランス（AWB）を無効化する． + + ・コスト: なし + ・効果: ○（色味変動によるグレースケール値の揺れを防止） + + 8-3. 赤外 LED + IR カメラ + + 赤外光源と IR パスフィルタを使用して撮影する． + + ・コスト: 高（ハードウェア追加が必要） + ・効果: ◎（黒は赤外線を吸収するため，可視光の影響を完全に排除） + ・備考: コスト面で優先度は低いが，原理的には最も頑健な手法 + + +9. 推奨する組み合わせ案 (Recommended Combinations) +------------------------------------------------------------------------ + + 9-0. 選定の考え方 + + コースアウトを防ぐ上で「安定性」は「精度」より優先する． + 誤検出が 1 フレームでも入るとコースアウトしうるため， + 頑健性（ロバスト性）の高い手法を選択すること． + + 以下の3案はいずれも穴・陰の両方に対して高い耐性を持つ． + 処理時間は 320x240 画像での概算値であり， + 30fps（~33ms/フレーム）に対していずれも十分な余裕がある． + + 9-1. 案A: Black-hat 中心型（推奨） + + Black-hat が「背景より暗い構造」を直接抽出するため， + 照明正規化後の画像は非常にクリーンになり， + 固定閾値でも安定する．計算量が最も少なくパラメータも少ない． + まず試すべき案である． + + ・パイプライン: + + グレースケール → Black-hat → ブラー → 固定閾値 + → 等方クロージング → 距離変換マスク + → 行ごと中心抽出 → polyfit + + ・各段階の処理時間（概算）: + - Stage 1: Black-hat 変換 ~0.2ms + - Stage 1: ガウシアンブラー ~0.2ms + - Stage 2: 固定閾値 ~0.05ms + - Stage 3: 等方クロージング ~0.1ms + - Stage 3: 距離変換 + 閾値マスク ~0.2ms + - Stage 4: 行ごと中心抽出 + polyfit ~0.2ms + - 合計 ~1.0ms + + ・穴: ◎（クロージングで穴を埋め，距離変換で中心を抽出） + ・陰: ◎（Black-hat が背景輝度を除去 + 距離変換が外縁を削る + + 中心抽出が幅変動を無視） + ・変更量: 中 + ・追加パラメータ: 3個（Black-hat カーネルサイズ， + 等方クロージングサイズ，距離変換閾値） + ・追加依存: なし + + 9-2. 案B: 二重正規化型 + + 背景除算で大域的な照明勾配を除去した上で， + 適応的閾値が局所的なムラも処理する． + 原理の異なる2手法が補完し合う「二重防壁」構成であり， + どちらか一方では対処できないケースにも対応できる． + + ・パイプライン: + + グレースケール → 背景除算正規化 + → 適応的閾値（ガウシアン） + → 等方クロージング → 距離変換マスク + → 行ごと中心抽出 → polyfit + + ・各段階の処理時間（概算）: + - Stage 1: 背景除算（大カーネルブラー + 除算） ~0.5ms + - Stage 2: 適応的閾値（ガウシアン） ~0.3ms + - Stage 3: 等方クロージング ~0.1ms + - Stage 3: 距離変換 + 閾値マスク ~0.2ms + - Stage 4: 行ごと中心抽出 + polyfit ~0.2ms + - 合計 ~1.3ms + + ・穴: ◎（背景除算で反射の影響を低減 + + 適応的閾値が局所的に追従） + ・陰: ◎（背景除算が影の勾配を除去 + + 適応的閾値が局所閾値を調整） + ・変更量: 中 + ・追加パラメータ: 4個（背景除算カーネルサイズ， + blockSize，C，距離変換閾値） + ・追加依存: なし + + 9-3. 案C: 最高ロバスト型 + + 全段階で最もロバストな手法を選択した構成． + Black-hat + 適応的閾値の二重正規化に加え， + RANSAC で前段を突破した外れ値も排除する． + 全段階に防壁があるため，極端な照明環境でも破綻しにくい． + + ・パイプライン: + + グレースケール → Black-hat → 適応的閾値（ガウシアン） + → 等方クロージング → 距離変換マスク + → 行ごと中央値抽出 → RANSAC polyfit + + ・各段階の処理時間（概算）: + - Stage 1: Black-hat 変換 ~0.2ms + - Stage 2: 適応的閾値（ガウシアン） ~0.3ms + - Stage 3: 等方クロージング ~0.1ms + - Stage 3: 距離変換 + 閾値マスク ~0.2ms + - Stage 4: 行ごと中央値抽出 ~0.2ms + - Stage 4: RANSAC polyfit ~1.5ms + - 合計 ~2.5ms + + ・穴: ◎ + ・陰: ◎ + ・変更量: 大 + ・追加パラメータ: 5個（Black-hat カーネルサイズ， + blockSize，C，距離変換閾値，RANSAC 閾値） + ・追加依存: scikit-learn（RANSAC） + ・備考: RANSAC が処理時間の大部分を占める． + 案A・B で十分な精度が得られる場合はオーバースペック + + 9-4. 3案の総合比較 + + ・処理時間: + - 案A: ~1.0ms（対現在比 ~1.0x） + - 案B: ~1.3ms（対現在比 ~1.3x） + - 案C: ~2.5ms（対現在比 ~2.5x） + + ・穴耐性: 案A ◎，案B ◎，案C ◎ + ・陰耐性: 案A ◎，案B ◎，案C ◎ + ・追加パラメータ数: 案A 3個，案B 4個，案C 5個 + ・追加依存: 案A なし，案B なし，案C scikit-learn + ・実装変更量: 案A 中，案B 中，案C 大 + + いずれも 30fps に対して十分な余裕がある． + まず案A を実装して効果を確認し，不足があれば + 案B・案C に段階的に進めることを推奨する + + 9-5. 共通オプション: 透視補正付き幅フィルタの追加 + + 上記3案はいずれも等方クロージング + 距離変換マスク後に + 「透視補正付き幅フィルタ（6-8 節）」を追加できる． + カメラ高さ・線幅が固定されているため，width_near・width_far を + 一度実測して固定すれば，パラメータ調整なしに + 陰で広がった行を正確に除外できる． + + ・処理時間への影響: 極小（+0.05ms 以下） + ・効果: 距離変換では除去しきれなかった広がり行を補完的に除外 + ・適用場所: _apply_dist_mask の後，_fit_row_centers の前 + ・追加パラメータ: + - width_near: 画像下端での期待線幅（px，実測値） + - width_far: 画像上端での期待線幅（px，実測値） + - width_tolerance: 上限倍率（デフォルト 1.8） + + +10. 評価方法 (Evaluation) +------------------------------------------------------------------------ + + 10-1. 定性評価 + + デバッグオーバーレイ（`TECH_03_デバッグオーバーレイ仕様.txt` 参照） + を使い，二値化画像と多項式フィッティング結果を目視確認する． + + ・確認ポイント + - 二値化画像で黒線が一本の連続した塊として描かれているか + - 床面（背景）の誤検出塊がないか + - 多項式フィッティング線が実際の黒線に重なっているか + - 光の穴や陰の広がりが後処理で正しく補正されているか + + 10-2. 定量評価 + + 実走テストでコースアウトに至るまでの周回数・走行距離を記録し， + 手法変更前後で比較する． + + ・評価環境: 通常照明，強照明，照明ムラありの3条件を推奨 + ・計測項目: 周回数，コースアウト回数，position_error の分散 diff --git "a/docs/04_ENV/ENV_01_\346\212\200\350\241\223\343\202\271\343\202\277\343\203\203\343\202\257\351\201\270\345\256\232.txt" "b/docs/04_ENV/ENV_01_\346\212\200\350\241\223\343\202\271\343\202\277\343\203\203\343\202\257\351\201\270\345\256\232.txt" index 68c79f2..924416d 100644 --- "a/docs/04_ENV/ENV_01_\346\212\200\350\241\223\343\202\271\343\202\277\343\203\203\343\202\257\351\201\270\345\256\232.txt" +++ "b/docs/04_ENV/ENV_01_\346\212\200\350\241\223\343\202\271\343\202\277\343\203\203\343\202\257\351\201\270\345\256\232.txt" @@ -15,7 +15,7 @@ 1-1. 全体方針 ・言語: Python で統一する（Pi 側・PC 側ともに）． - ・既存資産: モーター制御コード（`src/pi/motor.py`）を流用する． + ・既存資産: モーター制御コード（`src_old/pi/motor.py`）を参考にする． ・選定基準: 低遅延・軽量・Python との親和性を重視する． @@ -91,6 +91,18 @@ ・PWM 制御・GPIO 出力に必要な機能が揃っている． + 2-6. 環境変数管理: python-dotenv + + ・用途: .env ファイルから環境変数を読み込む + - PC の IP アドレス + - 通信ポート番号 + + ■ 選定理由 + ・ネットワーク設定など環境固有の値をコードから分離できる． + ・.env ファイルを .gitignore に含めることで， + Git に環境固有情報をコミットしない運用が可能． + + 3. 構成まとめ (Summary) ------------------------------------------------------------------------ @@ -99,9 +111,11 @@ ・Picamera2: カメラフレーム取得 ・RPi.GPIO: モーター制御 ・pyzmq: PC との通信 + ・python-dotenv: 環境変数管理 ■ PC 側 ・PySide6: GUI アプリケーション ・OpenCV: 画像処理・線検出 ・pyzmq: Pi との通信 + ・python-dotenv: 環境変数管理 diff --git "a/docs/04_ENV/ENV_02_PC\347\222\260\345\242\203\346\247\213\347\257\211\346\211\213\351\240\206.txt" "b/docs/04_ENV/ENV_02_PC\347\222\260\345\242\203\346\247\213\347\257\211\346\211\213\351\240\206.txt" index c27c07e..f9f2b97 100644 --- "a/docs/04_ENV/ENV_02_PC\347\222\260\345\242\203\346\247\213\347\257\211\346\211\213\351\240\206.txt" +++ "b/docs/04_ENV/ENV_02_PC\347\222\260\345\242\203\346\247\213\347\257\211\346\211\213\351\240\206.txt" @@ -72,6 +72,7 @@ ・opencv-python (4.13.0.92): 画像処理 ・pyzmq (27.1.0): ZMQ 通信 ・numpy (2.4.3): 数値計算（OpenCV の依存ライブラリ） + ・python-dotenv (1.2.2): .env ファイルからの環境変数読み込み 3-3. インストール確認 @@ -88,4 +89,4 @@ 以下を Python で実行し，エラーが出ないことを確認する． - $ python -c "import PySide6; import cv2; import zmq; print('OK')" + $ python -c "import PySide6; import cv2; import zmq; import dotenv; print('OK')" diff --git "a/docs/04_ENV/ENV_03_RaspPi\347\222\260\345\242\203\346\247\213\347\257\211\346\211\213\351\240\206.txt" "b/docs/04_ENV/ENV_03_RaspPi\347\222\260\345\242\203\346\247\213\347\257\211\346\211\213\351\240\206.txt" index d40ac62..99edb9b 100644 --- "a/docs/04_ENV/ENV_03_RaspPi\347\222\260\345\242\203\346\247\213\347\257\211\346\211\213\351\240\206.txt" +++ "b/docs/04_ENV/ENV_03_RaspPi\347\222\260\345\242\203\346\247\213\347\257\211\346\211\213\351\240\206.txt" @@ -51,15 +51,30 @@ - 例: /home/user/RobotCar/ 6. 「Send」を押すと転送が開始される． - 2-3. フォルダ転送の注意 + 2-3. 転送スクリプト（推奨） - ・SCP では単一ファイルの転送が基本となる． - ・複数ファイルをまとめて転送したい場合は，PC 側で zip に圧縮し， - 転送後に Pi 側で展開する． + プロジェクトルートの deploy.sh を使用すると，Pi 側のフォルダを + 初期化してからまとめて転送できる． - $ unzip filename.zip + $ bash deploy.sh - 2-4. 転送対象 + 処理内容: + 1. Pi 側の common/，pi/ を削除 + 2. src/common/，src/pi/ を転送 + 3. .env，requirements_pi.txt を転送 + + ※ パスワードを複数回入力する必要がある． + + 2-4. 手動転送の方法 + + ・Tera Term の SCP では単一ファイルの転送が基本となる． + ・コマンドラインから scp -r を使うとフォルダごと転送できる． + + $ ssh user@192.168.23.224 "rm -rf /home/user/RobotCar/common /home/user/RobotCar/pi" + $ scp -r src/common user@192.168.23.224:/home/user/RobotCar/ + $ scp -r src/pi user@192.168.23.224:/home/user/RobotCar/ + + 2-5. 転送対象 以下の2ディレクトリを Pi に転送する． @@ -79,10 +94,52 @@ 3. venv の作成・ライブラリインストール (Virtual Environment) ------------------------------------------------------------------------ - ※ Pi 側の環境構築は実機作業時に手順を確定し，追記する． + 3-1. venv の作成 + + Picamera2 や libcamera などシステムにプリインストールされた + ライブラリを使用するため，--system-site-packages オプションが必須． + + $ cd /home/user/RobotCar + $ python3 -m venv --system-site-packages .venv + + 3-2. venv の有効化 + + $ source .venv/bin/activate + + 3-3. pip のアップグレード + + (.venv) $ pip install --upgrade pip + + 3-4. ライブラリのインストール + + (.venv) $ pip install -r requirements_pi.txt + + ※ requirements_pi.txt は deploy.sh で転送済みであること． + + 3-5. .env ファイルの確認 + + deploy.sh で .env が転送される． + PC_IP に PC の IP アドレスが設定されていることを確認する． + + (.venv) $ cat .env 4. 動作確認 (Verification) ------------------------------------------------------------------------ - ※ Pi 側の動作確認は実機作業時に手順を確定し，追記する． + 4-1. Pi 側の起動 + + $ cd /home/user/RobotCar + $ source .venv/bin/activate + (.venv) $ python -m pi.main + + 「Pi: カメラ・通信を開始」と表示されれば成功． + + 4-2. PC 側の起動 + + PC 側で以下を実行し，GUI の「接続開始」ボタンを押す． + + $ cd src + $ ../.venv/Scripts/python.exe -m pc.main + + カメラ映像がリアルタイムで表示されれば通信成功． 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 4d4d0d2..1aa7eaf 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" @@ -13,9 +13,8 @@ 1-1. 状態の表記 - ・(実装済み): ファイルが存在し，動作する状態． ・(未実装): 今後作成予定のファイル． - ・表記なし: ディレクトリのみ作成済み． + ・表記なし: 実装済み，またはディレクトリ． 2. ディレクトリ構成 (Directory Structure) @@ -24,9 +23,12 @@ 2-1. 全体構成 RobotCar/ - ├── CLAUDE.md (実装済み) - ├── requirements_pc.txt (実装済み) - ├── requirements_pi.txt (実装済み) + ├── CLAUDE.md + ├── requirements_pc.txt + ├── requirements_pi.txt + ├── deploy.sh Pi への転送スクリプト + ├── .env.example 環境変数テンプレート + ├── pd_params.json パラメータ保存ファイル（.gitignore） ├── docs/ ドキュメント ├── src/ 自律走行用ソースコード │ ├── common/ 共通設定（PC・Pi 両方で使用） @@ -37,7 +39,7 @@ 2-2. src/common/ common/ - └── config.py (未実装) + └── config.py ・PC・Pi 間で共有する設定値を定義する． ・内容: ネットワーク設定，画像フォーマット，通信設定等． @@ -45,24 +47,26 @@ 2-3. src/pc/ pc/ - ├── main.py (未実装) エントリーポイント + ├── main.py エントリーポイント ├── gui/ GUI 関連 - │ └── main_window.py (未実装) メインウィンドウ + │ └── main_window.py メインウィンドウ ├── comm/ 通信関連 - │ └── zmq_client.py (未実装) ZMQ 送受信 + │ └── zmq_client.py ZMQ 送受信 ├── steering/ 操舵量計算（独立モジュール） - │ ├── base.py (未実装) 共通インターフェース - │ └── pd_control.py (未実装) PD 制御の実装 + │ ├── base.py 共通インターフェース + │ ├── pd_control.py PD 制御の実装 + │ └── param_store.py パラメータ保存・読み込み └── vision/ 画像処理 - └── line_detector.py (未実装) 線検出 + ├── line_detector.py 線検出（多項式フィッティング） + └── overlay.py デバッグオーバーレイ描画 2-4. src/pi/ pi/ - ├── main.py (未実装) エントリーポイント + ├── main.py エントリーポイント ├── comm/ 通信関連 - │ └── zmq_client.py (未実装) ZMQ 送受信 + │ └── zmq_client.py ZMQ 送受信 ├── camera/ カメラ関連 - │ └── capture.py (未実装) フレーム取得 + │ └── capture.py フレーム取得 └── motor/ モーター関連 - └── driver.py (未実装) TB6612FNG 制御 + └── driver.py TB6612FNG 制御 diff --git a/requirements_pc.txt b/requirements_pc.txt index 4877e6c..506720a 100644 --- a/requirements_pc.txt +++ b/requirements_pc.txt @@ -2,3 +2,4 @@ opencv-python==4.13.0.92 pyzmq==27.1.0 numpy==2.4.3 +python-dotenv==1.2.2 diff --git a/requirements_pi.txt b/requirements_pi.txt index d073deb..8b753d4 100644 --- a/requirements_pi.txt +++ b/requirements_pi.txt @@ -1,3 +1,4 @@ pyzmq==27.1.0 picamera2 RPi.GPIO +python-dotenv==1.2.2 diff --git a/src/common/__init__.py b/src/common/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/common/__init__.py diff --git a/src/common/config.py b/src/common/config.py new file mode 100644 index 0000000..8973b3b --- /dev/null +++ b/src/common/config.py @@ -0,0 +1,94 @@ +""" +config +プロジェクト共通の設定値を管理するモジュール +環境固有の値は .env ファイルから読み込む +""" + +import os +from pathlib import Path + +from dotenv import load_dotenv + +# .env ファイルの読み込み +# config.py から上方向に .env を探索する +_search_dir = Path(__file__).resolve().parent +_env_path = _search_dir / ".env" +while not _env_path.exists() and _search_dir != _search_dir.parent: + _search_dir = _search_dir.parent + _env_path = _search_dir / ".env" +load_dotenv(_env_path) + +# ── ネットワーク設定（.env から読み込み） ────────────────────── + +# PC の IP アドレス +PC_IP: str = os.getenv("PC_IP", "127.0.0.1") + +# 画像送信ポート（Pi → PC） +IMAGE_PORT: int = int(os.getenv("IMAGE_PORT", "5555")) + +# 操舵量送信ポート（PC → Pi） +CONTROL_PORT: int = int(os.getenv("CONTROL_PORT", "5556")) + +# ── 画像設定 ────────────────────────────────────────────── + +# カメラ画像の幅 (px) +FRAME_WIDTH: int = 320 + +# カメラ画像の高さ (px) +FRAME_HEIGHT: int = 240 + +# JPEG 圧縮品質 (0-100) +JPEG_QUALITY: int = 55 + +# ── 通信設定 ────────────────────────────────────────────── + +# 操舵量の送信頻度 (Hz) +CONTROL_PUBLISH_HZ: float = 20.0 + +# 操舵量の受信タイムアウト (秒)，超過でモーター停止 +CONTROL_TIMEOUT_SEC: float = 0.5 + +# ── モーター設定（GPIO ピン番号，BOARD モード） ─────────── + +# モーター A（左） +MA_IN1: int = 19 +MA_IN2: int = 21 +MA_PWM: int = 23 + +# モーター B（右） +MB_IN1: int = 15 +MB_IN2: int = 13 +MB_PWM: int = 11 + +# モーター PWM 周波数 (Hz) +MOTOR_PWM_FREQ: int = 100 + +# モーター極性反転フラグ +MOTOR_LEFT_REVERSED: bool = True +MOTOR_RIGHT_REVERSED: bool = True + +# ステアリング方向反転フラグ +STEER_REVERSED: bool = True + + +# ── アドレス生成ヘルパー ────────────────────────────────── + + +def image_bind_address() -> str: + """画像受信側（PC）のバインドアドレスを返す""" + return f"tcp://*:{IMAGE_PORT}" + + +def image_connect_address() -> str: + """画像送信側（Pi）の接続先アドレスを返す""" + return f"tcp://{PC_IP}:{IMAGE_PORT}" + + +def control_bind_address() -> str: + """操舵量送信側（PC）のバインドアドレスを返す""" + return f"tcp://*:{CONTROL_PORT}" + + +def control_connect_address() -> str: + """操舵量受信側（Pi）の接続先アドレスを返す""" + return f"tcp://{PC_IP}:{CONTROL_PORT}" diff --git a/src/pc/__init__.py b/src/pc/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/pc/__init__.py diff --git a/src/pc/comm/__init__.py b/src/pc/comm/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/pc/comm/__init__.py diff --git a/src/pc/comm/zmq_client.py b/src/pc/comm/zmq_client.py new file mode 100644 index 0000000..625a580 --- /dev/null +++ b/src/pc/comm/zmq_client.py @@ -0,0 +1,86 @@ +""" +zmq_client +PC 側の ZMQ 通信を担当するモジュール +画像の受信と操舵量の送信を行う +""" + +import json + +import cv2 +import numpy as np +import zmq + +from common import config + + +class PcZmqClient: + """PC 側の ZMQ 通信クライアント + + 画像受信（SUB）と操舵量送信（PUB）の2チャネルを管理する + """ + + def __init__(self) -> None: + self._context: zmq.Context | None = None + self._image_socket: zmq.Socket | None = None + self._control_socket: zmq.Socket | None = None + + def start(self) -> None: + """通信ソケットを初期化してバインドする""" + self._context = zmq.Context() + + # 画像受信ソケット（SUB，Pi からの画像を受信） + self._image_socket = self._context.socket(zmq.SUB) + self._image_socket.setsockopt(zmq.CONFLATE, 1) + self._image_socket.setsockopt_string(zmq.SUBSCRIBE, "") + self._image_socket.bind(config.image_bind_address()) + + # 操舵量送信ソケット（PUB，Pi へ操舵量を送信） + self._control_socket = self._context.socket(zmq.PUB) + self._control_socket.bind(config.control_bind_address()) + + def receive_image(self) -> np.ndarray | None: + """画像を非ブロッキングで受信する + + Returns: + 受信した画像の NumPy 配列，受信データがない場合は None + """ + if self._image_socket is None: + return None + try: + data = self._image_socket.recv(zmq.NOBLOCK) + frame = cv2.imdecode( + np.frombuffer(data, dtype=np.uint8), + cv2.IMREAD_COLOR, + ) + return frame + except zmq.Again: + return None + + def send_control( + self, throttle: float, steer: float, + ) -> None: + """操舵量を送信する + + Args: + throttle: 前後方向の出力 (-1.0 ~ +1.0) + steer: 左右方向の出力 (-1.0 ~ +1.0) + """ + if self._control_socket is None: + return + payload = json.dumps({ + "throttle": throttle, + "steer": steer, + }).encode("utf-8") + self._control_socket.send(payload, zmq.NOBLOCK) + + def stop(self) -> None: + """通信ソケットを閉じる""" + if self._image_socket is not None: + self._image_socket.close() + self._image_socket = None + if self._control_socket is not None: + self._control_socket.close() + self._control_socket = None + if self._context is not None: + self._context.term() + self._context = None diff --git a/src/pc/gui/__init__.py b/src/pc/gui/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/pc/gui/__init__.py diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py new file mode 100644 index 0000000..81e35a6 --- /dev/null +++ b/src/pc/gui/main_window.py @@ -0,0 +1,1137 @@ +""" +main_window +PC 側のメインウィンドウを定義するモジュール +カメラ映像のリアルタイム表示と操作 UI を提供する +""" + +import numpy as np +from PySide6.QtCore import Qt, QTimer +from PySide6.QtGui import QImage, QKeyEvent, QPixmap +from PySide6.QtWidgets import ( + QCheckBox, + QComboBox, + QDoubleSpinBox, + QFormLayout, + QGroupBox, + QHBoxLayout, + QInputDialog, + QLabel, + QMainWindow, + QMessageBox, + QPushButton, + QScrollArea, + QSpinBox, + QVBoxLayout, + QWidget, +) + +from common import config +from pc.comm.zmq_client import PcZmqClient +from pc.steering.auto_params import ( + load_control, + load_detect_params, + save_control, + save_detect_params, +) +from pc.steering.pd_control import PdControl, PdParams +from pc.steering.param_store import ( + ImagePreset, + PdPreset, + add_image_preset, + add_pd_preset, + delete_image_preset, + delete_pd_preset, + load_image_presets, + load_pd_presets, +) +from pc.vision.line_detector import ( + DETECT_METHODS, + ImageParams, + LineDetectResult, + detect_line, +) +from pc.vision.overlay import OverlayFlags, draw_overlay + +# 映像更新間隔 (ms) +FRAME_INTERVAL_MS: int = 33 + +# 操舵量送信間隔 (ms) +CONTROL_INTERVAL_MS: int = int( + 1000 / config.CONTROL_PUBLISH_HZ +) + +# 映像表示のスケール倍率 +DISPLAY_SCALE: float = 2.0 + +# 手動操作の throttle / steer 量 +MANUAL_THROTTLE: float = 0.5 +MANUAL_STEER: float = 0.4 + + +class MainWindow(QMainWindow): + """PC 側のメインウィンドウ""" + + def __init__(self) -> None: + super().__init__() + self._zmq_client = PcZmqClient() + self._is_connected = False + self._is_auto = False + + # 手動操作の状態 + self._pressed_keys: set[int] = set() + self._throttle: float = 0.0 + self._steer: float = 0.0 + + # 自動保存の制御フラグ + self._auto_save_enabled = False + + # 前回のパラメータを復元 + pd_params, last_method = load_control() + image_params = load_detect_params(last_method) + self._pd_control = PdControl( + params=pd_params, + image_params=image_params, + ) + + # 最新フレームの保持（自動操縦で使用） + self._latest_frame: np.ndarray | None = None + + # オーバーレイ + self._overlay_flags = OverlayFlags() + self._last_detect_result: LineDetectResult | None = None + + self._setup_ui() + self._setup_timers() + self._auto_save_enabled = True + + def _setup_ui(self) -> None: + """UI を構築する""" + self.setWindowTitle("RobotCar Controller") + + # 中央ウィジェット + central = QWidget() + self.setCentralWidget(central) + root_layout = QHBoxLayout(central) + + # 左側: 映像表示 + self._video_label = QLabel("カメラ映像待機中...") + self._video_label.setAlignment( + Qt.AlignmentFlag.AlignCenter, + ) + self._video_label.setMinimumSize( + int(config.FRAME_WIDTH * DISPLAY_SCALE), + int(config.FRAME_HEIGHT * DISPLAY_SCALE), + ) + self._video_label.setStyleSheet( + "background-color: #222;" + " color: #aaa; font-size: 16px;" + ) + root_layout.addWidget(self._video_label, stretch=3) + + # 右側: スクロール可能なコントロールパネル + scroll = QScrollArea() + scroll.setWidgetResizable(True) + scroll.setHorizontalScrollBarPolicy( + Qt.ScrollBarPolicy.ScrollBarAlwaysOff, + ) + control_widget = QWidget() + control_layout = QVBoxLayout(control_widget) + scroll.setWidget(control_widget) + root_layout.addWidget(scroll, stretch=1) + + # 接続ボタン + self._connect_btn = QPushButton("接続開始") + self._connect_btn.clicked.connect( + self._toggle_connection, + ) + control_layout.addWidget(self._connect_btn) + + # 自動操縦ボタン + self._auto_btn = QPushButton("自動操縦 ON") + self._auto_btn.setEnabled(False) + self._auto_btn.clicked.connect( + self._toggle_auto, + ) + control_layout.addWidget(self._auto_btn) + + # ステータス表示 + self._status_label = QLabel("未接続") + self._status_label.setAlignment( + Qt.AlignmentFlag.AlignCenter, + ) + control_layout.addWidget(self._status_label) + + # 操舵量表示 + self._control_label = QLabel( + "throttle: 0.00\nsteer: 0.00" + ) + self._control_label.setAlignment( + Qt.AlignmentFlag.AlignCenter, + ) + self._control_label.setStyleSheet("font-size: 14px;") + control_layout.addWidget(self._control_label) + + # 画像処理パラメータ調整（Stage 1〜4） + self._setup_image_param_ui(control_layout) + + # PD 制御パラメータ（操舵量計算） + self._setup_param_ui(control_layout) + + # デバッグ表示 + self._setup_overlay_ui(control_layout) + + # 操作ガイド + guide = QLabel( + "--- キー操作 ---\n" + "W/↑: 前進 S/↓: 後退\n" + "A/←: 左 D/→: 右\n" + "Space: 停止\n" + "Q: 自動操縦 切替" + ) + guide.setAlignment(Qt.AlignmentFlag.AlignLeft) + guide.setStyleSheet("font-size: 12px; color: #666;") + control_layout.addWidget(guide) + + # 余白を下に詰める + control_layout.addStretch() + + def _setup_param_ui( + self, parent_layout: QVBoxLayout, + ) -> None: + """PD パラメータ調整 UI を構築する""" + group = QGroupBox("PD 制御パラメータ") + layout = QVBoxLayout() + group.setLayout(layout) + + form = QFormLayout() + layout.addLayout(form) + + params = self._pd_control.params + + self._spin_kp = self._create_spin( + params.kp, 0.0, 5.0, 0.05, + ) + form.addRow("Kp (位置):", self._spin_kp) + + self._spin_kh = self._create_spin( + params.kh, 0.0, 5.0, 0.05, + ) + form.addRow("Kh (傾き):", self._spin_kh) + + self._spin_kd = self._create_spin( + params.kd, 0.0, 5.0, 0.05, + ) + form.addRow("Kd (微分):", self._spin_kd) + + self._spin_max_steer_rate = self._create_spin( + params.max_steer_rate, 0.01, 1.0, 0.01, + ) + form.addRow("操舵制限:", self._spin_max_steer_rate) + + self._spin_max_throttle = self._create_spin( + params.max_throttle, 0.0, 1.0, 0.05, + ) + form.addRow("最大速度:", self._spin_max_throttle) + + self._spin_speed_k = self._create_spin( + params.speed_k, 0.0, 5.0, 0.05, + ) + form.addRow("減速係数:", self._spin_speed_k) + + # --- プリセット管理 --- + self._pd_preset_combo = QComboBox() + self._pd_preset_combo.setPlaceholderText( + "プリセット", + ) + layout.addWidget(self._pd_preset_combo) + + self._pd_preset_memo = QLabel("") + self._pd_preset_memo.setWordWrap(True) + self._pd_preset_memo.setStyleSheet( + "font-size: 11px; color: #888;", + ) + layout.addWidget(self._pd_preset_memo) + + btn_layout = QHBoxLayout() + load_btn = QPushButton("読込") + load_btn.clicked.connect( + self._on_load_pd_preset, + ) + btn_layout.addWidget(load_btn) + save_btn = QPushButton("保存") + save_btn.clicked.connect( + self._on_save_pd_preset, + ) + btn_layout.addWidget(save_btn) + del_btn = QPushButton("削除") + del_btn.clicked.connect( + self._on_delete_pd_preset, + ) + btn_layout.addWidget(del_btn) + layout.addLayout(btn_layout) + + # コールバック接続 + for spin in [ + self._spin_kp, self._spin_kh, + self._spin_kd, self._spin_max_steer_rate, + self._spin_max_throttle, self._spin_speed_k, + ]: + spin.valueChanged.connect( + self._on_param_changed, + ) + self._pd_preset_combo.currentIndexChanged \ + .connect(self._on_pd_preset_selected) + + self._pd_presets: list[PdPreset] = [] + self._refresh_pd_presets() + + parent_layout.addWidget(group) + + def _setup_image_param_ui( + self, parent_layout: QVBoxLayout, + ) -> None: + """画像処理パラメータ調整 UI を構築する""" + group = QGroupBox("画像処理パラメータ") + layout = QVBoxLayout() + group.setLayout(layout) + + ip = self._pd_control.image_params + + # 検出手法の選択コンボボックス + self._method_combo = QComboBox() + for key, label in DETECT_METHODS.items(): + self._method_combo.addItem(label, key) + idx = self._method_combo.findData(ip.method) + if idx >= 0: + self._method_combo.setCurrentIndex(idx) + layout.addWidget(self._method_combo) + + # パラメータフォーム + self._image_form = QFormLayout() + layout.addLayout(self._image_form) + + # 各パラメータの可視性マッピング + # (widget, 表示する手法の集合) + self._image_param_vis: list[ + tuple[QWidget, set[str]] + ] = [] + + # --- 現行手法パラメータ --- + self._spin_clahe_clip = self._create_spin( + ip.clahe_clip, 0.5, 10.0, 0.5, + ) + self._add_image_row( + "CLAHE強度:", self._spin_clahe_clip, + {"current"}, + ) + + self._spin_binary_thresh = QSpinBox() + self._spin_binary_thresh.setRange(10, 200) + self._spin_binary_thresh.setValue( + ip.binary_thresh, + ) + self._add_image_row( + "二値化閾値:", self._spin_binary_thresh, + {"current", "blackhat"}, + ) + + self._spin_open_size = QSpinBox() + self._spin_open_size.setRange(1, 31) + self._spin_open_size.setSingleStep(2) + self._spin_open_size.setValue(ip.open_size) + self._add_image_row( + "ノイズ除去:", self._spin_open_size, + {"current"}, + ) + + self._spin_close_width = QSpinBox() + self._spin_close_width.setRange(1, 51) + self._spin_close_width.setSingleStep(2) + self._spin_close_width.setValue(ip.close_width) + self._add_image_row( + "途切れ補間:", self._spin_close_width, + {"current"}, + ) + + # --- 案A/C: Black-hat --- + self._spin_blackhat_ksize = QSpinBox() + self._spin_blackhat_ksize.setRange(11, 101) + self._spin_blackhat_ksize.setSingleStep(2) + self._spin_blackhat_ksize.setValue( + ip.blackhat_ksize, + ) + self._add_image_row( + "BHカーネル:", self._spin_blackhat_ksize, + {"blackhat", "robust"}, + ) + + # --- 案B: 背景除算 --- + self._spin_bg_blur_ksize = QSpinBox() + self._spin_bg_blur_ksize.setRange(31, 201) + self._spin_bg_blur_ksize.setSingleStep(2) + self._spin_bg_blur_ksize.setValue( + ip.bg_blur_ksize, + ) + self._add_image_row( + "背景ブラー:", self._spin_bg_blur_ksize, + {"dual_norm"}, + ) + + # --- 案B/C: 適応的閾値 --- + self._spin_adaptive_block = QSpinBox() + self._spin_adaptive_block.setRange(11, 101) + self._spin_adaptive_block.setSingleStep(2) + self._spin_adaptive_block.setValue( + ip.adaptive_block, + ) + self._add_image_row( + "適応ブロック:", self._spin_adaptive_block, + {"dual_norm", "robust"}, + ) + + self._spin_adaptive_c = QSpinBox() + self._spin_adaptive_c.setRange(1, 30) + self._spin_adaptive_c.setValue(ip.adaptive_c) + self._add_image_row( + "適応定数C:", self._spin_adaptive_c, + {"dual_norm", "robust"}, + ) + + # --- 案A/B/C: 後処理 --- + self._spin_iso_close = QSpinBox() + self._spin_iso_close.setRange(1, 51) + self._spin_iso_close.setSingleStep(2) + self._spin_iso_close.setValue( + ip.iso_close_size, + ) + self._add_image_row( + "穴埋め:", self._spin_iso_close, + {"blackhat", "dual_norm", "robust"}, + ) + + self._spin_dist_thresh = self._create_spin( + ip.dist_thresh, 0.0, 10.0, 0.5, + ) + self._add_image_row( + "距離閾値:", self._spin_dist_thresh, + {"blackhat", "dual_norm", "robust"}, + ) + + self._spin_min_line_width = QSpinBox() + self._spin_min_line_width.setRange(1, 20) + self._spin_min_line_width.setValue( + ip.min_line_width, + ) + self._add_image_row( + "最小線幅:", self._spin_min_line_width, + {"blackhat", "dual_norm", "robust"}, + ) + + # --- 案C: RANSAC --- + self._spin_ransac_thresh = self._create_spin( + ip.ransac_thresh, 1.0, 30.0, 1.0, + ) + self._add_image_row( + "RANSAC閾値:", self._spin_ransac_thresh, + {"robust"}, + ) + + # --- 幅フィルタ（透視補正） --- + self._spin_width_near = QSpinBox() + self._spin_width_near.setRange(0, 200) + self._spin_width_near.setValue(ip.width_near) + self._spin_width_near.setSpecialValueText("無効") + self._add_image_row( + "線幅(近)px:", self._spin_width_near, + {"blackhat", "dual_norm", "robust"}, + ) + + self._spin_width_far = QSpinBox() + self._spin_width_far.setRange(0, 200) + self._spin_width_far.setValue(ip.width_far) + self._spin_width_far.setSpecialValueText("無効") + self._add_image_row( + "線幅(遠)px:", self._spin_width_far, + {"blackhat", "dual_norm", "robust"}, + ) + + self._spin_width_tolerance = self._create_spin( + ip.width_tolerance, 1.0, 5.0, 0.1, + ) + self._add_image_row( + "幅フィルタ倍率:", self._spin_width_tolerance, + {"blackhat", "dual_norm", "robust"}, + ) + + # --- プリセット管理 --- + self._image_preset_combo = QComboBox() + self._image_preset_combo.setPlaceholderText( + "プリセット", + ) + layout.addWidget(self._image_preset_combo) + + self._image_preset_memo = QLabel("") + self._image_preset_memo.setWordWrap(True) + self._image_preset_memo.setStyleSheet( + "font-size: 11px; color: #888;", + ) + layout.addWidget(self._image_preset_memo) + + btn_layout = QHBoxLayout() + load_btn = QPushButton("読込") + load_btn.clicked.connect( + self._on_load_image_preset, + ) + btn_layout.addWidget(load_btn) + save_btn = QPushButton("保存") + save_btn.clicked.connect( + self._on_save_image_preset, + ) + btn_layout.addWidget(save_btn) + del_btn = QPushButton("削除") + del_btn.clicked.connect( + self._on_delete_image_preset, + ) + btn_layout.addWidget(del_btn) + layout.addLayout(btn_layout) + + # コールバック接続 + self._method_combo.currentIndexChanged.connect( + self._on_method_changed, + ) + for widget, _ in self._image_param_vis: + widget.valueChanged.connect( + self._on_image_param_changed, + ) + self._image_preset_combo.currentIndexChanged \ + .connect(self._on_image_preset_selected) + + self._image_presets: list[ImagePreset] = [] + self._image_filtered: list[int] = [] + + parent_layout.addWidget(group) + + # 初期表示の更新 + self._on_method_changed() + + def _add_image_row( + self, + label: str, + widget: QWidget, + methods: set[str], + ) -> None: + """画像処理パラメータの行を追加する""" + self._image_form.addRow(label, widget) + self._image_param_vis.append( + (widget, methods), + ) + + def _on_method_changed(self) -> None: + """検出手法の変更を反映する""" + method = self._method_combo.currentData() + + # 旧手法のパラメータを保存 + if self._auto_save_enabled: + ip = self._pd_control.image_params + save_detect_params(ip.method, ip) + + # 新手法のパラメータを読み込み + if self._auto_save_enabled: + new_ip = load_detect_params(method) + self._pd_control.image_params = new_ip + self._sync_image_spinboxes() + save_control( + self._pd_control.params, method, + ) + else: + self._pd_control.image_params.method = ( + method + ) + + # パラメータの表示/非表示を更新 + for widget, methods in self._image_param_vis: + visible = method in methods + widget.setVisible(visible) + label = self._image_form.labelForField( + widget, + ) + if label: + label.setVisible(visible) + + # 保存済みプリセットを手法でフィルタ + if hasattr(self, "_image_preset_combo"): + self._refresh_image_presets() + + def _sync_image_spinboxes(self) -> None: + """画像処理パラメータの SpinBox を現在値に同期する""" + self._auto_save_enabled = False + try: + ip = self._pd_control.image_params + self._spin_clahe_clip.setValue( + ip.clahe_clip, + ) + self._spin_binary_thresh.setValue( + ip.binary_thresh, + ) + self._spin_open_size.setValue(ip.open_size) + self._spin_close_width.setValue( + ip.close_width, + ) + self._spin_blackhat_ksize.setValue( + ip.blackhat_ksize, + ) + self._spin_bg_blur_ksize.setValue( + ip.bg_blur_ksize, + ) + self._spin_adaptive_block.setValue( + ip.adaptive_block, + ) + self._spin_adaptive_c.setValue( + ip.adaptive_c, + ) + self._spin_iso_close.setValue( + ip.iso_close_size, + ) + self._spin_dist_thresh.setValue( + ip.dist_thresh, + ) + self._spin_min_line_width.setValue( + ip.min_line_width, + ) + self._spin_ransac_thresh.setValue( + ip.ransac_thresh, + ) + self._spin_width_near.setValue(ip.width_near) + self._spin_width_far.setValue(ip.width_far) + self._spin_width_tolerance.setValue( + ip.width_tolerance, + ) + finally: + self._auto_save_enabled = True + + def _on_image_param_changed(self) -> None: + """画像処理パラメータの変更を反映する""" + ip = self._pd_control.image_params + # 現行手法 + ip.clahe_clip = self._spin_clahe_clip.value() + ip.binary_thresh = ( + self._spin_binary_thresh.value() + ) + ip.open_size = self._spin_open_size.value() + ip.close_width = self._spin_close_width.value() + # 案A/C: Black-hat + ip.blackhat_ksize = ( + self._spin_blackhat_ksize.value() + ) + # 案B: 背景除算 + ip.bg_blur_ksize = ( + self._spin_bg_blur_ksize.value() + ) + # 案B/C: 適応的閾値 + ip.adaptive_block = ( + self._spin_adaptive_block.value() + ) + ip.adaptive_c = self._spin_adaptive_c.value() + # 案A/B/C: 後処理 + ip.iso_close_size = ( + self._spin_iso_close.value() + ) + ip.dist_thresh = ( + self._spin_dist_thresh.value() + ) + ip.min_line_width = ( + self._spin_min_line_width.value() + ) + # 案C: RANSAC + ip.ransac_thresh = ( + self._spin_ransac_thresh.value() + ) + # 幅フィルタ（透視補正） + ip.width_near = self._spin_width_near.value() + ip.width_far = self._spin_width_far.value() + ip.width_tolerance = ( + self._spin_width_tolerance.value() + ) + + if self._auto_save_enabled: + save_detect_params(ip.method, ip) + + # ── 画像処理プリセット ────────────────────────── + + def _refresh_image_presets(self) -> None: + """選択中の手法のプリセットだけ表示する""" + self._image_presets = load_image_presets() + method = self._method_combo.currentData() + self._image_preset_combo.clear() + self._image_filtered = [] + for i, p in enumerate(self._image_presets): + if p.image_params.method == method: + self._image_preset_combo.addItem( + p.title, + ) + self._image_filtered.append(i) + self._image_preset_memo.setText("") + + def _on_image_preset_selected(self) -> None: + """画像処理プリセット選択時にメモを表示""" + idx = self._get_image_preset_idx() + if idx >= 0: + self._image_preset_memo.setText( + self._image_presets[idx].memo, + ) + else: + self._image_preset_memo.setText("") + + def _get_image_preset_idx(self) -> int: + """コンボの選択を全体インデックスに変換""" + ci = self._image_preset_combo.currentIndex() + if ci < 0 or ci >= len(self._image_filtered): + return -1 + return self._image_filtered[ci] + + def _on_load_image_preset(self) -> None: + """画像処理プリセットを読み込む""" + idx = self._get_image_preset_idx() + if idx < 0: + return + self._auto_save_enabled = False + try: + ip = self._image_presets[idx].image_params + self._pd_control.image_params = ip + self._sync_image_spinboxes() + finally: + self._auto_save_enabled = True + save_detect_params(ip.method, ip) + + def _on_save_image_preset(self) -> None: + """画像処理プリセットを保存する""" + title, ok = QInputDialog.getText( + self, "画像処理プリセット保存", "タイトル:", + ) + if not ok or not title.strip(): + return + memo, ok = QInputDialog.getText( + self, "画像処理プリセット保存", "メモ:", + ) + if not ok: + return + ip = self._pd_control.image_params + add_image_preset(ImagePreset( + title=title.strip(), + memo=memo.strip(), + image_params=ImageParams(**{ + f.name: getattr(ip, f.name) + for f in ip.__dataclass_fields__.values() + }), + )) + self._refresh_image_presets() + self._image_preset_combo.setCurrentIndex( + self._image_preset_combo.count() - 1, + ) + + def _on_delete_image_preset(self) -> None: + """画像処理プリセットを削除する""" + idx = self._get_image_preset_idx() + if idx < 0: + return + title = self._image_presets[idx].title + reply = QMessageBox.question( + self, "削除確認", + f"「{title}」を削除しますか？", + QMessageBox.StandardButton.Yes + | QMessageBox.StandardButton.No, + QMessageBox.StandardButton.No, + ) + if reply == QMessageBox.StandardButton.Yes: + delete_image_preset(idx) + self._refresh_image_presets() + + # ── PD 制御プリセット ───────────────────────── + + def _refresh_pd_presets(self) -> None: + """PD 制御プリセット一覧を更新する""" + self._pd_presets = load_pd_presets() + self._pd_preset_combo.clear() + for p in self._pd_presets: + self._pd_preset_combo.addItem(p.title) + self._pd_preset_memo.setText("") + + def _on_pd_preset_selected(self) -> None: + """PD 制御プリセット選択時にメモを表示""" + idx = self._pd_preset_combo.currentIndex() + if 0 <= idx < len(self._pd_presets): + self._pd_preset_memo.setText( + self._pd_presets[idx].memo, + ) + else: + self._pd_preset_memo.setText("") + + def _on_load_pd_preset(self) -> None: + """PD 制御プリセットを読み込む""" + idx = self._pd_preset_combo.currentIndex() + if idx < 0 or idx >= len(self._pd_presets): + return + self._auto_save_enabled = False + try: + p = self._pd_presets[idx].params + self._spin_kp.setValue(p.kp) + self._spin_kh.setValue(p.kh) + self._spin_kd.setValue(p.kd) + self._spin_max_steer_rate.setValue( + p.max_steer_rate, + ) + self._spin_max_throttle.setValue( + p.max_throttle, + ) + self._spin_speed_k.setValue(p.speed_k) + self._pd_control.params = p + finally: + self._auto_save_enabled = True + save_control( + p, + self._pd_control.image_params.method, + ) + + def _on_save_pd_preset(self) -> None: + """PD 制御プリセットを保存する""" + title, ok = QInputDialog.getText( + self, "PD プリセット保存", "タイトル:", + ) + if not ok or not title.strip(): + return + memo, ok = QInputDialog.getText( + self, "PD プリセット保存", "メモ:", + ) + if not ok: + return + p = self._pd_control.params + add_pd_preset(PdPreset( + title=title.strip(), + memo=memo.strip(), + params=PdParams( + 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_pd_presets() + self._pd_preset_combo.setCurrentIndex( + self._pd_preset_combo.count() - 1, + ) + + def _on_delete_pd_preset(self) -> None: + """PD 制御プリセットを削除する""" + idx = self._pd_preset_combo.currentIndex() + if idx < 0 or idx >= len(self._pd_presets): + return + title = self._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_pd_preset(idx) + self._refresh_pd_presets() + + def _setup_overlay_ui( + self, parent_layout: QVBoxLayout, + ) -> None: + """デバッグ表示のチェックボックス UI を構築する""" + group = QGroupBox("デバッグ表示") + layout = QVBoxLayout() + group.setLayout(layout) + + items = [ + ("二値化画像", "binary"), + ("検出領域", "detect_region"), + ("フィッティング曲線", "poly_curve"), + ("中心線", "center_line"), + ("検出情報", "info_text"), + ] + for label, attr in items: + cb = QCheckBox(label) + cb.toggled.connect( + lambda checked, a=attr: + setattr(self._overlay_flags, a, checked), + ) + layout.addWidget(cb) + + parent_layout.addWidget(group) + + def _has_any_overlay(self) -> bool: + """いずれかのオーバーレイが有効かを返す""" + f = self._overlay_flags + return ( + f.binary or f.detect_region + or f.poly_curve or f.center_line + or f.info_text + ) + + @staticmethod + def _create_spin( + value: float, min_val: float, + max_val: float, step: float, + ) -> QDoubleSpinBox: + """パラメータ用の SpinBox を作成する""" + spin = QDoubleSpinBox() + spin.setRange(min_val, max_val) + spin.setSingleStep(step) + spin.setDecimals(3) + spin.setValue(value) + return spin + + def _on_param_changed(self) -> None: + """パラメータ SpinBox の値が変更されたときに反映する""" + p = self._pd_control.params + p.kp = self._spin_kp.value() + p.kh = self._spin_kh.value() + p.kd = self._spin_kd.value() + p.max_steer_rate = ( + self._spin_max_steer_rate.value() + ) + p.max_throttle = self._spin_max_throttle.value() + p.speed_k = self._spin_speed_k.value() + + if self._auto_save_enabled: + save_control( + p, + self._pd_control.image_params.method, + ) + + def _setup_timers(self) -> None: + """タイマーを設定する""" + # 映像更新用 + self._frame_timer = QTimer(self) + self._frame_timer.timeout.connect(self._update_frame) + + # 操舵量送信用 + self._control_timer = QTimer(self) + self._control_timer.timeout.connect( + self._send_control, + ) + + # ── 接続 ────────────────────────────────────────────── + + def _toggle_connection(self) -> None: + """接続 / 切断を切り替える""" + if self._is_connected: + self._disconnect() + else: + self._connect() + + def _connect(self) -> None: + """ZMQ 通信を開始して映像受信を始める""" + self._zmq_client.start() + self._is_connected = True + self._connect_btn.setText("切断") + self._auto_btn.setEnabled(True) + self._status_label.setText("接続中 (手動操作)") + self._frame_timer.start(FRAME_INTERVAL_MS) + self._control_timer.start(CONTROL_INTERVAL_MS) + + def _disconnect(self) -> None: + """ZMQ 通信を停止する""" + self._frame_timer.stop() + self._control_timer.stop() + if self._is_auto: + self._is_auto = False + self._auto_btn.setText("自動操縦 ON") + self._zmq_client.stop() + self._is_connected = False + self._auto_btn.setEnabled(False) + self._pressed_keys.clear() + self._throttle = 0.0 + self._steer = 0.0 + self._latest_frame = None + self._connect_btn.setText("接続開始") + self._status_label.setText("未接続") + self._video_label.setText("カメラ映像待機中...") + self._control_label.setText( + "throttle: 0.00\nsteer: 0.00" + ) + + # ── 自動操縦 ────────────────────────────────────────── + + def _toggle_auto(self) -> None: + """自動操縦の ON / OFF を切り替える""" + if self._is_auto: + self._disable_auto() + else: + self._enable_auto() + + def _enable_auto(self) -> None: + """自動操縦を開始する""" + self._is_auto = True + self._pd_control.reset() + self._pressed_keys.clear() + self._auto_btn.setText("自動操縦 OFF") + self._status_label.setText("接続中 (自動操縦)") + + def _disable_auto(self) -> None: + """自動操縦を停止して手動に戻る""" + self._is_auto = False + self._throttle = 0.0 + self._steer = 0.0 + self._auto_btn.setText("自動操縦 ON") + self._status_label.setText("接続中 (手動操作)") + self._update_control_label() + + # ── 映像更新 ────────────────────────────────────────── + + def _update_frame(self) -> None: + """タイマーから呼び出され，最新フレームを表示する""" + frame = self._zmq_client.receive_image() + if frame is None: + return + self._latest_frame = frame + + # 自動操縦時は操舵量を計算 + if self._is_auto: + output = self._pd_control.compute(frame) + self._throttle = output.throttle + self._steer = output.steer + self._update_control_label() + self._last_detect_result = ( + self._pd_control.last_detect_result + ) + elif self._has_any_overlay(): + # 手動操作中でもオーバーレイ用に線検出を実行 + self._last_detect_result = detect_line( + frame, + self._pd_control.image_params, + ) + else: + self._last_detect_result = None + + self._display_frame(frame) + + def _display_frame(self, frame: np.ndarray) -> None: + """NumPy 配列の画像を QLabel に表示する + + Args: + frame: BGR 形式の画像 + """ + # オーバーレイ描画 + frame = draw_overlay( + frame, self._last_detect_result, + self._overlay_flags, + ) + + # BGR → RGB 変換 + rgb = frame[:, :, ::-1].copy() + h, w, ch = rgb.shape + image = QImage( + rgb.data, w, h, ch * w, + QImage.Format.Format_RGB888, + ) + # スケーリングして表示 + scaled_w = int(w * DISPLAY_SCALE) + scaled_h = int(h * DISPLAY_SCALE) + pixmap = QPixmap.fromImage(image).scaled( + scaled_w, + scaled_h, + Qt.AspectRatioMode.KeepAspectRatio, + Qt.TransformationMode.SmoothTransformation, + ) + self._video_label.setPixmap(pixmap) + + # ── 手動操作 ────────────────────────────────────────── + + def keyPressEvent(self, event: QKeyEvent) -> None: + """キー押下時の処理""" + if event.isAutoRepeat(): + return + + # Q キーで自動操縦切り替え + if event.key() == Qt.Key.Key_Q: + if self._is_connected: + self._toggle_auto() + return + + # 自動操縦中はキー操作を無視 + if self._is_auto: + return + + self._pressed_keys.add(event.key()) + self._update_manual_control() + + def keyReleaseEvent(self, event: QKeyEvent) -> None: + """キー離上時に操舵量を更新する""" + if event.isAutoRepeat() or self._is_auto: + return + self._pressed_keys.discard(event.key()) + self._update_manual_control() + + def _update_manual_control(self) -> None: + """押下中のキーから throttle と steer を計算する""" + keys = self._pressed_keys + + # Space で緊急停止 + if Qt.Key.Key_Space in keys: + self._throttle = 0.0 + self._steer = 0.0 + self._pressed_keys.clear() + # 自動操縦中なら停止 + if self._is_auto: + self._disable_auto() + self._update_control_label() + return + + # throttle: W/↑ で前進，S/↓ で後退 + forward = ( + Qt.Key.Key_W in keys + or Qt.Key.Key_Up in keys + ) + backward = ( + Qt.Key.Key_S in keys + or Qt.Key.Key_Down in keys + ) + if forward and not backward: + self._throttle = MANUAL_THROTTLE + elif backward and not forward: + self._throttle = -MANUAL_THROTTLE + else: + self._throttle = 0.0 + + # steer: A/← で左，D/→ で右 + left = ( + Qt.Key.Key_A in keys + or Qt.Key.Key_Left in keys + ) + right = ( + Qt.Key.Key_D in keys + or Qt.Key.Key_Right in keys + ) + if left and not right: + self._steer = -MANUAL_STEER + elif right and not left: + self._steer = MANUAL_STEER + else: + self._steer = 0.0 + + self._update_control_label() + + def _update_control_label(self) -> None: + """操舵量の表示を更新する""" + self._control_label.setText( + f"throttle: {self._throttle:+.2f}\n" + f"steer: {self._steer:+.2f}" + ) + + def _send_control(self) -> None: + """操舵量を Pi に送信する""" + if not self._is_connected: + return + self._zmq_client.send_control( + self._throttle, self._steer, + ) + + def closeEvent(self, event) -> None: + """ウィンドウを閉じるときに通信を停止する""" + if self._is_connected: + self._disconnect() + event.accept() diff --git a/src/pc/main.py b/src/pc/main.py new file mode 100644 index 0000000..4352aee --- /dev/null +++ b/src/pc/main.py @@ -0,0 +1,22 @@ +""" +main +PC 側アプリケーションのエントリーポイント +""" + +import sys + +from PySide6.QtWidgets import QApplication + +from pc.gui.main_window import MainWindow + + +def main() -> None: + """PC 側 GUI アプリケーションを起動する""" + app = QApplication(sys.argv) + window = MainWindow() + window.show() + sys.exit(app.exec()) + + +if __name__ == "__main__": + main() diff --git a/src/pc/steering/__init__.py b/src/pc/steering/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/pc/steering/__init__.py diff --git a/src/pc/steering/auto_params.py b/src/pc/steering/auto_params.py new file mode 100644 index 0000000..fb3167d --- /dev/null +++ b/src/pc/steering/auto_params.py @@ -0,0 +1,130 @@ +""" +auto_params +パラメータの自動保存・読み込みを管理するモジュール +アプリ起動時に前回のパラメータを復元し，変更時に自動保存する + +ファイル構成: + params/ + ├── control.json PD 制御 + 最後に使用した手法 + ├── detect_current.json 現行手法の画像処理パラメータ + ├── detect_blackhat.json 案A の画像処理パラメータ + ├── detect_dual_norm.json 案B の画像処理パラメータ + └── detect_robust.json 案C の画像処理パラメータ +""" + +import json +from dataclasses import asdict +from pathlib import Path + +from pc.steering.pd_control import PdParams +from pc.vision.line_detector import ImageParams + +# パラメータ保存ディレクトリ +_PARAMS_DIR: Path = ( + Path(__file__).resolve().parent.parent.parent.parent + / "params" +) + +# PD 制御パラメータファイル +_CONTROL_FILE: Path = _PARAMS_DIR / "control.json" + +# 検出手法ごとのファイル名 +_DETECT_FILES: dict[str, str] = { + "current": "detect_current.json", + "blackhat": "detect_blackhat.json", + "dual_norm": "detect_dual_norm.json", + "robust": "detect_robust.json", +} + + +def save_control( + params: PdParams, method: str, +) -> None: + """PD 制御パラメータと最後に使用した手法を保存する + + Args: + params: PD 制御パラメータ + method: 最後に使用した検出手法の識別子 + """ + _PARAMS_DIR.mkdir(exist_ok=True) + data = asdict(params) + data["last_method"] = method + _write_json(_CONTROL_FILE, data) + + +def load_control() -> tuple[PdParams, str]: + """PD 制御パラメータと最後に使用した手法を読み込む + + Returns: + (PD 制御パラメータ, 最後に使用した手法の識別子) + """ + if not _CONTROL_FILE.exists(): + return PdParams(), "current" + + data = _read_json(_CONTROL_FILE) + method = data.pop("last_method", "current") + known = PdParams.__dataclass_fields__ + filtered = { + k: v for k, v in data.items() + if k in known + } + return PdParams(**filtered), method + + +def save_detect_params( + method: str, params: ImageParams, +) -> None: + """検出手法のパラメータを保存する + + Args: + method: 検出手法の識別子 + params: 画像処理パラメータ + """ + filename = _DETECT_FILES.get(method) + if filename is None: + return + _PARAMS_DIR.mkdir(exist_ok=True) + data = asdict(params) + data["method"] = method + _write_json(_PARAMS_DIR / filename, data) + + +def load_detect_params(method: str) -> ImageParams: + """検出手法のパラメータを読み込む + + Args: + method: 検出手法の識別子 + + Returns: + 画像処理パラメータ（ファイルがない場合はデフォルト） + """ + filename = _DETECT_FILES.get(method) + if filename is None: + return ImageParams(method=method) + + path = _PARAMS_DIR / filename + if not path.exists(): + return ImageParams(method=method) + + data = _read_json(path) + known = ImageParams.__dataclass_fields__ + filtered = { + k: v for k, v in data.items() + if k in known + } + filtered["method"] = method + return ImageParams(**filtered) + + +def _write_json(path: Path, data: dict) -> None: + """JSON ファイルに書き込む""" + with open(path, "w", encoding="utf-8") as f: + json.dump( + data, f, ensure_ascii=False, indent=2, + ) + + +def _read_json(path: Path) -> dict: + """JSON ファイルを読み込む""" + with open(path, "r", encoding="utf-8") as f: + return json.load(f) diff --git a/src/pc/steering/base.py b/src/pc/steering/base.py new file mode 100644 index 0000000..40eff1a --- /dev/null +++ b/src/pc/steering/base.py @@ -0,0 +1,50 @@ +""" +base +操舵量計算の共通インターフェースを定義するモジュール +全ての操舵量計算クラスはこのインターフェースに従う +""" + +from abc import ABC, abstractmethod +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class SteeringOutput: + """操舵量計算の出力を格納するデータクラス + + Attributes: + throttle: 前後方向の出力 (-1.0 ~ +1.0) + steer: 左右方向の出力 (-1.0 ~ +1.0) + """ + throttle: float + steer: float + + +class SteeringBase(ABC): + """操舵量計算の基底クラス + + 全ての操舵量計算クラスはこのクラスを継承し， + compute メソッドを実装する + """ + + @abstractmethod + def compute( + self, frame: np.ndarray, + ) -> SteeringOutput: + """カメラ画像から操舵量を計算する + + Args: + frame: BGR 形式のカメラ画像 + + Returns: + 計算された操舵量 + """ + + @abstractmethod + def reset(self) -> None: + """内部状態をリセットする + + 自動操縦の開始時に呼び出される + """ diff --git a/src/pc/steering/param_store.py b/src/pc/steering/param_store.py new file mode 100644 index 0000000..c51ee3f --- /dev/null +++ b/src/pc/steering/param_store.py @@ -0,0 +1,159 @@ +""" +param_store +パラメータプリセットの保存・読み込みを管理するモジュール +画像処理パラメータと PD 制御パラメータを独立して管理する +""" + +import json +from dataclasses import asdict, dataclass +from pathlib import Path + +from pc.steering.pd_control import PdParams +from pc.vision.line_detector import ImageParams + +# プリセット保存ディレクトリ +_PARAMS_DIR: Path = ( + Path(__file__).resolve().parent.parent.parent.parent + / "params" +) + +_PD_FILE: Path = _PARAMS_DIR / "presets_pd.json" +_IMAGE_FILE: Path = _PARAMS_DIR / "presets_image.json" + + +# ── PD 制御プリセット ───────────────────────── + + +@dataclass +class PdPreset: + """PD 制御パラメータのプリセット + + Attributes: + title: プリセットのタイトル + memo: メモ + params: PD 制御パラメータ + """ + + title: str + memo: str + params: PdParams + + +def load_pd_presets() -> list[PdPreset]: + """PD 制御プリセット一覧を読み込む""" + return _load_presets( + _PD_FILE, PdPreset, "params", PdParams, + ) + + +def add_pd_preset(preset: PdPreset) -> None: + """PD 制御プリセットを追加する""" + presets = load_pd_presets() + presets.append(preset) + _save_presets( + _PD_FILE, presets, "params", + ) + + +def delete_pd_preset(index: int) -> None: + """PD 制御プリセットを削除する""" + presets = load_pd_presets() + if 0 <= index < len(presets): + presets.pop(index) + _save_presets( + _PD_FILE, presets, "params", + ) + + +# ── 画像処理プリセット ──────────────────────── + + +@dataclass +class ImagePreset: + """画像処理パラメータのプリセット + + Attributes: + title: プリセットのタイトル + memo: メモ + image_params: 画像処理パラメータ + """ + + title: str + memo: str + image_params: ImageParams + + +def load_image_presets() -> list[ImagePreset]: + """画像処理プリセット一覧を読み込む""" + return _load_presets( + _IMAGE_FILE, ImagePreset, + "image_params", ImageParams, + ) + + +def add_image_preset(preset: ImagePreset) -> None: + """画像処理プリセットを追加する""" + presets = load_image_presets() + presets.append(preset) + _save_presets( + _IMAGE_FILE, presets, "image_params", + ) + + +def delete_image_preset(index: int) -> None: + """画像処理プリセットを削除する""" + presets = load_image_presets() + if 0 <= index < len(presets): + presets.pop(index) + _save_presets( + _IMAGE_FILE, presets, "image_params", + ) + + +# ── 共通処理 ────────────────────────────────── + + +def _load_presets(path, preset_cls, params_key, params_cls): + """プリセットファイルを読み込む""" + if not path.exists(): + return [] + + with open(path, "r", encoding="utf-8") as f: + data = json.load(f) + + known = params_cls.__dataclass_fields__ + presets = [] + for item in data: + if params_key in item: + filtered = { + k: v + for k, v in item[params_key].items() + if k in known + } + params = params_cls(**filtered) + else: + params = params_cls() + + presets.append(preset_cls( + title=item["title"], + memo=item["memo"], + **{params_key: params}, + )) + return presets + + +def _save_presets(path, presets, params_key): + """プリセットファイルに保存する""" + path.parent.mkdir(exist_ok=True) + data = [] + for preset in presets: + data.append({ + "title": preset.title, + "memo": preset.memo, + params_key: asdict( + getattr(preset, params_key), + ), + }) + + with open(path, "w", encoding="utf-8") as f: + json.dump(data, f, ensure_ascii=False, indent=2) diff --git a/src/pc/steering/pd_control.py b/src/pc/steering/pd_control.py new file mode 100644 index 0000000..fde5a4b --- /dev/null +++ b/src/pc/steering/pd_control.py @@ -0,0 +1,128 @@ +""" +pd_control +PD 制御による操舵量計算モジュール +多項式フィッティングの位置・傾き・曲率から操舵量と速度を算出する +""" + +import time +from dataclasses import dataclass + +import numpy as np + +from pc.steering.base import SteeringBase, SteeringOutput +from pc.vision.line_detector import ImageParams, detect_line + + +@dataclass +class PdParams: + """PD 制御のパラメータ + + Attributes: + kp: 位置偏差ゲイン + kh: 傾き（ヘディング）ゲイン + 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 = 0.3 + + +class PdControl(SteeringBase): + """PD 制御による操舵量計算クラス""" + + def __init__( + self, + params: PdParams | None = None, + image_params: ImageParams | None = None, + ) -> None: + self.params: PdParams = params or PdParams() + 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 + + def compute( + self, frame: np.ndarray, + ) -> SteeringOutput: + """カメラ画像から PD 制御で操舵量を計算する + + Args: + frame: BGR 形式のカメラ画像 + + Returns: + 計算された操舵量 + """ + p = self.params + + # 線検出 + result = detect_line(frame, self.image_params) + self._last_result = result + + # 線が検出できなかった場合は停止 + if not result.detected: + return SteeringOutput(throttle=0.0, steer=0.0) + + # 位置偏差 + 傾きによる操舵量 + error = ( + p.kp * result.position_error + + p.kh * result.heading + ) + + # 時間差分の計算 + now = time.time() + dt = ( + now - self._prev_time + if self._prev_time > 0 + else 0.033 + ) + dt = max(dt, 0.001) + + # 微分項 + 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(result.curvature) + ) + 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 + + @property + def last_detect_result(self): + """直近の線検出結果を取得する""" + return self._last_result diff --git a/src/pc/vision/__init__.py b/src/pc/vision/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/pc/vision/__init__.py diff --git a/src/pc/vision/line_detector.py b/src/pc/vision/line_detector.py new file mode 100644 index 0000000..5a97102 --- /dev/null +++ b/src/pc/vision/line_detector.py @@ -0,0 +1,618 @@ +""" +line_detector +カメラ画像から黒線の位置を検出するモジュール +複数の検出手法を切り替えて使用できる +""" + +from dataclasses import dataclass + +import cv2 +import numpy as np + +from common import config + +# 検出領域の y 範囲（画像全体） +DETECT_Y_START: int = 0 +DETECT_Y_END: int = config.FRAME_HEIGHT + +# フィッティングに必要な最小数 +MIN_FIT_PIXELS: int = 50 +MIN_FIT_ROWS: int = 10 + +# 検出手法の定義（キー: 識別子，値: 表示名） +DETECT_METHODS: dict[str, str] = { + "current": "現行（CLAHE + 固定閾値）", + "blackhat": "案A（Black-hat 中心）", + "dual_norm": "案B（二重正規化）", + "robust": "案C（最高ロバスト）", +} + + +@dataclass +class ImageParams: + """画像処理パラメータ + + Attributes: + method: 検出手法の識別子 + clahe_clip: CLAHE のコントラスト増幅上限 + clahe_grid: CLAHE の局所領域分割数 + blur_size: ガウシアンブラーのカーネルサイズ（奇数） + binary_thresh: 二値化の閾値 + open_size: オープニングのカーネルサイズ + close_width: クロージングの横幅 + close_height: クロージングの高さ + blackhat_ksize: Black-hat のカーネルサイズ + bg_blur_ksize: 背景除算のブラーカーネルサイズ + adaptive_block: 適応的閾値のブロックサイズ + adaptive_c: 適応的閾値の定数 C + iso_close_size: 等方クロージングのカーネルサイズ + dist_thresh: 距離変換の閾値 + min_line_width: 行ごと中心抽出の最小線幅 + ransac_thresh: RANSAC の外れ値判定閾値 + ransac_iter: RANSAC の反復回数 + width_near: 画像下端での期待線幅（px，0 で無効） + width_far: 画像上端での期待線幅（px，0 で無効） + width_tolerance: 幅フィルタの上限倍率 + """ + + # 検出手法 + method: str = "current" + + # 現行手法パラメータ + clahe_clip: float = 2.0 + clahe_grid: int = 8 + blur_size: int = 5 + binary_thresh: int = 80 + open_size: int = 5 + close_width: int = 25 + close_height: int = 3 + + # 案A/C: Black-hat + blackhat_ksize: int = 45 + + # 案B: 背景除算 + bg_blur_ksize: int = 101 + + # 案B/C: 適応的閾値 + adaptive_block: int = 51 + adaptive_c: int = 10 + + # 案A/B/C: 後処理 + iso_close_size: int = 15 + dist_thresh: float = 3.0 + min_line_width: int = 3 + + # 案C: RANSAC + ransac_thresh: float = 5.0 + ransac_iter: int = 50 + + # 透視補正付き幅フィルタ（0 で無効） + width_near: int = 0 + width_far: int = 0 + width_tolerance: float = 1.8 + + +@dataclass +class LineDetectResult: + """線検出の結果を格納するデータクラス + + Attributes: + detected: 線が検出できたか + position_error: 画像下端での位置偏差（-1.0～+1.0） + heading: 線の傾き（dx/dy，画像下端での値） + curvature: 線の曲率（d²x/dy²） + poly_coeffs: 多項式の係数（描画用，未検出時は None） + binary_image: 二値化後の画像（デバッグ用） + """ + + detected: bool + position_error: float + heading: float + curvature: float + poly_coeffs: np.ndarray | None + binary_image: np.ndarray | None + + +def detect_line( + frame: np.ndarray, + params: ImageParams | None = None, +) -> LineDetectResult: + """画像から黒線の位置を検出する + + params.method に応じて検出手法を切り替える + + Args: + frame: BGR 形式のカメラ画像 + params: 画像処理パラメータ（None でデフォルト） + + Returns: + 線検出の結果 + """ + if params is None: + params = ImageParams() + + method = params.method + if method == "blackhat": + return _detect_blackhat(frame, params) + if method == "dual_norm": + return _detect_dual_norm(frame, params) + if method == "robust": + return _detect_robust(frame, params) + return _detect_current(frame, params) + + +# ── 検出手法の実装 ───────────────────────────── + + +def _detect_current( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """現行手法: CLAHE + 固定閾値 + 全ピクセルフィッティング""" + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # CLAHE でコントラスト強調 + clahe = cv2.createCLAHE( + clipLimit=params.clahe_clip, + tileGridSize=( + params.clahe_grid, + params.clahe_grid, + ), + ) + enhanced = clahe.apply(gray) + + # ガウシアンブラー + blur_k = params.blur_size | 1 + blurred = cv2.GaussianBlur( + enhanced, (blur_k, blur_k), 0, + ) + + # 固定閾値で二値化（黒線を白に反転） + _, binary = cv2.threshold( + blurred, params.binary_thresh, 255, + cv2.THRESH_BINARY_INV, + ) + + # オープニング（孤立ノイズ除去） + if params.open_size >= 3: + open_k = params.open_size | 1 + open_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (open_k, open_k), + ) + binary = cv2.morphologyEx( + binary, cv2.MORPH_OPEN, open_kernel, + ) + + # 横方向クロージング（途切れ補間） + if params.close_width >= 3: + close_h = max(params.close_height | 1, 1) + close_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, + (params.close_width, close_h), + ) + binary = cv2.morphologyEx( + binary, cv2.MORPH_CLOSE, close_kernel, + ) + + # 全ピクセルフィッティング（従来方式） + return _fit_all_pixels(binary) + + +def _detect_blackhat( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案A: Black-hat 中心型 + + Black-hat 変換で背景より暗い構造を直接抽出し， + 固定閾値 + 距離変換 + 行ごと中心抽出で検出する + """ + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # Black-hat 変換（暗い構造の抽出） + bh_k = params.blackhat_ksize | 1 + bh_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (bh_k, bh_k), + ) + blackhat = cv2.morphologyEx( + gray, cv2.MORPH_BLACKHAT, bh_kernel, + ) + + # ガウシアンブラー + blur_k = params.blur_size | 1 + blurred = cv2.GaussianBlur( + blackhat, (blur_k, blur_k), 0, + ) + + # 固定閾値（Black-hat 後は線が白） + _, binary = cv2.threshold( + blurred, params.binary_thresh, 255, + cv2.THRESH_BINARY, + ) + + # 等方クロージング + 距離変換マスク + 幅フィルタ + binary = _apply_iso_closing( + binary, params.iso_close_size, + ) + binary = _apply_dist_mask( + binary, params.dist_thresh, + ) + if params.width_near > 0 and params.width_far > 0: + binary = _apply_width_filter( + binary, + params.width_near, + params.width_far, + params.width_tolerance, + ) + + # 行ごと中心抽出 + フィッティング + return _fit_row_centers( + binary, params.min_line_width, + ) + + +def _detect_dual_norm( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案B: 二重正規化型 + + 背景除算で照明勾配を除去し， + 適応的閾値で局所ムラにも対応する二重防壁構成 + """ + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # 背景除算正規化 + bg_k = params.bg_blur_ksize | 1 + bg = cv2.GaussianBlur( + gray, (bg_k, bg_k), 0, + ) + normalized = ( + gray.astype(np.float32) * 255.0 + / (bg.astype(np.float32) + 1.0) + ) + normalized = np.clip( + normalized, 0, 255, + ).astype(np.uint8) + + # 適応的閾値（ガウシアン，BINARY_INV） + block = max(params.adaptive_block | 1, 3) + binary = cv2.adaptiveThreshold( + normalized, 255, + cv2.ADAPTIVE_THRESH_GAUSSIAN_C, + cv2.THRESH_BINARY_INV, + block, params.adaptive_c, + ) + + # 等方クロージング + 距離変換マスク + 幅フィルタ + binary = _apply_iso_closing( + binary, params.iso_close_size, + ) + binary = _apply_dist_mask( + binary, params.dist_thresh, + ) + if params.width_near > 0 and params.width_far > 0: + binary = _apply_width_filter( + binary, + params.width_near, + params.width_far, + params.width_tolerance, + ) + + # 行ごと中心抽出 + フィッティング + return _fit_row_centers( + binary, params.min_line_width, + ) + + +def _detect_robust( + frame: np.ndarray, params: ImageParams, +) -> LineDetectResult: + """案C: 最高ロバスト型 + + Black-hat + 適応的閾値の二重正規化に加え， + RANSAC で外れ値を除去する最もロバストな構成 + """ + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # Black-hat 変換 + bh_k = params.blackhat_ksize | 1 + bh_kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (bh_k, bh_k), + ) + blackhat = cv2.morphologyEx( + gray, cv2.MORPH_BLACKHAT, bh_kernel, + ) + + # 適応的閾値（BINARY: Black-hat 後は線が白） + block = max(params.adaptive_block | 1, 3) + binary = cv2.adaptiveThreshold( + blackhat, 255, + cv2.ADAPTIVE_THRESH_GAUSSIAN_C, + cv2.THRESH_BINARY, + block, -params.adaptive_c, + ) + + # 等方クロージング + 距離変換マスク + 幅フィルタ + binary = _apply_iso_closing( + binary, params.iso_close_size, + ) + binary = _apply_dist_mask( + binary, params.dist_thresh, + ) + if params.width_near > 0 and params.width_far > 0: + binary = _apply_width_filter( + binary, + params.width_near, + params.width_far, + params.width_tolerance, + ) + + # 行ごと中央値抽出 + RANSAC フィッティング + return _fit_row_centers( + binary, params.min_line_width, + use_median=True, + ransac_thresh=params.ransac_thresh, + ransac_iter=params.ransac_iter, + ) + + +# ── 共通処理 ─────────────────────────────────── + + +def _apply_iso_closing( + binary: np.ndarray, size: int, +) -> np.ndarray: + """等方クロージングで穴を埋める + + Args: + binary: 二値画像 + size: カーネルサイズ + + Returns: + クロージング後の二値画像 + """ + if size < 3: + return binary + k = size | 1 + kernel = cv2.getStructuringElement( + cv2.MORPH_ELLIPSE, (k, k), + ) + return cv2.morphologyEx( + binary, cv2.MORPH_CLOSE, kernel, + ) + + +def _apply_width_filter( + binary: np.ndarray, + width_near: int, + width_far: int, + tolerance: float, +) -> np.ndarray: + """透視補正付き幅フィルタで広がりすぎた行を除外する + + 各行の期待線幅を線形補間で算出し， + 実際の幅が上限（期待幅 × tolerance）を超える行をマスクする + + Args: + binary: 二値画像 + width_near: 画像下端での期待線幅（px） + width_far: 画像上端での期待線幅（px） + tolerance: 上限倍率 + + Returns: + 幅フィルタ適用後の二値画像 + """ + result = binary.copy() + h = binary.shape[0] + denom = max(h - 1, 1) + + for y_local in range(h): + xs = np.where(binary[y_local] > 0)[0] + if len(xs) == 0: + continue + # 画像下端（近距離）ほど t=1，上端（遠距離）ほど t=0 + t = (h - 1 - y_local) / denom + expected = float(width_far) + ( + float(width_near) - float(width_far) + ) * t + max_w = expected * tolerance + actual_w = int(xs[-1]) - int(xs[0]) + 1 + if actual_w > max_w: + result[y_local] = 0 + + return result + + +def _apply_dist_mask( + binary: np.ndarray, thresh: float, +) -> np.ndarray: + """距離変換で中心部のみを残す + + Args: + binary: 二値画像 + thresh: 距離の閾値（ピクセル） + + Returns: + 中心部のみの二値画像 + """ + if thresh <= 0: + return binary + dist = cv2.distanceTransform( + binary, cv2.DIST_L2, 5, + ) + _, mask = cv2.threshold( + dist, thresh, 255, cv2.THRESH_BINARY, + ) + return mask.astype(np.uint8) + + +def _fit_all_pixels( + binary: np.ndarray, +) -> LineDetectResult: + """全白ピクセルに多項式をフィッティングする + + 従来方式．全ピクセルを等しく扱うため， + 陰で幅が広がった行がフィッティングを支配する弱点がある + + Args: + binary: 二値画像 + + Returns: + 線検出の結果 + """ + region = binary[DETECT_Y_START:DETECT_Y_END, :] + ys_local, xs = np.where(region > 0) + + if len(xs) < MIN_FIT_PIXELS: + return _no_detection(binary) + + ys = ys_local + DETECT_Y_START + coeffs = np.polyfit(ys, xs, 2) + return _build_result(coeffs, binary) + + +def _fit_row_centers( + binary: np.ndarray, + min_width: int, + use_median: bool = False, + ransac_thresh: float = 0.0, + ransac_iter: int = 0, +) -> LineDetectResult: + """行ごとの中心点に多項式をフィッティングする + + 各行の白ピクセルの中心（平均または中央値）を1点抽出し， + 中心点列に対してフィッティングする． + 幅の変動に強く，各行が等しく寄与する + + Args: + binary: 二値画像 + min_width: 線として認識する最小ピクセル数 + use_median: True の場合は中央値を使用 + ransac_thresh: RANSAC 閾値（0 以下で無効） + ransac_iter: RANSAC 反復回数 + + Returns: + 線検出の結果 + """ + region = binary[DETECT_Y_START:DETECT_Y_END, :] + centers_y: list[float] = [] + centers_x: list[float] = [] + + for y_local in range(region.shape[0]): + xs = np.where(region[y_local] > 0)[0] + if len(xs) < min_width: + continue + y = float(y_local + DETECT_Y_START) + centers_y.append(y) + if use_median: + centers_x.append(float(np.median(xs))) + else: + centers_x.append(float(np.mean(xs))) + + if len(centers_y) < MIN_FIT_ROWS: + return _no_detection(binary) + + cy = np.array(centers_y) + cx = np.array(centers_x) + + if ransac_thresh > 0 and ransac_iter > 0: + coeffs = _ransac_polyfit( + cy, cx, 2, ransac_iter, ransac_thresh, + ) + if coeffs is None: + return _no_detection(binary) + else: + coeffs = np.polyfit(cy, cx, 2) + + return _build_result(coeffs, binary) + + +def _ransac_polyfit( + ys: np.ndarray, xs: np.ndarray, + degree: int, n_iter: int, thresh: float, +) -> np.ndarray | None: + """RANSAC で外れ値を除去して多項式フィッティング + + Args: + ys: y 座標配列 + xs: x 座標配列 + degree: 多項式の次数 + n_iter: 反復回数 + thresh: 外れ値判定閾値（ピクセル） + + Returns: + 多項式係数（フィッティング失敗時は None） + """ + n = len(ys) + sample_size = degree + 1 + if n < sample_size: + return None + + best_coeffs: np.ndarray | None = None + best_inliers = 0 + rng = np.random.default_rng() + + for _ in range(n_iter): + idx = rng.choice(n, sample_size, replace=False) + coeffs = np.polyfit(ys[idx], xs[idx], degree) + poly = np.poly1d(coeffs) + residuals = np.abs(xs - poly(ys)) + n_inliers = int(np.sum(residuals < thresh)) + if n_inliers > best_inliers: + best_inliers = n_inliers + best_coeffs = coeffs + + # インライアで再フィッティング + if best_coeffs is not None: + poly = np.poly1d(best_coeffs) + inlier_mask = np.abs(xs - poly(ys)) < thresh + if np.sum(inlier_mask) >= sample_size: + best_coeffs = np.polyfit( + ys[inlier_mask], + xs[inlier_mask], + degree, + ) + + return best_coeffs + + +def _no_detection( + binary: np.ndarray, +) -> LineDetectResult: + """未検出の結果を返す""" + return LineDetectResult( + detected=False, + position_error=0.0, + heading=0.0, + curvature=0.0, + poly_coeffs=None, + binary_image=binary, + ) + + +def _build_result( + coeffs: np.ndarray, + binary: np.ndarray, +) -> LineDetectResult: + """多項式係数から LineDetectResult を構築する""" + poly = np.poly1d(coeffs) + center_x = config.FRAME_WIDTH / 2.0 + + # 画像下端での位置偏差 + x_bottom = poly(DETECT_Y_END) + position_error = (center_x - x_bottom) / center_x + + # 傾き: dx/dy（画像下端での値） + poly_deriv = poly.deriv() + heading = float(poly_deriv(DETECT_Y_END)) + + # 曲率: d²x/dy² + poly_deriv2 = poly_deriv.deriv() + curvature = float(poly_deriv2(DETECT_Y_END)) + + return LineDetectResult( + detected=True, + position_error=position_error, + heading=heading, + curvature=curvature, + poly_coeffs=coeffs, + binary_image=binary, + ) diff --git a/src/pc/vision/overlay.py b/src/pc/vision/overlay.py new file mode 100644 index 0000000..188a1e8 --- /dev/null +++ b/src/pc/vision/overlay.py @@ -0,0 +1,186 @@ +""" +overlay +画像処理の結果をカメラ映像に重ねて描画するモジュール +チェックボックスで個別に ON/OFF できる +""" + +from dataclasses import dataclass + +import cv2 +import numpy as np + +from common import config +from pc.vision import line_detector +from pc.vision.line_detector import LineDetectResult + +# 描画色の定義 (BGR) +COLOR_LINE: tuple = (0, 255, 0) +COLOR_CENTER: tuple = (0, 255, 255) +COLOR_TEXT: tuple = (255, 255, 255) +COLOR_REGION: tuple = (255, 0, 0) + +# 二値化オーバーレイの不透明度 +BINARY_OPACITY: float = 0.4 + + +@dataclass +class OverlayFlags: + """オーバーレイ表示項目のフラグ + + Attributes: + binary: 二値化画像の半透明表示 + detect_region: 検出領域の枠 + poly_curve: フィッティング曲線 + center_line: 画像中心線 + info_text: 検出情報の数値表示 + """ + binary: bool = False + detect_region: bool = False + poly_curve: bool = False + center_line: bool = False + info_text: bool = False + + +def draw_overlay( + frame: np.ndarray, + result: LineDetectResult | None, + flags: OverlayFlags, +) -> np.ndarray: + """カメラ映像にオーバーレイを描画する + + Args: + frame: 元の BGR カメラ画像 + result: 線検出の結果（None の場合はオーバーレイなし） + flags: 表示項目のフラグ + + Returns: + オーバーレイ描画済みの画像 + """ + display = frame.copy() + + if result is None: + return display + + # 二値化画像の半透明オーバーレイ + if flags.binary and result.binary_image is not None: + display = _draw_binary_overlay( + display, result.binary_image, + ) + + # 検出領域の枠 + if flags.detect_region: + cv2.rectangle( + display, + (0, line_detector.DETECT_Y_START), + ( + config.FRAME_WIDTH - 1, + line_detector.DETECT_Y_END - 1, + ), + COLOR_REGION, 1, + ) + + # 画像中心線 + if flags.center_line: + center_x = config.FRAME_WIDTH // 2 + cv2.line( + display, + (center_x, 0), + (center_x, config.FRAME_HEIGHT), + COLOR_CENTER, 1, + ) + + # フィッティング曲線 + if flags.poly_curve and result.poly_coeffs is not None: + _draw_poly_curve(display, result.poly_coeffs) + + # 検出情報の数値表示 + if flags.info_text: + _draw_info_text(display, result) + + return display + + +def _draw_binary_overlay( + frame: np.ndarray, + binary: np.ndarray, +) -> np.ndarray: + """二値化画像を半透明で重ねる + + Args: + frame: 元の BGR 画像 + binary: 二値化画像（グレースケール） + + Returns: + 合成された画像 + """ + binary_bgr = np.zeros_like(frame) + binary_bgr[:, :, 2] = binary + + return cv2.addWeighted( + frame, 1.0 - BINARY_OPACITY, + binary_bgr, BINARY_OPACITY, 0, + ) + + +def _draw_poly_curve( + frame: np.ndarray, + coeffs: np.ndarray, +) -> None: + """フィッティング曲線を描画する + + Args: + frame: 描画先の画像 + coeffs: 多項式の係数 + """ + poly = np.poly1d(coeffs) + y_start = line_detector.DETECT_Y_START + y_end = line_detector.DETECT_Y_END + + # 曲線上の点を生成 + ys = np.arange(y_start, y_end) + xs = poly(ys) + + # 画像範囲内の点のみ描画 + points = [] + for x, y in zip(xs, ys): + ix = int(round(x)) + if 0 <= ix < config.FRAME_WIDTH: + points.append([ix, int(y)]) + + if len(points) >= 2: + pts = np.array(points, dtype=np.int32) + cv2.polylines( + frame, [pts], False, + COLOR_LINE, 2, + ) + + +def _draw_info_text( + frame: np.ndarray, + result: LineDetectResult, +) -> None: + """検出情報の数値を画像に描画する + + Args: + frame: 描画先の画像 + result: 線検出の結果 + """ + if not result.detected: + cv2.putText( + frame, "LINE: N/A", (5, 15), + cv2.FONT_HERSHEY_SIMPLEX, 0.4, + COLOR_TEXT, 1, + ) + return + + lines = [ + f"pos: {result.position_error:+.3f}", + f"head: {result.heading:+.4f}", + f"curv: {result.curvature:+.6f}", + ] + for i, text in enumerate(lines): + cv2.putText( + frame, text, (5, 15 + i * 15), + cv2.FONT_HERSHEY_SIMPLEX, 0.35, + COLOR_TEXT, 1, + ) diff --git a/src/pi/__init__.py b/src/pi/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/pi/__init__.py diff --git a/src/pi/camera/__init__.py b/src/pi/camera/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/pi/camera/__init__.py diff --git a/src/pi/camera/capture.py b/src/pi/camera/capture.py new file mode 100644 index 0000000..68fdbc7 --- /dev/null +++ b/src/pi/camera/capture.py @@ -0,0 +1,43 @@ +""" +capture +Picamera2 を使用してカメラ画像を取得するモジュール +""" + +import numpy as np +from picamera2 import Picamera2 + +from common import config + + +class CameraCapture: + """Picamera2 でフレームを取得するクラス""" + + def __init__(self) -> None: + self._camera: Picamera2 | None = None + + def start(self) -> None: + """カメラを初期化して撮影を開始する""" + self._camera = Picamera2() + camera_config = self._camera.create_preview_configuration( + main={ + "size": (config.FRAME_WIDTH, config.FRAME_HEIGHT), + "format": "BGR888", + }, + ) + self._camera.configure(camera_config) + self._camera.start() + + def capture(self) -> np.ndarray: + """1フレームを取得する + + Returns: + BGR 形式の画像（NumPy 配列） + """ + return self._camera.capture_array() + + def stop(self) -> None: + """カメラを停止する""" + if self._camera is not None: + self._camera.stop() + self._camera.close() + self._camera = None diff --git a/src/pi/comm/__init__.py b/src/pi/comm/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/pi/comm/__init__.py diff --git a/src/pi/comm/zmq_client.py b/src/pi/comm/zmq_client.py new file mode 100644 index 0000000..f553511 --- /dev/null +++ b/src/pi/comm/zmq_client.py @@ -0,0 +1,93 @@ +""" +zmq_client +Pi 側の ZMQ 通信を担当するモジュール +画像の送信と操舵量の受信を行う +""" + +import json +import time + +import cv2 +import numpy as np +import zmq + +from common import config + + +class PiZmqClient: + """Pi 側の ZMQ 通信クライアント + + 画像送信（PUB）と操舵量受信（SUB）の2チャネルを管理する + """ + + def __init__(self) -> None: + self._context = zmq.Context() + self._image_socket: zmq.Socket | None = None + self._control_socket: zmq.Socket | None = None + self._last_receive_time: float = 0.0 + + def start(self) -> None: + """通信ソケットを初期化して接続する""" + + # 画像送信ソケット（PUB，PC へ画像を送信） + self._image_socket = self._context.socket(zmq.PUB) + self._image_socket.setsockopt(zmq.CONFLATE, 1) + self._image_socket.connect(config.image_connect_address()) + + # 操舵量受信ソケット（SUB，PC からの操舵量を受信） + self._control_socket = self._context.socket(zmq.SUB) + self._control_socket.setsockopt(zmq.CONFLATE, 1) + self._control_socket.setsockopt_string(zmq.SUBSCRIBE, "") + self._control_socket.connect(config.control_connect_address()) + + self._last_receive_time = time.time() + + def send_image(self, frame: np.ndarray) -> None: + """画像を JPEG 圧縮して送信する + + Args: + frame: カメラから取得した画像の NumPy 配列 + """ + if self._image_socket is None: + return + _, encoded = cv2.imencode( + ".jpg", + frame, + [cv2.IMWRITE_JPEG_QUALITY, config.JPEG_QUALITY], + ) + self._image_socket.send(encoded.tobytes(), zmq.NOBLOCK) + + def receive_control(self) -> tuple[float, float] | None: + """操舵量を非ブロッキングで受信する + + Returns: + (throttle, steer) のタプル，受信データがない場合は None + """ + if self._control_socket is None: + return None + try: + data = self._control_socket.recv(zmq.NOBLOCK) + payload = json.loads(data.decode("utf-8")) + self._last_receive_time = time.time() + return (payload["throttle"], payload["steer"]) + except zmq.Again: + return None + + def is_timeout(self) -> bool: + """操舵量の受信がタイムアウトしたか判定する + + Returns: + タイムアウトしていれば True + """ + elapsed = time.time() - self._last_receive_time + return elapsed > config.CONTROL_TIMEOUT_SEC + + def stop(self) -> None: + """通信ソケットを閉じる""" + if self._image_socket is not None: + self._image_socket.close() + self._image_socket = None + if self._control_socket is not None: + self._control_socket.close() + self._control_socket = None + self._context.term() diff --git a/src/pi/main.py b/src/pi/main.py new file mode 100644 index 0000000..17b7630 --- /dev/null +++ b/src/pi/main.py @@ -0,0 +1,52 @@ +""" +main +Pi 側アプリケーションのエントリーポイント +カメラ画像の送信と操舵量の受信・モーター制御を行う +""" + +import time + +from pi.camera.capture import CameraCapture +from pi.comm.zmq_client import PiZmqClient +from pi.motor.driver import MotorDriver + + +def main() -> None: + """Pi 側のメインループを実行する""" + camera = CameraCapture() + zmq_client = PiZmqClient() + motor = MotorDriver() + + try: + camera.start() + zmq_client.start() + motor.start() + print("Pi: カメラ・通信・モーターを開始") + + while True: + # カメラ画像を取得して送信 + frame = camera.capture() + zmq_client.send_image(frame) + + # 操舵量を受信してモーターに反映 + control = zmq_client.receive_control() + if control is not None: + throttle, steer = control + motor.set_drive(throttle, steer) + + # タイムアウト時はモーター停止 + if zmq_client.is_timeout(): + motor.stop() + + time.sleep(0.01) + + except KeyboardInterrupt: + print("\nPi: 終了") + finally: + motor.cleanup() + camera.stop() + zmq_client.stop() + + +if __name__ == "__main__": + main() diff --git a/src/pi/motor/__init__.py b/src/pi/motor/__init__.py new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/src/pi/motor/__init__.py diff --git a/src/pi/motor/driver.py b/src/pi/motor/driver.py new file mode 100644 index 0000000..7e0dd99 --- /dev/null +++ b/src/pi/motor/driver.py @@ -0,0 +1,132 @@ +""" +driver +TB6612FNG モータードライバを制御するモジュール +差動2輪駆動で左右のモーターを制御する +""" + +from common import config + +try: + import RPi.GPIO as GPIO +except Exception: + GPIO = None + + +class MotorDriver: + """TB6612FNG を介して左右のモーターを制御するクラス""" + + def __init__(self) -> None: + self._pwm_a: object = None + self._pwm_b: object = None + self._ready: bool = False + + def start(self) -> None: + """GPIO を初期化して PWM を開始する""" + if GPIO is None: + print("GPIO 未検出: モーター無効（非 RPi 環境）") + return + + GPIO.setmode(GPIO.BOARD) + + # モーター A（左） + GPIO.setup(config.MA_IN1, GPIO.OUT, initial=GPIO.LOW) + GPIO.setup(config.MA_IN2, GPIO.OUT, initial=GPIO.LOW) + GPIO.setup(config.MA_PWM, GPIO.OUT, initial=GPIO.LOW) + + # モーター B（右） + GPIO.setup(config.MB_IN1, GPIO.OUT, initial=GPIO.LOW) + GPIO.setup(config.MB_IN2, GPIO.OUT, initial=GPIO.LOW) + GPIO.setup(config.MB_PWM, GPIO.OUT, initial=GPIO.LOW) + + self._pwm_a = GPIO.PWM( + config.MA_PWM, config.MOTOR_PWM_FREQ, + ) + self._pwm_b = GPIO.PWM( + config.MB_PWM, config.MOTOR_PWM_FREQ, + ) + self._pwm_a.start(0) + self._pwm_b.start(0) + self._ready = True + print("モーター初期化完了") + + @staticmethod + def _clamp( + value: float, low: float, high: float, + ) -> float: + """値を指定範囲に制限する""" + return max(low, min(high, value)) + + def _apply_one_motor( + self, in1: int, in2: int, pwm: object, + speed: float, + ) -> None: + """1つのモーターに速度を適用する + + Args: + in1: IN1 の GPIO ピン番号 + in2: IN2 の GPIO ピン番号 + pwm: PWM オブジェクト + speed: -1.0 ~ +1.0 の速度値 + """ + if speed > 0: + GPIO.output(in1, GPIO.LOW) + GPIO.output(in2, GPIO.HIGH) + elif speed < 0: + GPIO.output(in1, GPIO.HIGH) + GPIO.output(in2, GPIO.LOW) + else: + GPIO.output(in1, GPIO.LOW) + GPIO.output(in2, GPIO.LOW) + pwm.ChangeDutyCycle(abs(speed) * 100.0) + + def set_drive( + self, throttle: float, steer: float, + ) -> None: + """throttle と steer からモーターを駆動する + + Args: + throttle: 前後方向 (-1.0 ~ +1.0) + steer: 左右方向 (-1.0 ~ +1.0) + """ + if not self._ready: + return + + throttle = self._clamp(throttle, -1.0, 1.0) + steer = self._clamp(steer, -1.0, 1.0) + + # ステアリング方向の補正 + if config.STEER_REVERSED: + steer = -steer + + # 差動2輪: 左右の速度を計算 + left = self._clamp(throttle + steer, -1.0, 1.0) + right = self._clamp(throttle - steer, -1.0, 1.0) + + # モーター配線の極性補正 + if config.MOTOR_LEFT_REVERSED: + left = -left + if config.MOTOR_RIGHT_REVERSED: + right = -right + + self._apply_one_motor( + config.MA_IN1, config.MA_IN2, + self._pwm_a, left, + ) + self._apply_one_motor( + config.MB_IN1, config.MB_IN2, + self._pwm_b, right, + ) + + def stop(self) -> None: + """モーターを停止する""" + self.set_drive(0.0, 0.0) + + def cleanup(self) -> None: + """GPIO リソースを解放する""" + if not self._ready: + return + self.stop() + self._pwm_a.stop() + self._pwm_b.stop() + GPIO.cleanup() + self._ready = False