[PythonClient/cv] Fix cv_mode.py

PythonClient/computer_vision/cv_mode.py

@@ -1,25 +1,36 @@
-# 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)
 
 client = airsim.VehicleClient()
 client.confirmConnection()
 
-airsim.wait_key('Press any key to set camera-0 gimble to 15-degree pitch')
-client.simSetCameraOrientation("0", airsim.to_quaternion(0.261799, 0, 0)); #radians
+airsim.wait_key('Press any key to set camera-0 gimbal to 15-degree pitch')
+client.simSetCameraOrientation("0", airsim.to_quaternion(math.radians(15), 0, 0)) #radians
 
 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
@@ -35,17 +46,18 @@
         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)
 
     time.sleep(3)
 
 # currently reset() doesn't work in CV mode. Below is the workaround
-client.simSetPose(airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True)
+client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True)