536 lines
17 KiB
Python
536 lines
17 KiB
Python
'''
|
||
@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() |