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 7af8283..52a3fef 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" @@ -52,10 +52,10 @@ ├── comm/ 通信関連 │ └── zmq_client.py ZMQ 送受信 ├── steering/ 操舵量計算（独立モジュール） - │ ├── base.py (未実装) 共通インターフェース - │ └── pd_control.py (未実装) PD 制御の実装 + │ ├── base.py 共通インターフェース + │ └── pd_control.py PD 制御の実装 └── vision/ 画像処理 - └── line_detector.py (未実装) 線検出 + └── line_detector.py 線検出 2-4. src/pi/ @@ -66,4 +66,4 @@ ├── camera/ カメラ関連 │ └── capture.py フレーム取得 └── motor/ モーター関連 - └── driver.py (未実装) TB6612FNG 制御 + └── driver.py TB6612FNG 制御 diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index 92d9223..013f921 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -8,6 +8,9 @@ from PySide6.QtCore import Qt, QTimer from PySide6.QtGui import QImage, QKeyEvent, QPixmap from PySide6.QtWidgets import ( + QDoubleSpinBox, + QFormLayout, + QGroupBox, QHBoxLayout, QLabel, QMainWindow, @@ -18,6 +21,7 @@ from common import config from pc.comm.zmq_client import PcZmqClient +from pc.steering.pd_control import PdControl, PdParams # 映像更新間隔 (ms) FRAME_INTERVAL_MS: int = 33 @@ -42,12 +46,19 @@ 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._pd_control = PdControl() + + # 最新フレームの保持（自動操縦で使用） + self._latest_frame: np.ndarray | None = None + self._setup_ui() self._setup_timers() @@ -86,6 +97,14 @@ ) 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( @@ -103,14 +122,16 @@ self._control_label.setStyleSheet("font-size: 14px;") control_layout.addWidget(self._control_label) + # PD パラメータ調整 + self._setup_param_ui(control_layout) + # 操作ガイド guide = QLabel( - "--- 手動操作 ---\n" - "W / ↑ : 前進\n" - "S / ↓ : 後退\n" - "A / ← : 左旋回\n" - "D / → : 右旋回\n" - "Space : 停止" + "--- キー操作 ---\n" + "W/↑: 前進 S/↓: 後退\n" + "A/←: 左 D/→: 右\n" + "Space: 停止\n" + "Q: 自動操縦 切替" ) guide.setAlignment(Qt.AlignmentFlag.AlignLeft) guide.setStyleSheet("font-size: 12px; color: #666;") @@ -119,6 +140,91 @@ # 余白を下に詰める control_layout.addStretch() + def _setup_param_ui( + self, parent_layout: QVBoxLayout, + ) -> None: + """PD パラメータ調整 UI を構築する""" + group = QGroupBox("PD パラメータ") + form = QFormLayout() + group.setLayout(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_kd = self._create_spin( + params.kd, 0.0, 5.0, 0.05, + ) + form.addRow("Kd:", self._spin_kd) + + self._spin_alpha = self._create_spin( + params.alpha, 0.0, 1.0, 0.1, + ) + form.addRow("α (近方):", self._spin_alpha) + + self._spin_beta = self._create_spin( + params.beta, 0.0, 1.0, 0.1, + ) + form.addRow("β (遠方):", self._spin_beta) + + 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, 2.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, + ) + + parent_layout.addWidget(group) + + @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.kd = self._spin_kd.value() + p.alpha = self._spin_alpha.value() + p.beta = self._spin_beta.value() + p.max_throttle = self._spin_max_throttle.value() + p.speed_k = self._spin_speed_k.value() + def _setup_timers(self) -> None: """タイマーを設定する""" # 映像更新用 @@ -131,6 +237,8 @@ self._send_control, ) + # ── 接続 ────────────────────────────────────────────── + def _toggle_connection(self) -> None: """接続 / 切断を切り替える""" if self._is_connected: @@ -143,6 +251,7 @@ 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) @@ -151,11 +260,16 @@ """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("カメラ映像待機中...") @@ -163,11 +277,48 @@ "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._display_frame(frame) def _display_frame(self, frame: np.ndarray) -> None: @@ -197,15 +348,26 @@ # ── 手動操作 ────────────────────────────────────────── 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(): + if event.isAutoRepeat() or self._is_auto: return self._pressed_keys.discard(event.key()) self._update_manual_control() @@ -219,6 +381,9 @@ self._throttle = 0.0 self._steer = 0.0 self._pressed_keys.clear() + # 自動操縦中なら停止 + if self._is_auto: + self._disable_auto() self._update_control_label() return 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/pd_control.py b/src/pc/steering/pd_control.py new file mode 100644 index 0000000..9e160db --- /dev/null +++ b/src/pc/steering/pd_control.py @@ -0,0 +1,111 @@ +""" +pd_control +PD 制御による操舵量計算モジュール +近方・遠方の2領域偏差から操舵量と速度を算出する +""" + +import time +from dataclasses import dataclass, field + +import numpy as np + +from pc.steering.base import SteeringBase, SteeringOutput +from pc.vision.line_detector import detect_line + + +@dataclass +class PdParams: + """PD 制御のパラメータ + + Attributes: + kp: 比例ゲイン + kd: 微分ゲイン + alpha: 近方偏差の重み + beta: 遠方偏差の重み + max_throttle: 直線での最大速度 + speed_k: 減速係数 + """ + kp: float = 0.5 + kd: float = 0.1 + alpha: float = 0.6 + beta: float = 0.4 + max_throttle: float = 0.4 + speed_k: float = 0.3 + + +class PdControl(SteeringBase): + """PD 制御による操舵量計算クラス""" + + def __init__( + self, params: PdParams | None = None, + ) -> None: + self.params: PdParams = params or PdParams() + self._prev_error: float = 0.0 + self._prev_time: float = 0.0 + self._last_result = None + + def compute( + self, frame: np.ndarray, + ) -> SteeringOutput: + """カメラ画像から PD 制御で操舵量を計算する + + Args: + frame: BGR 形式のカメラ画像 + + Returns: + 計算された操舵量 + """ + # 線検出 + result = detect_line(frame) + self._last_result = result + + # 線が検出できなかった場合は停止 + if result.near_x is None and result.far_x is None: + 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 + + # 時間差分の計算 + now = time.time() + 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 = max(-1.0, min(1.0, steer)) + + # 速度制御（操舵量連動） + throttle = p.max_throttle - p.speed_k * abs(steer) + throttle = max(0.0, throttle) + + # 状態の更新 + self._prev_error = error + self._prev_time = now + + return SteeringOutput( + throttle=throttle, steer=steer, + ) + + def reset(self) -> None: + """内部状態をリセットする""" + self._prev_error = 0.0 + self._prev_time = 0.0 + self._last_result = None + + @property + def last_detect_result(self): + """直近の線検出結果を取得する""" + return self._last_result diff --git a/src/pc/vision/line_detector.py b/src/pc/vision/line_detector.py new file mode 100644 index 0000000..8f7bced --- /dev/null +++ b/src/pc/vision/line_detector.py @@ -0,0 +1,115 @@ +""" +line_detector +カメラ画像から黒線の位置を検出するモジュール +近方・遠方の2領域で重心 x 座標を算出する +""" + +from dataclasses import dataclass + +import cv2 +import numpy as np + +from common import config + +# ガウシアンブラーのカーネルサイズ +BLUR_KERNEL_SIZE: int = 5 + +# 二値化の閾値（黒線検出用，この値以下を黒とみなす） +BINARY_THRESHOLD: int = 80 + +# 近方領域の y 範囲（画像下部） +NEAR_Y_START: int = int(config.FRAME_HEIGHT * 0.7) +NEAR_Y_END: int = config.FRAME_HEIGHT + +# 遠方領域の y 範囲（画像上部） +FAR_Y_START: int = int(config.FRAME_HEIGHT * 0.3) +FAR_Y_END: int = int(config.FRAME_HEIGHT * 0.5) + + +@dataclass +class LineDetectResult: + """線検出の結果を格納するデータクラス + + Attributes: + near_x: 近方領域の線の重心 x 座標（未検出時は None） + far_x: 遠方領域の線の重心 x 座標（未検出時は None） + near_error: 近方偏差（画像中心からのずれ） + far_error: 遠方偏差（画像中心からのずれ） + binary_image: 二値化後の画像（デバッグ用） + """ + near_x: float | None + far_x: float | None + near_error: float + far_error: float + binary_image: np.ndarray | None + + +def detect_line(frame: np.ndarray) -> LineDetectResult: + """画像から黒線の位置を検出する + + Args: + frame: BGR 形式のカメラ画像 + + Returns: + 線検出の結果 + """ + # グレースケール変換 + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # ガウシアンブラー + blurred = cv2.GaussianBlur( + gray, + (BLUR_KERNEL_SIZE, BLUR_KERNEL_SIZE), + 0, + ) + + # 二値化（黒線を白，背景を黒に反転） + _, binary = cv2.threshold( + blurred, BINARY_THRESHOLD, 255, + cv2.THRESH_BINARY_INV, + ) + + # 画像の中心 x 座標 + center_x = config.FRAME_WIDTH / 2.0 + + # 近方領域の重心算出 + near_roi = binary[NEAR_Y_START:NEAR_Y_END, :] + near_x = _calc_centroid_x(near_roi) + + # 遠方領域の重心算出 + far_roi = binary[FAR_Y_START:FAR_Y_END, :] + far_x = _calc_centroid_x(far_roi) + + # 偏差の計算 + 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 + + return LineDetectResult( + near_x=near_x, + far_x=far_x, + near_error=near_error, + far_error=far_error, + 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"]