90 lines
3.4 KiB
Python
90 lines
3.4 KiB
Python
|
import time
|
|||
|
|
import math
|
|||
|
|
import sys
|
|||
|
|
import VisionCaptureApi
|
|||
|
|
import PX4MavCtrlV4 as PX4MavCtrl
|
|||
|
|
import cv2
|
|||
|
|
import UE4CtrlAPI
|
|||
|
|
ue = UE4CtrlAPI.UE4CtrlAPI()
|
|||
|
|
|
|||
|
|
#Create a new MAVLink communication instance, UDP sending port (CopterSim’s receving port) is 20100
|
|||
|
|
mav = PX4MavCtrl.PX4MavCtrler(1)
|
|||
|
|
|
|||
|
|
# The IP should be specified by the other computer
|
|||
|
|
vis = VisionCaptureApi.VisionCaptureApi()
|
|||
|
|
|
|||
|
|
# Send command to UE4 Window 1 to change resolution
|
|||
|
|
ue.sendUE4Cmd('r.setres 1280x720',0) # 设置UE4窗口分辨率，注意本窗口仅限于显示，取图分辨率在json中配置，本窗口设置越小，资源需求越少。
|
|||
|
|
ue.sendUE4Cmd('t.MaxFPS 30',0) # 设置UE4最大刷新频率，同时也是取图频率
|
|||
|
|
time.sleep(2)
|
|||
|
|
|
|||
|
|
# VisionCaptureApi 中的配置函数
|
|||
|
|
vis.jsonLoad() # 加载Config.json中的传感器配置文件
|
|||
|
|
|
|||
|
|
# vis.RemotSendIP = '192.168.3.80'
|
|||
|
|
# 注意，手动修改RemotSendIP的值，可以将图片发送到本地址
|
|||
|
|
# 如果不修改这个值，那么发送的IP地址为json文件中SendProtocol[1:4]定义的IP
|
|||
|
|
# 图片的发送端口，为json中SendProtocol[5]定义好的。
|
|||
|
|
|
|||
|
|
isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
|
|||
|
|
if not isSuss: # 如果请求取图失败，则退出
|
|||
|
|
sys.exit(0)
|
|||
|
|
vis.startImgCap() # 开启取图，并启用共享内存图像转发，转发到填写的目录
|
|||
|
|
|
|||
|
|
|
|||
|
|
#mav.InitMavLoop(UDPMode), where UDPMode=0,1,2,3,4
|
|||
|
|
# Use MAVLink_Full Mode to control PX4
|
|||
|
|
# In this mode, this script will send MAVLinkOffboard message to PX4 directly
|
|||
|
|
# and receive MAVLink data from PX4
|
|||
|
|
# Obviously, MAVLink_Full mode is slower than UDP mode, but the functions and data are more comprehensive
|
|||
|
|
mav.InitMavLoop() # the same as mav.InitMavLoop() in other PythonVision demos
|
|||
|
|
|
|||
|
|
time.sleep(1)
|
|||
|
|
print('Start Offboard Send!')
|
|||
|
|
mav.initOffboard()
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# Check if the PX4'EKF has correctlly initialized, which is necessary for offboard control
|
|||
|
|
if not mav.isPX4Ekf3DFixed:
|
|||
|
|
print('CopterSim/PX4 still not 3DFxied, please wait and try again.')
|
|||
|
|
sys.exit(0)
|
|||
|
|
else:
|
|||
|
|
print('CopterSim/PX4 3D Fixed, ready to fly.')
|
|||
|
|
|
|||
|
|
mav.SendMavArm(True)
|
|||
|
|
print('Fly to pos 0, 0, -1!')
|
|||
|
|
mav.SendPosNED(0, 0, -1, 0)
|
|||
|
|
|
|||
|
|
# 下面的程序非必需，仅用于观察图像用，在电脑性能不足时，请删除
|
|||
|
|
lastTime = time.time()
|
|||
|
|
num=0
|
|||
|
|
lastClock=time.time()
|
|||
|
|
while True:
|
|||
|
|
lastTime = lastTime + 1/30.0
|
|||
|
|
sleepTime = lastTime - time.time()
|
|||
|
|
if sleepTime > 0:
|
|||
|
|
time.sleep(sleepTime)
|
|||
|
|
else:
|
|||
|
|
lastTime = time.time()
|
|||
|
|
|
|||
|
|
for i in range(len(vis.hasData)):
|
|||
|
|
if vis.hasData[i]:
|
|||
|
|
# Process your image here
|
|||
|
|
cv2.imshow('Img'+str(i),vis.Img[i])
|
|||
|
|
cv2.waitKey(1)
|
|||
|
|
|
|||
|
|
if i==0: # 更新0号相机的参数
|
|||
|
|
# 以下代码用于实时更新相机参数（位置、焦距、角度、装载飞机和形式）
|
|||
|
|
vs = vis.VisSensor[0] #获取第0号相机基本参数
|
|||
|
|
# 修改其中的可变部分，只修改需要改变的部分即可
|
|||
|
|
vs.TargetCopter=1 #修改视角绑定的飞机ID
|
|||
|
|
vs.TargetMountType=0 # 修改视角绑定类型，固连飞机还是地面
|
|||
|
|
vs.CameraFOV=90 # 修改视角的视场角（焦距），可以模拟对焦相机
|
|||
|
|
vs.SensorPosXYZ=[0.3,-0.15,0] # 修改相机的位置，可以调整相机初始位置
|
|||
|
|
vs.SensorAngEular=[0,0,0] # 修改相机的姿态，可以模拟云台转动
|
|||
|
|
vis.sendUpdateUEImage(vs) # 发送更新数据
|
|||
|
|
|
|||
|
|
|
|||
|
|
#mav.endOffboard()
|
|||
|
|
#mav.stopRun()
|