2023浙江省工训竞赛物流车识别部分开源

2023浙江省工训竞赛物流车初赛第二，同样适用于决赛，可惜队友没写扫描物块颜色的机械臂动作

这部分代码用于树莓派或旭日派识别物料等地图上的信息

import cv2
import numpy as np
import math
from math import *
import serial
import serial.tools.list_ports
import time

def DOpenPort(portx, bps, timeout):
    try:
        # 打开串口，并得到串口对象
        ser = serial.Serial(portx, bps, timeout=timeout)
        # 判断是否打开成功
        if(False == ser.is_open):
           ser = -1
    except Exception as e:
        print("---异常---：", e)
    return ser

def rotate(img, pt1, pt2, pt3, pt4):
    withRect = math.sqrt((pt4[0] - pt1[0]) ** 2 + (pt4[1] - pt1[1]) ** 2)  # 矩形框的宽度
    heightRect = math.sqrt((pt1[0] - pt2[0]) ** 2 + (pt1[1] - pt2[1]) ** 2)

    angle = acos((pt4[0] - pt1[0]) / withRect) * (180 / math.pi)  # 矩形框旋转角度

    if pt4[1] < pt1[1]:
        angle = -angle

    height = img.shape[0]  # 原始图像高度
    width = img.shape[1]  # 原始图像宽度
    rotateMat = cv2.getRotationMatrix2D((width / 2, height / 2), angle, 1)  # 按angle角度旋转图像
    heightNew = int(width * fabs(sin(radians(angle))) + height * fabs(cos(radians(angle))))
    widthNew = int(height * fabs(sin(radians(angle))) + width * fabs(cos(radians(angle))))

    rotateMat[0, 2] += (widthNew - width) / 2
    rotateMat[1, 2] += (heightNew - height) / 2
    imgRotation = cv2.warpAffine(img, rotateMat, (widthNew, heightNew), borderValue=(255, 255, 255))

    # 旋转后图像的四点坐标
    [[pt1[0]], [pt1[1]]] = np.dot(rotateMat, np.array([[pt1[0]], [pt1[1]], [1]]))
    [[pt3[0]], [pt3[1]]] = np.dot(rotateMat, np.array([[pt3[0]], [pt3[1]], [1]]))
    [[pt2[0]], [pt2[1]]] = np.dot(rotateMat, np.array([[pt2[0]], [pt2[1]], [1]]))
    [[pt4[0]], [pt4[1]]] = np.dot(rotateMat, np.array([[pt4[0]], [pt4[1]], [1]]))

    # 处理反转的情况
    if pt2[1] > pt4[1]:
        pt2[1], pt4[1] = pt4[1], pt2[1]
    if pt1[0] > pt3[0]:
        pt1[0], pt3[0] = pt3[0], pt1[0]

    imgOut = imgRotation[int(pt2[1]):int(pt4[1]), int(pt1[0]):int(pt3[0])]
    return imgOut

def solidity(img):
    x, y = img.shape
    # if x < 1 or y < 1:
    #     return 0
    bk = 0
    wt = 0
    for i in range(x):
        for j in range(y):
            if img[i, j] == 0:
                bk += 1
            else:
                wt += 1
    rate1 = wt / (x * y)
    rate2 = bk / (x * y)
    return rate1

