fix：新增例程

基础智能/e4.四旋翼与无人车异构集群协同控制实验/UDPSimpleMultiVehicleSwarm.py

@@ -0,0 +1,83 @@
+import time
+import math
+import sys
+import numpy as np
+import PX4MavCtrlV4 as PX4MavCtrl
+VehilceNum = 3
+CarNum = 3
+
+MavList=[]
+flagI = np.zeros(VehilceNum, dtype=bool)
+# Create MAV instance
+for i in range(VehilceNum + CarNum):
+    MavList=MavList+[PX4MavCtrl.PX4MavCtrler(1+i)]
+
+time.sleep(2)
+# Start MAV loop with UDP mode: Udp_Simple
+for i in range(VehilceNum+CarNum):
+    MavList[i].InitMavLoop(1)
+
+# Enter Offboard mode to start vehicle control
+time.sleep(2)
+for i in range(VehilceNum+CarNum):
+    MavList[i].initOffboard()
+    time.sleep(0.5)
+
+time.sleep(2)
+for i in range(VehilceNum+CarNum):
+    MavList[i].SendMavArm(True)
+# 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(5)
+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]
+
+        if abs(mav.uavPosNED[2] - (-10)) <2 and (flagI[j] == False):
+            flagI[j] = True
+            print(str(j)+"号飞机已到达指定高度")
+        
+        if flagI[j] == True:
+            mav.SendPosNED(MavList[j+3].uavPosNED[0],MavList[j+3].uavPosNED[1],mav.uavPosNED[2],0)
+    
+    for i in range(VehilceNum,VehilceNum+CarNum):
+        if flagI[i-3]  == True:
+            # print(f"编号 {i-3}：北 {MavList[i-3].uavPosNED[0]:.2f} m，东 {MavList[i-3].uavPosNED[1]:.2f} m，地 {MavList[i-3].uavPosNED[2]:.2f} m")
+            MavList[i].SendPosNEDNoYaw(MavList[i].uavPosNED[0]+2.5,MavList[i].uavPosNED[1],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()