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'))