def get_color(frame):
    # lower_red1 = np.array([0, 43, 46])
    # upper_red1 = np.array([6, 255, 255])
    lower_red1 = np.array([0, 50, 120])
    upper_red1 = np.array([10, 255, 255])
    # lower_red2 = np.array([170, 110, 128])
    # upper_red2 = np.array([180, 255, 255])
    lower_red2 = np.array([175, 0, 130])
    upper_red2 = np.array([179, 255, 255])

    #lower_green = np.array([35, 80, 80])
    lower_green = np.array([35, 100, 80])
    #upper_green = np.array([77, 255, 255])
    upper_green = np.array([120, 255, 255])

    # lower_blue = np.array([100, 43, 46])
    # upper_blue = np.array([124, 255, 255])
    lower_blue = np.array([120, 45, 50])
    upper_blue = np.array([140, 255, 255])

    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    maxsum = 250
    color = None

    red_mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
    red_mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
    red_mask = cv2.bitwise_or(red_mask1, red_mask2)
    green_mask = cv2.inRange(hsv, lower_green, upper_green)
    blue_mask = cv2.inRange(hsv, lower_blue, upper_blue)

    red_binary = cv2.threshold(red_mask, 127, 255, cv2.THRESH_BINARY)[1]
    green_binary = cv2.threshold(green_mask, 127, 255, cv2.THRESH_BINARY)[1]
    blue_binary = cv2.threshold(blue_mask, 127, 255, cv2.THRESH_BINARY)[1]

    red_cnts = cv2.findContours(red_binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
    green_cnts = cv2.findContours(green_binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
    blue_cnts = cv2.findContours(blue_binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]

    for c in red_cnts:
        rect = cv2.minAreaRect(c)
        box = cv2.boxPoints(rect)
        cv2.drawContours(frame, [np.int0(box)], -1, (0, 0, 255), 2)

        if cv2.contourArea(c) > maxsum:
            rect_image = rotate(red_binary, np.int0(box)[0], np.int0(box)[1], np.int0(box)[2], np.int0(box)[3])
            x, y = rect_image.shape
            if x > 10 and y > 10:
                rect_image = cv2.resize(rect_image, (0, 0), fx=0.25, fy=0.25)
            # rect_image = cv2.resize(rect_image, (0, 0), fx=0.5, fy=0.5)
            if solidity(rect_image) > 0.5 and rect[0][0] > 40 and rect[0][0] < 120:
                maxsum = cv2.contourArea(c)
                color = 'red'
                rect_image_max = rect_image
                cv2.imshow('rect_image_max', rect_image_max)

    for c in green_cnts:
        rect = cv2.minAreaRect(c)
        box = cv2.boxPoints(rect)
        cv2.drawContours(frame, [np.int0(box)], -1, (0, 0, 255), 2)

        if cv2.contourArea(c) > maxsum:
            rect_image = rotate(green_binary, np.int0(box)[0], np.int0(box)[1], np.int0(box)[2], np.int0(box)[3])
            x, y = rect_image.shape
            if x > 10 and y > 10:
                rect_image = cv2.resize(rect_image, (0, 0), fx=0.25, fy=0.25)
            # rect_image = cv2.resize(rect_image, (0, 0), fx=0.5, fy=0.5)
            if solidity(rect_image) > 0.5 and rect[0][0] > 40 and rect[0][0] < 120:
                maxsum = cv2.contourArea(c)
                color = 'green'
                rect_image_max = rect_image
                cv2.imshow('rect_image_max', rect_image_max)

    for c in blue_cnts:
        rect = cv2.minAreaRect(c)
        box = cv2.boxPoints(rect)
        cv2.drawContours(frame, [np.int0(box)], -1, (0, 0, 255), 2)

        if cv2.contourArea(c) > maxsum:
            rect_image = rotate(blue_binary, np.int0(box)[0], np.int0(box)[1], np.int0(box)[2], np.int0(box)[3])
            x, y = rect_image.shape
            if x > 10 and y > 10:
                rect_image = cv2.resize(rect_image, (0, 0), fx=0.25, fy=0.25)
            # rect_image = cv2.resize(rect_image, (0, 0), fx=0.5, fy=0.5)
            if solidity(rect_image) > 0.5 and rect[0][0] > 40 and rect[0][0] < 120:
                maxsum = cv2.contourArea(c)
                color = 'blue'
                rect_image_max = rect_image
                cv2.imshow('rect_image_max', rect_image_max)

    # cv2.imshow('rect_image_max', rect_image_max)
    cv2.imshow('frame', frame)
    return color

def TargetShooting1(frame):
    #lower_green = np.array([0, 40, 70])
    #upper_green = np.array([80, 255, 255])
    #lower_green = np.array([0, 40, 80])
    #upper_green = np.array([60, 255, 255])
    #lower_green = np.array([20, 90, 80])
    #upper_green = np.array([77, 255, 255])
    #lower_green = np.array([20, 40, 80])
    #upper_green = np.array([60, 255, 255])
    lower_green = np.array([130, 30, 0])
    upper_green = np.array([179, 255, 255])

    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    green_mask = cv2.inRange(hsv, lower_green, upper_green)
    dilated = cv2.dilate(green_mask, cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3)), iterations=3)
    #dilated = cv2.medianBlur(dilated, 7)
    cv2.imshow('dilated', dilated)
    circles = cv2.HoughCircles(dilated, cv2.HOUGH_GRADIENT, 1, 200, param1=15, param2=15, minRadius=20, maxRadius=120)

    if circles is not None:
        #print(circles)
        x, y, radius = circles[0][0]
        center = (int(x), int(y))
        frame1 = frame
        cv2.circle(frame1, center, int(radius), (0, 255, 0), 2)
        cv2.imshow("frame1", frame1)
        return circles[0][0]
    else:
        return [0,0,0]

