200 lines
5.1 KiB
Python
200 lines
5.1 KiB
Python
"""
|
||
视觉跟随圆移动控制飞机飞行。
|
||
圆形案板移动有前后，左右，上下移动
|
||
|
||
"""
|
||
import cv2
|
||
import threading
|
||
import time
|
||
import VisionCaptureApi
|
||
import PX4MavCtrlV4 as PX4MavCtrl
|
||
import sys
|
||
import numpy as np
|
||
import keyboard
|
||
import UE4CtrlAPI
|
||
ue = UE4CtrlAPI.UE4CtrlAPI()
|
||
|
||
|
||
"""
|
||
初始化飞机，起飞,创建圆形目标
|
||
"""
|
||
vis = VisionCaptureApi.VisionCaptureApi()
|
||
# ue.sendUE4Cmd(b"r.setres 720x405w", 0)
|
||
ue.sendUE4Cmd(b"t.MaxFPS 30", 0)
|
||
ue.sendUE4PosNew(3, vehicleType=809, PosE=[2, 0, -2])
|
||
vis.jsonLoad()
|
||
time.sleep(1)
|
||
ue.sendUE4ExtAct(3, ActExt=[1, 0.8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0])
|
||
isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
|
||
if not isSuss: # 如果请求取图失败，则退出
|
||
sys.exit(0)
|
||
vis.startImgCap() # 开启取图，并启用共享内存图像转发，转发到填写的目录
|
||
time.sleep(2)
|
||
|
||
mav = PX4MavCtrl.PX4MavCtrler()
|
||
mav.InitMavLoop()
|
||
time.sleep(2)
|
||
mav.initOffboard()
|
||
time.sleep(2)
|
||
mav.SendMavArm(True)
|
||
mav.SendPosNED(0, 0, -2, 0)
|
||
time.sleep(10)
|
||
detect_flag = False
|
||
|
||
"""
|
||
开辟线线程,控制圆形目标移动
|
||
"""
|
||
|
||
|
||
Circle_pose = [2, 0, -2, 0]
|
||
|
||
|
||
def MoveCircle(val):
|
||
"""
|
||
圆板目标移动没有旋转，只有平移，且始终朝向飞机
|
||
"""
|
||
global vis
|
||
global detect_flag
|
||
global CalCirle_pose
|
||
offset = 0.01 # 每次移动的距离
|
||
yaw_offset = np.deg2rad(1)
|
||
drone_pose = ue.getUE4Pos()
|
||
# if not detect_flag:
|
||
# time.sleep(0.001)
|
||
# continue
|
||
if val == "right": # 如果按键按下，这个值为1
|
||
# 向右移动
|
||
Circle_pose[1] += offset
|
||
if val == "left":
|
||
# 向左移动
|
||
Circle_pose[1] -= offset
|
||
if val == "up":
|
||
# 向上移动
|
||
Circle_pose[2] -= offset
|
||
if val == "down":
|
||
# 向下移动
|
||
Circle_pose[2] += offset
|
||
if val == "front":
|
||
# 向前移动
|
||
Circle_pose[0] += offset
|
||
if val == "back":
|
||
# 向后移动
|
||
Circle_pose[0] -= offset
|
||
ue.sendUE4PosNew(
|
||
3,
|
||
vehicleType=809,
|
||
PosE=[Circle_pose[0], Circle_pose[1], Circle_pose[2]],
|
||
AngEuler=[0, 0, Circle_pose[3]],
|
||
)
|
||
|
||
|
||
# move_circle_th = threading.Thread(target=MoveCircle)
|
||
# move_circle_th.start()
|
||
|
||
"""
|
||
开辟线程,获取图像,并控制飞机移动
|
||
"""
|
||
|
||
|
||
def CalCirle(img: np.array) -> tuple():
|
||
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
||
low_hsv = np.array([0, 43, 46])
|
||
high_hsv = np.array([10, 255, 255])
|
||
mask1 = cv2.inRange(hsv, lowerb=low_hsv, upperb=high_hsv)
|
||
|
||
lower2 = np.array([156, 43, 46])
|
||
upper2 = np.array([180, 255, 255])
|
||
mask2 = cv2.inRange(hsv, lower2, upper2)
|
||
mask = mask1 + mask2
|
||
nonzero = np.nonzero(mask)
|
||
if np.size(nonzero) == 0:
|
||
return [-1, -1], [-1, -1], [-1, -1]
|
||
row_min = np.min(nonzero[0])
|
||
row_max = np.max(nonzero[0])
|
||
col_min = np.min(nonzero[1])
|
||
col_max = np.max(nonzero[1])
|
||
mid_row = int((row_max + row_min) / 2)
|
||
mid_col = int((col_max + col_min) / 2)
|
||
|
||
cv2.line(
|
||
img,
|
||
(col_min, mid_row),
|
||
(col_max, mid_row),
|
||
(255, 255, 255),
|
||
1,
|
||
)
|
||
cv2.line(
|
||
img,
|
||
(mid_col, row_min),
|
||
(mid_col, row_max),
|
||
(255, 255, 255),
|
||
1,
|
||
)
|
||
cv2.imshow("img ", img)
|
||
cv2.waitKey(1)
|
||
# cv2.imshow("test", mask1 + mask2)
|
||
# cv2.waitKey(1)
|
||
|
||
return [col_min, col_max], [row_min, row_max], [mid_col, mid_row]
|
||
|
||
|
||
def CtrlDrone():
|
||
global vis
|
||
global mav
|
||
global detect_flag
|
||
img = None
|
||
init_dist_x = 0
|
||
init_dist_y = 0
|
||
img_cnt_x = 320
|
||
img_cnt_y = 240
|
||
tolerance_cnt = 15
|
||
tolerance_y = 15
|
||
tolerance_x = 20
|
||
kx = 0.015
|
||
ky = 0.009
|
||
kz = 0.005
|
||
while True:
|
||
if vis.hasData[0]:
|
||
img = vis.Img[0]
|
||
vis.hasData[0] = False
|
||
# cv2.imshow("img", vis.Img[0])
|
||
cv2.waitKey(1)
|
||
[x_min, x_max], [y_min, y_max], [cnt_x, cnt_y] = CalCirle(img)
|
||
if x_min == -1:
|
||
time.sleep(0.001)
|
||
continue
|
||
if not detect_flag:
|
||
detect_flag = True
|
||
dist_x = x_max - x_min
|
||
dist_y = y_max - y_min
|
||
if init_dist_x == 0:
|
||
init_dist_x = dist_x
|
||
if init_dist_y == 0:
|
||
init_dist_y = dist_y
|
||
vx = 0
|
||
vy = 0
|
||
vz = 0
|
||
yaw_rate = 0
|
||
if abs(cnt_x - img_cnt_x) > tolerance_cnt:
|
||
vy = ky * (cnt_x - img_cnt_x)
|
||
if abs(cnt_y - img_cnt_y) > tolerance_cnt:
|
||
vz = kz * (cnt_y - img_cnt_y)
|
||
if abs(dist_y - init_dist_y) > tolerance_y:
|
||
vx = kx * (init_dist_y - dist_y)
|
||
|
||
mav.SendVelFRD(vx, vy, vz, 0)
|
||
time.sleep(0.001)
|
||
|
||
|
||
ctrl_drone_th = threading.Thread(target=CtrlDrone)
|
||
ctrl_drone_th.start()
|
||
|
||
keyboard.add_hotkey("ctrl+up", MoveCircle, args=("front",))
|
||
keyboard.add_hotkey("ctrl+down", MoveCircle, args=("back",))
|
||
keyboard.add_hotkey("up", MoveCircle, args=("up",))
|
||
keyboard.add_hotkey("down", MoveCircle, args=("down",))
|
||
keyboard.add_hotkey("right", MoveCircle, args=("right",))
|
||
keyboard.add_hotkey("left", MoveCircle, args=("left",))
|
||
|
||
keyboard.wait()
|