import queue
import threading
import cv2
import serial
from time import sleep
import time
import numpy as np
ser = serial.Serial('/dev/ttyUSB0',9600) # 串口初始化,根据实际情况修改串口号和波特率
from pyzbar import pyzbar
img1122=cv2.imread("/home/ros/Desktop/ce/1122p.jpg")
img2211=cv2.imread("/home/ros/Desktop/ce/2211p.jpg")
img2112=cv2.imread("/home/ros/Desktop/ce/2112p.jpg")
img1221=cv2.imread("/home/ros/Desktop/ce/1221p.jpg")
#video.set(cv2.CAP_PROP_FOURCC,cv2.VideoWriter_fourcc('M','J','P','G'))
# #video.set(5,30)
class VideoCapture:
def __init__(self, camera_id):
# "camera_id" is a int type id or string name
self.cap = cv2.VideoCapture(camera_id)
self.q = queue.Queue(maxsize=3)
self.stop_threads = False # to gracefully close sub-thread
th = threading.Thread(target=self._reader)
th.daemon = True # 设置工作线程为后台运行
th.start()
def _reader(self):
while not self.stop_threads:
ret, frame = self.cap.read()
if not ret:
break
if not self.q.empty():
try:
self.q.get_nowait()
except queue.Empty:
pass
self.q.put(frame)
def read(self):
return self.q.get()
def terminate(self):
self.stop_threads = True
self.cap.release()
def show(img):
cv2.namedWindow("num",cv2.WINDOW_FREERATIO)
#cv2.resize(img,(100,100))
cv2.resizeWindow("num",800,600)
cv2.imshow("num", img)
#cv2.setWindowProperty("num", cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_NORMAL)
#cv2.waitKey(1)
def img_contrast_bright(img,a,b,g):
h,w,c=img.shape
blank=np.zeros([h,w,c],img.dtype)
dst=cv2.addWeighted(img,a,blank,b,g)
return dst
def show_data(myData):
if myData == "1221":
show(img1221)
if myData == "2112":
show(img2112)
if myData == "1122":
show(img1122)
if myData == "2211":
show(img2211)
def read_and_mersure(img):
myData=""
w=0.0
key=""
result = pyzbar.decode(img)
#print(result)
for barCode in result:#在序列中遍历特定参数
myData = barCode.data.decode('utf-8')#解码,去掉b
w = barCode.rect.width
#print(result[0][2][3])
W = 8 # 输入实际二维码的宽度
f = 610 # 控制相机与二维码的距离,进行标定
if w!= 0.0:
l = W*f/w
print("l=",l)
if l<55:
key="9999"
return key,myData
def judge_fin(img): #获取外接矩形,返回其外接矩形面积大小
ret,binary = cv2.threshold(img, 127, 255, cv2.THRESH_BINARY)
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
w=h=res=0
for i in range(0,len(contours)):
x, y, w, h = cv2.boundingRect(contours[i])
if w >= 5 and h >= 5:
res=1
return res,w*h #第一个为是否检测到,第二个为面积
def test_read(img): #此函数用于提取图片中二维码信息
myData=""
result = pyzbar.decode(img)
#print(result)
for barCode in result:#在序列中遍历特定参数
myData = barCode.data.decode('utf-8')#解码,去掉b
#print(myData)
return myData
def test_mersure_l(img):
w=0.0
key=""
result = pyzbar.decode(img)
#for barCode in result:#在序列中遍历特定参数
result.barCode.rect.width
W = 16.9 # 输入实际二维码的宽度
f = 610 # 控制相机与二维码的距离,进行标定
if w!= 0.0:
l = W*f/w
print("l=",l)
if l<150:
key="9999"
return key
"""
part two
"""
def reded(img): #绿色mask
img = img_contrast_bright(img,1,1,100)
img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# image_g = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
img_only_green = cv2.inRange(img_hsv, (35, 43, 46), (77, 255, 255)) # 低于lower、高于upper为0,中间为255
#cv2.imshow("bin_red_is_white",img_only_red)
return img_only_green
def pretreat(img): #清除噪点
img = cv2.erode(img, (3, 3), iterations=5) # 腐蚀
img = cv2.dilate(img, (3, 3), iterations=5) # 膨胀
#ret, binary = cv2.threshold(img, 0, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU)
return img
def mask(img,x1,y1,w1,h1): #创建roi区域
Mask = np.zeros((img.shape[0], img.shape[1]), dtype=np.uint8) # 返回与图像 img1 尺寸相同的全零数组
xmin, ymin, w, h = x1, y1, w1, h1 # 矩形裁剪区域 (ymin:ymin+h, xmin:xmin+w) 的位置参数
Mask[ymin:ymin + h, xmin:xmin + w] = 255 # 掩模图像,ROI 为白色,其它区域为黑色
#摄像头范围为多大来着
#print(img.shape, Mask.shape)
imgAddMask = cv2.add(img, np.zeros(np.shape(img), dtype=np.uint8), mask=Mask) # 提取 ROI
return imgAddMask
'''
def judge(img):
ret,binary = cv2.threshold(img, 127, 255, cv2.THRESH_BINARY)
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours)==0: #so samrt
return 0
for i in range(0,len(contours)):
w=h=0
x, y, w, h = cv2.boundingRect(contours[i])
if w>=5 and h>=5 :
return 1
if w<5 or h<5 :
return 0
'''
def judge(img):
ret,binary = cv2.threshold(img, 127, 255, cv2.THRESH_BINARY)
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
result=0
for i in range(0,len(contours)):
x, y, w, h = cv2.boundingRect(contours[i])
if w>=5 and h>=5 :
result=1
return result
def read2_mid_x(img): #此函数用于提取图片中二维码信息,slowwww
mid_x=0
result = pyzbar.decode(img)
#a=time.time()
for barCode in result:#在序列中遍历特定参数
print(barCode.polygon, "\n\n") # 轮廓信息
mid_x=(barCode.polygon[2][0]+barCode.polygon[0][0])/2
print(mid_x)
#print(time.time()-a)
return mid_x
def judge_mid(mid_x,mid): #第一个参数是二维码中心坐标,第二个是视频中心坐标,fast
if (mid_x-mid<120 and mid-mid_x<120):
return 2226
if __name__ == "__main__":
video = VideoCapture(0)
i=j=k=f=ff=kk=0
right="2277"
right_1="3366"
left="7722"
left_1="6633"
nothing="2222"
nothing_1="3333"
bothside="7777"
bothside_1="6666"
mid="2226"
left_side="5555"
right_side="8888"
left_first="5588"
right_first="8855"
over="4399"
over_over="9934"
zero="0000"
while open:
frame = video.read()
frame_c=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
if frame is None:
break
if (k==0):
key,myData=read_and_mersure(frame_c)
#cv2.putText(frame, myData, (100, 200), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 255, 0), 3)
#ser.write(myData.encode()) # 发送数字到串口
if myData != "":
i+=1
if i<=4 :
ser.write(myData.encode()) # 发送数字到串口
print (myData)
if key == "9999":
sleep(1.45)
k+=1
#print (key)
cv2.imshow('result', frame)
if ( k == 1):
cv2.destroyAllWindows()
if k>=1 and k<=11:
ser.write(key.encode()) # 发送数字到串口
k+=1
if k>=12 and k<=22:
ser.write(zero.encode()) # 发送数字到串口
k+=1
if (k>=23 and kk==0):
show_data(myData)
k+=1 #30Hz
if k==85: #2s
img_1 = img_contrast_bright(frame,1,1,100)
cv2.imshow("11111",img_1)
img_green=reded(frame) #绿色为白
img_green=pretreat(img_green) #清除噪点
masked_right= mask(img_green, 320, 10, 320, 460)
masked_left = mask(img_green,10,10,320,460)
#contour_l = cv2.findContours(masked_left, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[0]
#contour_r = cv2.findContours(masked_right, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[0]
#cv2.drawContours(frame, contour_l, -1, (0, 255, 0), 2)
#cv2.drawContours(frame, contour_r, -1, (0, 255, 0), 2)
#cv2.imshow('result_left', masked_left)
#cv2.imshow('result_right', frame)
#cv2.imshow('result2', frame)
result_left = judge (masked_left)
result_right = judge (masked_right)
k+=1
if result_right == 1 and result_left ==0:
print("右侧检测成功")
ser.write(right.encode())
if result_left == 1 and result_right ==0:
print("左侧检测成功")
ser.write(left.encode())
if result_right == 0 and result_left ==0 :
print("None")
ser.write(nothing.encode())
if result_right == 1 and result_left ==1 :
print("Bothside")
ser.write(bothside.encode())
if k==800 :
img_2 = img_contrast_bright(frame,1,1,100)
cv2.imshow("1115767",img_2)
img_green=reded(frame) #绿色为白
img_green=pretreat(img_green) #清除噪点
masked_right= mask(img_green, 320, 10, 320, 460)
masked_left = mask(img_green,10,10,320,460)
#contour_l = cv2.findContours(masked_left, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[0]
#contour_r = cv2.findContours(masked_right, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[0]
#cv2.drawContours(frame, contour_l, -1, (0, 255, 0), 2)
#cv2.drawContours(frame, contour_r, -1, (0, 255, 0), 2)
#cv2.imshow('result_left', masked_left)
#cv2.imshow('result_right', frame)
#cv2.imshow('result2', frame)
result_left_1 = judge (masked_left)
result_right_1 = judge(masked_right)
k+=1
if result_right_1 == 1 and result_left_1 ==0:
print("右侧检测成功")
ser.write(right_1.encode())
if result_left_1 == 1 and result_right_1 ==0:
print("左侧检测成功")
ser.write(left_1.encode())
if result_right_1 == 0 and result_left_1 ==0 :
print("None")
ser.write(nothing_1.encode())
if k>=890 and j==0:
mid_x=read2_mid_x(frame)
result_mid=judge_mid(mid_x, 320)
print(mid_x)
cv2.imshow('result2', frame)
if result_mid == 2226 and j==0:
ser.write(mid.encode())
cv2.imshow("mid",frame)
j+=1
cv2.destroyWindow('result2')
if(j>=1 and j<=174):
j+=1
if (kk==0 and j==175):
show_data(myData)
img_3 = img_contrast_bright(frame,1,1,100)
cv2.imshow("fin",img_3)
img_green=reded(frame) #绿色为白
img_green=pretreat(img_green) #清除噪点
kk+=1
masked_left= mask(img_green,10,10,320,460)
masked_right = mask(img_green, 320, 10, 320, 460)
#以下语句用于判断roi区域
#masked_right = mask(image_gray, 10, 10, 320, 460)
#masked_left = mask(image_gray, 320, 10, 320, 460)
res_left,s_left = judge_fin(masked_left)
res_right,s_right = judge_fin(masked_right)
if(res_left==0 and res_right==0):
print("nothing?")
if(res_left==0 and res_right==1):
print("右边2个")
fin=8888
ser.write(right_side.encode())
if (res_right == 0 and res_left == 1):
print("左边2个")
fin=5555
ser.write(left_side.encode())
if s_right >s_left and res_right==1 and res_left==1:
print("右前左后")
fin=8855
ser.write(right_first.encode())
if s_right < s_left and res_right == 1 and res_left == 1:
print("左前右后")
fin=5588
ser.write(left_first.encode())
if s_right == s_left and res_right == 1 and res_left == 1:
print("等死")
if (kk==1 or kk==2 or kk==3 ):
show_data(myData)
cv2.imshow("111",frame)
img_green = reded(frame) # 绿色为白
img_green = pretreat(img_green) # 清除噪点
masked_left = mask(img_green, 10, 10, 320, 460)
masked_right = mask(img_green, 320, 10, 320, 460)
# 以下语句用于判断roi区域
# masked_right = mask(image_gray, 10, 10, 320, 460)
# masked_left = mask(image_gray, 320, 10, 320, 460)
res_left = judge(masked_left)
res_right = judge(masked_right)
if(res_left==0 or res_right==0 ) and kk==1:
kk+=1
f+=1
t1=time.time()
if(f>=1 and f<=50):
f+=1
if(f>=50 and f<=53):
print(time.time()-t1)
ser.write(over.encode())
print("经过障碍物了_first")
f+=1
# if(f>=56 and f<=60) :
# ser.write(zero.encode())
# print("0_first")
# f+=1
#
# if(res_left==0 and res_right==0 and k==2):
# ff+=1
# t2=time.time()
# k+=1
#
# if(ff>=1 and ff<=65):
# ff+=1
#
#
# if(ff>=65 and ff<=68):
# print(time.time()-t2)
# ser.write(over_over.encode())
# print("经过障碍物了_second")
# ff+=1
#
if(f==53):
cv2.destroyWindow('111')
if cv2.waitKey(1) & 0xFF == 27:
break
#video.release()