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()
|