91 lines
3.5 KiB
Python
91 lines
3.5 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.initOffboard2()
|
||
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 10, 0,-100!')
|
||
time.sleep(1)
|
||
mav.sendMavTakeOff(10,0,-100)
|
||
|
||
# 下面的程序非必需，仅用于观察图像用，在电脑性能不足时，请删除
|
||
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,0,0] # 修改相机的位置，可以调整相机初始位置
|
||
vs.SensorAngEular=[0,0,0] # 修改相机的姿态，可以模拟云台转动
|
||
vis.sendUpdateUEImage(vs) # 发送更新数据
|
||
|
||
|
||
#mav.endOffboard()
|
||
#mav.stopRun() |