def talker():
#pubImu = rospy.Publisher("/imuTest", Imu, queue_size = 10)
pubLeft = rospy.Publisher("/left/image_encode", Image, queue_size=10)
pubRight = rospy.Publisher("/right/image_encode", Image, queue_size=10)
rospy.init_node('airsim_data', anonymous=False)
bridge = CvBridge()
#imu = Imu()
counter = 0
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
rawImageLeft = client.simGetImage("2", airsim.ImageType.Scene)
pngLeft = cv2.imdecode(airsim.string_to_uint8_array(rawImageLeft),
cv2.IMREAD_UNCHANGED)
cv2.imshow("Left Cam1", pngLeft)
try:
pubLeft.publish(bridge.cv2_to_imgmsg(pngLeft))
except CvBridgeError as e:
print(e)
rawImageRight = client.simGetImage("1", airsim.ImageType.Scene)
pngRight = cv2.imdecode(airsim.string_to_uint8_array(rawImageRight),
cv2.IMREAD_UNCHANGED)
cv2.imshow("Right Cam1", pngRight)
try:
pubRight.publish(
bridge.cv2_to_imgmsg(pngRight, encoding="passthrough"))
except CvBridgeError as e:
print(e)
#counter += 1
#imu_data = client.getImuData(imu_name = "", vehicle_name = "")
#imu.header.seq = counter
#imu.header.frame_id = "imu4"
#imu.header.stamp = rospy.Time.now()
#imu.angular_velocity.x = imu_data.angular_velocity.x_val
#imu.angular_velocity.y = imu_data.angular_velocity.y_val
#imu.angular_velocity.z = imu_data.angular_velocity.z_val
#imu.linear_acceleration.x = imu_data.linear_acceleration.x_val
#imu.linear_acceleration.y = imu_data.linear_acceleration.y_val
#imu.linear_acceleration.z = imu_data.linear_acceleration.z_val
#imu.orientation.x = imu_data.orientation.x_val
#imu.orientation.y = imu_data.orientation.y_val
#imu.orientation.z = imu_data.orientation.z_val
#imu.orientation.w = imu_data.orientation.w_val
#pubImu.publish(imu)
rate.sleep()
if cv2.waitKey(1) & 0xff == ord(' '):
break
def run(self, postprocessor=None, record=False):
counter = -1
while True:
# --> Fetch drone camera image
image = self.fetch_single_img()
# --> Encode image to png
image = cv2.imdecode(airsim.string_to_uint8_array(image),
cv2.IMREAD_UNCHANGED)
# --> Run image through supplied pre-processor
if postprocessor is not None:
image = postprocessor(image)
# --> Display using open cv
self.camera_memory.append(image)
cv2.imshow("Camera " + self.camera_name, image)
# --> Wait for a key to be pressed to close window and resume simulation
cv2.waitKey(self.shutter_speed)
# --> Record image is record is enabled
if record:
counter += 1
# --> Save png
cv2.imwrite(os.path.normpath("image_" + str(counter) + '.png'),
image)
def frame_generator():
while True:
# because this method returns std::vector<uint8>, msgpack decides to encode it as a string unfortunately.
rawImage = client.simGetImage("0", cameraTypeMap[cameraType])
if (rawImage == None):
print(
"Camera is not returning image, please check airsim for error messages"
)
sys.exit(0)
else:
#png = cv2.imdecode(airsim.string_to_uint8_array(rawImage), cv2.IMREAD_UNCHANGED)
#cv2.putText(png,'FPS ' + str(fps),textOrg, fontFace, fontScale,(255,0,255),thickness)
#cv2.imshow("Depth", png)
png_2 = cv2.imdecode(airsim.string_to_uint8_array(rawImage),
cv2.IMREAD_UNCHANGED)
cv2.putText(png_2, 'Koreanair ' + str(fps), textOrg, fontFace,
fontScale, (255, 0, 255), thickness)
ret, encoded_jpeg = cv2.imencode(DECODE_EXTENSION, png_2)
frame = encoded_jpeg.tobytes()
yield (b'--frame\r\n'
b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n\r\n')
"""
frameCount = frameCount + 1
endTime = time.time()
diff = endTime - startTime
if (diff > 1):
fps = frameCount
frameCount = 0
startTime = endTime
"""
key = cv2.waitKey(1) & 0xFF
if (key == 27 or key == ord('q') or key == ord('x')
or key == ord('X')):
break
def showImages():
global mutex, client
cameraType = "scene"
cameraTypeMap = {
"depth": airsim.ImageType.DepthVis,
"segmentation": airsim.ImageType.Segmentation,
"seg": airsim.ImageType.Segmentation,
"scene": airsim.ImageType.Scene,
"disparity": airsim.ImageType.DisparityNormalized,
"normals": airsim.ImageType.SurfaceNormals
}
print(cameraTypeMap[cameraType])
help = False
fontFace = cv2.FONT_HERSHEY_SIMPLEX
fontScale = 0.5
thickness = 2
textSize, baseline = cv2.getTextSize("FPS", fontFace, fontScale, thickness)
print(textSize)
textOrg = (10, 10 + textSize[1])
frameCount = 0
startTime = time.clock()
fps = 0
i = 0
while True:
#client.moveByVelocityAsync(1,0,0,100) #.join()等待程序执行完毕，不加则不等待
# because this method returns std::vector<uint8>, msgpack decides to encode it as a string unfortunately.
if mutex.acquire():
rawImage = client.simGetImage("0", cameraTypeMap[cameraType])
mutex.release()
if (rawImage == None):
print(
"Camera is not returning image, please check airsim for error messages"
)
sys.exit(0)
else:
png = cv2.imdecode(airsim.string_to_uint8_array(rawImage),
cv2.IMREAD_UNCHANGED)
cv2.putText(png, 'FPS ' + str(fps), textOrg, fontFace, fontScale,
(255, 0, 255), thickness)
cv2.imshow("Scene", png)
frameCount = frameCount + 1
endTime = time.clock()
diff = endTime - startTime
if (diff > 1):
fps = frameCount
frameCount = 0
startTime = endTime
time.sleep(0.01)
key = cv2.waitKey(1) & 0xFF
if (key == 27 or key == ord('q') or key == ord('x')):
break
def getImage(self, drone, cam=0):
"""
Get image for single drone
"""
raw_img = self.client.simGetImage(cam,
airsim.ImageType.Scene,
vehicle_name=drone)
return cv2.imdecode(airsim.string_to_uint8_array(raw_img),
cv2.IMREAD_UNCHANGED)
def fetch_and_record_single_image(self, file_name="Cam_shot"):
# --> Fetch drone camera image
image = self.fetch_single_img()
# --> Encode image to png
png = cv2.imdecode(airsim.string_to_uint8_array(image),
cv2.IMREAD_UNCHANGED)
# --> Save png
cv2.imwrite(os.path.normpath(file_name + '.png'), png)
return
def _camera_thread(self):
rate = 10.
r = rospy.Rate(rate)
image_pub = rospy.Publisher('/airsim/image_raw', Image, queue_size=1)
while not rospy.is_shutdown():
image = self.client.simGetImage("0", airsim.ImageType.Scene).join()
image = cv2.imdecode(airsim.string_to_uint8_array(image),
cv2.IMREAD_UNCHANGED)[:, :, :3]
image_pub.publish(self.bridge.cv2_to_imgmsg(image, 'bgr8'))
r.sleep()
def getFrame(self):
rawImage = self.client.simGetImage("0", self.cameraType)
if rawImage is None:
print("No image returned")
sys.exit(0)
else:
# Decode the image
png = cv2.imdecode(airsim.string_to_uint8_array(rawImage),
cv2.IMREAD_UNCHANGED)
return png
def process_frame(self, response):
frame = airsim.string_to_uint8_array(response.image_data_uint8)
try:
frame = frame.reshape(self.img_shape[0], self.img_shape[1], 3)
except ValueError: # Bug with client.simGetImages:randomly drops Image response sometimes
frame = np.zeros((self.img_shape[0], self.img_shape[1]))
self.dropped_frames += 1
return frame
frame = Image.fromarray(frame).convert('L')
frame = np.asarray(frame)
return frame
def getRGBImg(self):
rawImage = self.client.simGetImage("0", airsim.ImageType.Scene)
if (rawImage == None):
print(
"Camera is not returning image, please check airsim for error messages"
)
sys.exit(0)
else:
try:
png = cv2.imdecode(airsim.string_to_uint8_array(rawImage),
cv2.IMREAD_UNCHANGED)
img = cv2.cvtColor(png, cv2.COLOR_RGBA2RGB)
return img
except:
return np.zeros((144, 256, 3))
def talker():
pub = rospy.Publisher("/cam1/image_raw", Image, queue_size=10)
rospy.init_node('rightStereo', anonymous=False)
bridge = CvBridge()
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
rawImage = client.simGetImage("1", airsim.ImageType.Scene)
png = cv2.imdecode(airsim.string_to_uint8_array(rawImage),
cv2.IMREAD_UNCHANGED)
cv2.imshow("Right Cam1", png)
pub.publish(bridge.cv2_to_imgmsg(png))
rate.sleep()
if cv2.waitKey(1) & 0xff == ord(' '):
break
def capture_image(self):
requests = [
airsim.ImageRequest('front_center',
airsim.ImageType.Scene,
pixels_as_float=False,
compress=False)
]
responses = self.env.client.simGetImages(requests)
response = responses[0]
bgr = np.reshape(
airsim.string_to_uint8_array(response.image_data_uint8),
(response.height, response.width, 3))
rgb = np.array(bgr[:, :, [2, 1, 0]])
img = Image.fromarray(rgb)
img.save(self.save_path + "/rgb/image{}.jpg".format(self.img_idx))
print("Image {} captured!".format(self.img_idx))
self.img_idx += 1
def getVideoFrameAndDrawMarkers(self, client, imageType):
# Display FPS
thickness = 2
textSize, baseline = cv2.getTextSize("FPS", self.font_face,
self.font_scale, thickness)
textOrg = (10, 10 + textSize[1])
frameCount = 0
startTime = time.time()
fps = 0
# Infinite loop
while True:
# because this method returns std::vector<uint8>, msgpack decides to encode it as a string unfortunately.
rawImage = client.simGetImage("0", self.imageTypes[imageType])
if (rawImage == None):
print(
"Camera is not returning image, please check airsim for error messages"
)
sys.exit(0)
else:
png = cv2.imdecode(airsim.string_to_uint8_array(rawImage),
cv2.IMREAD_UNCHANGED)
cv2.putText(png, 'FPS ' + str(fps), textOrg, self.font_face,
self.font_scale, (255, 0, 255), thickness)
# Pass the image into the detection function to draw boundaries
self.drawMarkers(png)
frameCount = frameCount + 1
endTime = time.time()
diff = endTime - startTime
if (diff > 1):
fps = frameCount
frameCount = 0
startTime = endTime
# Quit on q or x
key = cv2.waitKey(1) & 0xFF
if (key == 27 or key == ord('q') or key == ord('x')):
break
def display_camera_view(self):
# --> Pause simulation
self.client.simPause(True)
# --> Fetch drone camera image
image = self.fetch_single_img()
# --> Encode image to png
png = cv2.imdecode(airsim.string_to_uint8_array(image),
cv2.IMREAD_UNCHANGED)
# --> Display using open cv
cv2.imshow("Camera " + self.camera_name, png)
# --> Wait for a key to be pressed to close window and resume simulation
cv2.waitKey(0)
cv2.destroyAllWindows()
self.client.simPause(False)
return
def talker():
pub = rospy.Publisher("/cam0/image_raw", Image, queue_size=10)
rospy.init_node('leftStereo', anonymous=False)
bridge = CvBridge()
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
rawImage = client.simGetImage("2", airsim.ImageType.Scene)
png = cv2.imdecode(airsim.string_to_uint8_array(rawImage),
cv2.IMREAD_UNCHANGED)
cv2.imshow("Left Cam1", png)
pub.publish(bridge.cv2_to_imgmsg(png))
imu_data = client.getImuData(imu_name="", vehicle_name="")
print(imu_data)
#hello_str = "hello world %s" % rospy.get_time()
#rospy.loginfo(hello_str)
#pub.publish(hello_str)
rate.sleep()
if cv2.waitKey(1) & 0xff == ord(' '):
break
airsim.ImageRequest("RGB_Left",
airsim.ImageType.DepthPlanar,
pixels_as_float=True,
compress=_compress),
airsim.ImageRequest("RGB_Left",
airsim.ImageType.Segmentation,
compress=_compress)
])
rRight = client.simGetImages(
[airsim.ImageRequest("RGB_Right", airsim.ImageType.Scene)])
pose = client.simGetVehiclePose()
p = pose.position
o = pose.orientation
imgL_0 = cv2.imdecode(
airsim.string_to_uint8_array(rLeft[0].image_data_uint8),
cv2.IMREAD_UNCHANGED)
imgR_0 = cv2.imdecode(
airsim.string_to_uint8_array(rRight[0].image_data_uint8),
cv2.IMREAD_UNCHANGED)
img = np.concatenate((imgL_0, imgR_0), axis=1)
cv2.imshow('***', img)
key = cv2.waitKey(1)
if key == ord('s'):
loc = [[p.x_val, p.y_val, p.z_val],
[o.w_val, o.x_val, o.y_val, o.z_val]]
trajactory.append(loc)
fname = str(id).zfill(5)
airsim.write_file((saveDir + fname + '_L.png'),
rLeft[0].image_data_uint8)
def takePicture(self):
# get camera images from the car
rawImage = self.client.simGetImage("0", airsim.ImageType.Scene)
return cv2.imdecode(airsim.string_to_uint8_array(rawImage),
cv2.IMREAD_UNCHANGED)
def main(args):
client = airsim.VehicleClient()
client.confirmConnection()
ts = datetime.datetime.now().isoformat()[:-7].replace(':', '')
tmp_dir = os.path.join(args.out_path, args.env, ts)
print("Saving images to %s" % tmp_dir)
try:
os.makedirs(tmp_dir)
except OSError:
if not os.path.isdir(tmp_dir):
raise
nseqs = 3600
h5file = tables.open_file(os.path.join(tmp_dir, 'output.h5'),
mode="w",
title="Flythroughs")
seq_len = 40
short_seq_len = 10
nominal_fps = 30
labels_table = h5file.create_table('/',
'labels',
Particle,
expectedrows=nseqs)
video_table = h5file.create_earray('/',
'videos',
tables.UInt8Atom(),
shape=(0, seq_len, 3, 112, 112),
expectedrows=nseqs)
short_video_table = h5file.create_earray('/',
'short_videos',
tables.UInt8Atom(),
shape=(0, short_seq_len, 3, 112,
112),
expectedrows=nseqs)
depth_table = h5file.create_earray('/',
'depth',
tables.Float64Atom(),
shape=(0, 112, 112),
expectedrows=nseqs)
for i in tqdm(range(nseqs)): # do few times
# For flat environments, start ground plane localization not too high.
ground_from = 5
# 3 meters/s -> jogging speed
MAX_SPEED = 3
collisions = True
pause = 0
# Manually define areas of interest. Note that inside one of the airsim
# environments, you can pull up coordinates using `;`. However, the
# coordinates listed are multiplied by 100 (i.e. the numbers are in cm
# rather than meters); divide by 100 to define reasonable boundaries
# for the capture area.
if args.env == 'blocks':
x = np.random.uniform(-50, 50)
y = np.random.uniform(-50, 50)
pause = .05 # blocks is too fast sometimes
elif args.env.startswith('nh'):
x = np.random.uniform(-150, 150)
y = np.random.uniform(-150, 150)
client.simEnableWeather(True)
for k in range(8):
client.simSetWeatherParameter(k, 0.0)
if args.env == 'nh_fall':
client.simSetWeatherParameter(
airsim.WeatherParameter.MapleLeaf, 1.0)
client.simSetWeatherParameter(airsim.WeatherParameter.RoadLeaf,
1.0)
if args.env == 'nh_winter':
client.simSetWeatherParameter(airsim.WeatherParameter.Snow,
1.0)
client.simSetWeatherParameter(airsim.WeatherParameter.RoadSnow,
1.0)
elif args.env == 'mountains':
# Most of the interesting stuff (e.g. the lake, roads) is on the
# diagonal of this very large environment (several kilometers!).
xy = np.random.uniform(0, 2500)
xmy = np.random.uniform(-100, 100)
x = xy + xmy
y = xy - xmy
# This environment varies a lot in height, start from higher
ground_from = 100
elif args.env == 'trap':
x = np.random.uniform(-10, 10)
y = np.random.uniform(-10, 10)
# This one has lots of branches, keep sequences that have collisions
collisions = False
else:
raise NotImplementedError(args.env)
# Time of day effects works in blocks and trap only.
time_of_day = np.random.randint(5, 21)
client.simSetTimeOfDay(
True,
start_datetime=f"2020-03-21 {time_of_day:02}:00:00",
is_start_datetime_dst=True,
celestial_clock_speed=0,
update_interval_secs=60,
move_sun=True)
if pause > 0:
time.sleep(pause)
pitch = np.random.uniform(
-.25,
.25) # Sometimes we look a little up, sometimes a little down
roll = 0 # Should be 0 unless something is wrong
# 360 degrees lookaround
yaw = np.random.uniform(-np.pi, np.pi)
heading_yaw = draw_von_mises(2.5)
heading_pitch = draw_von_mises(16)
# Max ~90 degrees per second head rotation
rotation_yaw = 30 * np.pi / 180 * np.random.randn()
rotation_pitch = 10 * np.pi / 180 * np.random.randn()
speed = MAX_SPEED * np.random.rand()
# Figure out the height of the ground. Move the camera way far above
# ground, and get the distance to the ground via the depth buffer
# from the bottom camera.
client.simSetVehiclePose(
airsim.Pose(airsim.Vector3r(x, y, -ground_from),
airsim.to_quaternion(0, 0, 0)), True)
if pause > 0:
time.sleep(pause)
responses = client.simGetImages([
airsim.ImageRequest("bottom_center",
airsim.ImageType.DepthPlanner,
pixels_as_float=True)
])
response = responses[0]
the_arr = airsim.list_to_2d_float_array(response.image_data_float,
response.width,
response.height)
# Then move to ~5.5 feet above the ground
rgy = range(int(.4 * the_arr.shape[0]), int(.6 * the_arr.shape[0]))
rgx = range(int(.4 * the_arr.shape[0]), int(.6 * the_arr.shape[0]))
the_min = np.median(the_arr[rgy, rgx])
z = the_min - ground_from - np.random.uniform(1.4, 2)
if z > 50:
# More than 50 meters below sea level, bad draw.
continue
#client.startRecording()
z = z.item()
depths = []
frames = []
booped = False
for k in range(seq_len):
if booped:
continue
# In nominal seconds
t = (k - (seq_len - 1) / 2) / nominal_fps
d = t * speed
client.simSetVehiclePose(
airsim.Pose(
airsim.Vector3r(
x + d * np.cos(yaw + heading_yaw) *
np.cos(pitch + heading_pitch),
y + d * np.sin(yaw + heading_yaw) *
np.cos(pitch + heading_pitch),
z + d * np.sin(pitch + heading_pitch)),
airsim.to_quaternion(pitch + t * rotation_pitch, roll,
yaw + t * rotation_yaw)), True)
responses = client.simGetImages([
airsim.ImageRequest("front_center", airsim.ImageType.Scene,
False, False),
airsim.ImageRequest("front_center",
airsim.ImageType.DepthPlanner, True,
False),
])
for j, response in enumerate(responses):
if j == 0:
frames.append(
airsim.string_to_uint8_array(
response.image_data_uint8).reshape(
response.height, response.width,
3)[:, :, ::-1])
if j == 1:
zbuff = airsim.list_to_2d_float_array(
response.image_data_float, response.width,
response.height)
depths.append(zbuff)
# Did we bump into something?
if collisions:
closest = zbuff[zbuff.shape[0] // 4:-zbuff.shape[0] //
4, zbuff.shape[1] //
4:-zbuff.shape[1] // 4].min()
if closest < .5:
# oops I booped it again.
booped = True
if j == 0 and args.save_images:
filename = os.path.join(tmp_dir, f"{i:02}_{k:02}.png")
matplotlib.image.imsave(filename, frames[-1])
if pause > 0:
time.sleep(pause)
row = labels_table.row
if not booped and not args.save_images:
# Let's save this!
row['x'] = x
row['y'] = y
row['z'] = z
row['yaw'] = yaw
row['pitch'] = pitch
row['speed'] = speed
row['heading_yaw'] = heading_yaw
row['heading_pitch'] = heading_pitch
row['rotation_yaw'] = rotation_yaw
row['rotation_pitch'] = rotation_pitch
row.append()
V = np.stack(frames, axis=0).transpose((0, 3, 1, 2))
V = V.reshape((1, ) + V.shape)
video_table.append(V)
# Take a subset of the data.
mid_seq = range((seq_len - short_seq_len) // 2,
(seq_len + short_seq_len) // 2)
assert V.shape[1] == seq_len
short_video_table.append(V[:, mid_seq, :, :, :])
# Only record the mid-point z buffer
n = seq_len // 2
D = depths[n]
D = D.reshape((1, ) + D.shape)
depth_table.append(D)
h5file.close()
# currently reset() doesn't work in CV mode. Below is the workaround
client.simSetVehiclePose(
airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)),
True)
# Client for gathering images from camera
imageClient = airsim.MultirotorClient()
# Loop and get the images
while True:
# because this method returns std::vector<uint8>, msgpack decides to encode it as a string unfortunately.
rawImage = imageClient.simGetImage("0", airsim.ImageType.Scene)
if (rawImage == None):
print("Camera is not returning image, please check airsim for error messages")
sys.exit(0)
# Get the frame
frame = cv2.imdecode(airsim.string_to_uint8_array(rawImage), cv2.IMREAD_UNCHANGED)
# Convert the frame to gray
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Find markers and corners in the image
corners, ids, rejected = aruco.detectMarkers(gray, aruco_dict)
if ids is not None:
# rvecs and tvecs are the rotation and translation vectors for each of the markers in corners.
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners, 1, cameraMatrix, distCoeffs)
# Join and enumerate the vectors
for id, rvec, tvec in zip(ids, rvecs, tvecs):
print(textSize)
textOrg = (10, 10 + textSize[1])
frameCount = 0
startTime = time.clock()
fps = 0
while True:
# because this method returns std::vector<uint8>, msgpack decides to encode it as a string unfortunately.
rawImage = client.simGetImage("0", cameraTypeMap[cameraType])
if (rawImage == None):
print(
"Camera is not returning image, please check airsim for error messages"
)
sys.exit(0)
else:
png = cv2.imdecode(airsim.string_to_uint8_array(rawImage),
cv2.IMREAD_UNCHANGED)
cv2.putText(png, 'FPS ' + str(fps), textOrg, fontFace, fontScale,
(255, 0, 255), thickness)
cv2.imshow("Depth", png)
frameCount = frameCount + 1
endTime = time.clock()
diff = endTime - startTime
if (diff > 1):
fps = frameCount
frameCount = 0
startTime = endTime
key = cv2.waitKey(1) & 0xFF
if (key == 27 or key == ord('q') or key == ord('x')):
thickness = 2
textSize, baseline = cv2.getTextSize("FPS", fontFace, fontScale, thickness)
print (textSize)
textOrg = (10, 10 + textSize[1])
frameCount = 0
startTime=time.clock()
fps = 0
while True:
# because this method returns std::vector<uint8>, msgpack decides to encode it as a string unfortunately.
rawImage = client.simGetImage("0", cameraTypeMap[cameraType])
if (rawImage == None):
print("Camera is not returning image, please check airsim for error messages")
sys.exit(0)
else:
png = cv2.imdecode(airsim.string_to_uint8_array(rawImage), cv2.IMREAD_UNCHANGED)
cv2.putText(png,'FPS ' + str(fps),textOrg, fontFace, fontScale,(255,0,255),thickness)
cv2.imshow("Depth", png)
frameCount = frameCount + 1
endTime=time.clock()
diff = endTime - startTime
if (diff > 1):
fps = frameCount
frameCount = 0
startTime = endTime
key = cv2.waitKey(1) & 0xFF;
if (key == 27 or key == ord('q') or key == ord('x')):
break;
def get_camera_observation(client,
sensor_types=['rgb', 'depth'],
max_dist=10,
height=64,
width=64):
"""
Makes call to AirSim and extract depth and/or RGB images from the UAV's camera. Due to a bug in AirSim
an empty array is sometimes returned which is caught in this method.
:param client: AirsimEnv object
:param sensor_types: List of the image types to extract. 'rgb' and 'depth' available.
:param max_dist: Max depth at which the depth map is capped.
:param height: height of the images
:param width: width of the images
:return: dict containing the 'rgb' and/or 'depth' images
"""
requests = []
sensor_idx = {}
idx_counter = 0
if 'rgb' in sensor_types:
requests.append(
airsim.ImageRequest('front_center',
airsim.ImageType.Scene,
pixels_as_float=False,
compress=False))
sensor_idx.update({'rgb': idx_counter})
idx_counter += 1
if 'depth' in sensor_types:
requests.append(
airsim.ImageRequest('front_center',
airsim.ImageType.DepthPlanner,
pixels_as_float=True,
compress=False))
sensor_idx.update({'depth': idx_counter})
idx_counter += 1
responses = client.simGetImages(requests)
images = {}
if 'rgb' in sensor_types:
idx = sensor_idx['rgb']
# convert to uint and reshape to matrix with 3 color channels
try:
bgr = np.reshape(
airsim.string_to_uint8_array(responses[idx].image_data_uint8),
(height, width, 3))
# move color channels around
rgb = np.array(bgr[:, :, [2, 1, 0]], dtype=np.float32)
except ValueError as err:
print('========================================================')
print('Value err when reshaping RGB image: {0}'.format(err))
print('Replacing rgb with all zeros')
print('========================================================')
rgb = np.zeros((height, width, 3), dtype=np.float32)
images.update({'rgb': rgb})
if 'depth' in sensor_types:
idx = sensor_idx['depth']
# convert to 2D numpy array. Had unexpected exception here. Try: Catch
try:
depth = airsim.list_to_2d_float_array(
responses[idx].image_data_float, width, height)
except ValueError as err:
print('========================================================')
print('Value err when reshaping depth image: {0}'.format(err))
print('Replacing depth map with all max dist values')
print('========================================================')
depth = np.ones((height, width), dtype=np.float32) * max_dist
depth = np.expand_dims(depth, axis=2)
images.update({'depth': depth})
return images
def main():
depth_model = tf.keras.models.load_model(
filepath="./depth/model.h5",
custom_objects={
"relu6": tf.nn.relu6,
"tf": tf.nn
},
compile=False,
)
print("depth model loaded")
#
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
client.takeoffAsync().join()
print("multirotor took off")
#
client.simSetVehiclePose(
airsim.Pose(
airsim.Vector3r(*START_POS),
airsim.Quaternionr(0, 0, 0, 1),
),
ignore_collison=True,
)
move_to_target(client)
#
record = pathlib.Path(RECORD_PATH).joinpath(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M"))
scene_record = record.joinpath("scene")
depth_record = record.joinpath("depth")
scene_record.mkdir(mode=0o755, parents=True)
depth_record.mkdir(mode=0o755, parents=True)
record_counter = 1
#
try:
while True:
pose = client.simGetVehiclePose().position
if pose.distance_to(airsim.Vector3r(*TARGET_POS)) < 2:
print("target position arrived")
client.moveByVelocityAsync(0, 0, 0, 5).join()
break
#
response = client.simGetImages([
airsim.ImageRequest(
camera_name="front_center",
image_type=airsim.ImageType.Scene,
compress=False,
)
])
scene = airsim.string_to_uint8_array(
response[0].image_data_uint8).reshape(response[0].height,
response[0].width, 3)
depth = np.maximum(
np.squeeze(
depth_model.predict(np.expand_dims(scene, axis=0))[0]), 1)
#
result = algorithm.two_step_decision(depth)
print(result)
#
filename = str(record_counter).zfill(3) + ".png"
Image.frombytes(
mode="RGB",
size=(response[0].width, response[0].height),
data=response[0].image_data_uint8,
).save(scene_record.joinpath(filename))
util.save_algorithm(
depth_data=depth,
algo_result=result,
path=depth_record.joinpath(filename),
)
record_counter += 1
#
dodge_duration = DODGE_DIST / FLY_SPEED
if result == algorithm.Result.UP_RIGHT:
client.moveByVelocityAsync(
vx=FLY_SPEED / 4,
vy=FLY_SPEED / 2**0.5,
vz=-FLY_SPEED / 2**0.5,
duration=dodge_duration,
).join()
move_to_target(client)
elif result == algorithm.Result.RIGHT:
client.moveByVelocityAsync(
vx=FLY_SPEED,
vy=FLY_SPEED,
vz=0,
duration=dodge_duration,
).join()
move_to_target(client)
elif result == algorithm.Result.DOWN_RIGHT:
client.moveByVelocityAsync(
vx=FLY_SPEED / 4,
vy=FLY_SPEED / 2**0.5,
vz=FLY_SPEED / 2**0.5,
duration=dodge_duration,
).join()
move_to_target(client)
elif result == algorithm.Result.DOWN:
client.moveByVelocityAsync(
vx=FLY_SPEED / 4,
vy=0,
vz=FLY_SPEED,
duration=dodge_duration,
).join()
move_to_target(client)
elif result == algorithm.Result.DOWN_LEFT:
client.moveByVelocityAsync(
vx=FLY_SPEED / 4,
vy=-FLY_SPEED / 2**0.5,
vz=FLY_SPEED / 2**0.5,
duration=dodge_duration,
).join()
move_to_target(client)
elif result == algorithm.Result.LEFT:
client.moveByVelocityAsync(
vx=FLY_SPEED,
vy=-FLY_SPEED,
vz=0,
duration=dodge_duration,
).join()
move_to_target(client)
elif result == algorithm.Result.UP_LEFT:
client.moveByVelocityAsync(
vx=FLY_SPEED / 4,
vy=-FLY_SPEED / 2**0.5,
vz=-FLY_SPEED / 2**0.5,
duration=dodge_duration,
).join()
move_to_target(client)
elif result == algorithm.Result.UP:
client.moveByVelocityAsync(
vx=FLY_SPEED / 4,
vy=0,
vz=-FLY_SPEED,
duration=dodge_duration,
).join()
move_to_target(client)
elif result == algorithm.Result.STOP:
pass
else:
move_to_target(client)
except:
traceback.print_exc()
#
client.reset()
def get_image_and_publish(self):
image = self.client.simGetImage("0", airsim.ImageType.Scene)
image = cv2.imdecode(airsim.string_to_uint8_array(image),
cv2.IMREAD_UNCHANGED)[:, :, :3]
self.image_pub.publish(self.bridge.cv2_to_imgmsg(image, 'bgr8'))
