diff --git a/.gitignore b/.gitignore index 050c8ab..ea2b0bb 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,9 @@ # 環境変数 .env +# PD パラメータ保存ファイル +pd_params.json + # 旧コード（参照用，Git 管理外） src_old/ 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 bc6e7d7..89542ff 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,62 +29,85 @@ 1. カメラからフレームを取得する． 2. グレースケールに変換する． - 3. ガウシアンブラーを適用する（軽いノイズ除去）． - 4. 固定閾値で二値化する（BINARY_INV）． - 5. 横方向クロージングで反射による途切れを補間する． - 6. 近方領域・遠方領域それぞれで黒ピクセルの重心 x 座標を算出する． + 3. CLAHE でコントラストを強調する（照明ムラの影響を低減）． + 4. ガウシアンブラーを適用する（軽いノイズ除去）． + 5. 固定閾値で二値化する（BINARY_INV）． + 6. オープニングで孤立ノイズを除去する． + 7. 横方向クロージングで反射による途切れを補間する． + 8. 検出領域内の白ピクセルに2次多項式をフィッティングする． - ■ 横方向クロージング + ■ CLAHE（コントラスト制限付き適応ヒストグラム均等化） + + 照明ムラや反射により黒線と灰色の床の差が小さくなる問題への + 対策として，グレースケール変換後に CLAHE を適用する． + 局所的にコントラストを強調し，黒線と背景の分離を改善する． + + ・clahe_clip: コントラスト増幅の上限（デフォルト: 2.0） + ・clahe_grid: 局所領域の分割数（デフォルト: 8） + + ■ オープニング（収縮→膨張） + + 影や汚れにより発生する孤立した小さなノイズピクセルを除去する． + 小さい楕円カーネルで収縮→膨張を行い，小さな塊を消す． + + ・open_size: カーネルサイズ（デフォルト: 5） + ・効果: 線とつながっていない孤立ピクセルを除去 + + ■ 横方向クロージング（膨張→収縮） 光の反射により線が途切れる問題への対策として， - 二値化後にモルフォロジーのクロージング処理（膨張→収縮）を行う． + 二値化後にモルフォロジーのクロージング処理を行う． 横長の楕円カーネルを使用することで，縦方向への影響を抑えつつ 線の途切れを左右から埋める． - ・カーネル形状: 横長楕円（幅 25 × 高さ 3） + ・close_width: カーネルの横幅（デフォルト: 25） + ・close_height: カーネルの高さ（デフォルト: 3） ・効果: 反射で欠けた部分を，左右の検出済みピクセルからつなぐ - ・注意: カーネル幅は途切れの大きさに応じて調整する - 2-2. 2領域方式 + 2-2. 多項式フィッティング - カメラは真下を向いているため，画像内の位置が路面上の距離に対応する． - 画像を上下2つの領域に分割し，それぞれで線の位置を検出する． + 検出領域（画像全体）内の白ピクセル座標に対して， + 2次多項式 x = f(y) = ay² + by + c をフィッティングする． + これにより線全体の形状を1つの式で表現し，以下の情報を得る． - 画像上部 ─── 遠方領域（これから通る場所） - │ - 画像下部 ─── 近方領域（今いる場所） - - ・近方領域: 画像下部の水平帯．現在の車体位置に対する線のずれを示す． - ・遠方領域: 画像上部の水平帯．この先のコース方向を示す． - - ※ 各領域の具体的な y 座標範囲は実走テストで調整する． + ・位置偏差: 画像下端での 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) @@ -90,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 項を使用しない． 理由: カーブ中に偏差が蓄積し，カーブ出口でオーバーシュート @@ -117,49 +155,48 @@ 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 - ・ブラーカーネルサイズ: 5 - ・二値化閾値: 80（固定閾値） - ・クロージングカーネル: 幅 25 × 高さ 3（横長楕円） - ・近方領域の y 範囲: 画像高さの 70% ～ 100% - ・遠方領域の y 範囲: 画像高さの 30% ～ 50% + ・CLAHE 強度（clahe_clip）: 2.0 + ・CLAHE 分割数（clahe_grid）: 8 + ・ブラーカーネルサイズ（blur_size）: 5 + ・二値化閾値（binary_thresh）: 80 + ・オープニングサイズ（open_size）: 5 + ・クロージング横幅（close_width）: 25 + ・クロージング高さ（close_height）: 3 - ■ 偏差合成パラメータ + ■ PD 制御パラメータ（GUI で調整可能） - ・α（近方の重み）: 実走テストで決定 - ・β（遠方の重み）: 実走テストで決定 + ・Kp（位置偏差ゲイン）: 0.5 + ・Kh（傾きゲイン）: 0.3 + ・Kd（微分ゲイン）: 0.1 + ・max_steer_rate（最大操舵変化量）: 0.1 - ■ PD 制御パラメータ + ■ 速度制御パラメータ（GUI で調整可能） - ・Kp（比例ゲイン）: 実走テストで決定 - ・Kd（微分ゲイン）: 実走テストで決定 + ・max_throttle（最大速度）: 0.4 + ・speed_k（曲率減速係数）: 0.3 - ■ 速度制御パラメータ + ■ パラメータ管理 - ・max_throttle（最大速度）: 実走テストで決定 - ・k（減速係数）: 実走テストで決定 + ・全パラメータ（画像処理 + 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" index d95088c..3793ab1 100644 --- "a/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" +++ "b/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" @@ -25,17 +25,16 @@ 2-1. 一覧 ・二値化画像: 二値化結果を赤色の半透明で重ねる（不透明度 0.4） - ・近方領域: 近方 ROI の枠を緑色で表示 - ・遠方領域: 遠方 ROI の枠を青色で表示 - ・重心マーカー: 検出した重心位置に円を描画 + ・検出領域: 検出対象領域の枠を青色で表示 + ・フィッティング曲線: 多項式の曲線を緑色で描画 ・中心線: 画像の中心 x に垂直線を描画（黄色） - ・偏差値: 近方・遠方の偏差数値を画像左上に表示 + ・検出情報: 位置偏差・傾き・曲率の数値を画像左上に表示 2-2. 描画色 (BGR) - ・近方関連（枠・重心）: (0, 255, 0) 緑 - ・遠方関連（枠・重心）: (255, 0, 0) 青 + ・フィッティング曲線: (0, 255, 0) 緑 ・中心線: (0, 255, 255) 黄 + ・検出領域: (255, 0, 0) 青 ・テキスト: (255, 255, 255) 白 ・二値化オーバーレイ: 赤チャンネルに二値化画像を割り当て 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 bcc2b37..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" @@ -28,6 +28,7 @@ ├── requirements_pi.txt ├── deploy.sh Pi への転送スクリプト ├── .env.example 環境変数テンプレート + ├── pd_params.json パラメータ保存ファイル（.gitignore） ├── docs/ ドキュメント ├── src/ 自律走行用ソースコード │ ├── common/ 共通設定（PC・Pi 両方で使用） @@ -53,9 +54,10 @@ │ └── zmq_client.py ZMQ 送受信 ├── steering/ 操舵量計算（独立モジュール） │ ├── base.py 共通インターフェース - │ └── pd_control.py PD 制御の実装 + │ ├── pd_control.py PD 制御の実装 + │ └── param_store.py パラメータ保存・読み込み └── vision/ 画像処理 - ├── line_detector.py 線検出 + ├── line_detector.py 線検出（多項式フィッティング） └── overlay.py デバッグオーバーレイ描画 2-4. src/pi/ diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index aea0edc..f43b77f 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -9,13 +9,17 @@ from PySide6.QtGui import QImage, QKeyEvent, QPixmap from PySide6.QtWidgets import ( QCheckBox, + QComboBox, QDoubleSpinBox, QFormLayout, QGroupBox, QHBoxLayout, + QInputDialog, QLabel, QMainWindow, + QMessageBox, QPushButton, + QSpinBox, QVBoxLayout, QWidget, ) @@ -23,7 +27,17 @@ from common import config from pc.comm.zmq_client import PcZmqClient from pc.steering.pd_control import PdControl, PdParams -from pc.vision.line_detector import LineDetectResult, detect_line +from pc.steering.param_store import ( + ParamEntry, + add_entry, + delete_entry, + load_entries, +) +from pc.vision.line_detector import ( + ImageParams, + LineDetectResult, + detect_line, +) from pc.vision.overlay import OverlayFlags, draw_overlay # 映像更新間隔 (ms) @@ -132,6 +146,12 @@ # PD パラメータ調整 self._setup_param_ui(control_layout) + # 画像処理パラメータ調整 + self._setup_image_param_ui(control_layout) + + # パラメータ保存・読み込み + self._setup_param_store_ui(control_layout) + # デバッグ表示 self._setup_overlay_ui(control_layout) @@ -163,22 +183,22 @@ self._spin_kp = self._create_spin( params.kp, 0.0, 5.0, 0.05, ) - form.addRow("Kp:", self._spin_kp) + 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) + form.addRow("Kd (微分):", self._spin_kd) - self._spin_alpha = self._create_spin( - params.alpha, 0.0, 1.0, 0.1, + self._spin_max_steer_rate = self._create_spin( + params.max_steer_rate, 0.01, 1.0, 0.01, ) - form.addRow("α (近方):", self._spin_alpha) - - self._spin_beta = self._create_spin( - params.beta, 0.0, 1.0, 0.1, - ) - form.addRow("β (遠方):", self._spin_beta) + form.addRow("操舵制限:", self._spin_max_steer_rate) self._spin_max_throttle = self._create_spin( params.max_throttle, 0.0, 1.0, 0.05, @@ -186,32 +206,239 @@ form.addRow("最大速度:", self._spin_max_throttle) self._spin_speed_k = self._create_spin( - params.speed_k, 0.0, 2.0, 0.05, + params.speed_k, 0.0, 5.0, 0.05, ) form.addRow("減速係数:", self._spin_speed_k) # パラメータ変更時のコールバック - self._spin_kp.valueChanged.connect( - self._on_param_changed, - ) - self._spin_kd.valueChanged.connect( - self._on_param_changed, - ) - self._spin_alpha.valueChanged.connect( - self._on_param_changed, - ) - self._spin_beta.valueChanged.connect( - self._on_param_changed, - ) - self._spin_max_throttle.valueChanged.connect( - self._on_param_changed, - ) - self._spin_speed_k.valueChanged.connect( - self._on_param_changed, - ) + 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, + ) parent_layout.addWidget(group) + def _setup_image_param_ui( + self, parent_layout: QVBoxLayout, + ) -> None: + """画像処理パラメータ調整 UI を構築する""" + group = QGroupBox("画像処理パラメータ") + form = QFormLayout() + group.setLayout(form) + + ip = self._pd_control.image_params + + self._spin_clahe_clip = self._create_spin( + ip.clahe_clip, 0.5, 10.0, 0.5, + ) + form.addRow("CLAHE強度:", self._spin_clahe_clip) + + self._spin_binary_thresh = QSpinBox() + self._spin_binary_thresh.setRange(10, 200) + self._spin_binary_thresh.setValue(ip.binary_thresh) + form.addRow("二値化閾値:", self._spin_binary_thresh) + + 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) + form.addRow("ノイズ除去:", self._spin_open_size) + + 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) + form.addRow("途切れ補間:", self._spin_close_width) + + # パラメータ変更時のコールバック + for spin in [ + self._spin_clahe_clip, + self._spin_binary_thresh, + self._spin_open_size, + self._spin_close_width, + ]: + spin.valueChanged.connect( + self._on_image_param_changed, + ) + + parent_layout.addWidget(group) + + 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() + + def _setup_param_store_ui( + self, parent_layout: QVBoxLayout, + ) -> None: + """パラメータ保存・読み込み UI を構築する""" + group = QGroupBox("パラメータ管理") + layout = QVBoxLayout() + group.setLayout(layout) + + # コンボボックス（パラメータ選択） + self._param_combo = QComboBox() + self._param_combo.setPlaceholderText( + "保存済みパラメータ" + ) + layout.addWidget(self._param_combo) + + # メモ表示ラベル + self._param_memo_label = QLabel("") + self._param_memo_label.setWordWrap(True) + self._param_memo_label.setStyleSheet( + "font-size: 11px; color: #888;" + ) + layout.addWidget(self._param_memo_label) + + # ボタン行 + btn_layout = QHBoxLayout() + + load_btn = QPushButton("読込") + load_btn.clicked.connect(self._on_load_param) + btn_layout.addWidget(load_btn) + + save_btn = QPushButton("保存") + save_btn.clicked.connect(self._on_save_param) + btn_layout.addWidget(save_btn) + + delete_btn = QPushButton("削除") + delete_btn.clicked.connect(self._on_delete_param) + btn_layout.addWidget(delete_btn) + + layout.addLayout(btn_layout) + parent_layout.addWidget(group) + + # コンボボックスの選択変更でメモを表示 + self._param_combo.currentIndexChanged.connect( + self._on_param_combo_changed, + ) + + # 保存済みエントリの読み込み + self._param_entries: list[ParamEntry] = [] + self._refresh_param_combo() + + def _refresh_param_combo(self) -> None: + """コンボボックスの内容を更新する""" + self._param_entries = load_entries() + self._param_combo.clear() + for entry in self._param_entries: + self._param_combo.addItem(entry.title) + self._param_memo_label.setText("") + + def _on_param_combo_changed( + self, index: int, + ) -> None: + """コンボボックスの選択変更時にメモを表示する""" + if 0 <= index < len(self._param_entries): + self._param_memo_label.setText( + self._param_entries[index].memo, + ) + else: + self._param_memo_label.setText("") + + def _on_load_param(self) -> None: + """選択したパラメータを SpinBox に反映する""" + index = self._param_combo.currentIndex() + if index < 0 or index >= len(self._param_entries): + return + entry = self._param_entries[index] + p = entry.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) + + ip = entry.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) + + def _on_save_param(self) -> None: + """現在のパラメータを保存する""" + title, ok = QInputDialog.getText( + self, "パラメータ保存", "タイトル:", + ) + if not ok or not title.strip(): + return + + memo, ok = QInputDialog.getText( + self, "パラメータ保存", "メモ:", + ) + if not ok: + return + + entry = ParamEntry( + title=title.strip(), + memo=memo.strip(), + params=PdParams( + kp=self._spin_kp.value(), + kh=self._spin_kh.value(), + kd=self._spin_kd.value(), + max_steer_rate=( + self._spin_max_steer_rate.value() + ), + max_throttle=( + self._spin_max_throttle.value() + ), + speed_k=self._spin_speed_k.value(), + ), + image_params=ImageParams( + clahe_clip=( + self._spin_clahe_clip.value() + ), + binary_thresh=( + self._spin_binary_thresh.value() + ), + open_size=( + self._spin_open_size.value() + ), + close_width=( + self._spin_close_width.value() + ), + ), + ) + add_entry(entry) + self._refresh_param_combo() + # 追加したエントリを選択 + self._param_combo.setCurrentIndex( + self._param_combo.count() - 1, + ) + + def _on_delete_param(self) -> None: + """選択したパラメータを削除する""" + index = self._param_combo.currentIndex() + if index < 0 or index >= len(self._param_entries): + return + + title = self._param_entries[index].title + reply = QMessageBox.question( + self, "削除確認", + f"「{title}」を削除しますか？", + QMessageBox.StandardButton.Yes + | QMessageBox.StandardButton.No, + QMessageBox.StandardButton.No, + ) + if reply == QMessageBox.StandardButton.Yes: + delete_entry(index) + self._refresh_param_combo() + def _setup_overlay_ui( self, parent_layout: QVBoxLayout, ) -> None: @@ -222,11 +449,10 @@ items = [ ("二値化画像", "binary"), - ("近方領域", "near_region"), - ("遠方領域", "far_region"), - ("重心マーカー", "centroids"), + ("検出領域", "detect_region"), + ("フィッティング曲線", "poly_curve"), ("中心線", "center_line"), - ("偏差値", "error_text"), + ("検出情報", "info_text"), ] for label, attr in items: cb = QCheckBox(label) @@ -242,9 +468,9 @@ """いずれかのオーバーレイが有効かを返す""" f = self._overlay_flags return ( - f.binary or f.near_region or f.far_region - or f.centroids or f.center_line - or f.error_text + f.binary or f.detect_region + or f.poly_curve or f.center_line + or f.info_text ) @staticmethod @@ -264,9 +490,11 @@ """パラメータ 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.alpha = self._spin_alpha.value() - p.beta = self._spin_beta.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() @@ -368,7 +596,10 @@ ) elif self._has_any_overlay(): # 手動操作中でもオーバーレイ用に線検出を実行 - self._last_detect_result = detect_line(frame) + self._last_detect_result = detect_line( + frame, + self._pd_control.image_params, + ) else: self._last_detect_result = None diff --git a/src/pc/steering/param_store.py b/src/pc/steering/param_store.py new file mode 100644 index 0000000..892eab5 --- /dev/null +++ b/src/pc/steering/param_store.py @@ -0,0 +1,103 @@ +""" +param_store +パラメータの保存・読み込みを管理するモジュール +1つの JSON ファイルに複数のパラメータセットを格納する +""" + +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 + +# パラメータ保存先のファイルパス +_STORE_PATH: Path = ( + Path(__file__).resolve().parent.parent.parent.parent + / "pd_params.json" +) + + +@dataclass +class ParamEntry: + """パラメータセットの1エントリ + + Attributes: + title: パラメータセットのタイトル + memo: メモ（用途や特徴の説明） + params: PD 制御パラメータ + image_params: 画像処理パラメータ + """ + title: str + memo: str + params: PdParams + image_params: ImageParams + + +def load_entries() -> list[ParamEntry]: + """保存済みのパラメータ一覧を読み込む + + Returns: + パラメータエントリのリスト + """ + if not _STORE_PATH.exists(): + return [] + + with open(_STORE_PATH, "r", encoding="utf-8") as f: + data = json.load(f) + + entries: list[ParamEntry] = [] + for item in data: + image_params = ImageParams( + **item["image_params"], + ) if "image_params" in item else ImageParams() + + entries.append(ParamEntry( + title=item["title"], + memo=item["memo"], + params=PdParams(**item["params"]), + image_params=image_params, + )) + return entries + + +def save_entries(entries: list[ParamEntry]) -> None: + """パラメータ一覧をファイルに保存する + + Args: + entries: パラメータエントリのリスト + """ + data = [] + for entry in entries: + data.append({ + "title": entry.title, + "memo": entry.memo, + "params": asdict(entry.params), + "image_params": asdict(entry.image_params), + }) + + with open(_STORE_PATH, "w", encoding="utf-8") as f: + json.dump(data, f, ensure_ascii=False, indent=2) + + +def add_entry(entry: ParamEntry) -> None: + """パラメータセットを追加する + + Args: + entry: 追加するエントリ + """ + entries = load_entries() + entries.append(entry) + save_entries(entries) + + +def delete_entry(index: int) -> None: + """パラメータセットを削除する + + Args: + index: 削除するエントリのインデックス + """ + entries = load_entries() + if 0 <= index < len(entries): + entries.pop(index) + save_entries(entries) diff --git a/src/pc/steering/pd_control.py b/src/pc/steering/pd_control.py index 9e160db..fde5a4b 100644 --- a/src/pc/steering/pd_control.py +++ b/src/pc/steering/pd_control.py @@ -1,16 +1,16 @@ """ pd_control PD 制御による操舵量計算モジュール -近方・遠方の2領域偏差から操舵量と速度を算出する +多項式フィッティングの位置・傾き・曲率から操舵量と速度を算出する """ import time -from dataclasses import dataclass, field +from dataclasses import dataclass import numpy as np from pc.steering.base import SteeringBase, SteeringOutput -from pc.vision.line_detector import detect_line +from pc.vision.line_detector import ImageParams, detect_line @dataclass @@ -18,17 +18,17 @@ """PD 制御のパラメータ Attributes: - kp: 比例ゲイン + kp: 位置偏差ゲイン + kh: 傾き（ヘディング）ゲイン kd: 微分ゲイン - alpha: 近方偏差の重み - beta: 遠方偏差の重み + max_steer_rate: 1フレームあたりの最大操舵変化量 max_throttle: 直線での最大速度 - speed_k: 減速係数 + speed_k: 曲率ベースの減速係数 """ kp: float = 0.5 + kh: float = 0.3 kd: float = 0.1 - alpha: float = 0.6 - beta: float = 0.4 + max_steer_rate: float = 0.1 max_throttle: float = 0.4 speed_k: float = 0.3 @@ -37,11 +37,17 @@ """PD 制御による操舵量計算クラス""" def __init__( - self, params: PdParams | None = None, + 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( @@ -55,45 +61,55 @@ Returns: 計算された操舵量 """ + p = self.params + # 線検出 - result = detect_line(frame) + result = detect_line(frame, self.image_params) self._last_result = result # 線が検出できなかった場合は停止 - if result.near_x is None and result.far_x is None: + if not result.detected: return SteeringOutput(throttle=0.0, steer=0.0) - # 片方のみ検出できた場合はもう片方の値で代用 - near_error = result.near_error - far_error = result.far_error - if result.near_x is None: - near_error = far_error - if result.far_x is None: - far_error = near_error - - # 制御用偏差の合成 - p = self.params - error = p.alpha * near_error + p.beta * far_error + # 位置偏差 + 傾きによる操舵量 + 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 = ( + now - self._prev_time + if self._prev_time > 0 + else 0.033 + ) dt = max(dt, 0.001) - # PD 制御 + # 微分項 derivative = (error - self._prev_error) / dt - steer = p.kp * error + p.kd * derivative + steer = error + p.kd * derivative # 操舵量のクランプ steer = max(-1.0, min(1.0, steer)) - # 速度制御（操舵量連動） - throttle = p.max_throttle - p.speed_k * abs(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, @@ -103,6 +119,7 @@ """内部状態をリセットする""" self._prev_error = 0.0 self._prev_time = 0.0 + self._prev_steer = 0.0 self._last_result = None @property diff --git a/src/pc/vision/line_detector.py b/src/pc/vision/line_detector.py index 1c77ff0..d20a263 100644 --- a/src/pc/vision/line_detector.py +++ b/src/pc/vision/line_detector.py @@ -1,7 +1,8 @@ """ line_detector カメラ画像から黒線の位置を検出するモジュール -近方・遠方の2領域で重心 x 座標を算出する +二値化画像の白ピクセルに多項式をフィッティングして +線の位置・傾き・曲率を算出する """ from dataclasses import dataclass @@ -11,23 +12,34 @@ from common import config -# ガウシアンブラーのカーネルサイズ -BLUR_KERNEL_SIZE: int = 5 +# 検出領域の y 範囲（画像全体） +DETECT_Y_START: int = 0 +DETECT_Y_END: int = config.FRAME_HEIGHT -# 二値化の閾値（黒線検出用，この値以下を黒とみなす） -BINARY_THRESHOLD: int = 80 +# フィッティングに必要な最小ピクセル数 +MIN_FIT_PIXELS: int = 50 -# 横方向クロージングのカーネルサイズ（反射による途切れを補間） -MORPH_CLOSE_WIDTH: int = 25 -MORPH_CLOSE_HEIGHT: int = 3 -# 近方領域の y 範囲（画像下部） -NEAR_Y_START: int = int(config.FRAME_HEIGHT * 0.7) -NEAR_Y_END: int = config.FRAME_HEIGHT +@dataclass +class ImageParams: + """画像処理パラメータ -# 遠方領域の y 範囲（画像上部） -FAR_Y_START: int = int(config.FRAME_HEIGHT * 0.3) -FAR_Y_END: int = int(config.FRAME_HEIGHT * 0.5) + Attributes: + clahe_clip: CLAHE のコントラスト増幅上限 + clahe_grid: CLAHE の局所領域分割数 + blur_size: ガウシアンブラーのカーネルサイズ（奇数） + binary_thresh: 二値化の閾値 + open_size: オープニングのカーネルサイズ（孤立ノイズ除去） + close_width: クロージングの横幅（途切れ補間） + close_height: クロージングの高さ + """ + 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 @dataclass @@ -35,94 +47,126 @@ """線検出の結果を格納するデータクラス Attributes: - near_x: 近方領域の線の重心 x 座標（未検出時は None） - far_x: 遠方領域の線の重心 x 座標（未検出時は None） - near_error: 近方偏差（画像中心からのずれ） - far_error: 遠方偏差（画像中心からのずれ） + detected: 線が検出できたか + position_error: 画像下端での位置偏差（-1.0～+1.0） + heading: 線の傾き（dx/dy，画像下端での値） + curvature: 線の曲率（d²x/dy²） + poly_coeffs: 多項式の係数（描画用，未検出時は None） binary_image: 二値化後の画像（デバッグ用） """ - near_x: float | None - far_x: float | None - near_error: float - far_error: float + 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) -> LineDetectResult: +def detect_line( + frame: np.ndarray, + params: ImageParams | None = None, +) -> LineDetectResult: """画像から黒線の位置を検出する + 二値化後の白ピクセルに2次多項式をフィッティングし， + 線の位置・傾き・曲率を算出する + Args: frame: BGR 形式のカメラ画像 + params: 画像処理パラメータ（None の場合はデフォルト） Returns: 線検出の結果 """ + if params is None: + params = ImageParams() + # グレースケール変換 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( - gray, - (BLUR_KERNEL_SIZE, BLUR_KERNEL_SIZE), - 0, + enhanced, (blur_k, blur_k), 0, ) # 固定閾値で二値化（黒線を白，背景を黒に反転） _, binary = cv2.threshold( - blurred, BINARY_THRESHOLD, 255, + blurred, params.binary_thresh, 255, cv2.THRESH_BINARY_INV, ) - # 横方向クロージング（反射で途切れた線を左右からつなぐ） - kernel = cv2.getStructuringElement( - cv2.MORPH_ELLIPSE, - (MORPH_CLOSE_WIDTH, MORPH_CLOSE_HEIGHT), - ) - binary = cv2.morphologyEx( - binary, cv2.MORPH_CLOSE, kernel, - ) + # オープニング（収縮→膨張で孤立ノイズを除去） + 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, + ) - # 画像の中心 x 座標 + # 横方向クロージング（反射で途切れた線を左右からつなぐ） + 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, + ) + + # 検出領域内の白ピクセル座標を取得 + region = binary[DETECT_Y_START:DETECT_Y_END, :] + ys_local, xs = np.where(region > 0) + + if len(xs) < MIN_FIT_PIXELS: + return LineDetectResult( + detected=False, + position_error=0.0, + heading=0.0, + curvature=0.0, + poly_coeffs=None, + binary_image=binary, + ) + + # 全体画像座標に変換 + ys = ys_local + DETECT_Y_START + + # 2次多項式フィッティング: x = f(y) = ay² + by + c + coeffs = np.polyfit(ys, xs, 2) + poly = np.poly1d(coeffs) + + # 画像中心 center_x = config.FRAME_WIDTH / 2.0 - # 近方領域の重心算出 - near_roi = binary[NEAR_Y_START:NEAR_Y_END, :] - near_x = _calc_centroid_x(near_roi) + # 画像下端（近方）での位置偏差 + x_bottom = poly(DETECT_Y_END) + position_error = (center_x - x_bottom) / center_x - # 遠方領域の重心算出 - far_roi = binary[FAR_Y_START:FAR_Y_END, :] - far_x = _calc_centroid_x(far_roi) + # 傾き: dx/dy（画像下端での値） + poly_deriv = poly.deriv() + heading = float(poly_deriv(DETECT_Y_END)) - # 偏差の計算 - near_error = 0.0 - if near_x is not None: - near_error = (center_x - near_x) / center_x - - far_error = 0.0 - if far_x is not None: - far_error = (center_x - far_x) / center_x + # 曲率: d²x/dy²（2次多項式では定数 = 2a） + poly_deriv2 = poly_deriv.deriv() + curvature = float(poly_deriv2(DETECT_Y_END)) return LineDetectResult( - near_x=near_x, - far_x=far_x, - near_error=near_error, - far_error=far_error, + detected=True, + position_error=position_error, + heading=heading, + curvature=curvature, + poly_coeffs=coeffs, binary_image=binary, ) - - -def _calc_centroid_x( - roi: np.ndarray, -) -> float | None: - """ROI 内の白ピクセルの重心 x 座標を算出する - - Args: - roi: 二値化された領域画像 - - Returns: - 重心の x 座標，白ピクセルがない場合は None - """ - moments = cv2.moments(roi) - if moments["m00"] == 0: - return None - return moments["m10"] / moments["m00"] diff --git a/src/pc/vision/overlay.py b/src/pc/vision/overlay.py index 2fb0809..188a1e8 100644 --- a/src/pc/vision/overlay.py +++ b/src/pc/vision/overlay.py @@ -14,10 +14,10 @@ from pc.vision.line_detector import LineDetectResult # 描画色の定義 (BGR) -COLOR_NEAR: tuple = (0, 255, 0) -COLOR_FAR: tuple = (255, 0, 0) +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 @@ -29,18 +29,16 @@ Attributes: binary: 二値化画像の半透明表示 - near_region: 近方領域の枠 - far_region: 遠方領域の枠 - centroids: 重心マーカー + detect_region: 検出領域の枠 + poly_curve: フィッティング曲線 center_line: 画像中心線 - error_text: 偏差の数値表示 + info_text: 検出情報の数値表示 """ binary: bool = False - near_region: bool = False - far_region: bool = False - centroids: bool = False + detect_region: bool = False + poly_curve: bool = False center_line: bool = False - error_text: bool = False + info_text: bool = False def draw_overlay( @@ -69,28 +67,16 @@ display, result.binary_image, ) - # 近方領域の枠 - if flags.near_region: + # 検出領域の枠 + if flags.detect_region: cv2.rectangle( display, - (0, line_detector.NEAR_Y_START), + (0, line_detector.DETECT_Y_START), ( config.FRAME_WIDTH - 1, - line_detector.NEAR_Y_END - 1, + line_detector.DETECT_Y_END - 1, ), - COLOR_NEAR, 1, - ) - - # 遠方領域の枠 - if flags.far_region: - cv2.rectangle( - display, - (0, line_detector.FAR_Y_START), - ( - config.FRAME_WIDTH - 1, - line_detector.FAR_Y_END - 1, - ), - COLOR_FAR, 1, + COLOR_REGION, 1, ) # 画像中心線 @@ -103,32 +89,13 @@ COLOR_CENTER, 1, ) - # 重心マーカー - if flags.centroids: - if result.near_x is not None: - near_y = ( - line_detector.NEAR_Y_START - + line_detector.NEAR_Y_END - ) // 2 - cv2.circle( - display, - (int(result.near_x), near_y), - 6, COLOR_NEAR, -1, - ) - if result.far_x is not None: - far_y = ( - line_detector.FAR_Y_START - + line_detector.FAR_Y_END - ) // 2 - cv2.circle( - display, - (int(result.far_x), far_y), - 6, COLOR_FAR, -1, - ) + # フィッティング曲線 + if flags.poly_curve and result.poly_coeffs is not None: + _draw_poly_curve(display, result.poly_coeffs) - # 偏差の数値表示 - if flags.error_text: - _draw_error_text(display, result) + # 検出情報の数値表示 + if flags.info_text: + _draw_info_text(display, result) return display @@ -146,44 +113,74 @@ Returns: 合成された画像 """ - # 二値化画像を赤色の BGR に変換 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_error_text( +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: 線検出の結果 """ - near_str = ( - f"near: {result.near_error:+.3f}" - if result.near_x is not None - else "near: N/A" - ) - far_str = ( - f"far: {result.far_error:+.3f}" - if result.far_x is not None - else "far: N/A" - ) - cv2.putText( - frame, near_str, (5, 15), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, - COLOR_TEXT, 1, - ) - cv2.putText( - frame, far_str, (5, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.4, - COLOR_TEXT, 1, - ) + 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, + )