方程式寻迹和识别算法

"""Run inference with a YOLOv5 model on images, videos, directories, streams

Usage:
    $ python path/to/detect.py --source path/to/img.jpg --weights yolov5s.pt --img 640
"""

import argparse
import sys
import time
from pathlib import Path

import cv2
import torch
import torch.backends.cudnn as cudnn

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
from matplotlib.pyplot import MultipleLocator
import math

import serial
import serial.tools.list_ports
import threading
import struct


FILE = Path(__file__).absolute()
sys.path.append(FILE.parents[0].as_posix())  # add yolov5/ to path

from models.experimental import attempt_load
from utils.datasets import LoadStreams, LoadImages
from utils.general import check_img_size, check_requirements, check_imshow, colorstr, non_max_suppression, \
    apply_classifier, scale_coords, xyxy2xywh, strip_optimizer, set_logging, increment_path, save_one_box
from utils.plots import colors, plot_one_box
from utils.torch_utils import select_device, load_classifier, time_synchronized



def int_to_hex(num):
    high_8bits = eval(hex(num)) >> 8
    low_8bits  = eval(hex(num)) & 0xff
    return high_8bits,low_8bits
def int_to_hex2(num):

    low_8bits  = eval(hex(num)) & 0xff
    return low_8bits

def serial_init():
    port_list = list(serial.tools.list_ports.comports())
    print(port_list)
    if len(port_list) == 0:
        print('无可用串口')
    else:
        for i in range(0,len(port_list)):
            print(port_list[i])
    try:
        portx="/dev/ttyUSB0"
        bps=115200
        #超时设置,None：永远等待操作，0为立即返回请求结果，其他值为等待超时时间(单位为秒）
        timex=None
        ser=serial.Serial(portx,bps,timeout=timex)
        print("串口详情参数：", ser)
        print(ser.port)#获取到当前打开的串口名
        return ser
    except Exception as e:
        print("---异常---：",e)

def ReadData(ser):
    global STRGLO,BOOL
    # 循环接收数据，此为死循环，可用线程实现
    while BOOL:
        if ser.in_waiting:
            STRGLO = ser.read(ser.in_waiting).decode("gbk")
            print('STRGLO',STRGLO)


def DWritePort(ser,text):

    result = ser.write(text.encode("utf-8"))  # 写数据
    #print('result',result)
    return result

cmd=[]
def control(turn_angle):
    print("{:#016x}".format(200))
    #cmd = ['\x12','\x34','\x16','\x15','\x00','\x45','\x12']
    cmd = [0x12,0x34,0x22,0x22,0x00,0x12,0x12]
    if turn_angle < 0:
        turn_angle = -turn_angle
        cmd[3]=0x01

    cmd[4],cmd[5] = int_to_hex(turn_angle)
    print("cmd",cmd)
    ser = serial_init()
    serial_com = serial.Serial("/dev/ttyUSB0", 115200)
    #threading.Thread(target=ReadData, args=(ser,)).start()
    #threading.Thread(target=DWritePort, args=(ser, cmd)).start()
    for i in range(len(cmd)):
        #count=DWritePort(ser,hex(cmd[i]))
        ser.write(cmd)
        print('cmd[i]',cmd[i])

#control(-244)

