107 lines
2.6 KiB
Python
107 lines
2.6 KiB
Python
|
||
# import required libraries
|
||
# pip3 install pymavlink pyserial
|
||
|
||
import cv2
|
||
import numpy as np
|
||
import time
|
||
import VisionCaptureApi
|
||
import PX4MavCtrlV4 as PX4MavCtrl
|
||
import math
|
||
# import matplotlib.pyplot as plt
|
||
import ReqCopterSim
|
||
import Open3DShow
|
||
|
||
show3d=Open3DShow.Open3DShow()
|
||
|
||
|
||
vis = VisionCaptureApi.VisionCaptureApi()
|
||
|
||
# VisionCaptureApi 中的配置函数
|
||
vis.jsonLoad()
|
||
isSuss = vis.sendReqToUE4()
|
||
|
||
vis.startImgCap() # 开启取图循环，执行本语句之后，已经可以通过vis.Img[i]读取到图片了
|
||
print('Start Image Reciver')
|
||
|
||
# vis.sendImuReqCopterSim(1,'127.0.0.1') # 发送请求，从目标飞机CopterSim读取IMU数据,回传地址为127.0.0.1，默认频率为200Hz
|
||
# 执行本语句之后，会自动开启数据监听，已经可以通过vis.imu读取到IMU数据了。
|
||
|
||
|
||
mav = PX4MavCtrl.PX4MavCtrler(1)
|
||
|
||
|
||
time.sleep(2)
|
||
# Start MAV loop with UDP mode: MAVLINK_FULL
|
||
mav.InitMavLoop()
|
||
mav.InitTrueDataLoop()
|
||
|
||
# Enter Offboard mode to start vehicle control
|
||
time.sleep(2)
|
||
mav.initOffboard()
|
||
|
||
# Get the takeoff position of each vehicle to the UE4 Map
|
||
# this can be adopted to obtain the global position of a vehicle in swarm simulation
|
||
time.sleep(2)
|
||
Error2UE4Map = []
|
||
|
||
Error2UE4Map = -np.array([mav.uavGlobalPos[0]-mav.uavPosNED[0],
|
||
mav.uavGlobalPos[1]-mav.uavPosNED[1], mav.uavGlobalPos[2]-mav.uavPosNED[2]])
|
||
|
||
# fly to 10m high above its takeoff position
|
||
mav.SendPosNED(0, 0, -8, 0)
|
||
mav.SendCopterSpeed(3)
|
||
#time.sleep(10)
|
||
|
||
|
||
#创建点云显示窗口
|
||
show3d.CreatShow(0)
|
||
|
||
|
||
lastTime = time.time()
|
||
startTime = time.time()
|
||
timeInterval = 1/30.0 # here is 0.0333s (30Hz)
|
||
while True:
|
||
lastTime = lastTime + timeInterval
|
||
sleepTime = lastTime - time.time()
|
||
if sleepTime > 0:
|
||
time.sleep(sleepTime) # sleep until the desired clock
|
||
else:
|
||
lastTime = time.time()
|
||
# The following code will be executed 30Hz (0.0333s)
|
||
|
||
t = time.time()-startTime
|
||
|
||
|
||
# target position in UE4 map global frame
|
||
if t < 10:
|
||
# fly to 15 meters high in 0 to 10s
|
||
targetPosE = np.array([-0, 0, -5])
|
||
elif t<20: # fly circle after 10s
|
||
targetPosE = np.array(
|
||
[15,5, -5])
|
||
else:
|
||
targetPosE = np.array([15,-10, -5])
|
||
|
||
# target position in vehilce takeoff frame
|
||
targetPosE = targetPosE+Error2UE4Map
|
||
mav.SendPosNED(targetPosE[0], targetPosE[1], targetPosE[2], 0)
|
||
|
||
if vis.hasData[0]:
|
||
|
||
# 更新点云
|
||
show3d.UpdateShow(vis.Img[0])
|
||
|
||
# 标记
|
||
vis.hasData[0]=False
|
||
|
||
#isShowCloud = False
|
||
if t > 200: # end simulation when 200s
|
||
break
|
||
|
||
|
||
mav.endOffboard()
|
||
|
||
time.sleep(1)
|
||
mav.stopRun()
|