feat: route track from kazimiyuuka
This commit is contained in:
raiots
parent
259ab2ada3
commit
6ed7a603fc
|
|
@ -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()
|
||||
Loading…
Reference in New Issue