def image_callback(self, stack): # get uncompressed fpv cam image request = [ airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False) ] response = self.airsim_client_images.simGetImages(request) time.sleep(0.1) img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3) if self.start_time == 0: self.start_time = datetime.now() print(datetime.now() - self.start_time) drone_state = self.airsim_client_images.getMultirotorState() # in world frame: print(drone_state.kinematics_estimated.position) if stack.qsize() > 5: while not stack.empty(): try: stack.get(False) except queue.Empty: continue stack.task_done() stack.put(img_rgb)
def image_callback(self): #print("image callback") # get uncompressed fpv cam image request = [ airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False) ] response = self.airsim_client_images.simGetImages(request) img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3) path = "./picture_depth/picture_depth_" + str(self.day) + "_" + str( self.hour) + "_" + str(self.minute) + "/" if not (os.path.isdir(path)): os.makedirs(os.path.join(path)) if (self.count % 3 == 0): cv2.imwrite(path + str(self.count) + ".jpg", img_rgb) self.count += 1 self.gate_detection(img_rgb) if self.viz_image_cv2: cv2.imshow('Object detector', img_rgb) cv2.waitKey(1)
def image_callback(self): # get uncompressed fpv cam image request = [airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False)] response = self.airsim_client_images.simGetImages(request) img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3) if self.viz_image_cv2: cv2.imshow("img_rgb", img_rgb) cv2.waitKey(1)
def image_callback(self): #print("image callback") # get uncompressed fpv cam image #path = "./picture_mt/picture_mt_" + str(self.day) + "_" + str(self.hour) + "_" + str(self.minute) + "/" #if not(os.path.isdir(path)): # os.makedirs(os.path.join(path)) #if(self.count % 3 == 0): # cv2.imwrite(path+str(self.count) + ".jpg", img_rgb) #self.count += 1 if (self.next_gate_idx == 21): request = [ airsim.ImageRequest("bottom_cam", airsim.ImageType.Scene, False, False) ] response = self.airsim_client_images.simGetImages(request) img_rgb_1d_b = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb_b = img_rgb_1d_b.reshape(response[0].height, response[0].width, 3) self.gate_detection(img_rgb_b) cv2.imshow('Object detector', img_rgb_b) cv2.waitKey(1) else: request = [ airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False) ] response = self.airsim_client_images.simGetImages(request) img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3) self.gate_detection(img_rgb) # if self.viz_image_cv2: # kerne_gain = 15 # kernel = np.ones((kerne_gain, kerne_gain), np.uint8) # img_rgb = cv2.morphologyEx(img_rgb, cv2.MORPH_CLOSE, kernel) - cv2.morphologyEx(img_rgb, cv2.MORPH_OPEN, kernel) cv2.imshow('Object detector', img_rgb) cv2.waitKey(1)
def image_callback(self): # get uncompressed fpv cam image request = [ airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False) ] response = self.airsim_client_images.simGetImages(request) img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3) name = time.strftime("%Y%m%d-%H%M%S") + ".png" savePath = os.path.join('.', 'checkpoints\screenshots', name) cv2.imwrite(savePath, img_rgb) """if self.viz_image_cv2:
def image_callback_benchmark_simGetImages(self): self.image_benchmark_num_images += 1 iter_start_time = time.time() request = [ airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False) ] response = self.airsim_client_images.simGetImages(request) img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3) self.image_benchmark_total_time += time.time() - iter_start_time avg_fps = 1.0 / ((self.image_benchmark_total_time) / float(self.image_benchmark_num_images)) print(self.level_name + ": {} avg_fps for {} num of images".format( avg_fps, self.image_benchmark_num_images))
def image_callback(self): # get uncompressed fpv cam image request = [airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False)] response = self.airsim_client_images.simGetImages(request) img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3) ############################# path = "./picture_joy/" if self.viz_image_cv2: # cv2.imshow("img_rgb", img_rgb) if(self.count % 2 == 0): cv2.imwrite(path+str(self.count) + ".jpg", img_rgb) # cv2.waitKey(1) self.count += 1
def image_callback(self): # get uncompressed fpv cam image request = [ airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False) ] response = self.airsim_client_images.simGetImages(request) img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3) if self.viz_image_cv2: cv2.imshow("img_rgb", img_rgb) cv2.waitKey(1) img_g = cv2.cvtColor(img_rgb, cv2.COLOR_BGR2GRAY) self.Img_g = cv2.resize(img_g, (160, 120), interpolation=cv2.INTER_CUBIC) self.Img_rgb = cv2.resize(img_rgb, (64, 64), interpolation=cv2.INTER_CUBIC)
def image_callback(self): # get uncompressed fpv cam image # Scene = 0, # DepthPlanner = 1, # DepthPerspective = 2, # DepthVis = 3, # DisparityNormalized = 4, # Segmentation = 5, # SurfaceNormals = 6, # Infrared = 7 request = [ airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False) ] response = self.airsim_client_images.simGetImages(request) img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3) if self.viz_image_cv2: cv2.imshow("img_rgb", img_rgb) cv2.waitKey(1)
import airsimneurips as airsim import numpy as np import json client = airsim.MultirotorClient() client.confirmConnection() id2rgb = {} rgb2id = {} for id in range(256): client.simSetSegmentationObjectID("Mesh_soccerField_3", id) responses = client.simGetImages([ # floating point uncompressed image airsim.ImageRequest("fpv_cam", airsim.ImageType.Segmentation, pixels_as_float=False, compress=False), ]) seg_response = responses[0] seg_1d = np.frombuffer(seg_response.image_data_uint8, dtype=np.uint8) img_seg = seg_1d.reshape(seg_response.height, seg_response.width, 3) midpt = (int(seg_response.height / 2), int(seg_response.height / 2)) id_rgb = img_seg[int(seg_response.height / 2), int(seg_response.height / 2), :].astype(int).tolist() id2rgb[id] = id_rgb rgb2id[(id_rgb[0], id_rgb[1], id_rgb[2])] = id for id in range(256): test_rgb = id2rgb[id]
def take_images_dataset(self): # get uncompressed fpv cam image # Scene = 0, # DepthPlanner = 1, # DepthPerspective = 2, # DepthVis = 3, # DisparityNormalized = 4, # Segmentation = 5, # SurfaceNormals = 6, # Infrared = 7 img_rgb_cam = {} img_seg_rgb_cam = {} img_seg_id_cam = {} img_depth_cam = {} images_dict = {} for camera in self.cameras: # Take images from fpv_cam, back, starboard, port, bottom # RGB, Segmentation, Depth responses = self.client.simGetImages([ # uncompressed RGBA array bytes airsim.ImageRequest(camera, airsim.ImageType.Scene, pixels_as_float=False, compress=False), # floating point uncompressed image airsim.ImageRequest(camera, airsim.ImageType.Segmentation, pixels_as_float=False, compress=False), # Depth airsim.ImageRequest(camera, airsim.ImageType.DepthPerspective, pixels_as_float=True, compress=False), ]) rgb_response = responses[0] img_rgb_1d = np.frombuffer(rgb_response.image_data_uint8, dtype=np.uint8) img_rgb = img_rgb_1d.reshape(rgb_response.height, rgb_response.width, 3) img_rgb_cam[camera] = img_rgb seg_response = responses[1] seg_1d = np.frombuffer(seg_response.image_data_uint8, dtype=np.uint8) img_seg = seg_1d.reshape(seg_response.height, seg_response.width, 3) img_seg_rgb_cam[camera] = img_seg img_seg_id_cam[camera] = rgb2id_seg_img(img_seg, self.rgb2id) depth_response = responses[2] depth_1d = np.array(depth_response.image_data_float) img_depth = depth_1d.reshape(depth_response.height, depth_response.width) if self.max_depth: img_depth[img_depth > self.max_depth] = self.max_depth img_depth_cam[camera] = img_depth images_dict['rgb'] = img_rgb_cam images_dict['seg_rgb'] = img_seg_rgb_cam images_dict['seg_id'] = img_seg_id_cam images_dict['depth'] = img_depth_cam return images_dict
def run(self): self.get_ground_truth_gate_poses( ) # Tier 2 gives noisy prior of gate poses mu_1_airsimvector = self.gate_poses_ground_truth[0].position mu_1 = np.reshape( np.array([ mu_1_airsimvector.x_val, mu_1_airsimvector.y_val, mu_1_airsimvector.z_val ]), (3, 1)) sigma_1 = 50 * np.identity(3) sigma_2 = 50 * np.identity(3) while self.airsim_client.isApiControlEnabled( vehicle_name=self.drone_name): # Take image response = self.airsim_client.simGetImages([ airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False) ]) img1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) image_rgb = img1d.reshape(response[0].height, response[0].width, 3) # Get quad state when the image was taken state = self.airsim_client.simGetVehiclePose() rot_matrix_quad2global = self.get_rotation_matrix_quad_frame_to_global_frame( state) # Get gate mask mask1 = cv2.inRange(image_rgb, self.lower_green1, self.upper_green1) mask2 = cv2.inRange(image_rgb, self.lower_green2, self.upper_green2) mask3 = cv2.inRange(image_rgb, self.lower_green3, self.upper_green3) mask4 = cv2.inRange(image_rgb, self.lower_green4, self.upper_green4) mask = mask1 + mask2 + mask3 + mask4 dilated_gate = cv2.dilate(mask, self.kernel, iterations=8) gate_contours = None gate_contours, ____ = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) cv2.imshow("mask", mask) cv2.imshow("dilated_gate", dilated_gate) cv2.waitKey(1) gate_corners = None smallest_measurement_error = 100 # find four gate corners that give the smallest error between the gate center and mean # check each contour for contour in gate_contours: gate_contour_epsilon = 0.01 * cv2.arcLength(contour, True) gate_contour_approx = cv2.approxPolyDP(contour, gate_contour_epsilon, True) gate_corners = cv2.convexHull(gate_contour_approx) gate_corner_combinations = [None] gate_corner_combinations = list(combinations(gate_corners, 4)) # check for each combination of four gate corner points within each contour for comb in gate_corner_combinations: four_gate_corners = np.empty((0, 2)) for i in comb: i = np.reshape(i, (1, 2)) four_gate_corners = np.append(four_gate_corners, i, axis=0) measurement_error, waypoint_glob_1, waypoint_glob_2, gate_center_pixel, gate_corners_plot = self.find_measurement_error( four_gate_corners, mu_1, state, rot_matrix_quad2global) aspect_ratio_too_large = self.find_aspect_ratio( four_gate_corners) if measurement_error < smallest_measurement_error and aspect_ratio_too_large == False: waypoint_1_best = copy.deepcopy(waypoint_glob_1) waypoint_2_best = copy.deepcopy(waypoint_glob_2) smallest_measurement_error = copy.deepcopy( measurement_error) gate_center_pixel_best = copy.deepcopy( gate_center_pixel) gate_corners_plot_best = copy.deepcopy( gate_corners_plot) next_gate_position_dist = abs( np.linalg.norm( np.reshape( np.array([ state.position.x_val, state.position.y_val, state.position.z_val ]), (3, 1)) - mu_1)) self.moveonspline_count += 1 # If the smallest error if large, then continue toward the best estimate of the gate location if smallest_measurement_error > 5.0: self.no_gate_count += 1 print("Gate NOT detected") if next_gate_position_dist > 30.0: vmax = 10.0 amax = 3.0 no_gate_count_max = 15 else: vmax = 5.0 amax = 3.0 no_gate_count_max = 3 if self.no_gate_count == no_gate_count_max and next_gate_position_dist >= self.largest_next_gate_position_dist: self.no_gate_count = 0 self.too_close_count = 0 print("Move toward best estimate", next_gate_position_dist) self.airsim_client.moveOnSplineAsync( [airsim.Vector3r(mu_1[0, 0], mu_1[1, 0], mu_1[2, 0])], vel_max=vmax, acc_max=amax, add_position_constraint=True, add_velocity_constraint=True, add_acceleration_constraint=True, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=False, replan_lookahead_sec=1.0) # else, input the measurement to the Kalman Filter and proceed to the updated estimate else: self.no_gate_count = 0 self.measurement_count += 1 if self.measurement_count == 1: mu_2 = np.reshape(waypoint_2_best, (3, 1)) #initialize mu_2 # filter both waypoints mu_1, sigma_1 = self.kalman_filter( mu_1, sigma_1, np.reshape(waypoint_1_best, (3, 1))) mu_2, sigma_2 = self.kalman_filter( mu_2, sigma_2, np.reshape(waypoint_2_best, (3, 1))) waypoint_1 = airsim.Vector3r(mu_1[0, 0], mu_1[1, 0], mu_1[2, 0]) waypoint_2 = airsim.Vector3r(mu_2[0, 0], mu_2[1, 0], mu_2[2, 0]) if next_gate_position_dist > self.largest_next_gate_position_dist: print("Gate detected, Measurement Taken") print(self.measurement_count) self.too_close_count = 0 #cv2.circle(image_rgb, (int(gate_center_pixel_best[0]), int(gate_center_pixel_best[1])), 10, (255, 0, 0), -1) cv2.circle(image_rgb, (int(gate_corners_plot_best[0][0]), int(gate_corners_plot_best[0][1])), 10, (255, 100, 0), -1) cv2.circle(image_rgb, (int(gate_corners_plot_best[1][0]), int(gate_corners_plot_best[1][1])), 10, (255, 0, 100), -1) cv2.circle(image_rgb, (int(gate_corners_plot_best[2][0]), int(gate_corners_plot_best[2][1])), 10, (255, 200, 0), -1) cv2.circle(image_rgb, (int(gate_corners_plot_best[3][0]), int(gate_corners_plot_best[3][1])), 10, (255, 0, 200), -1) cv2.imshow("image_rgb", image_rgb) cv2.waitKey(1) if self.moveonspline_count >= 5: if self.next_gate_idx >= len( self.gate_poses_ground_truth) - 1: self.next_gate_idx = -1 self.airsim_client.moveOnSplineAsync( [ waypoint_1, waypoint_2, self.gate_poses_ground_truth[self.next_gate_idx + 1].position ], vel_max=7.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=True, add_acceleration_constraint=True, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=False, replan_lookahead_sec=1.0) self.moveonspline_count = 0 # If too close to the gate don't collect measurements, continue to waypoint, reset filter with new mean and covariance if next_gate_position_dist < self.largest_next_gate_position_dist: print("Gate too close") time.sleep(2) self.no_gate_count = 0 self.too_close_count += 1 if self.too_close_count == 1: print("reset gate too close") self.next_gate_idx += 1 if self.next_gate_idx >= len(self.gate_poses_ground_truth): self.next_gate_idx = 0 print("race end") mu_1_airsimvector = self.gate_poses_ground_truth[ self.next_gate_idx].position mu_1 = np.reshape( np.array([ mu_1_airsimvector.x_val, mu_1_airsimvector.y_val, mu_1_airsimvector.z_val ]), (3, 1)) sigma_1 = 50 * np.identity(3) sigma_2 = 50 * np.identity(3) self.measurement_count = 0