From 6ed7a603fcd3faf9b4e85cfb299ab7a681f2a783 Mon Sep 17 00:00:00 2001 From: raiots Date: Mon, 29 May 2023 23:52:08 +0800 Subject: [PATCH] feat: route track from kazimiyuuka --- 单摄像头.py | 535 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 535 insertions(+) create mode 100644 单摄像头.py diff --git a/单摄像头.py b/单摄像头.py new file mode 100644 index 0000000..6eabfce --- /dev/null +++ b/单摄像头.py @@ -0,0 +1,535 @@ +''' +@Author : kazimiyuuka +@Date : 2023/05/29 23:51 +@LastEditors : raiot +''' + + +import serial +import cv2 +import numpy as np +import time + +import serial + +class Ship: + # @parameter badudrate 波特率 + def __init__(self): + self.connect = serial.Serial("/dev/ttyAMA0",baudrate=9600,timeout=0.5) + + self.Target_Pitch_Angle = 0 + self.Target_Roll_Angle = 0 + self.Target_Yaw_Angle = 0 + self.Target_Pitch_Palstance = 0 + self.Target_Roll_Palstance = 0 + self.Target_Yaw_Palstance = 0 + self.Target_X = 0 + self.Target_Y = 0 + self.Target_Depth = 0 + self.Target_Height = 0 + self.Target_X_Speed = 0 + self.Target_Y_Speed = 0 + self.Target_Z_Speed = 0 + self.Target_X_Acc = 0 + self.Target_Y_Acc = 0 + self.LED0 = False + self.LED1 = False + + def sendMessage(self,tmp:int , val:float): + end = 255 + end = end.to_bytes(1,'big') + id = tmp.to_bytes(1, 'big') + try: + self.connect.write(id) + self.connect.write(str(float()).encode('ascii')) + self.connect.write(str(' ').encode('ascii')) + self.connect.write(end) + except: + return + + def setSlopeAndInter(self , slope : float , val : float): + tmp = 0x02 + end = 0xff + id = tmp.to_bytes(1, 'big') + mark = end.to_bytes(1, 'big') + self.connect.write(id) + self.connect.write(str(slope).encode('ascii')) + self.connect.write(str(' ').encode('ascii')) + self.connect.write(str(val).encode('ascii')) + self.connect.write(str(' ').encode('ascii')) + self.connect.write(end) + + def LED0ON(self): + if self.LED0 == False: + self.LED0 = True + tmp = 0x10 + self.sendMessage(tmp , 0.0) + + def LED0OFF(self): + if self.LED0 == True: + self.LED0 = False + tmp = 0x0f + self.sendMessage(tmp , 0.0) + + def LED1OFF(self): + if self.LED1 == True: + self.LED1 = False + tmp = 0x11 + self.endMessage(tmp , 0.0) + + def LED1ON(self): + if self.LED1 == False: + self.LED1 = False + tmp = 0x12 + self.sendMessage(tmp, 0.0) + + def setTarget_Depth(self , val : float): + tmp = 0x08 + self.Target_Depth = val + self.sendMessage(tmp, val) + + def setTarget_Yaw_Angle(self, val : float): + tmp = 0x02 + self.Target_Yaw_Angle = val + self.sendMessage(tmp, val) + + def setTarget_X_Speed(self , val : float): + tmp = 0x0A + self.Target_X_Speed = val + self.sendMessage(tmp, val) + + def setTarget_Y_Speed(self , val : float): + tmp = 0x0B + self.Target_Y_Speed = val + self.sendMessage(tmp, val) + + + def sendAlpha(self , ch : int): + tmp = 0x13 + ch + self.sendMessage(tmp, 0.0) + + def setFinal(self): + tmp = 0x2d + self.sendMessage(tmp, 0.0) + + def setForward_Thrust(self): + tmp = 0x2f + self.sendMessage(tmp, val) + + def setSidesway_Thrust(sekf , val : float): + tmp = 0x30 + self.sendMessage(tep ,val) + + def setYaw_Rotate_Torque(self , val : float): + tmp = 0x32 + self.sendMessage(tmp, val) + + +class SVM: + def __init__(self , path : str): + self.model = cv2.ml.SVM_load(path) + + def predicted(img): + _ , res = self.model.predicted(img) + return res + +ship = Ship() + +''' + 定义状态1是从开始点到识别点1 + 定义状态2是导引线消失但是未识别到识别点1 + 定义状态3是从导引线开始寻找识别点4中 + 定义状态4是根据识别点4开始进行巡线 + 定义状态5 +''' +state = 1 +alphaSvm = SVM("./alpha.mat") +stateSvm = SVM("./state.mat") + +roateForce = 0 + +# capFront = cv2.VideoCapture("/dev/people_video") +capFront = cv2.VideoCapture(0) +capButton = cv2.VideoCapture(1) + +# @parameter 待检测图片 +def line_detect(image): + # 将图片转换为HSV + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + # 设置阈值 + lowera = np.array([0, 0, 0]) + uppera = np.array([180, 255, 46]) + mask1 = cv2.inRange(hsv, lowera, uppera) + kernel = np.ones((3, 3), np.uint8) + + # 对得到的图像进行形态学操作（闭运算和开运算） + mask = cv2.morphologyEx(mask1, cv2.MORPH_CLOSE, kernel) #闭运算：表示先进行膨胀操作，再进行腐蚀操作 + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) #开运算：表示的是先进行腐蚀，再进行膨胀操作 + + # 绘制轮廓 + edges = cv2.Canny(mask, 50, 150, apertureSize=3) + # 显示图片 + # cv2.imshow("edges", edges) + # 检测白线 这里是设置检测直线的条件，可以去读一读HoughLinesP()函数，然后根据自己的要求设置检测条件 + lines = cv2.HoughLinesP(edges, 1, np.pi / 180, 40,minLineLength=10,maxLineGap=10) + slope = -999 + start = 0 + interx = 0 + intery = 0 + end = 0 + for line in lines: + x1,y1,x2,y2 = line[0] #两点确定一条直线，这里就是通过遍历得到的两个点的数据 （x1,y1）(x2,y2) + # 转换为浮点数，计算斜率 + x1 = float(x1) + x2 = float(x2) + y1 = float(y1) + y2 = float(y2) + if y1 < y2: + tmp = y1 + y1 = y2 + y2 = tmp + tmp = x1 + x1 = x2 + x2 = tmp + + if x2 - x1 == 0: + # print "直线是竖直的" + result=90 + elif y2 - y1 == 0 : + # print "直线是水平的" + result=0 + else: + # 计算斜率 + k = -(y2 - y1) / (x2 - x1) + # 求反正切，再将得到的弧度转换为度 + result = np.arctan(k) * 57.29577 + print ("直线倾斜角度为：" + str(result) + "度") + + if slope == -999 or y1 < start or (y1 == start and y2 < end): + slope = result + if slope < 0 : + slope = slope + 180 + start = y1 + end = y2 + interx = ((x1 + x2) / 2) - 120 + intery = ((y1 + y2) / 2) - 120 + + return (slope , interx , -intery) + +def diffColor(img): + # 检测红色 + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + kernel = np.ones((3, 3), np.uint8) + + # 设置阈值 + lowera = np.array([0, 43, 46]) + uppera = np.array([10, 255, 255]) + mask1 = cv2.inRange(hsv, lowera, uppera) + + + mask = cv2.morphologyEx(mask1, cv2.MORPH_CLOSE, kernel) #闭运算：表示先进行膨胀操作，再进行腐蚀操作 + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) #开运算：表示的是先进行腐蚀，再进行膨胀操作 + + cv2.imshow("test" , mask) + # 检测白点数 + count = 0 + pos = 0 + for i in range(mask.shape[0]): + for j in range(mask.shape[1]): + if mask[i,j] != 0: + count = count + 1 + pos = pos + j + + if count >= 20: + return (1,float(pos) / float(count)) + + # 检测绿色 + # 设置阈值 + lowera = np.array([35, 43, 46]) + uppera = np.array([77, 255, 255]) + mask1 = cv2.inRange(hsv, lowera, uppera) + + mask = cv2.morphologyEx(mask1, cv2.MORPH_CLOSE, kernel) #闭运算：表示先进行膨胀操作，再进行腐蚀操作 + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) #开运算：表示的是先进行腐蚀，再进行膨胀操作 + + # 检测白点数 + count = 0 + pos = 0 + for i in range(mask.shape[0]): + for j in range(mask.shape[1]): + if mask[i,j] != 0: + count = count + 1 + pos = pos + j + + if count >= 20: + return (2,float(pos) / float(count)) + + # 检测蓝色 + lowera = np.array([115, 250, 250]) + uppera = np.array([125, 255, 255]) + mask1 = cv2.inRange(hsv, lowera, uppera) + + mask = cv2.morphologyEx(mask1, cv2.MORPH_CLOSE, kernel) #闭运算：表示先进行膨胀操作，再进行腐蚀操作 + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) #开运算：表示的是先进行腐蚀，再进行膨胀操作 + + # 检测白点数 + count = 0 + pos = 0 + for i in range(mask.shape[0]): + for j in range(mask.shape[1]): + if mask[i,j] != 0: + count = count + 1 + pos = pos + j + + if count >= 20: + return (3,float(pos) / float(count)) + + return (0,0) + +def findEllipse(img): + imgray=cv2.Canny(img,50,100,3)#Canny边缘检测，参数可更改 + ret,thresh = cv2.threshold(imgray,127,255,cv2.THRESH_BINARY) + contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)#contours为轮廓集，可以计算轮廓的长度、面积等 + for cnt in contours: + if len(cnt)>10: + S1=cv2.contourArea(cnt) + ell=cv2.fitEllipse(cnt) + S2 =math.pi*ell[1][0]*ell[1][1] + if (S1/S2)>0.2 :#面积比例，可以更改，根据数据集。。。 + # img = cv2.ellipse(img, ell, (0, 255, 0), 2) + return ell[0] + return (-1,-1) + +def getImage(id : int): + match id: + case 1 : + while capFront.isOpened() == False: + capFront = cv2.VideoCapture(0) + ret,img = capFront.read() + while ret == False: + ret,img = capFront.read() + img = cv2.resize(img , (240,240)) + return img + case 2: + while capButton.isOpened() == False: + capButton = cv2.VideoCapture(1) + ret,img = capButton.read() + while ret == False: + ret,img = capButton.read() + img = cv2.resize(img , (240,240)) + return img + while capFront.isOpened() == False: + capFront = cv2.VideoCapture(0) + ret,img = capFront.read() + while ret == False: + ret,img = capFront.read() + img = cv2.resize(img , (240,240)) + return img + +def lineThrust(degree : float): + if degree >= 89.0 and degree <= 91.0: + return # 无须分配横向推力 + elif degree <= 90: + # 偏左 向右分配推力 + # 暂定直接分配角度 + ship.setSidesway_Thrust(degree - 90.0) + else: + # 偏右 向左分配推力 + ship.setSidesway_Thrust(degree - 90.0) + +ship.setForward_Thrust(10.0) + +# 控制代码开始 +color = 0 +while True: + match state: + case 1 : + # 从开始到字母识别点 + # 计算斜率 + img = getImage(0) + res = line_detect(img) + if res[0] == -999: + # 无导引 + pass + else: + # 传递斜率和截距 + ship.setSlopeAndInter(res[0], res[1]) # 期望角度为90度,截距为0 + + res = stateSvm.predicted(img) # 是否有字母存在的svm + if res != 0: + ans = alphaSvm(img) + for ch in ans: + ship.sendAlpha(ch) + time.sleep(0.1) + cv2.imsave("./point1.jpg") + state = 2 + case 2: + # 向左转向20~30度 + + + # 利用左侧摄像头开始巡线 + # img = getImage(2) + # res = line_detect(img) + # if res[0] != 999: + # if res[0] <= 90: + # ship.setSlopeAndInter(res[0] - 90, res[2]) # 期望角度为0,截距为0 + # else: + # ship.setSlopeAndInter(res[0] - 90, res[2]) # 期望角度为0,截距为0 + + # img = getImage(1) + + res = diffColor(img) + if res[0] != 0: + state = 4 + case 4: + # # 竖直推力为0,水平推力为20 + # ship.setForward_Thrust(0) + # ship.setSidesway_Thrust(10) + + # # 根据方向调整 + img = getImage(0) + res = diffColor(img) + + if res[0] == 0: + # 亮灯 + ship.LED0ON() + time.sleep(0.5) + ship.LED0OFF() + state = 5 + else: + color = res[1] + + # 分配水平推力调整 , 或者进入时旋转90度使用巡线的调整 + + case 5: + # # 若之前旋转90度,现在需要旋转回来 + + + # # 右侧巡线 + # img = getImage(2) + # res = line_detect(img) + # if res[0] != -999: + # if res[0] < 90: + # ship.setSlopeAndInter(res[0] - 90, res[2] + 60) + # else: + # ship.setSlopeAndInter(res[0] - 90, res[2] + 60) + # else: + # ship.setSlopeAndInter(0, 190) + + # if res[2] >= 0: + # # 恢复前摄像头巡线 + # state = 6 + + img = getImage(2) + res = line_detect(img) + if res[0] != -999: + state = 6 + + case 6: + img = getImage(0) + res = line_detect(img) + + if res[0] != -999: + ship.setSlopeAndInter(res[0], res[1]) # 期望角度为90度,截距为0 + + res = diffColor(img) + if res[0] != 0: + ship.LED1ON() + time.sleep(0.2) + img = getImage(0) + cv2.imwrite("/kazimi/point1.jpg",img) + ship.LED1OFF() + state = 7 + case 7: + img = getImage(0) + res = line_detect(img) + + if res[0] != -999: + ship.setSlopeAndInter(res[0], res[1]) # 期望角度为90度,截距为0 + + res = diffColor(img) + if res[0] != 0: + ship.LED1ON() + time.sleep(0.2) + img = getImage(0) + cv2.imwrite("/kazimi/point2.jpg",img) + ship.LED1OFF() + state = 8 + case 8: + img = getImage(0) + res = line_detect(img) + + if res[0] != -999: + ship.setSlopeAndInter(res[0], res[1]) # 期望角度为90度,截距为0 + + res = diffColor(img) + if res[0] != 0: + ship.LED1ON() + time.sleep(0.2) + img = getImage(0) + cv2.imwrite("/kazimi/point3.jpg",img) + ship.LED1OFF() + state = 9 + case 9: + img = getImage(0) + res = line_detect(img) + + if res[0] != -999: + ship.setSlopeAndInter(res[0], res[1]) # 期望角度为90度,截距为0 + + res = diffColor(img) + if res[0] != 0: + color = res[0] + ship.LED1ON() + time.sleep(0.2) + img = getImage(0) + cv2.imwrite("/kazimi/point5.jpg",img) + ship.LED1OFF() + state = 10 + case 10: + # 先旋转一个小角度避免寻到第一段导引 + + + # 巡线直到无线可寻 + res = line_detect(img) + while res[0] != 999: + img = getImage(img) + ship.setSlopeAndInter(res[0], res[1]) # 期望角度为90度,截距为0 + time.sleep(0.03) + res = line_detect(img) + + state = 11 + case 11: + res = diffColor(img) + if res[0] != 0: + color = res[0] + ship.LED1ON() + time.sleep(0.2) + img = getImage(0) + cv2.imwrite("/kazimi/point6.jpg",img) + ship.LED1OFF() + state = 12 + case 12: + ship.setFinal() + state = 13 + case 13: + img = getImage(0) + res = line_detect(img) + if res[0] != -999: + state = 14 + case 14: + img = getImage(0) + res = line_detect(img) + + if res[0] != -999: + ship.setSlopeAndInter(res[0], res[1]) # 期望角度为90度,截距为0 + + res = diffColor(img) + if res[0] != 0: + color = res[0] + ship.LED1ON() + time.sleep(0.2) + img = getImage(0) + cv2.imwrite("/kazimi/point5.jpg",img) + ship.LED1OFF() + quit() \ No newline at end of file