def TargetShooting2(frame):
    #lower_green = np.array([30, 40, 80])
    #upper_green = np.array([60, 255, 255])
    lower_green = np.array([130, 30, 0])
    upper_green = np.array([179, 255, 255])
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    maxsum = 1000
    green_mask = cv2.inRange(hsv, lower_green, upper_green)
    green_binary = cv2.dilate(green_mask, cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3)), iterations=5)
    cv2.imshow('green_binary', green_binary)
    green_cnts = cv2.findContours(green_binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
    maxrect = None
    if green_cnts is not None:
        for c in green_cnts:
            rect = cv2.minAreaRect(c)

            if cv2.contourArea(c) > maxsum:
                maxrect = rect
                box = cv2.boxPoints(maxrect)
                cv2.drawContours(frame, [np.int0(box)], -1, (0, 0, 255), 2)
                maxsum = cv2.contourArea(c)
        cv2.imshow('frame', frame)
        if maxrect is not None:
            return maxrect[0]
        else:
            return [0,0]
    else:
        return [0,0] 

def videox():
    ser = DOpenPort('/dev/ttyS3', 115200, 1)
    
    cap = cv2.VideoCapture(0)
    width = 160
    height = 120
     
    #cap.set(6,cv2.VideoWriter_fourcc('M','J','P','G'))
    #cap.set(5,60)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)  # 设置图像宽度
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)  # 设置图像高度
    # 显示图像
    while True:
        T1 = time.perf_counter()
        ret, frame = cap.read()  # 读取图像(frame就是读取的视频帧，对frame处理就是对整个视频的处理)
        #retval = cap.get(5)
        #print(retval)
        cv2.imshow("frame", frame)
        
        readflag=b'\x00'
        if ser.in_waiting:
            readflag = ser.read(ser.in_waiting)
            print(readflag[0])
        
        if readflag[0] == 1:
            qrCodeDetector = cv2.QRCodeDetector()
            data, bbox, straight_qrcode = qrCodeDetector.detectAndDecode(frame)
            print(data)
            if data != '':
                #send_data = '\xA5' + '\x00' + data[0] + data[1] + data[2] + data[4] + data[5] + data[6] + '\x5A'
                #send_data_encode = send_data.encode('utf-8')
                send_data = bytearray([0xA5,0x00,int(data[0]),int(data[1]),int(data[2]),int(data[4]),int(data[5]),int(data[6]),0x5A])
                ser.write(send_data)
            
        if readflag[0] == 2:
            color_flag = get_color(frame)
            print(color_flag)
            if(color_flag == 'red'):
                ser.write(b'\xA5\x01\x00\x00\x00\x5A')
            if(color_flag == 'green'):
                ser.write(b'\xA5\x02\x00\x00\x00\x5A')
            if(color_flag == 'blue'):
                ser.write(b'\xA5\x03\x00\x00\x00\x5A')

        if readflag[0] == 3:
            a1 = TargetShooting2(frame)
            #a2 = TargetShooting1(frame)
            #a3 = TargetShooting1(frame)
            
            x1 = int(a1[0])
            x2 = 0
            y1 = int(a1[1])
            y2 = 0
            print([x1,y1])
            if x1>255:
                x1=255
                x2=x1-255
            if y1>255:
                y1=255
                y2=y1-255
            send_data = bytearray([0xA5,int(x1),int(x2),int(y1),int(y2),0x5A])
            print(send_data)
            ser.write(send_data)
            
        if readflag[0] == 4:
            a = TargetShooting2(frame)
            print(a)
            x1 = int(a[0])
            x2 = 0
            y1 = int(a[1])
            y2 = 0
            if a[0]>255:
                x1=255
                x2=a[0]-255
            if a[1]>255:
                y1=255
                y2=a[1]-255
            send_data = bytearray([0xA5,int(x1),int(x2),int(y1),int(y2),0x5A])
            #print(send_data)
            ser.write(send_data)
        T2 =time.perf_counter()
        #print('程序运行时间:%s毫秒' % ((T2 - T1)*1000))
        input = cv2.waitKey(20)
        if input == ord('q'):  # 如过输入的是q就break，结束图像显示，鼠标点击视频画面输入字符
            break

    cap.release()  # 释放摄像头
    cv2.destroyAllWindows()  # 销毁窗口

if __name__ == '__main__':
    videox()