Graduation_Project/main/camera.py

import cv2
import numpy as np
import motor
import time
import signal
import sys

# 信号处理函数，用于捕获Ctrl+C
def signal_handler(sig, frame):
    print("\n接收到中断信号，正在停止电机...")
    motor.stop()
    print("电机已停止，程序退出")
    sys.exit(0)

# 注册信号处理函数
signal.signal(signal.SIGINT, signal_handler)

print("开始运行摄像头程序...")
print(f"OpenCV版本: {cv2.__version__}")

# 尝试打开摄像头
cap = cv2.VideoCapture(0)

if not cap.isOpened():
    print("无法打开摄像头！")
    exit()

print("摄像头打开成功")

# 设置摄像头参数
cap.set(3, 320)
cap.set(4, 240)
cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)

print(f"摄像头分辨率: {cap.get(3)}x{cap.get(4)}")

# 初始化稳定阶段
print("初始化摄像头...")
success_count = 0
for i in range(10):
    ret, frame = cap.read()
    if not ret:
        print(f"第{i+1}帧读取失败")
        continue
    success_count += 1
    print(f"第{i+1}帧读取成功")
    time.sleep(0.1)
print(f"摄像头初始化完成，成功读取{success_count}帧")

# 读取初始帧
ret, prev_frame = cap.read()
if not ret:
    print("无法读取初始帧！")
    cap.release()
    exit()

print("初始帧读取成功")
prev_gray = cv2.cvtColor(prev_frame, cv2.COLOR_BGR2GRAY)
prev_gray = cv2.GaussianBlur(prev_gray, (5,5), 0)
print("初始帧处理完成")

# 记录上一帧目标的位置和面积
prev_cx = None
prev_cy = None
prev_area = None
# 目标跟踪状态
tracking_target = None

