105 lines
3.0 KiB
Python
105 lines
3.0 KiB
Python
|
# import required libraries
|
|||
|
|
import cv2
|
|||
|
|
import time
|
|||
|
|
import math
|
|||
|
|
import sys
|
|||
|
|
|
|||
|
|
# import RflySim APIs
|
|||
|
|
import PX4MavCtrlV4 as PX4MavCtrl
|
|||
|
|
|
|||
|
|
# import RflySim APIs
|
|||
|
|
import PX4MavCtrlV4 as PX4MavCtrl
|
|||
|
|
|
|||
|
|
import VisionCaptureApi
|
|||
|
|
import UE4CtrlAPI
|
|||
|
|
ue = UE4CtrlAPI.UE4CtrlAPI()
|
|||
|
|
|
|||
|
|
vis = VisionCaptureApi.VisionCaptureApi()
|
|||
|
|
|
|||
|
|
# VisionCaptureApi 中的配置函数
|
|||
|
|
vis.jsonLoad() # 加载Config.json中的传感器配置文件
|
|||
|
|
|
|||
|
|
isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
|
|||
|
|
if not isSuss: # 如果请求取图失败，则退出
|
|||
|
|
sys.exit(0)
|
|||
|
|
vis.startImgCap(True) # 开启取图，并启用共享内存图像转发，转发到填写的目录
|
|||
|
|
|
|||
|
|
# Send command to UE4 Window 1 to change resolution
|
|||
|
|
ue.sendUE4Cmd('r.setres 720x405w',0) # 设置UE4窗口分辨率，注意本窗口仅限于显示，取图分辨率在json中配置，本窗口设置越小，资源需求越少。
|
|||
|
|
ue.sendUE4Cmd('t.MaxFPS 30',0) # 设置UE4最大刷新频率，同时也是取图频率
|
|||
|
|
time.sleep(2)
|
|||
|
|
|
|||
|
|
# Create MAVLink control API instance
|
|||
|
|
mav = PX4MavCtrl.PX4MavCtrler(1)
|
|||
|
|
mav2 = PX4MavCtrl.PX4MavCtrler(2)
|
|||
|
|
# Init MAVLink data receiving loop
|
|||
|
|
mav.InitMavLoop()
|
|||
|
|
mav2.InitMavLoop()
|
|||
|
|
|
|||
|
|
|
|||
|
|
# send vehicle position command to create a man, with copterID=100
|
|||
|
|
# the man is located before the drone, and rotated to 180 degree (face the drone)
|
|||
|
|
ue.sendUE4Pos(100,30,0,[1,0,-8.086],[0,0,math.pi])
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
# send command to change object with copterID=100 (the man just created) to a walking style
|
|||
|
|
ue.sendUE4Cmd('RflyChange3DModel 100 16')
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
|
|||
|
|
# send command to the first RflySim3D window, to switch to vehicle 1
|
|||
|
|
ue.sendUE4Cmd('RflyChangeViewKeyCmd B 1',0)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
# change the first window to size 720x405
|
|||
|
|
ue.sendUE4Cmd('r.setres 720x405w',0)
|
|||
|
|
time.sleep(2)
|
|||
|
|
|
|||
|
|
|
|||
|
|
#send command to the second RflySim3D window to configure camera's position
|
|||
|
|
ue.sendUE4Cmd('RflyChangeViewKeyCmd B 2',1)
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
ue.sendUE4Cmd('r.setres 720x405w',1)
|
|||
|
|
time.sleep(2)
|
|||
|
|
|
|||
|
|
print("5s, Arm the drone")
|
|||
|
|
mav.initOffboard()
|
|||
|
|
mav2.initOffboard()
|
|||
|
|
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
mav.SendMavArm(True) # Arm the drone
|
|||
|
|
mav2.SendMavArm(True) # Arm the drone
|
|||
|
|
print("Arm the drone!, and fly to NED 0,0,-5")
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
|
|||
|
|
mav.SendPosNED(0, 0, -1.7, 0) # Fly to target position 0,0，-1.5
|
|||
|
|
mav2.SendPosNED(0, 0, -1.7, 0) # Fly to target position 0,0，-1.5
|
|||
|
|
|
|||
|
|
# Process the image in the following timer
|
|||
|
|
startTime = time.time()
|
|||
|
|
lastTime = time.time()
|
|||
|
|
timeInterval = 1/30.0 # time interval of the timer
|
|||
|
|
num=0
|
|||
|
|
lastClock=time.time()
|
|||
|
|
while True:
|
|||
|
|
# The above code will be executed 30Hz (0.033333s)
|
|||
|
|
lastTime = lastTime + timeInterval
|
|||
|
|
sleepTime = lastTime - time.time()
|
|||
|
|
if sleepTime > 0:
|
|||
|
|
time.sleep(sleepTime)
|
|||
|
|
else:
|
|||
|
|
lastTime = time.time()
|
|||
|
|
|
|||
|
|
for i in range(4):
|
|||
|
|
if vis.hasData[i]:
|
|||
|
|
img = vis.Img[i]
|
|||
|
|
cv2.imshow("pic"+str(i),img)
|
|||
|
|
cv2.waitKey(1)
|
|||
|
|
# add target tracking algorithm here with mav.SendVelNED or SendVelFRD API
|
|||
|
|
|
|||
|
|
num=num+1
|
|||
|
|
if num%100==0:
|
|||
|
|
tiem=time.time()
|
|||
|
|
print(100/(tiem-lastClock))
|
|||
|
|
lastClock=tiem
|
|||
|
|
|
|||
|
|
|