-
Notifications
You must be signed in to change notification settings - Fork 25
/
Copy pathmulti_agent_drone.py
105 lines (87 loc) · 3.56 KB
/
multi_agent_drone.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
import setup_path
import airsim
import numpy as np
import os
import tempfile
import pprint
# Use below in settings.json with Blocks environment
"""
{
"SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
"SettingsVersion": 1.2,
"SimMode": "Multirotor",
"ClockSpeed": 1,
"Vehicles": {
"Drone1": {
"VehicleType": "SimpleFlight",
"X": 4, "Y": 0, "Z": -2
},
"Drone2": {
"VehicleType": "SimpleFlight",
"X": 8, "Y": 0, "Z": -2
}
}
}
"""
# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True, "Drone1")
client.enableApiControl(True, "Drone2")
client.armDisarm(True, "Drone1")
client.armDisarm(True, "Drone2")
airsim.wait_key('Press any key to takeoff')
f1 = client.takeoffAsync(vehicle_name="Drone1")
f2 = client.takeoffAsync(vehicle_name="Drone2")
f1.join()
f2.join()
state1 = client.getMultirotorState(vehicle_name="Drone1")
s = pprint.pformat(state1)
print("state: %s" % s)
state2 = client.getMultirotorState(vehicle_name="Drone2")
s = pprint.pformat(state2)
print("state: %s" % s)
airsim.wait_key('Press any key to move vehicles')
f1 = client.moveToPositionAsync(-5, 5, -10, 5, vehicle_name="Drone1")
f2 = client.moveToPositionAsync(5, -5, -10, 5, vehicle_name="Drone2")
f1.join()
f2.join()
airsim.wait_key('Press any key to take images')
# get camera images from the car
responses1 = client.simGetImages([
airsim.ImageRequest("0", airsim.ImageType.DepthVis), #depth visualization image
airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)], vehicle_name="Drone1") #scene vision image in uncompressed RGBA array
print('Drone1: Retrieved images: %d' % len(responses1))
responses2 = client.simGetImages([
airsim.ImageRequest("0", airsim.ImageType.DepthVis), #depth visualization image
airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)], vehicle_name="Drone2") #scene vision image in uncompressed RGBA array
print('Drone2: Retrieved images: %d' % len(responses2))
tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_drone")
print ("Saving images to %s" % tmp_dir)
try:
os.makedirs(tmp_dir)
except OSError:
if not os.path.isdir(tmp_dir):
raise
for idx, response in enumerate(responses1 + responses2):
filename = os.path.join(tmp_dir, str(idx))
if response.pixels_as_float:
print("Type %d, size %d" % (response.image_type, len(response.image_data_float)))
airsim.write_pfm(os.path.normpath(filename + '.pfm'), airsim.get_pfm_array(response))
elif response.compress: #png format
print("Type %d, size %d" % (response.image_type, len(response.image_data_uint8)))
airsim.write_file(os.path.normpath(filename + '.png'), response.image_data_uint8)
else: #uncompressed array
print("Type %d, size %d" % (response.image_type, len(response.image_data_uint8)))
img1d = np.fromstring(response.image_data_uint8, dtype=np.uint8) #get numpy array
img_rgba = img1d.reshape(response.height, response.width, 4) #reshape array to 4 channel image array H X W X 4
img_rgba = np.flipud(img_rgba) #original image is flipped vertically
img_rgba[:,:,1:2] = 100 #just for fun add little bit of green in all pixels
airsim.write_png(os.path.normpath(filename + '.greener.png'), img_rgba) #write to png
airsim.wait_key('Press any key to reset to original state')
client.armDisarm(False, "Drone1")
client.armDisarm(False, "Drone2")
client.reset()
# that's enough fun for now. let's quit cleanly
client.enableApiControl(False, "Drone1")
client.enableApiControl(False, "Drone2")