while True:
    ret, frame = cap.read()
    if not ret:
        break

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    gray = cv2.GaussianBlur(gray, (5,5), 0)

    # 帧差
    diff = cv2.absdiff(prev_gray, gray)
    diff = cv2.convertScaleAbs(diff, alpha=2.5)

    # 应用形态学操作，去除噪声
    kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
    diff = cv2.morphologyEx(diff, cv2.MORPH_OPEN, kernel)
    diff = cv2.morphologyEx(diff, cv2.MORPH_CLOSE, kernel)

    _, thresh = cv2.threshold(diff, 30, 255, cv2.THRESH_BINARY)

    contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    # ===== 核心：目标检测和跟踪 =====
    target = None
    max_area = 0
    
    # 找到所有符合条件的轮廓
    valid_contours = []
    for cnt in contours:
        area = cv2.contourArea(cnt)

        if area < 75:  # 过滤噪声，增大阈值
            continue
        
        if area > 8000:  # 过滤大面积假目标（如整个画面），减小阈值
            continue

        # 计算轮廓的宽高比，过滤不符合垃圾特征的目标
        x, y, w, h = cv2.boundingRect(cnt)
        aspect_ratio = float(w) / h if h > 0 else 0
        if aspect_ratio > 3 or aspect_ratio < 0.3:  # 过滤过于狭长的目标
            continue
        
        valid_contours.append((area, cnt, x, y, w, h))
    
    # 如果有有效的轮廓
    if valid_contours:
        # 如果正在跟踪目标，优先选择与上一目标位置接近的轮廓
        if tracking_target is not None and prev_cx is not None and prev_cy is not None:
            best_match = None
            min_distance = float('inf')
            
            search_radius = 100  # 目标锁定区域半径

            for area, cnt, x, y, w, h in valid_contours:
                cx = x + w // 2
                cy = y + h // 2
                # 计算与上一目标的距离
                distance = ((cx - prev_cx) ** 2 + (cy - prev_cy) ** 2) ** 0.5

                # 只考虑在锁定区域内的目标
                if distance > search_radius:
                    continue

                # 如果距离小于阈值，认为是同一个目标
                if distance < 50:  # 距离阈值
                    if distance < min_distance:
                        min_distance = distance
                        best_match = (area, cnt, x, y, w, h)
            
            # 如果找到匹配的目标
            if best_match:
                area, cnt, x, y, w, h = best_match
                target = cnt
                max_area = area
            else:
                # 如果没有找到匹配的目标，选择最大的轮廓
                valid_contours.sort(key=lambda x: x[0], reverse=True)
                area, cnt, x, y, w, h = valid_contours[0]
                target = cnt
                max_area = area
                # 开始跟踪新目标
                tracking_target = True
        else:
            # 如果没有正在跟踪的目标，选择最大的轮廓
            valid_contours.sort(key=lambda x: x[0], reverse=True)
            area, cnt, x, y, w, h = valid_contours[0]
            target = cnt
            max_area = area
            # 开始跟踪新目标
            tracking_target = True

    # ===== 只处理一个目标 =====
    if target is not None:
        x, y, w, h = cv2.boundingRect(target)
        cx = x + w // 2
        cy = y + h // 2

        # 目标位置平滑处理
        alpha = 0.6
        if prev_cx is not None and prev_cy is not None:
            cx = int(alpha * prev_cx + (1 - alpha) * cx)
            cy = int(alpha * prev_cy + (1 - alpha) * cy)

        # 绘制画面中心
        center_x = 50  # 画面中心x坐标
        center_y = 120  # 画面中心y坐标
        cv2.circle(frame, (center_x, center_y), 5, (255, 0, 0), -1)
        
        # 绘制目标矩形和中心
        cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2)
        cv2.circle(frame, (cx, cy), 5, (0, 0, 255), -1)

        print("唯一目标:", cx, cy, "area:", max_area)
        
        # 检测目标是否突然跳变
        target_jumped = False
        if prev_cx is not None and prev_cy is not None and prev_area is not None:
            # 计算位置变化
            dx = abs(cx - prev_cx)
            dy = abs(cy - prev_cy)
            # 计算面积变化比例
            area_ratio = max_area / prev_area if prev_area > 0 else 0
            
            # 如果位置变化过大或面积变化过大，认为目标跳变
            if dx > 100 or dy > 100 or area_ratio < 0.3 or area_ratio > 3:
                target_jumped = True
                print("目标突然跳变，可能已离开视野范围")
                # 重置跟踪状态
                tracking_target = None
        
        # 控制逻辑：根据x轴和y轴坐标控制垃圾桶移动
        center_x = 160  # 画面中心x坐标
        center_y = 120  # 画面中心y坐标
        threshold = 15  # 阈值范围

        if target_jumped:
            # 目标跳变，停止移动
            print("目标跳变，停止移动")
            motor.stop()
        else:
            # 计算目标与中心的偏差
            dx = cx - center_x
            dy = cy - center_y

            # 确定移动方向
            if abs(dx) < threshold and abs(dy) < threshold:
                # 目标在中心附近，停止移动
                print("目标在中心，停止移动")
                motor.stop()
            elif abs(dx) < threshold:
                # 只在y轴方向有偏差
                if dy > threshold:
                    # 目标在下侧，垃圾桶向右移动
                    print("目标在下侧，向右移动")
                    motor.move_right(speed=1.0)
                else:
                    # 目标在上侧，垃圾桶向左移动
                    print("目标在上侧，向左移动")
                    motor.move_left(speed=1.0)
            elif abs(dy) < threshold:
                # 只在x轴方向有偏差
                if dx > threshold:
                    # 目标在右侧，垃圾桶向后移动
                    print("目标在右侧，向后移动")
                    motor.backward(speed=1.0)
                else:
                    # 目标在左侧，垃圾桶向前移动
                    print("目标在左侧，向前移动")
                    motor.forward(speed=1.0)
            else:
                # 在x轴和y轴方向都有偏差，使用斜向移动
                if dx > threshold and dy > threshold:
                    # 目标在右下侧，垃圾桶向右后移动
                    print("目标在右下侧，向右后移动")
                    motor.move_right_backward(speed=1.0)
                elif dx > threshold and dy < -threshold:
                    # 目标在右上侧，垃圾桶向左后移动
                    print("目标在右上侧，向左后移动")
                    motor.move_left_backward(speed=1.0)
                elif dx < -threshold and dy > threshold:
                    # 目标在左下侧，垃圾桶向右前移动
                    print("目标在左下侧，向右前移动")
                    motor.move_right_forward(speed=1.0)
                else:
                    # 目标在左上侧，垃圾桶向左前移动
                    print("目标在左上侧，向左前移动")
                    motor.move_left_forward(speed=1.0)
        
        # 更新上一帧的目标信息
        prev_cx = cx
        prev_cy = cy
        prev_area = max_area
    else:
        # 没有检测到目标，停止移动
        print("未检测到目标，停止移动")
        motor.stop()
        # 重置上一帧的目标信息
        prev_cx = None
        prev_cy = None
        prev_area = None
        # 重置跟踪状态
        tracking_target = None

    cv2.imshow("tracking", frame)

    prev_gray = gray.copy()

    if cv2.waitKey(1) & 0xFF == 27:
        break

cap.release()
cv2.destroyAllWindows()
motor.stop()