'''
@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("/shot/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("/shot/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("/shot/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("/shot/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("/shot/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("/shot/point5.jpg",img)
ship.LED1OFF()
quit()