Tello ピンクのボールを追尾する(ピンク色のボール検出（OpenCV HSV）を使い追尾)

import pyttsx3  # 音声合成ライブラリを読み込み

engine = pyttsx3.init()  # 音声エンジンを初期化

engine.say("こんにちは、Telloが追尾します")  # 読み上げたいテキスト
engine.runAndWait()  # 実際に再生

import pyttsx3
＃音声エンジン初期化
engine = pyttsx3.init()
ball_detected = False  # ボールが見つかったかどうかのフラグ

def findBall(img):
    global ball_detected
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    # ピンク色のHSV範囲（明るさ・環境によって調整）
    lower_pink = np.array([140, 100, 100])
    upper_pink = np.array([170, 255, 255])
    mask = cv2.inRange(hsv, lower_pink, upper_pink)

    contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

    BallList_center = []
    BallList_area = []
    ball_center_y = 0

    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area > 500:  # ノイズ除去
            x, y, w, h = cv2.boundingRect(cnt)
            cx = x + w // 2
            cy = y + h // 2
            BallList_center.append([cx, cy])
            BallList_area.append(area)
            cv2.rectangle(img, (x, y), (x + w, y + h), (255, 0, 255), 2)
            cv2.circle(img, (cx, cy), 10, (0, 0, 255), -1)
            cv2.putText(img, f"ballcenter: ({cx}, {cy})", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (204, 0, 255), 1)

    if len(BallList_area) != 0:
        i = BallList_area.index(max(BallList_area))
        if not ball_detected:
            engine.say("ピンクのボール発見！追尾します")
            engine.runAndWait()
            ball_detected = True
        return img, [BallList_center[i], BallList_area[i]]
    else:
        ball_detected = False
        return img, [[0, 0], 0]

import threading
import socket
import time
import cv2
import numpy as np
import pyttsx3

# 音声再生（非同期）
def speak_async(text):
    def _speak():
        engine = pyttsx3.init()
        engine.say(text)
        engine.runAndWait()
    threading.Thread(target=_speak, daemon=True).start()

# Telloネットワーク設定
host = '0.0.0.0'
port = 8889
locaddr = (host, port)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
tello_address = ('192.168.10.1', 8889)
sock.bind(locaddr)

# Telloからの応答を受信
def recv():
    while True:
        try:
            data, _ = sock.recvfrom(1518)
            print(data.decode(encoding="utf-8"))
        except Exception:
            print('\nExit . . . RECV\n')
            break

recvThread = threading.Thread(target=recv)
recvThread.daemon = True
recvThread.start()

# 初期化コマンド
sock.sendto(b'command', tello_address)
time.sleep(0.5)
sock.sendto(b'streamon', tello_address)
time.sleep(1)
sock.sendto(b'takeoff', tello_address)
time.sleep(2)

# カメラ映像取得
addr = 'udp://192.168.10.1:11111'
cap = cv2.VideoCapture(addr)
print('start cap')

# PID制御パラメータ
pid = [0.4, 0.4, 0]
pError = 0
w, h = 480, 360

# RCコマンド初期値
a = b = c = d = 0

ball_detected = False  # 音声を一度だけ再生するためのフラグ

# ピンクボール検出関数
def findBall(img):
    global ball_detected
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    lower_pink = np.array([140, 100, 100])
    upper_pink = np.array([170, 255, 255])
    mask = cv2.inRange(hsv, lower_pink, upper_pink)
    contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

    BallList_center = []
    BallList_area = []

    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area > 500:
            x, y, w_box, h_box = cv2.boundingRect(cnt)
            cx = x + w_box // 2
            cy = y + h_box // 2
            BallList_center.append([cx, cy])
            BallList_area.append(area)
            cv2.rectangle(img, (x, y), (x + w_box, y + h_box), (255, 0, 255), 2)
            cv2.circle(img, (cx, cy), 10, (0, 0, 255), -1)

    if len(BallList_area) != 0:
        i = BallList_area.index(max(BallList_area))
        if not ball_detected:
            speak_async("ピンクのボール発見！追尾します")
            ball_detected = True
        return img, [BallList_center[i], BallList_area[i]]
    else:
        ball_detected = False
        return img, [[0, 0], 0]

# 左右・前後制御（PID）
def TrackBall(info, w, pid, pError):
    global b, d
    area = info[1]
    x, y = info[0]
    b = 0

    error = x - w / 2
    d = pid[0] * error + pid[1] * (error - pError)
    d = int(np.clip(d, -100, 100))

    if 6200 < area < 6800:
        b = 0
    elif area > 6800:
        b = -20
    elif area < 6200 and area != 0:
        b = 20
    if x == 0:
        d = 0
        error = 0

    return error

# 上下制御（オプション）
def throttle(y):
    global c
    c = 0
    if 50 < y < 110:
        c = 20
    elif 280 > y > 190:
        c = -20

# メインループ
while True:
    for i in range(5):  # フレームスキップで安定化
        ret, frame = cap.read()
    if frame is None or frame.size == 0:
        continue

    frame = cv2.resize(frame, (w, h))
    img, info = findBall(frame)
    pError = TrackBall(info, w, pid, pError)
    throttle(info[0][1])

    rc_command = f"rc {a} {b} {c} {d}"
    sock.sendto(rc_command.encode('utf-8'), tello_address)
    print("Sending RC command with values:", a, b, c, d)

    # 画面に情報表示
    cv2.putText(img, f'Pitch: {b}  Throttle: {c}  Yaw: {d}', (20, 20),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
    cv2.imshow('Tracking', img)

    # ESCキーで終了
    if cv2.waitKey(1) & 0xFF == 27:
        sock.sendto(b'land', tello_address)
        cap.release()
        cv2.destroyAllWindows()
        sock.close()
        break

lower_pink = np.array([140, 100, 100])
upper_pink = np.array([170, 255, 255])