2021-09-11

朱雀 2022-08-28 15:55 6阅读 0赞

import cv2
    import numpy as np
    import time
    import serial
    
    
    frameWidth = 640
    frameHeight = 480
    
    
    
    cap = cv2.VideoCapture(0)  # 0对应笔记本自带摄像头
    
    
    cap.set(3, frameWidth)  # set中，这里的3，下面的4和10是类似于功能号的东西，数字的值没有实际意义
    cap.set(4, frameHeight)
    cap.set(10, 80)        # 设置亮度
    pulse_ms = 30
    
    
    lower = np.array([100, 100, 60])
    upper = np.array([180, 255, 255])
    
    ball_color = 'red'
    ball_color2 = 'green'
    
    color_dist = {'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
                  'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
                  'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
                  }
    
    targetPos_x = 0
    targetPos_y = 0
    lastPos_x = 0
    lastPos_y = 0
    
    
    def draw_direction(img, lx, ly, nx, ny):
        # 根据上一位置与当前位置计算移动方向并绘制箭头
        dx = nx - lx
        dy = ny - ly
        if abs(dx) < 4 and abs(dy) < 4:
            dx = 0
            dy = 0
        else:
            r = (dx**2 + dy**2)**0.5
            dx = int(dx/r*40)
            dy = int(dy/r*40)
            # print(dx, dy)
        cv2.arrowedLine(img, (60, 100), (60+dx, 100+dy), (0, 255, 0), 2)
        # print(nx-lx, ny-ly)   # 噪声一般为+-1
        # cv2.arrowedLine(img, (150, 150), (150+(nx-lx)*4, 150+(ny-ly)*4), (0, 0, 255), 2, 0, 0, 0.2)
    
    
    flag = 0
    ted=serial.Serial(port="/dev/ttyAMA1",baudrate=9600)
    
    
    while True:
        #cv2.namedWindow('camera', cv2.WINDOW_AUTOSIZE)
    
        _, img = cap.read()
    
        imgHsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    
        imgMask = cv2.inRange(imgHsv, color_dist[ball_color]['Lower'], color_dist[ball_color]['Upper'])  # 获取遮罩
        imgOutput = cv2.bitwise_and(img, img, mask=imgMask)
        contours, hierarchy = cv2.findContours(imgMask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)  # 查找轮廓
        imgMask = cv2.cvtColor(imgMask, cv2.COLOR_GRAY2BGR)  # 转换后，后期才能够与原画面拼接，否则与原图维数不同
    
        # 下面的代码查找包围框，并绘制
        x, y, w, h = 0, 0, 0, 0
        for cnt in contours:
            area = cv2.contourArea(cnt)
            # print(area)
            if area > 300:
                x, y, w, h = cv2.boundingRect(cnt)
                lastPos_x = targetPos_x
                lastPos_y = targetPos_y
                targetPos_x = int(x + w / 2)
                targetPos_y = int(y + h / 2)
    
                if flag == 0 and targetPos_x >= 150 and targetPos_x <= 500 and targetPos_y >= 100 and targetPos_y <= 400:
                    flag = 1
                    print("targetPos_x" + str(targetPos_x))
                    print("targetPos_y" + str(targetPos_y))
                    ted.write(("targetPos_x" +str(targetPos_x)).encode("gbk"))
                    ted.write(("targetPos_y" +str(targetPos_y)).encode("gbk"))
                    time.sleep(7)
    
                if targetPos_x < 150 or targetPos_x > 500 or targetPos_y < 100 or targetPos_y > 400:
                    flag = 0
    
                cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)
                cv2.circle(img, (targetPos_x, targetPos_y), 2, (0, 255, 0), 4)
                cv2.rectangle(img, (150, 100), (500, 400), (0, 255, 0), 2)
    
        # 坐标（图像内的）
        cv2.putText(img, "({:0<2d}, {:0<2d})".format(targetPos_x, targetPos_y), (20, 30),
                    cv2.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 2)  # 文字
        draw_direction(img, lastPos_x, lastPos_y, targetPos_x, targetPos_y)
    
        imgStack = np.hstack([img, imgOutput])
        # imgStack = np.hstack([img, imgMask])            # 拼接
        cv2.imshow('Horizontal Stacking', imgStack)  # 显示
    
    
    
        if cv2.waitKey(pulse_ms) & 0xFF == ord('q'):          # 按下“q”推出（英文输入法）
            break
    
    #time.sleep(5)
    
    #打开第二个摄像头以识别路障
    
    
    
    #cv2.namedWindow('camera', cv2.WINDOW_AUTOSIZE)
    
    
    
    
    cap.release()
    cv2.destroyAllWindows()