diff --git "a/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" "b/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" index 4117323..5c4bc20 100644 --- "a/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" +++ "b/docs/03_TECH/TECH_03_\343\203\207\343\203\220\343\203\203\343\202\260\343\202\252\343\203\274\343\203\220\343\203\274\343\203\254\343\202\244\344\273\225\346\247\230.txt" @@ -27,7 +27,12 @@ ・二値化画像: 二値化結果を赤色の半透明で重ねる（不透明度 0.4） ・検出領域: 検出対象領域の枠を青色で表示 ・フィッティング曲線: 多項式の曲線を緑色で描画 + ・行中心点: 各行の線中心 x 座標をオレンジ色の点で描画 + ・Theil-Sen 直線: 行中心点の Theil-Sen 近似直線をマゼンタで描画 ・中心線: 画像の中心 x に垂直線を描画（黄色） + ・パシュート目標点: 2点パシュートの near/far 目標点を赤色の + 円（半径 2px）で描画．制御手法がパシュート，かつ自動操縦中 + のみ有効 2-2. 検出情報ラベル（常時表示） @@ -39,6 +44,9 @@ ・フィッティング曲線: (0, 255, 0) 緑 ・中心線: (0, 255, 255) 黄 ・検出領域: (255, 0, 0) 青 + ・行中心点: (0, 165, 255) オレンジ + ・Theil-Sen 直線: (255, 0, 255) マゼンタ + ・パシュート目標点: (0, 0, 255) 赤 ・二値化オーバーレイ: 赤チャンネルに二値化画像を割り当て diff --git a/src/pc/gui/main_window.py b/src/pc/gui/main_window.py index 08de403..9efbacc 100644 --- a/src/pc/gui/main_window.py +++ b/src/pc/gui/main_window.py @@ -392,9 +392,15 @@ bgr = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR) # オーバーレイ描画 + pursuit_pts = None + if self._steering_method == "pursuit": + pursuit_pts = ( + self._pursuit_control.last_pursuit_points + ) bgr = draw_overlay( bgr, self._last_detect_result, self._overlay_panel.get_flags(), + pursuit_points=pursuit_pts, ) # 検出情報をラベルに表示 diff --git a/src/pc/gui/panels/overlay_panel.py b/src/pc/gui/panels/overlay_panel.py index 1a86ffa..ea8aeee 100644 --- a/src/pc/gui/panels/overlay_panel.py +++ b/src/pc/gui/panels/overlay_panel.py @@ -28,6 +28,7 @@ ("行中心点", "row_centers"), ("Theil-Sen直線", "theil_sen"), ("中心線", "center_line"), + ("パシュート目標点", "pursuit_points"), ] for label, attr in items: cb = QCheckBox(label) diff --git a/src/pc/steering/pursuit_control.py b/src/pc/steering/pursuit_control.py index 833edd4..db0b793 100644 --- a/src/pc/steering/pursuit_control.py +++ b/src/pc/steering/pursuit_control.py @@ -60,6 +60,10 @@ ) self._prev_steer: float = 0.0 self._last_result = None + self._last_pursuit_points: ( + tuple[tuple[float, float], tuple[float, float]] + | None + ) = None def compute( self, frame: np.ndarray, @@ -79,6 +83,7 @@ self._last_result = result if not result.detected or result.row_centers is None: + self._last_pursuit_points = None return SteeringOutput( throttle=0.0, steer=0.0, ) @@ -91,6 +96,7 @@ xs = centers[valid] if len(ys) < 2: + self._last_pursuit_points = None return SteeringOutput( throttle=0.0, steer=0.0, ) @@ -107,6 +113,12 @@ near_x = slope * near_y + intercept far_x = slope * far_y + intercept + # 目標点を保持（デバッグ表示用） + self._last_pursuit_points = ( + (near_x, near_y), + (far_x, far_y), + ) + # 各点の偏差（正: 線が左にある → 右に曲がる） near_err = (center_x - near_x) / center_x far_err = (center_x - far_x) / center_x @@ -136,9 +148,24 @@ """内部状態をリセットする""" self._prev_steer = 0.0 self._last_result = None + self._last_pursuit_points = None reset_valley_tracker() @property def last_detect_result(self): """直近の線検出結果を取得する""" return self._last_result + + @property + def last_pursuit_points( + self, + ) -> ( + tuple[tuple[float, float], tuple[float, float]] + | None + ): + """直近の2点パシュート目標点を取得する + + Returns: + ((near_x, near_y), (far_x, far_y)) または None + """ + return self._last_pursuit_points diff --git a/src/pc/vision/overlay.py b/src/pc/vision/overlay.py index 27436f2..44cbe87 100644 --- a/src/pc/vision/overlay.py +++ b/src/pc/vision/overlay.py @@ -18,6 +18,10 @@ COLOR_REGION: tuple = (255, 0, 0) COLOR_ROW_CENTER: tuple = (0, 165, 255) COLOR_THEIL_SEN: tuple = (255, 0, 255) +COLOR_PURSUIT: tuple = (0, 0, 255) + +# パシュート目標点の描画半径 +PURSUIT_POINT_RADIUS: int = 2 # 二値化オーバーレイの不透明度 BINARY_OPACITY: float = 0.4 @@ -34,6 +38,7 @@ row_centers: 各行の線中心点 theil_sen: Theil-Sen 近似直線 center_line: 画像中心線 + pursuit_points: 2点パシュートの目標点 """ binary: bool = False detect_region: bool = False @@ -41,12 +46,17 @@ row_centers: bool = False theil_sen: bool = False center_line: bool = False + pursuit_points: bool = False def draw_overlay( frame: np.ndarray, result: LineDetectResult | None, flags: OverlayFlags, + pursuit_points: ( + tuple[tuple[float, float], tuple[float, float]] + | None + ) = None, ) -> np.ndarray: """カメラ映像にオーバーレイを描画する @@ -54,6 +64,8 @@ frame: 元の BGR カメラ画像 result: 線検出の結果（None の場合はオーバーレイなし） flags: 表示項目のフラグ + pursuit_points: 2点パシュートの目標点 + ((near_x, near_y), (far_x, far_y)) Returns: オーバーレイ描画済みの画像 @@ -103,6 +115,10 @@ display, result.row_centers, ) + # 2点パシュートの目標点 + if flags.pursuit_points and pursuit_points is not None: + _draw_pursuit_points(display, pursuit_points) + return display @@ -207,3 +223,29 @@ frame, [pts], False, COLOR_LINE, 1, ) + + +def _draw_pursuit_points( + frame: np.ndarray, + points: tuple[ + tuple[float, float], tuple[float, float] + ], +) -> None: + """2点パシュートの目標点を描画する + + Args: + frame: 描画先の画像 + points: ((near_x, near_y), (far_x, far_y)) + """ + w = frame.shape[1] + for x, y in points: + ix = int(round(x)) + iy = int(round(y)) + if 0 <= ix < w: + cv2.circle( + frame, + (ix, iy), + PURSUIT_POINT_RADIUS, + COLOR_PURSUIT, + -1, + )