@torch.no_grad()
def run(weights='yolov5s.pt',  # model.pt path(s)
        source='data/images',  # file/dir/URL/glob, 0 for webcam
        imgsz=640,  # inference size (pixels)
        conf_thres=0.25,  # confidence threshold
        iou_thres=0.45,  # NMS IOU threshold
        max_det=1000,  # maximum detections per image
        device='',  # cuda device, i.e. 0 or 0,1,2,3 or cpu
        view_img=True,  # show results
        save_txt=False,  # save results to *.txt
        save_conf=False,  # save confidences in --save-txt labels
        save_crop=False,  # save cropped prediction boxes
        nosave=True,  # do not save images/videos
        classes=None,  # filter by class: --class 0, or --class 0 2 3
        agnostic_nms=False,  # class-agnostic NMS
        augment=False,  # augmented inference
        update=False,  # update all models
        project='runs/detect',  # save results to project/name
        name='exp',  # save results to project/name
        exist_ok=False,  # existing project/name ok, do not increment
        line_thickness=2,  # bounding box thickness (pixels)
        hide_labels=False,  # hide labels
        hide_conf=False,  # hide confidences
        half=False,  # use FP16 half-precision inference
        ):
    save_img = not nosave and not source.endswith('.txt')  # save inference images
    webcam = source.isnumeric() or source.endswith('.txt') or source.lower().startswith(
        ('rtsp://', 'rtmp://', 'http://', 'https://'))

    central_points = []
    left_points = []
    left_points_tmp = []
    right_points = []
    left_points_x = []
    left_points_y = []
    left_points_x_tmp = []
    left_points_y_tmp = []

    right_points_x = []
    right_points_y = []
    right_points_x_tmp = []
    right_points_y_tmp = []
    right_points_tmp = []


    center_points_x = []
    center_points_y = []
    center_points_x_tmp = []
    center_points_y_tmp = []
    center_points_tmp = []

    # Directories
    save_dir = increment_path(Path(project) / name, exist_ok=exist_ok)  # increment run
    (save_dir / 'labels' if save_txt else save_dir).mkdir(parents=True, exist_ok=True)  # make dir

    # Initialize
    set_logging()
    device = select_device(device)
    half &= device.type != 'cpu'  # half precision only supported on CUDA

    # Load model
    model = attempt_load(weights, map_location=device)  # load FP32 model
    stride = int(model.stride.max())  # model stride
    imgsz = check_img_size(imgsz, s=stride)  # check image size
    names = model.module.names if hasattr(model, 'module') else model.names  # get class names
    if half:
        model.half()  # to FP16

    # Second-stage classifier
    classify = False
    if classify:
        modelc = load_classifier(name='resnet50', n=2)  # initialize
        modelc.load_state_dict(torch.load('resnet50.pt', map_location=device)['model']).to(device).eval()

    # Set Dataloader
    vid_path, vid_writer = None, None
    if webcam:
        view_img = check_imshow()
        cudnn.benchmark = True  # set True to speed up constant image size inference
        dataset = LoadStreams(source, img_size=imgsz, stride=stride)
    else:
        dataset = LoadImages(source, img_size=imgsz, stride=stride)

    # Run inference
    if device.type != 'cpu':
        model(torch.zeros(1, 3, imgsz, imgsz).to(device).type_as(next(model.parameters())))  # run once
    t0 = time.time()
    for path, img, im0s, vid_cap in dataset:
        img = torch.from_numpy(img).to(device)
        img = img.half() if half else img.float()  # uint8 to fp16/32
        img /= 255.0  # 0 - 255 to 0.0 - 1.0
        if img.ndimension() == 3:
            img = img.unsqueeze(0)

        # Inference
        t1 = time_synchronized()
        pred = model(img, augment=augment)[0]

        # Apply NMS
        pred = non_max_suppression(pred, conf_thres, iou_thres, classes, agnostic_nms, max_det=max_det)
        t2 = time_synchronized()

        # Apply Classifier
        if classify:
            pred = apply_classifier(pred, modelc, img, im0s)

        # Process detections
        for i, det in enumerate(pred):  # detections per image
            if webcam:  # batch_size >= 1
                p, s, im0, frame = path[i], f'{i}: ', im0s[i].copy(), dataset.count
            else:
                p, s, im0, frame = path, '', im0s.copy(), getattr(dataset, 'frame', 0)

            p = Path(p)  # to Path
            save_path = str(save_dir / p.name)  # img.jpg
            txt_path = str(save_dir / 'labels' / p.stem) + ('' if dataset.mode == 'image' else f'_{frame}')  # img.txt
            s += '%gx%g ' % img.shape[2:]  # print string
            gn = torch.tensor(im0.shape)[[1, 0, 1, 0]]  # normalization gain whwh
            imc = im0.copy() if save_crop else im0  # for save_crop

            ########
            countsL,countsR = (0,0)
            c_range = []
            if len(det):
                # Rescale boxes from img_size to im0 size
                det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round()

                # Print results
                for c in det[:, -1].unique():
                    n = (det[:, -1] == c).sum()  # detections per class
                    s += f"{n} {names[int(c)]}{'s' * (n > 1)}, "  # add to string

                # Write results

                for *xyxy, conf, cls in reversed(det):
                    if save_txt:  # Write to file
                        xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist()  # normalized xywh
                        line = (cls, *xywh, conf) if save_conf else (cls, *xywh)  # label format
                        with open(txt_path + '.txt', 'a') as f:
                            f.write(('%g ' * len(line)).rstrip() % line + '\n')

                    if save_img or save_crop or view_img:  # Add bbox to image
                        c = int(cls)  # integer class
                        label = None if hide_labels else (names[c] if hide_conf else f'{names[c]} {conf:.2f}')
                        point1,point2 = plot_one_box(xyxy, im0, label=label, color=colors(c, True), line_thickness=line_thickness)
                        box_center =(int((point1[0]+point2[0])/2),int((point1[1]+point2[1])/2))
                        if save_crop:
                            save_one_box(xyxy, imc, file=save_dir / 'crops' / names[c] / f'{p.stem}.jpg', BGR=True)
                    if label == 'blue_cb' and box_center>(400,210) and box_center[1]<450:
                        right_points_x.append(int((point1[0]+point2[0])/2))
                        right_points_y.append(1044-int((point1[1]+point2[1])/2))
                        center_xy = (int((point1[0]+point2[0])/2),int((point1[1]+point2[1])/2))
                        cv2.circle(im0,center_xy,4,(255,0,255),2)   #1044   1920
                        #if countsR>=2:
                            #cv2.line(im0,center_xy,center_xy_last,(3,125,125),2,8)
                        countsR = countsR +1
                        center_xy_last = (int((point1[0]+point2[0])/2),int((point1[1]+point2[1])/2))
                    if label == 'red_cb'and box_center<(1400,450) and box_center[1]>210:
                        left_points_x.append(int((point1[0]+point2[0])/2))
                        left_points_y.append(1044-int((point1[1]+point2[1])/2))
                        center_xy = (int((point1[0]+point2[0])/2),int((point1[1]+point2[1])/2))
                        cv2.circle(im0,center_xy,4,(0,255,0),2)
                        countsL =  countsL +1


                if countsL >=4 and countsR>=4:
                        #paixu

                    left_points_y_arr = np.array(left_points_y)
                    indexL=np.lexsort([left_points_y_arr])
                    right_points_y_arr = np.array(right_points_y)
                    indexR=np.lexsort([right_points_y_arr])
                    tmp_countsL,tmp_countsR = (0,0)
                    for iiiR in  indexR:
                        right_points_x_tmp.append(right_points_x[iiiR])
                        right_points_y_tmp.append(1044-right_points_y[iiiR])
                        new_pointR = (right_points_x[iiiR],1044-right_points_y[iiiR])
                        if(tmp_countsR>=1):
                            cv2.line(im0,new_pointR,old_pointR,(255,0,0),3,8)
                        old_pointR = new_pointR
                        tmp_countsR = tmp_countsR + 1
                    right_points_x=right_points_x_tmp
                    right_points_y=right_points_y_tmp
                    #print('right_points_x_tmp:',right_points_x_tmp)
                    #print('right_points_y_tmp:',right_points_y_tmp)
                    for iiiL in  indexL:
                        left_points_x_tmp.append(left_points_x[iiiL])
                        left_points_y_tmp.append(1044-left_points_y[iiiL])
                        new_pointL = (left_points_x[iiiL],1044-left_points_y[iiiL])
                        if(tmp_countsL>=1):
                            cv2.line(im0,new_pointL,old_pointL,(0,0,255),3,8)
                        old_pointL = new_pointL
                        tmp_countsL = tmp_countsL + 1
                    left_points_x=left_points_x_tmp
                    left_points_y=left_points_y_tmp
                    index_comin = min(len(indexL),len(indexR))
                    tmp_countsC1 = 0
                    cx=[]
                    cy=[]
                    for iiiCC in range(index_comin):
                        c_x = int((right_points_x[iiiCC]+left_points_x[iiiCC])/2)
                        c_y = int((right_points_y[iiiCC]+left_points_y[iiiCC])/2)
                        cx.append(c_x)
                        cy.append(c_y)
                        center_points = (c_x,c_y)
                        if tmp_countsC1 >=1:
                            cv2.circle(im0,center_points,3,(0,0,0),3,8)
                            cv2.line(im0,center_points_last,center_points,(0,255,255),3,8)
                        tmp_countsC1 = tmp_countsC1 + 1
                        center_points_last  = center_points
                    cx = np.array(cx)
                    cy = np.array(cy)

                    F_center = np.polyfit(cy, cx, 1)  #i  chazhi
                    c_tan = F_center[0]
                    turn_angle = int(math.atan(c_tan)*180/3.14)
                    control(turn_angle)
                    #print('turn_angle:',turn_angle)

                    left_points_x = left_points_x_tmp
                    left_points_y = left_points_y_tmp
                    #print('left_points_x_tmp:',left_points_x_tmp)
                    #print('left_points_y_tmp:',left_points_y_tmp)
                    center_line_start = max(left_points_y[0],right_points_y[0])
                    center_line_end = min(left_points_y[countsL-1],right_points_y[countsR-1])
                    f2L = np.polyfit(left_points_y, left_points_x, 2)  #i  chazhi
                    f2R = np.polyfit(right_points_y, right_points_x, 2)  #i  chazhi
                    p2L = np.poly1d(f2L)
                    p2R = np.poly1d(f2R)
                    pC = (p2L+p2R)/2




                    for iiiC_tmp in range(10):
                        if center_line_start+(center_line_end-center_line_start)/10*iiiC_tmp >=400:
                            c_range.append(400)
                        else:
                            c_range.append(center_line_start+(center_line_end-center_line_start)/10*iiiC_tmp)
                    print(c_range)

                    tmp_countsC = 0
                    for iiiC in c_range:
                        center_points_y.append(int(iiiC))
                        center_points_x.append(pC(iiiC))

                        if int(pC(iiiC))<20 :
                            center_points_x[tmp_countsC] = 20
                        if int(pC(iiiC))>1900:
                            center_points_x[tmp_countsC] = 1900
                        current_center_point = (int(center_points_x[tmp_countsC]),int(iiiC))
                        #if tmp_countsC >=1:
                            #cv2.line(im0,last_center_point,current_center_point,(0,255,255),3,8)
                        last_center_point = current_center_point
                        tmp_countsC = tmp_countsC + 1

                    print('center_points_y',center_points_y)
                    print('center_points_x',center_points_x)

                    #print('center_points_y',center_points_y)
                    #print('xvalsC',xvalsC)



                    #cv2.circle(im0,(right_points_x,right_points_y),3,(255,255,0),2)


                #print('\nright_points_x:\n',right_points_x)
                #print('\nright_points_y:\n',right_points_y)





            # Print time (inference + NMS)
            print(f'{s}Done. ({t2 - t1:.3f}s)')
            #image = cv2.resize(image,(640,400), interpolation=cv2.INTER_LINEAR)
            im0=cv2.resize(im0,(1600,800), interpolation=cv2.INTER_LINEAR)
            cv2.imshow(str(p), im0)
            cv2.waitKey(1)  # 1 millisecond
            # Stream results

            central_points = []
            left_points = []
            left_points_tmp = []
            right_points = []
            left_points_x = []
            left_points_y = []
            left_points_x_tmp = []
            left_points_y_tmp = []

            right_points_x = []
            right_points_y = []
            right_points_x_tmp = []
            right_points_y_tmp = []
            right_points_tmp = []

            center_points_x = []
            center_points_y = []
            center_points_x_tmp = []
            center_points_y_tmp = []
            center_points_tmp = []


            if view_img:
                cv2.imshow(str(p), im0)
                cv2.waitKey(1)  # 1 millisecond

            # Save results (image with detections)
            if save_img == False:
                if dataset.mode == 'image':
                    cv2.imwrite(save_path, im0)
                else:  # 'video' or 'stream'
                    if vid_path != save_path:  # new video
                        vid_path = save_path
                        if isinstance(vid_writer, cv2.VideoWriter):
                            vid_writer.release()  # release previous video writer
                        if vid_cap:  # video
                            fps = vid_cap.get(cv2.CAP_PROP_FPS)
                            w = int(vid_cap.get(cv2.CAP_PROP_FRAME_WIDTH))
                            h = int(vid_cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
                        else:  # stream
                            fps, w, h = 30, im0.shape[1], im0.shape[0]
                            save_path += '.mp4'
                        vid_writer = cv2.VideoWriter(save_path, cv2.VideoWriter_fourcc(*'mp4v'), fps, (w, h))

                    vid_writer.write(im0)



    if save_txt or save_img:
        s = f"\n{len(list(save_dir.glob('labels/*.txt')))} labels saved to {save_dir / 'labels'}" if save_txt else ''
        print(f"Results saved to {save_dir}{s}")

    if update:
        strip_optimizer(weights)  # update model (to fix SourceChangeWarning)

    print(f'Done. ({time.time() - t0:.3f}s)')




def parse_opt():
    parser = argparse.ArgumentParser()
    parser.add_argument('--weights', nargs='+', type=str, default='../F1_train3/best.pt', help='model.pt path(s)')
    parser.add_argument('--source', type=str, default='E20.mp4', help='file/dir/URL/glob, 0 for webcam')
    parser.add_argument('--imgsz', '--img', '--img-size', type=int, default=640, help='inference size (pixels)')
    parser.add_argument('--conf-thres', type=float, default=0.25, help='confidence threshold')
    parser.add_argument('--iou-thres', type=float, default=0.45, help='NMS IoU threshold')
    parser.add_argument('--max-det', type=int, default=1000, help='maximum detections per image')
    parser.add_argument('--device', default='', help='cuda device, i.e. 0 or 0,1,2,3 or cpu')
    parser.add_argument('--view-img', action='store_true', help='show results')
    parser.add_argument('--save-txt', action='store_true', help='save results to *.txt')
    parser.add_argument('--save-conf', action='store_true', help='save confidences in --save-txt labels')
    parser.add_argument('--save-crop', action='store_true', help='save cropped prediction boxes')
    parser.add_argument('--nosave', action='store_true', help='do not save images/videos')
    parser.add_argument('--classes', nargs='+', type=int, help='filter by class: --class 0, or --class 0 2 3')
    parser.add_argument('--agnostic-nms', action='store_true', help='class-agnostic NMS')
    parser.add_argument('--augment', action='store_true', help='augmented inference')
    parser.add_argument('--update', action='store_true', help='update all models')
    parser.add_argument('--project', default='runs/detect', help='save results to project/name')
    parser.add_argument('--name', default='exp', help='save results to project/name')
    parser.add_argument('--exist-ok', action='store_true', help='existing project/name ok, do not increment')
    parser.add_argument('--line-thickness', default=2, type=int, help='bounding box thickness (pixels)')
    parser.add_argument('--hide-labels', default=False, action='store_true', help='hide labels')
    parser.add_argument('--hide-conf', default=True, action='store_true', help='hide confidences')   #屏蔽检测概率
    parser.add_argument('--half', action='store_true', help='use FP16 half-precision inference')
    opt = parser.parse_args()
    return opt


def main(opt):
    print(colorstr('detect: ') + ', '.join(f'{k}={v}' for k, v in vars(opt).items()))
    check_requirements(exclude=('tensorboard', 'thop'))
    run(**vars(opt))


if __name__ == "__main__":
    opt = parse_opt()
    main(opt)