From 045766d17628d59580d6348a4dc2d1ef9c26b8b4 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Thu, 3 Sep 2020 02:15:15 +0530 Subject: [PATCH] Fix Python API example files (#2683) * [Pythonclient] Fix hello_car.py print, temp folder for images, cleanup * [Pythonclient/car] Delete legacy_hello_car.py * [Pythonclient/types] Add utility functions to check for NaN in Pose * [PythonClient/cv] Fix cv_mode.py * [Unity] Don't send Alpha channel in image Maintain consistency with Unreal * [Pythonclient/utils] Use cv2 to write images --- PythonClient/airsim/types.py | 10 +++ PythonClient/airsim/utils.py | 33 +++----- PythonClient/car/hello_car.py | 36 +++++---- PythonClient/car/legacy_hello_car.py | 76 ------------------- PythonClient/computer_vision/cv_mode.py | 24 ++++-- .../Scripts/Utilities/DataCaptureScript.cs | 2 +- 6 files changed, 59 insertions(+), 122 deletions(-) delete mode 100644 PythonClient/car/legacy_hello_car.py diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index 14f73f1682..021dc6dd88 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -1,6 +1,7 @@ from __future__ import print_function import msgpackrpc #install as admin: pip install msgpack-rpc-python import numpy as np #pip install numpy +import math class MsgpackMixin: def __repr__(self): @@ -62,6 +63,9 @@ def __init__(self, x_val = 0.0, y_val = 0.0, z_val = 0.0): def nanVector3r(): return Vector3r(np.nan, np.nan, np.nan) + def containsNan(self): + return (math.isnan(self.x_val) or math.isnan(self.y_val) or math.isnan(self.z_val)) + def __add__(self, other): return Vector3r(self.x_val + other.x_val, self.y_val + other.y_val, self.z_val + other.z_val) @@ -122,6 +126,9 @@ def __init__(self, x_val = 0.0, y_val = 0.0, z_val = 0.0, w_val = 1.0): def nanQuaternionr(): return Quaternionr(np.nan, np.nan, np.nan, np.nan) + def containsNan(self): + return (math.isnan(self.w_val) or math.isnan(self.x_val) or math.isnan(self.y_val) or math.isnan(self.z_val)) + def __add__(self, other): if type(self) == type(other): return Quaternionr( self.x_val+other.x_val, self.y_val+other.y_val, self.z_val+other.z_val, self.w_val+other.w_val ) @@ -207,6 +214,9 @@ def __init__(self, position_val = None, orientation_val = None): def nanPose(): return Pose(Vector3r.nanVector3r(), Quaternionr.nanQuaternionr()) + def containsNan(self): + return (self.position.containsNan() or self.orientation.containsNan()) + class CollisionInfo(MsgpackMixin): has_collided = False diff --git a/PythonClient/airsim/utils.py b/PythonClient/airsim/utils.py index cd56292417..665921db24 100644 --- a/PythonClient/airsim/utils.py +++ b/PythonClient/airsim/utils.py @@ -6,6 +6,8 @@ import inspect import types import re +import cv2 # pip install opencv-python +import logging from .types import * @@ -41,6 +43,10 @@ def to_str(obj): def write_file(filename, bstr): + """ + Write binary data to file. + Used for writing compressed PNG images + """ with open(filename, 'wb') as afile: afile.write(bstr) @@ -196,27 +202,6 @@ def write_pfm(file, image, scale=1): def write_png(filename, image): """ image must be numpy array H X W X channels """ - import zlib, struct - - buf = image.flatten().tobytes() - width = image.shape[1] - height = image.shape[0] - - # reverse the vertical line order and add null bytes at the start - width_byte_3 = width * 3 - raw_data = b''.join(b'\x00' + buf[span:span + width_byte_3] - for span in range((height - 1) * width_byte_3, -1, - width_byte_3)) - - def png_pack(png_tag, data): - chunk_head = png_tag + data - return (struct.pack("!I", len(data)) + - chunk_head + - struct.pack("!I", 0xFFFFFFFF & zlib.crc32(chunk_head))) - - png_bytes = b''.join([ - b'\x89PNG\r\n\x1a\n', - png_pack(b'IHDR', struct.pack("!2I5B", width, height, 8, 6, 0, 0, 0)), - png_pack(b'IDAT', zlib.compress(raw_data, 9)), - png_pack(b'IEND', b'')]) - - write_file(filename, png_bytes) + ret = cv2.imwrite(filename, image) + if not ret: + logging.error(f"Writing PNG file {filename} failed") diff --git a/PythonClient/car/hello_car.py b/PythonClient/car/hello_car.py index a1402b1f5d..22aef13964 100644 --- a/PythonClient/car/hello_car.py +++ b/PythonClient/car/hello_car.py @@ -1,16 +1,26 @@ +import setup_path import airsim import cv2 import numpy as np import os -import setup_path import time +import tempfile -# connect to the AirSim simulator +# connect to the AirSim simulator client = airsim.CarClient() client.confirmConnection() client.enableApiControl(True) +print("API Control enabled: %s" % client.isApiControlEnabled()) car_controls = airsim.CarControls() +tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_car") +print ("Saving images to %s" % tmp_dir) +try: + os.makedirs(tmp_dir) +except OSError: + if not os.path.isdir(tmp_dir): + raise + for idx in range(3): # get state of the car car_state = client.getCarState() @@ -32,14 +42,14 @@ # go reverse car_controls.throttle = -0.5 - car_controls.is_manual_gear = True; + car_controls.is_manual_gear = True car_controls.manual_gear = -1 car_controls.steering = 0 client.setCarControls(car_controls) print("Go reverse, steer right") time.sleep(3) # let car drive a bit - car_controls.is_manual_gear = False; # change back gear to auto - car_controls.manual_gear = 0 + car_controls.is_manual_gear = False # change back gear to auto + car_controls.manual_gear = 0 # apply brakes car_controls.brake = 1 @@ -47,19 +57,18 @@ print("Apply brakes") time.sleep(3) # let car drive a bit car_controls.brake = 0 #remove brake - + # get camera images from the car responses = client.simGetImages([ airsim.ImageRequest("0", airsim.ImageType.DepthVis), #depth visualization image airsim.ImageRequest("1", airsim.ImageType.DepthPerspective, True), #depth in perspective projection airsim.ImageRequest("1", airsim.ImageType.Scene), #scene vision image in png format airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)]) #scene vision image in uncompressed RGB array - print('Retrieved images: %d', len(responses)) + print('Retrieved images: %d' % len(responses)) + + for response_idx, response in enumerate(responses): + filename = os.path.join(tmp_dir, f"{idx}_{response.image_type}_{response_idx}") - for response in responses: - filename = 'c:/temp/py' + str(idx) - if not os.path.exists('c:/temp/'): - os.makedirs('c:/temp/') 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)) @@ -70,12 +79,9 @@ 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_rgb = img1d.reshape(response.height, response.width, 3) # reshape array to 3 channel image array H X W X 3 - cv2.imwrite(os.path.normpath(filename + '.png'), img_rgb) # write to png + cv2.imwrite(os.path.normpath(filename + '.png'), img_rgb) # write to png #restore to original state client.reset() client.enableApiControl(False) - - - diff --git a/PythonClient/car/legacy_hello_car.py b/PythonClient/car/legacy_hello_car.py deleted file mode 100644 index 1c37bd1b5f..0000000000 --- a/PythonClient/car/legacy_hello_car.py +++ /dev/null @@ -1,76 +0,0 @@ -""" -For connecting to the AirSim drone environment and testing API functionality -""" - -import os -import tempfile -import pprint - -import setup_path -import airsim - - -# connect to the AirSim simulator -client = airsim.MultirotorClient() -client.confirmConnection() -client.enableApiControl(True) -client.armDisarm(True) - -state = client.getMultirotorState() -s = pprint.pformat(state) -print("state: %s" % s) - -airsim.wait_key('Press any key to takeoff') -client.takeoff() - -state = client.getMultirotorState() -print("state: %s" % pprint.pformat(state)) - -airsim.wait_key('Press any key to move vehicle to (-10, 10, -10) at 5 m/s') -client.moveToPosition(-10, 10, -10, 5) - -client.hover() - -state = client.getMultirotorState() -print("state: %s" % pprint.pformat(state)) - -airsim.wait_key('Press any key to take images') -# get camera images from the car -responses = client.simGetImages([ - ImageRequest(0, airsim.AirSimImageType.DepthVis), #depth visualiztion image - ImageRequest(1, airsim.AirSimImageType.DepthPerspective, True), #depth in perspective projection - ImageRequest(1, airsim.AirSimImageType.Scene), #scene vision image in png format - ImageRequest(1, airsim.AirSimImageType.Scene, False, False)]) #scene vision image in uncompressed RGB array -print('Retrieved images: %d' % len(responses)) - -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(responses): - - 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))) - AirSimClientBase.write_pfm(os.path.normpath(filename + '.pfm'), AirSimClientBase.getPfmArray(response)) - elif response.compress: #png format - print("Type %d, size %d" % (response.image_type, len(response.image_data_uint8))) - AirSimClientBase.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_rgb = img1d.reshape(response.height, response.width, 3) #reshape array to 3 channel image array H X W X 3 - AirSimClientBase.write_png(os.path.normpath(filename + '.png'), img_rgb) #write to png - -AirSimClientBase.wait_key('Press any key to reset to original state') - -client.armDisarm(False) -client.reset() - -# that's enough fun for now. let's quit cleanly -client.enableApiControl(False) diff --git a/PythonClient/computer_vision/cv_mode.py b/PythonClient/computer_vision/cv_mode.py index 11ea523cfe..66bca0c97a 100644 --- a/PythonClient/computer_vision/cv_mode.py +++ b/PythonClient/computer_vision/cv_mode.py @@ -1,12 +1,14 @@ -# In settings.json first activate computer vision mode: +# In settings.json first activate computer vision mode: # https://github.com/Microsoft/AirSim/blob/master/docs/image_apis.md#computer-vision-mode -import setup_path +import setup_path import airsim import pprint import os import time +import math +import tempfile pp = pprint.PrettyPrinter(indent=4) @@ -14,13 +16,22 @@ client.confirmConnection() airsim.wait_key('Press any key to set camera-0 gimbal to 15-degree pitch') -camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.261799, 0, 0)) #radians +camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(math.radians(15), 0, 0)) #radians client.simSetCameraPose("0", camera_pose) airsim.wait_key('Press any key to get camera parameters') for camera_name in range(5): camera_info = client.simGetCameraInfo(str(camera_name)) - print("CameraInfo %d: %s" % (camera_name, pp.pprint(camera_info))) + print("CameraInfo %d:" % camera_name) + pp.pprint(camera_info) + +tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_cv_mode") +print ("Saving images to %s" % tmp_dir) +try: + os.makedirs(tmp_dir) +except OSError: + if not os.path.isdir(tmp_dir): + raise airsim.wait_key('Press any key to get images') for x in range(3): # do few times @@ -36,12 +47,13 @@ airsim.ImageRequest("4", airsim.ImageType.SurfaceNormals)]) for i, response in enumerate(responses): + filename = os.path.join(tmp_dir, str(x) + "_" + str(i)) if response.pixels_as_float: print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_float), pprint.pformat(response.camera_position))) - airsim.write_pfm(os.path.normpath('/temp/cv_mode_' + str(x) + "_" + str(i) + '.pfm'), airsim.get_pfm_array(response)) + airsim.write_pfm(os.path.normpath(filename + '.pfm'), airsim.get_pfm_array(response)) else: print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_uint8), pprint.pformat(response.camera_position))) - airsim.write_file(os.path.normpath('/temp/cv_mode_' + str(x) + "_" + str(i) + '.png'), response.image_data_uint8) + airsim.write_file(os.path.normpath(filename + '.png'), response.image_data_uint8) pose = client.simGetVehiclePose() pp.pprint(pose) diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs index 1a970a7227..d25900a587 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs @@ -150,7 +150,7 @@ private List GetFrameData(ImageType type, bool isCompress) { frameData.Add(c.r); frameData.Add(c.g); frameData.Add(c.b); - frameData.Add(c.a); + // frameData.Add(c.a); // Unreal is just sending RGB images, so don't include Alpha channel here } } RenderTexture.active = null;