''' @Author : kazimiyuuka @Date : 2023/05/29 23:51 @LastEditors : raiot @Environment : python 3.10 ''' import serial import cv2 import numpy as np import time import math 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, val:float): tmp = 0x2f self.sendMessage(tmp, val) def setSidesway_Thrust(self , val: float): tmp = 0x30 self.sendMessage(tmp ,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(self, 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()