90 lines
2.8 KiB
Python
90 lines
2.8 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 ReqCopterSim
|
|||
|
|
|
|||
|
|
req = ReqCopterSim.ReqCopterSim() # 获取局域网内所有CopterSim程序的电脑IP列表
|
|||
|
|
StartCopterID = 1 # 初始飞机的ID号
|
|||
|
|
TargetIP = req.getSimIpID(StartCopterID) # 获取CopterSim的1号程序所在电脑的IP，作为目标IP
|
|||
|
|
# 注意：如果是本电脑运行的话，那TargetIP是127.0.0.1的本机地址；如果是远程访问，则是192打头的局域网地址。
|
|||
|
|
# 因此本程序能同时在本机运行，也能在其他电脑运行。
|
|||
|
|
|
|||
|
|
VehilceNum = 1
|
|||
|
|
MavList=[]
|
|||
|
|
# Create MAV instance
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
CopterID=StartCopterID+i # 当前配置的飞机序号
|
|||
|
|
TargetIP = req.getSimIpID(CopterID) # 获取对应的电脑IP
|
|||
|
|
req.sendReSimIP(CopterID) # 请求回传数据到本电脑
|
|||
|
|
time.sleep(1)
|
|||
|
|
MavList = MavList+[PX4MavCtrl.PX4MavCtrler(CopterID,TargetIP)] # 初始化并建立i号飞机的MAVLink通信连接
|
|||
|
|
|
|||
|
|
time.sleep(2)
|
|||
|
|
# Start MAV loop with UDP mode: MAVLINK_FULL
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
MavList[i].InitMavLoop()
|
|||
|
|
|
|||
|
|
# Enter Offboard mode to start vehicle control
|
|||
|
|
time.sleep(2)
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
MavList[i].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=[]
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
mav=MavList[i]
|
|||
|
|
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
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
MavList[i].SendPosNED(0, 0, -10, 0)
|
|||
|
|
time.sleep(10)
|
|||
|
|
|
|||
|
|
|
|||
|
|
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
|
|||
|
|
for j in range(VehilceNum):
|
|||
|
|
mav=MavList[j]
|
|||
|
|
|
|||
|
|
# target position in UE4 map global frame
|
|||
|
|
if t<10:
|
|||
|
|
targetPosE=np.array([-0,0,-15]) # fly to 15 meters high in 0 to 10s
|
|||
|
|
else: #fly circle after 10s
|
|||
|
|
targetPosE=np.array([10*math.sin(t/2+math.pi/2)-10,10*math.sin(t/2.0),-15])
|
|||
|
|
|
|||
|
|
# target position in vehilce takeoff frame
|
|||
|
|
targetPosE=targetPosE+Error2UE4Map[j]
|
|||
|
|
mav.SendPosNED(targetPosE[0],targetPosE[1],targetPosE[2],0)
|
|||
|
|
|
|||
|
|
if t>200: # end simulation when 200s
|
|||
|
|
break
|
|||
|
|
|
|||
|
|
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
MavList[i].endOffboard()
|
|||
|
|
|
|||
|
|
time.sleep(1)
|
|||
|
|
for i in range(VehilceNum):
|
|||
|
|
MavList[i].stopRun()
|
|||
|
|
|