NJUPT第二次积分赛小结与视觉部分开源
NJUPT第二次积分赛小结与视觉部分开源
跟队友连肝一周多积分赛，写了一堆屎山，总算是今天完赛了。结果也还行，80分到手。其实题目是全做完了的，但验收时我nt了没操作好导致丢了不少分，而且整个控制流程也都基于一堆bug和屎山做的，所以其实能做成这样我也很满意了，下次积分赛再战，只要国赛不出bug就行。
先放题目吧，如图：
基础部分：
基础1：小车从1号区域出发，巡线到2号区域（15分）
基础2：在基础1的基础上，小车在2号区域成功转直角弯，并走到3号区域。（15分）
基础3：在基础2的基础上，小车从3号区域成功走到4号位置。（10分）
基础4：在基础3的基础上，小车停车成功，在框内，不压到边界线。（10分）
提高部分：
小车以正方形和圆心区分左右移动，其中正方形指示小车向左移动，圆形指示小车向右移动。在移动时，如果移动侧存在道路，为变道，不存在道路为避障。其中避障从指定方向（识别的指示方向）绕过障碍，然后立即再次回到之前的道路上。即存在道路即继续按照道路行驶，如果不存在则超过障碍需要切回道路。距离越短越好。
例如，小车从3区域走到A障碍物处，发现为方形障碍物，需要往左移动，所以变道到虚线道路上，继续沿着虚线行驶到B位置，看到B为圆形障碍物，则小车往右侧移动，变道到实线道路上，继续沿着实线前进，看到C处为方形障碍物，从障碍物右侧避障再立刻回到实线道路。继续往前走，由于道路上不经过D处障碍物，所以不做处理。（D为隔壁道路的指示牌，与当前道路无关，视为干扰项，随机在小车此刻相邻道路的某一位置）。
注：所有障碍物颜色均为红色，一共有且仅有三个需要做出反应的障碍物。
发挥1：小车每成功路过一个障碍物，并执行相应动作获得10分，共30分。（路过干扰障碍物不额外加分）
发挥2：小车在每次变道/避障时，需要提前进行声光提示，变完道需要关闭声光提示。注：声光提示必须体现处左转和右转区别。（5分）
发挥3：小车成功走到4区域并完成停车。（10分）
发挥部分：
自由发挥出额外特色功能。（5分）
注：禁止使用麦克纳姆轮小车。
当初看到题目的时候，没觉得有多难，但真正入手时，是真的感觉到了题目的恶意。。。
我们队用的是树莓派4B 4GB + STM32401CCU6，一个USB摄像头，一个mpu6050（屁用没有），一个oled，一个TB6612，还有车板（拿平衡车板子改的，机械结构很逆天。）
先说出我们队做题时的几个困难点：
1、线好细，2mm线径，灰度巡线的队伍直接寄。还好我和我做控制的队友都没用过灰度，所以整套传感器基本是纯摄像头（mpu6050只能算是摆设，几套控制全程开环）
2、场地数字干扰。数字和线离得很近，干扰蛮大的，图像部分方案选取不好的话容易寄。
3、场地地形干扰。从1区域到2区域时车子左边噪声干扰较大，因为紧挨桌子，同时圆弧顶部离墙特别近，我们队的车又比较宽，如果不做一些处理的话巡线会直接撞墙，同时墙与地板之间的夹线对图像部分也有一定干扰。
4、障碍物大小不同。障碍物做的太过随便，都是拿胶把红色卡纸贴快递盒子上，高度大小都不太一样，甚至还有卷边。两个圆方识别我也是调了不少时间。
5、控制方案是真不好做。四个动作，两个变道，两个避障。在做变道的时候试了不少方案，一开始是准备mpu转90度走一段然后再转90度就开始寻线，但是后来由于车子形态，pid控制，mpu零漂，代码屎山堆积等多方面因素影响，我们最终选择直接开环。。。然后程序直接少了上百行，可读性大大增强。至于变道怎么个开环法，那还得是车子强大的巡线功能，从一条线上转个角度就能直接巡到另一条线上😋然后就是避障，还是靠的巡线，转个角度走个圆弧就切进线了😋
6、通信。我们用的串口，因为最常用。至于其他乱七八糟的通信协议用的都不怎么熟练，所以直接选用串口，但由于第一次进行STM32与树莓派交互通信，bug频出，有一个bug找了整整一天，最后还是一个有经验的学长过来指导的。。。真的很感谢那位学长！红豆泥阿里嘎多！
废话不多说，直接上图像部分代码。这是我封装后的函数，所以要用的话直接复制就能用。
bgr转hsv
def TrackBar_Init():
cv2.namedWindow('hsv_binary')
cv2.createTrackbar('hmin', 'hsv_binary', 0, 179, call_back)
cv2.createTrackbar('hmax', 'hsv_binary', 179, 179, call_back)
cv2.createTrackbar('smin', 'hsv_binary', 0, 255, call_back)
cv2.createTrackbar('smax', 'hsv_binary', 255, 255, call_back)
cv2.createTrackbar('vmin', 'hsv_binary', 130, 255, call_back)
cv2.createTrackbar('vmax', 'hsv_binary', 255, 255, call_back)
def getHSV(image):
# 1 get trackbar's value
hmin = cv2.getTrackbarPos('hmin', 'hsv_binary')
hmax = cv2.getTrackbarPos('hmax', 'hsv_binary')
smin = cv2.getTrackbarPos('smin', 'hsv_binary')
smax = cv2.getTrackbarPos('smax', 'hsv_binary')
vmin = cv2.getTrackbarPos('vmin', 'hsv_binary')
vmax = cv2.getTrackbarPos('vmax', 'hsv_binary')
cv2.imshow("hsv_binary", win)
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
h, s, v = cv2.split(hsv)
h_binary = cv2.inRange(np.array(h), np.array(hmin), np.array(hmax))
s_binary = cv2.inRange(np.array(s), np.array(smin), np.array(smax))
v_binary = cv2.inRange(np.array(v), np.array(vmin), np.array(vmax))
binary = 255 - cv2.bitwise_and(h_binary, cv2.bitwise_and(s_binary, v_binary))
return binary
逆时针90度旋转摄像头读取的图像：
def rotateAntiClock_90(img):
img = cv2.flip(cv2.transpose(img), 0)
return img
图像处理
def Image_Processing():
global frame, binary, dst, blur
ret, frame = camera.read()
frame = rotateAntiClock_90(frame) # 逆时针旋转90度
binary = getHSV(frame)
blur = cv2.GaussianBlur(binary, (9, 9), 0)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (10, 10))
dst = cv2.morphologyEx(blur, cv2.MORPH_CLOSE, kernel)
cv2.imshow('Dilate', dst) # dst：处理后的图像
巡线（实线）
def Find_RealLine(src): # 从左扫线
dst = src
direc1_x = 0
direc1_y = 0
diffCRL = 0
filtRate = 1 / 20
dirLine = int(dst.shape[0]*9/13) # 扫线
dot_x = []
# Canny边缘检测
dst_canny = cv2.Canny(dst, 190, 250)
h, w = dst_canny.shape[0:2]
dst_canny = dst_canny[:, 0 + int(filtRate*w):int(w - w*filtRate)] # 提取ROI
# cv2.imshow('canny', dst_canny)
# 找基准点，利用与运算掩模
dst_mask_line = np.zeros(dst_canny.shape, np.uint8)
dst_mask_line[dirLine, :] = 255 # 图像尺寸：640x480
dotImg = cv2.bitwise_and(dst_mask_line, dst_canny) # 使用按位与找交点
# 从左往右找第一第二个交点，取中点即为基准点
for i in range(0, dotImg.shape[1]):
if dotImg[dirLine, i] == 255:
dot_x.append(i)
try: # 防止没有找到交点导致列表内无数据的情况
x1 = dot_x[0]
x2 = dot_x[1]
# 基准点坐标
direc1_x = int((x1 + x2) / 2)
direc1_y = dirLine
print((direc1_x, direc1_y))
# 计算偏差值
diffCRL = direc1_x - int(1/2*dst_canny.shape[1])
print(diffCRL)
except:
pass
return diffCRL
巡线（虚线）
def Find_DottedLine(img): # 巡虚线：利用图像矩（好用！）
# img = cv2.Canny(img, 50, 150)
area = 0
difDotLine = 0
h, w = img.shape[0:2]
img = img[int(3/5*h):h, :] # 提取ROI
m = cv2.moments(img)
try:
x = int(m['m10'] / m['m00'])
y = int(m['m01'] / m['m00'])
area = int(m['m00'])
difDotLine = int(x - 1 / 2 * img.shape[1])
# cv2.circle(frame, (x, y + int(2/3*h)), 7, (0, 255, 0), 4)
cv2.imshow('dstline', img)
except:
call_back()
return area, difDotLine
检测近似水平的线
def findHorizonLine(src): # 检测水平直线
flag = 0
src = src[int(1/2 * src.shape[0]):src.shape[0], :] # 提取ROI
src_canny = cv2.Canny(src, 50, 150)
lines = cv2.HoughLinesP(src_canny, 1, np.pi / 180, 70, minLineLength=40, maxLineGap=10)
try:
for line in lines:
x1, y1, x2, y2 = line[0]
slope = (y2 - y1) / (x2 - x1 + 1e-6)
if abs(slope) > 0.3: # 滤掉斜率大于0.3的线
continue
else:
print("Found Horizon Line")
flag = 1
# cv2.line(frame, (x1, y1+int(1/2 * src.shape[0])), (x2, y2+int(1/2 * src.shape[0])), (0, 0, 255), 2)
except:
call_back()
return flag
检测近似垂直的线
def findVerticalLine(src):
flag = 0
src = src[int(src.shape[0]*2/3):src.shape[0], :]
lines = cv2.HoughLinesP(src, 1, np.pi / 180, 70, minLineLength=150, maxLineGap=5)
try:
for line in lines:
x1, y1, x2, y2 = line[0]
slope = (x2 - x1) / (y2 - y1 + 1e-6)
if abs(slope) > 0.3:
continue
else:
print("Found vertical Line")
flag = 1
# cv2.line(frame, (x1, y1 + int(src.shape[0]*2/3)), (x2, y2 + int(src.shape[0]*2/3)), (0, 0, 255), 2)
except:
call_back()
return flag
串口发送消息
def comPrint(_string):
message = _string
ser.write(message.encode())
串口接收单字节消息（能接收，但有bug）
def getMessage():
if ser.inWaiting():
string = ser.read(1) # 监听消息
return string
识别矩形
def getRect(img):
# global frame
shape = 0
centerPoint = (0, 0)
area = 0
_, contours, hierarchy = cv2.findContours(img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) # openCV不同版本有区别
for obj in contours:
area = cv2.contourArea(obj) # 计算轮廓内区域的面积
perimeter = cv2.arcLength(obj, True) # 计算轮廓周长
approx = cv2.approxPolyDP(obj, 0.02 * perimeter, True) # 获取轮廓角点坐标
CornerNum = len(approx) # 轮廓角点的数量
x, y, w, h = cv2.boundingRect(approx) # 获取坐标值和宽度、高度
centerPoint = (int(x+(1/2)*w), int(y+(1/2)*w))
if CornerNum == 4:
shape = 4
# cv2.circle(frame, centerPoint, 3, (0,0,255), -1)
return shape, centerPoint, area
识别并提取圆形
def getCircle(src): # src:处理过的图像
signalCircle = 0
circle = []
edges = cv2.Canny(src, 128, 255)
cv2.imshow('canny', edges)
# 检测图像形状
# edges = edges[int(edges.shape[0] * 1 / 10):edges.shape[0], :]
circles = cv2.HoughCircles(edges, cv2.HOUGH_GRADIENT, 1, 300,
param1=13, param2=26, minRadius=50,
maxRadius=200)
# 确保至少发现一个圆
if circles is not None:
# 进行取整操作
print("Circle here")
edges = np.zeros(edges.shape, np.uint8)
circles = np.round(circles[0, :]).astype("int")
signalCircle = 1
# 循环遍历所有的坐标和半径
# 绘制结果
for (x, y, r) in circles:
cv2.circle(frame, (x, y), r, (255, 0, 0), 4)
cv2.rectangle(frame, (x - 5, y - 5), (x + 5, y + 5), (255, 128, 0), -1)
circle.append((x,y,r))
return signalCircle, circle
提取红色部分
def getRedImg(img):
grid_HSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# H、S、V范围一：
lower1 = np.array([0, 43, 46])
upper1 = np.array([10, 255, 255])
mask1 = cv2.inRange(grid_HSV, lower1, upper1) # mask1 为二值图像
# H、S、V范围二：
lower2 = np.array([156, 43, 46])
upper2 = np.array([180, 255, 255])
mask2 = cv2.inRange(grid_HSV, lower2, upper2)
# 将两个二值图像结果 相加
mask3 = mask1 + mask2
# 去除噪声
kernel = np.ones((5, 5), np.uint8)
dst3 = cv2.morphologyEx(mask3, cv2.MORPH_OPEN, kernel)
dst3 = cv2.medianBlur(dst3, 29)
return dst3
全套工程代码（一堆屎山）
import numpy as np
import cv2
import serial
import time
from numpy import *
import RPi.GPIO as GPIO
ser = serial.Serial("/dev/ttyAMA0", 9600)
ser.timeout = 5
if not ser.isOpen:
ser.open() # 打开串口
width, height = 160, 120
camera = cv2.VideoCapture(0)
camera.set(100, width)
camera.set(80, height)
camera.set(cv2.CAP_PROP_FPS, 10)
win = np.zeros((300, 512, 3), np.uint8)
dst = np.zeros((width, height), np.uint8)
blur = np.zeros((width, height), np.uint8)
direc1_x = 0
direc1_y = 0
diffCDL = 0
diffCRL = 0
modeFlag = 0
horizonFlag = 0
verticalFlag = 0
followReaLineFlag = 1
followDottedLineFlag = 1
findDotHorLineFlag = 0
rectFlag = 0
circleFlag = 0
whichLine = 1 # 1:实线，2：虚线，0：中间位置
stopFlag = 0
openFlag = 0
ticks = 0
tickFlag = 0
printFlag = 0
turnFlag = 0
newModeFlag = 0
def call_back(*arg):
pass
def TrackBar_Init():
# 1 create windows
cv2.namedWindow('hsl_binary')
# 2 Create Trackbar
cv2.createTrackbar('hmin', 'hsl_binary', 0, 179, call_back)
cv2.createTrackbar('hmax', 'hsl_binary', 179, 179, call_back)
cv2.createTrackbar('smin', 'hsl_binary', 0, 255, call_back)
cv2.createTrackbar('smax', 'hsl_binary', 255, 255, call_back)
cv2.createTrackbar('vmin', 'hsl_binary', 130, 255, call_back)
cv2.createTrackbar('vmax', 'hsl_binary', 255, 255, call_back)
def Init():
TrackBar_Init()
GPIO.setmode(GPIO.BCM)
GPIO.setup(5, GPIO.IN)
GPIO.setup(5, GPIO.IN, pull_up_down=GPIO.PUD_UP) # 可以调整上下拉状态
def rotateAntiClock_90(img):
img = cv2.flip(cv2.transpose(img), 0)
return img
# 在HSV色彩空间下得到二值图
def Get_HSV(image):
# 1 get trackbar's value
hmin = cv2.getTrackbarPos('hmin', 'hsl_binary')
hmax = cv2.getTrackbarPos('hmax', 'hsl_binary')
smin = cv2.getTrackbarPos('smin', 'hsl_binary')
smax = cv2.getTrackbarPos('smax', 'hsl_binary')
vmin = cv2.getTrackbarPos('vmin', 'hsl_binary') # 128
vmax = cv2.getTrackbarPos('vmax', 'hsl_binary')
cv2.imshow("hsl_binary", win)
# 2 to HSV
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
# cv2.imshow('hsv', hsv)
h, s, v = cv2.split(hsv)
# 3 set threshold (binary image)
# if value in (min, max):white; otherwise:black
h_binary = cv2.inRange(np.array(h), np.array(hmin), np.array(hmax))
s_binary = cv2.inRange(np.array(s), np.array(smin), np.array(smax))
v_binary = cv2.inRange(np.array(v), np.array(vmin), np.array(vmax))
# 4 get binary（对H、S、V三个通道分别与操作）
binary = 255 - cv2.bitwise_and(h_binary, cv2.bitwise_and(s_binary, v_binary))
# 5 Show
# cv2.imshow('binary', binary)
return binary
# 图像处理
def Image_Processing():
global frame, binary, dst, blur
ret, frame = camera.read()
frame = rotateAntiClock_90(frame) # 逆时针旋转90度
binary = Get_HSV(frame)
blur = cv2.GaussianBlur(binary, (9, 9), 0)
# cv2.imshow('blur', blur)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (10, 10))
dst = cv2.morphologyEx(blur, cv2.MORPH_CLOSE, kernel)
cv2.imshow('Dilate', dst) # dst：处理后的图像
def Find_RealLine(): # 从左扫线
global dst, direc1_x, direc1_y, diffCRL, frame
dirLine = int(dst.shape[0]*9/13)
dot_x = []
# Canny边缘检测
dst_canny = cv2.Canny(dst, 190, 250)
h, w = dst_canny.shape[0:2]
filtRate = 1/20
dst_canny = dst_canny[:, 0 + int(filtRate*w):int(w - w*filtRate)]
# cv2.imshow('canny', dst_canny)
# 找基准点，利用与运算掩模
dst_mask_line = np.zeros(dst_canny.shape, np.uint8)
dst_mask_line[dirLine, :] = 255 # 图像尺寸：640x480
dotImg = cv2.bitwise_and(dst_mask_line, dst_canny) # 使用按位与找交点
# 从左往右找第一第二个交点，取中点即为基准点
for i in range(0, dotImg.shape[1]):
if dotImg[dirLine, i] == 255:
dot_x.append(i)
try: # 防止没有找到交点导致列表内无数据的情况
x1 = dot_x[0]
x2 = dot_x[1]
# 基准点坐标
direc1_x = int((x1 + x2) / 2)
direc1_y = dirLine
# 画出基准点
frame = cv2.circle(frame, (direc1_x + int(filtRate*w), direc1_y), 5, (0, 0, 255), 2)
dst_canny = cv2.line(dst_canny, (dirLine, 0), (dirLine, dst_canny.shape[0]), (255, 255, 255), 2)
dst_canny = cv2.cvtColor(dst_canny, cv2.COLOR_GRAY2BGR)
# cv2.imshow('dst_canny', dst_canny)
print((direc1_x, direc1_y))
# 计算偏差值
diffCRL = direc1_x - int(1/2*dst_canny.shape[1])
print(diffCRL)
except:
pass
def findHorizonLine(src): # 检测水平直线
global modeFlag, horizonFlag, frame
flag = 0
src = src[int(1/2 * src.shape[0]):src.shape[0], :]
dst_y = cv2.Canny(src, 50, 150)
# cv2.imshow('dst_y', dst_y)
lines = cv2.HoughLinesP(dst_y, 1, np.pi / 180, 70, minLineLength=40, maxLineGap=10)
# dsty_bgr = cv2.cvtColor(dst_y, cv2.COLOR_GRAY2BGR)
try:
for line in lines:
x1, y1, x2, y2 = line[0]
slope = (y2 - y1) / (x2 - x1 + 1e-6)
if abs(slope) > 0.3:
continue
else:
print("Found Line")
flag = 1
horizonFlag = 1
cv2.line(frame, (x1, y1+int(1/2 * src.shape[0])), (x2, y2+int(1/2 * src.shape[0])), (0, 0, 255), 2)
except:
call_back()
# cv2.imshow('dsty_line', dsty_bgr)
return flag
def findVerticalLine(src):
global modeFlag, frame
flag = 0
src = src[int(src.shape[0]*2/3):src.shape[0], :]
lines = cv2.HoughLinesP(src, 1, np.pi / 180, 70, minLineLength=150, maxLineGap=5)
try:
for line in lines:
x1, y1, x2, y2 = line[0]
slope = (x2 - x1) / (y2 - y1 + 1e-6)
if abs(slope) > 0.45:
continue
else:
print("Found vertical Line")
flag = 1
cv2.line(frame, (x1, y1 + int(src.shape[0]*2/3)), (x2, y2 + int(src.shape[0]*2/3)), (0, 0, 255), 2)
except:
call_back()
return flag
def findChangeLine(src):
global modeFlag, horizonFlag, frame
# src = cv2.Canny(src, 50, 150)
flag = 0
src = src[int(1 / 3 * src.shape[0]):src.shape[0], 0:int(src.shape[1] * 5/6)]
# cv2.imshow('HorDotLine', src)
lines = cv2.HoughLinesP(src, 1, np.pi / 180, 70, minLineLength=40, maxLineGap=120)
try:
for line in lines:
x1, y1, x2, y2 = line[0]
slope = (y2 - y1) / (x2 - x1 + 1e-6)
if 1/100 <= abs(slope) <= 2:
print("Found Change Line")
flag = 1
# horizonFlag = 1
else:
continue
cv2.line(frame, (x1, y1+int(1 / 2 * src.shape[0])), (x2, y2+int(1 / 2 * src.shape[0])), (0, 0, 255), 2)
except:
call_back()
return flag
def findChangeRLine(src):
global modeFlag
# src = cv2.Canny(src, 50, 150)
flag = 0
src = src[int(1 / 4 * src.shape[0]):src.shape[0], :]
# cv2.imshow('HorDotLine', src)
lines = cv2.HoughLinesP(src, 1, np.pi / 180, 70, minLineLength=50, maxLineGap=20)
try:
for line in lines:
x1, y1, x2, y2 = line[0]
slope = (y2 - y1) / (x2 - x1 + 1e-6)
if 1/100 <= abs(slope) <= 2:
print("Found Change Line")
flag = 1
# horizonFlag = 1
else:
continue
# cv2.line(frame, (x1, y1+int(1 / 2 * src.shape[0])), (x2, y2+int(1 / 2 * src.shape[0])), (0, 0, 255), 2)
except:
call_back()
return flag
def Find_DottedLine(img): # 巡虚线：利用图像矩（好用！）
global diffCRL, frame
# img = cv2.Canny(img, 50, 150)
area = 0
difDotLine = 0
h, w = img.shape[0:2]
img = img[int(3/5*h):h, :]
m = cv2.moments(img)
try:
x = int(m['m10'] / m['m00'])
y = int(m['m01'] / m['m00'])
area = int(m['m00'])
difDotLine = int(x - 1 / 2 * img.shape[1])
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
cv2.circle(frame, (x, y + int(2/3*h)), 7, (0, 255, 0), 4)
cv2.imshow('dstline', img)
except:
call_back()
cv2.imshow("DotLine", img)
return area, difDotLine
# cv2.fitLine(points, distType, param, reps, aeps[, line ]) -> Line
def findDottedLine3(img): # 巡虚线方案3：利用cv2.fitLine方法
cnt = [] # 点集
minVal = 1 # 区间下限
maxVal = 255 # 区间上限
# 由于拟合出的直线几乎垂直，故使用 x = m * y + n 型直线
h, w = img.shape[0:2]
img = img[int(2 / 3 * h):h, :] # 提取ROI，滤去边缘噪声
for i in range(img.shape[0]):
for j in range(img.shape[1]):
if minVal <= img[i, j] <= maxVal:
cnt.append([j, i])
cnt = np.array(cnt) # cnt 必须为矩阵形式，且表示[x,y]坐标
[vx, vy, x, y] = cv2.fitLine(cnt, cv2.DIST_L2, 0, 0.01, 0.01)
# 其中：(vx,vy)为直线的方向向量，(x,y)为直线上的一个点
img_bgr = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) # 用来画线的BGR图
try: # 防止全黑图等情况
m = vx / vy
n = x - m * y
pt1 = (int(m * (y - 1000) + n), int(y - 1000))
pt2 = (int(m * (y + 1000) + n), int(y + 1000))
cv2.line(img_bgr, pt1, pt2, (0, 255, 0), 3)
cv2.imshow('DottedLine', img_bgr)
except:
pass
def comPrint(_string):
message = _string
ser.write(message.encode())
# ser.write(b"10%d" % diffCRL)
def getRect(img):
global frame
shape = 0
centerPoint = (0, 0)
area = 0
imgContour = img.copy()
_, contours, hierarchy = cv2.findContours(img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
for obj in contours:
area = cv2.contourArea(obj) # 计算轮廓内区域的面积
cv2.drawContours(imgContour, obj, -1, (255, 0, 0), 4) # 绘制轮廓线
perimeter = cv2.arcLength(obj, True) # 计算轮廓周长
approx = cv2.approxPolyDP(obj, 0.02 * perimeter, True) # 获取轮廓角点坐标
CornerNum = len(approx) # 轮廓角点的数量
x, y, w, h = cv2.boundingRect(approx) # 获取坐标值和宽度、高度
centerPoint = (int(x+(1/2)*w), int(y+(1/2)*w))
if CornerNum == 4:
shape = 4
cv2.circle(frame, centerPoint, 3, (0,0,255), -1)
return shape, centerPoint, area, imgContour
def findRed():
global frame
signalRect = 0 # 检测矩形标志位
signalCirlcle = 0 # 检测圆形标志位
img = frame
# cv2.imshow('ord', img)
grid_RGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
# 从RGB色彩空间转换到HSV色彩空间
grid_HSV = cv2.cvtColor(grid_RGB, cv2.COLOR_RGB2HSV)
# H、S、V范围一：
lower1 = np.array([0, 43, 46])
upper1 = np.array([10, 255, 255])
mask1 = cv2.inRange(grid_HSV, lower1, upper1) # mask1 为二值图像
# H、S、V范围二：
lower2 = np.array([156, 43, 46])
upper2 = np.array([180, 255, 255])
mask2 = cv2.inRange(grid_HSV, lower2, upper2)
# 将两个二值图像结果 相加
mask3 = mask1 + mask2
# 去除噪声
kernel = np.ones((5, 5), np.uint8)
dst3 = cv2.morphologyEx(mask3, cv2.MORPH_OPEN, kernel)
dst3 = cv2.medianBlur(dst3, 29)
# dst3 = dst3[:, int(dst3.shape[1]*1/10):dst3.shape[1]]
# 结果显示
# cv2.imshow("dst", dst3)
edges = cv2.Canny(dst3, 128, 255)
cv2.imshow('canny', edges)
# 检测图像形状
edges = edges[int(edges.shape[0] * 1/10):edges.shape[0], :]
circles = cv2.HoughCircles(edges, cv2.HOUGH_GRADIENT, 1, 300,
param1=13, param2=26, minRadius=50,
maxRadius=200)
# dst3 = cv2.cvtColor(dst3, cv2.COLOR_GRAY2BGR)
# 确保至少发现一个圆
if circles is not None:
# 进行取整操作
print("Circle here")
edges = np.zeros(edges.shape, np.uint8)
circles = np.round(circles[0, :]).astype("int")
signalCirlcle = 1
# 循环遍历所有的坐标和半径
for (x, y, r) in circles:
# 绘制结果
cv2.circle(frame, (x + int(dst3.shape[1]*1/5), y), r, (255, 0, 0), 4)
# cv2.rectangle(frame, (x - 5, y - 5), (x + 5, y + 5), (255, 128, 0), -1)
shape, centerPoint, area, _ = getRect(edges)
# 检测矩形：
if shape >= 2 and area >= 2500:
print("There is a Rectangular")
edges = np.zeros(edges.shape, np.uint8)
cv2.circle(frame, centerPoint, 3, (255,0,0), 3)
signalRect = 1
# cv2.imshow('dst3', dst3)
# edges = np.zeros(edges.shape(), np.uint8)
del edges
return signalRect, signalCirlcle
if __name__ == '__main__':
Init()
modeFlag = 2
stopFlag = 2
turnFlag = 0
while True:
Image_Processing()
# Find_RealLine()
# modeFlag = 10
# if findHorizonDotLine(frame):
# print("findHorizon Line")
# _, diffCDL = Find_DottedLine(dst)
# if findChangeLine(dst) == 1:
# print("Chnage!")
# findVerticalLine(dst)
# comPrint("14\r\n")
# if ser.read(1) == b'A':
# print("A")
# comPrint("10%d\r\n" % diffCDL)
# comPrint("10%d\r\n" % diffCRL)
# findRed()
# findHorizonLine(dst)
# comPrint("13\r\n")
# string = ser.read(1)
# print(string)
if modeFlag == 0: # 出发走直线
comPrint("17\r\n") # 开环走直线模式
if findHorizonLine(dst): # 如果看到水平线
modeFlag = 1 # 切换
# 模式
if modeFlag == 1: # 右转90度模式
turnFlag = 1
horizonFlag = 0
comPrint("12\r\n") # 让车子转弯
print("turn RIGHT, STM32!")
# string = ser.read(1) # 读取来自stm32的消息
# print(string)
# if string == b'A': # 如果收到"A"
# modeFlag = 2 # 切换模式
# print(modeFlag)
verticalFlag = findVerticalLine(dst)
if tickFlag == 0:
ticks = int(time.time())
tickFlag = 1
if verticalFlag == 1 and int(time.time()) - ticks >= 5:
print("Find Vertical Line")
followReaLineFlag = 1
modeFlag = 2
horizonFlag = 0
tickFlag = 0
if modeFlag == 2: # 巡实线模式，同时检测障碍物与停车线
whichLine = 1 # 实线
tickFlag = 0
openFlag = 1
if followReaLineFlag == 1:
Find_RealLine()
# _,diffCDL = Find_DottedLine()
comPrint("10%d\r\n" % diffCRL) # 发给stm32偏移量
print("10%d\n" % diffCRL)
rectFlag, circleFlag = findRed()
horizonFlag = findHorizonLine(dst)
if horizonFlag == 1 and stopFlag >= 2: # 经过多少障碍物停车
modeFlag = 8 # 实线停车模式
if rectFlag == 1:
followReaLineFlag = 0
modeFlag = 3 # 向左变道
# stopFlag = stopFlag + 1
if circleFlag == 1:
followReaLineFlag = 0
modeFlag = 6 # 向右避障
# stopFlag = stopFlag + 1
# 给stm32发变道或避障指令
# 如果读到来自stm32的动作完毕指令，就开始巡虚线或实线，并将形状flag置为0
if turnFlag == 0:
if findHorizonLine(dst):
horizonFlag = 1
if horizonFlag == 1:
tickFlag = 0
modeFlag = 1
if modeFlag == 7: # 巡虚线模式
whichLine = 2
tickFlag = 0
openFlag = 1
if followDottedLineFlag == 1:
print("Find Dot Line")
_, diffCDL = Find_DottedLine(dst)
comPrint("10%d\r\n" % diffCDL) # 发给stm32偏移量
rectFlag, circleFlag = findRed()
horizonFlag = findHorizonLine(dst)
if horizonFlag == 1 and stopFlag >= 2:
modeFlag = 9 # 虚线停车模式
if rectFlag == 1:
followDottedLineFlag = 0
modeFlag = 5 # 向左避障
if circleFlag == 1:
followDottedLineFlag = 0
modeFlag = 4 # 向右变道
# 此处再加一个停车位的判断
if modeFlag == 3: # 向左变道模式
whichLine = 0
if tickFlag == 0:
ticks = int(time.time())
tickFlag = 1
openFlag = 1
if int(time.time()) - ticks >= 2 and openFlag == 1:
stopFlag = stopFlag + 1
openFlag = 0
comPrint("13\r\n") # 向左变道指令
if ser.inWaiting():
string = ser.read(1) # 监听消息
if string == b'A': # 收到stm32的动作完毕指令
modeFlag = 7 # 直接进入巡线
print("Following Dot Line")
string = None
followDottedLineFlag= 1
rectFlag = 0
circleFlag = 0
# ser.flushInput() # 清除输入缓冲区数据
if modeFlag == 4: # 向右变道模式
whichLine = 0
if tickFlag == 0:
ticks = int(time.time())
tickFlag = 1
openFlag = 1
if int(time.time()) - ticks >= 2 and openFlag == 1:
stopFlag = stopFlag + 1
openFlag = 0
if printFlag == 0:
comPrint("15\r\n") # 向右变道指令
printFlag = 1
if ser.inWaiting():
string = ser.read(1) # 监听消息
if string == b'B': # 收到stm32的动作完毕指令
print("B")
modeFlag = 2
followReaLineFlag = 1
rectFlag = 0
circleFlag = 0
printFlag = 1
# ser.flushInput() # 清除输入缓冲区数据
if modeFlag == 5: # 向左避障模式
whichLine = 0
if tickFlag == 0:
ticks = int(time.time())
tickFlag = 1
openFlag = 1
if int(time.time()) - ticks >= 2 and openFlag == 1:
stopFlag = stopFlag + 1
openFlag = 0
comPrint("14\r\n") # 向左避障指令
if ser.inWaiting():
string = ser.read(1) # 监听消息
if string == b'C': # 收到stm32的动作完毕指令
print("left avoid")
# if findChangeLine(dst):
# print("142\r\n")
# comPrint("142\r\n")
time.sleep(2)
modeFlag = 7
followDottedLineFlag = 1
rectFlag = 0
circleFlag = 0
# ser.flushInput() # 清除输入缓冲区数据
if modeFlag == 6: # 向右避障模式
if tickFlag == 0:
ticks = int(time.time())
tickFlag = 1
openFlag = 1
if int(time.time()) - ticks >= 2 and openFlag == 1:
stopFlag = stopFlag + 1
openFlag = 0
comPrint("16\r\n") # 向右避障指令
whichLine = 0
if ser.inWaiting():
string = ser.read(1) # 监听消息
if string == b'D': # 收到stm32的动作完毕指令
string = None
print("right avoid")
if findChangeRLine(dst) == 1:
comPrint("163\r\n")
print("Turn right, STM32!")
modeFlag = 2
followReaLineFlag = 1
rectFlag = 0
circleFlag = 0
# ser.flushInput() # 清除输入缓冲区数据
# serial.flushInput() # 清除输入缓冲区数据
if modeFlag == 8: # 实线停车
comPrint("11\r\n")
print("STOP AT REAL LINE")
time.sleep(10)
if findHorizonLine(dst):
comPrint("18\r\n")
print("18")
modeFlag = 10
if modeFlag == 9: # 虚线停车
comPrint("19\r\n")
print("STOP AT DOT LINE")
time.sleep(15)
# if findHorizonLine(dst):
# comPrint("19\r\n")
# print("18")
# modeFlag = 10
modeFlag = 10
cv2.imshow('frame', frame)
if ser.inWaiting():
string = ser.read(1) # 监听消息
if string == b'E':
print("We can stop")
stopFlag = 3
print("stop flag : %d" % stopFlag)
print("mode : %d" % modeFlag)
if modeFlag == 10:
newModeFlag = int(input("please change mode:"))
if newModeFlag == 1:
print("change mode")
stopFlag = 0
modeFlag = 2
# getKeyVal()
# if GPIO.input(3):
# print("HIGH VALUE")
# GPIO.wait_for_edge(3, GPIO.RISING)
# print("HIGH!")
# if GPIO.input(5) == GPIO.HIGH:
# print("HIGH VALUE!")
if cv2.waitKey(1) == ord('q'):
cv2.destroyAllWindows()
break
因为自己比较懒，有些函数没怎么测试，反正思路在那，看着改改就好。
浙公网安备 33010602011771号