From 0b4d6573d1a63e79290704bd5be5500f6a97b3d9 Mon Sep 17 00:00:00 2001 From: madratman Date: Tue, 13 Aug 2019 09:50:29 -0700 Subject: [PATCH 01/27] [baselines/perception] homography baseline on 480x640 working on soccer field easy Co-authored-by: Keiko Co-authored-by: madratman --- baselines/baseline_racer_perception.py | 252 +++++++++++++++++++++++++ 1 file changed, 252 insertions(+) create mode 100644 baselines/baseline_racer_perception.py diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py new file mode 100644 index 0000000..06d207a --- /dev/null +++ b/baselines/baseline_racer_perception.py @@ -0,0 +1,252 @@ +from itertools import combinations +from imutils.perspective import order_points + +import airsimneurips as airsim +import numpy as np +import time +import cv2 +import copy +import math + +# Find area of polygon +def find_poly_area(x,y): + return 0.5*np.abs(np.dot(x,np.roll(y,1))-np.dot(y,np.roll(x,1))) + +# Find four gate corners given more than four points +# use the four points that produce the largest area +def find_gate_corners(hull_comp): + hull_comp_new = hull_comp.reshape(len(hull_comp),2) + all_combs = list(combinations(hull_comp_new,4)) + comb_count_out = 0 + largest_area = 0.0 + for comb in all_combs: + comb_count_out += 1 + comb_count = 0 + for i in comb: + comb_count += 1 + if comb_count == 1: + comb_array = i + else: + comb_array = np.vstack((comb_array,i)) + poly_area = find_poly_area(comb_array[:,0],comb_array[:,1]) + if poly_area > largest_area: + largest_area = copy.deepcopy(poly_area) + comb_array_largest = copy.deepcopy(comb_array) + return comb_array_largest + +# Find the highest aspect ratio of the gate +# if aspect ratio is too high, a gate side may have been detected instead of a full gate +def find_aspect_ratio(hull_comp): + aspect_ratio_mtx = np.empty((1,4)) + aspect_ratio_mtx[0,0] = abs(np.float32(hull_comp[1][0][1] - hull_comp[2][0][1])/np.float32(hull_comp[0][0][0] - hull_comp[1][0][0]+eps)) + aspect_ratio_mtx[0,1] = abs(np.float32(hull_comp[0][0][1] - hull_comp[3][0][1])/np.float32(hull_comp[2][0][0] - hull_comp[3][0][0]+eps)) + aspect_ratio_mtx[0,2] = abs(np.float32(hull_comp[0][0][1] - hull_comp[3][0][1])/np.float32(hull_comp[0][0][0] - hull_comp[1][0][0]+eps)) + aspect_ratio_mtx[0,3] = abs(np.float32(hull_comp[1][0][1] - hull_comp[2][0][1])/np.float32(hull_comp[2][0][0] - hull_comp[3][0][0]+eps)) + large_aspect_ratio = 1.0 + for i in range(4): + if aspect_ratio_mtx[0,i] < 1.0: + aspect_ratio_mtx[0,i] = 1/aspect_ratio_mtx[0,i] + if aspect_ratio_mtx[0,i] > 1.2: + large_aspect_ratio = aspect_ratio_mtx[0,i] + return large_aspect_ratio + +img_count = 1 +no_gate_count = 0 +eps = 0.01 +waypoint_ave_2 = np.zeros((1,3)) +last_good_waypoint = airsim.Vector3r(0,0,0) +kernel = np.ones((3,3),np.uint8) +#dst_pts = np.float32([[[240, 149],[399, 149],[399, 308], [240, 308]]]) +dst_pts = np.float32([[[200, 132],[412, 132],[412, 346], [200, 346]]]) + +# connect to airsim +drone_name = "drone_1" # should match settings.json +client = airsim.MultirotorClient() +client.confirmConnection() +client.simLoadLevel('Soccer_Field_Easy') +time.sleep(2.0) +client.enableApiControl(vehicle_name=drone_name) +client.arm(vehicle_name=drone_name) +client.setTrajectoryTrackerGains(airsim.TrajectoryTrackerGains().to_list(), vehicle_name=drone_name) +client.moveOnSplineAsync([airsim.Vector3r(0, 0, -0.3)], vel_max=5.0, acc_max=3.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=False, viz_traj=True, vehicle_name=drone_name).join() + +ctr = 0 +while client.isApiControlEnabled(vehicle_name=drone_name) == True: + img_count += 1 + + # Move through first gate + if img_count == 2: + gate_names_sorted_bad = sorted(client.simListSceneObjects("Gate.*")) + gate_indices_bad = [int(gate_name.split('_')[0][4:]) for gate_name in gate_names_sorted_bad] + gate_indices_correct = sorted(range(len(gate_indices_bad)), key=lambda k:gate_indices_bad[k]) + gate_names = [gate_names_sorted_bad[gate_idx] for gate_idx in gate_indices_correct] + gate_poses_rel = [client.simGetObjectPose(gate_name).position for gate_name in gate_names] + waypoint_next = gate_poses_rel[0] + # client.plot_tf([airsim.Pose(position_val=waypoint_next, orientation_val=airsim.Quaternionr())], duration=3.0, vehicle_name=drone_name) + client.moveOnSplineAsync([waypoint_next], vel_max = 2.0, acc_max = 5.0, viz_traj=True, vehicle_name=drone_name).join() + continue + + # Take image + response = client.simGetImages([airsim.ImageRequest("0", airsim.ImageType.Scene, False, False)]) + img1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) # get numpy array + image_rgb = img1d.reshape(response[0].height, response[0].width, 3) + + # Get quad state when the image was taken + state = client.simGetVehiclePose() + q0 = state.orientation.w_val + q1 = state.orientation.x_val + q2 = state.orientation.y_val + q3 = state.orientation.z_val + + # rotation/translation matrix between quad body frame and global frame + rt_mtx_quad2global = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3-q0*q2)], + [2*(q1*q2 + q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], + [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) + + # Process image + # Find greenest pixels (pixels with gate) + lower_green = np.array([0, 210, 0]) + upper_green = np.array([200, 255, 200]) + # Find corners of the gate + mask = cv2.inRange(image_rgb,lower_green,upper_green) + dilated_comp = cv2.dilate(mask,kernel, iterations=8) + eroded_comp = cv2.erode(dilated_comp,kernel, iterations=8) + # im_comp, contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + # im_comp, contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) + cv2.imshow("mask", mask) + cv2.imshow("dilated_comp", dilated_comp) + cv2.imshow("eroded_comp", eroded_comp) + #cv2.imshow("im_comp", im_comp) + cv2.waitKey(1) + largest_area = 0.0 + hull_comp = None + hull_comp_best = np.zeros((1,3)) + for c in contours_comp: + # print("area", cv2.contourArea(c)) + if cv2.contourArea(c) > largest_area: + epsilon_comp = 0.01 * cv2.arcLength(c, True) + approx_comp = cv2.approxPolyDP(c,epsilon_comp,True) + hull_comp = cv2.convexHull(approx_comp) + # check if there are more than four points given; if there are find four points that produce largest area polygon + if len(hull_comp)>4: + hull_comp = find_gate_corners(hull_comp) + hull_comp = hull_comp.astype(int).reshape(4,1,2) + if len(hull_comp)>=4: + aspect_ratio = find_aspect_ratio(hull_comp) + if aspect_ratio <= 1.2: + largest_area = cv2.contourArea(c) + gate_contour_comp = c + hull_comp_best = hull_comp + + # MOVE + if largest_area < 3500 or len(hull_comp_best)!=4: + no_gate_count += 1 + print("Gate NOT detected") + # If no gate has been detected for over 25 attempts, move to the last good waypoint + if no_gate_count > 25: + client.moveOnSplineAsync([last_good_waypoint + airsim.Vector3r(np.random.uniform(low=-0.25,high=0.25,size=1)[0], + np.random.uniform(low=-0.25,high=0.25,size=1)[0], + np.random.uniform(low=-0.25,high=0.25,size=1)[0])], + vel_max = 10.0, acc_max = 5.0, viz_traj=True, vehicle_name=drone_name).join() # move to last good waypoint + no_gate_count = 0 + # Rotate counterclockwise until gate has been found + else: + client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.5, vehicle_name=drone_name).join() # rotate counter clockwise to look for next gate + else: + # gate corner points + src_pts = order_points(hull_comp_best.reshape(4,2)) + gate_corners_plot = src_pts + src_pts = np.float32(src_pts).reshape(-1,1,2) + + # find homography matrix between gate corner points in baseline image and corner points in image taken + M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0) + + # find middle of gate pixel coordinates + center_pixel_0 = np.array([306,239,1]).T + center_pixel_comp = np.dot(np.linalg.inv(M), center_pixel_0) + center_pixel_comp_norm = (int(center_pixel_comp[0]/(center_pixel_comp[2]+eps)), int(center_pixel_comp[1]/(center_pixel_comp[2]+eps))) + + print("center_pixel_comp", center_pixel_comp[0], center_pixel_comp[1], center_pixel_comp[2]) + print("center_pixel_comp_norm", center_pixel_comp_norm[0], center_pixel_comp_norm[1]) + cv2.circle(image_rgb, (int(center_pixel_comp_norm[0]), int(center_pixel_comp_norm[1])), 10, (255, 0, 0), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[0][0]), int(gate_corners_plot[0][1])), 10, (255, 100, 0), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[1][0]), int(gate_corners_plot[1][1])), 10, (255, 0, 100), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[2][0]), int(gate_corners_plot[2][1])), 10, (255, 200, 0), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[3][0]), int(gate_corners_plot[3][1])), 10, (255, 0, 200), -1) + cv2.imshow("image_rgb", image_rgb) + ctr+=1 + cv2.waitKey(1) + + # find 3D coordinate relative to quad + # camera matrix + mtx = np.array([[320.000, 0.000000, 320.000], + [0.000000, 320.000, 240.000], + [0.000000, 0.000000, 1.000000]]) + dist_coeff = np.zeros((1,5)) + + # coordinates of three waypoints in gate centered frame + waypoint_gate_1 = np.array([0.0,0.0,+1.0,1.0]).T.reshape(4,1) + waypoint_gate_2 = np.array([0.0,0.0,0.0,1.0]).T.reshape(4,1) + waypoint_gate_3 = np.array([0,0.0,-2.0,1.0]).T.reshape(4,1) + + # coordinates of gate corners in gate centered frame + objpoints = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) + + # coordinates of gate corners in image (pixel coordinates) + imgpoints = np.concatenate((np.array(center_pixel_comp_norm).reshape(-1,1,2),src_pts),axis=0) + imgpoints = np.float32(imgpoints) + + # find rotation/translation matrix between image to gate centered frame + ret, rvecs, tvecs = cv2.solvePnP(objpoints,imgpoints,mtx,dist_coeff) + rvec_full, jac1 = cv2.Rodrigues(rvecs) + rt_mtx_gate2quad = np.concatenate((rvec_full, tvecs), axis=1) + rt_mtx_gate2quad = np.concatenate((rt_mtx_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) + + # relative location of gate from quad to global location of gate + waypoint_rel_1 = np.dot(rt_mtx_gate2quad, waypoint_gate_1) + waypoint_rel_1 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_1[2], waypoint_rel_1[0], -waypoint_rel_1[1]])) + + waypoint_rel_2 = np.dot(rt_mtx_gate2quad, waypoint_gate_2) + waypoint_rel_2 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_2[2], waypoint_rel_2[0], -waypoint_rel_2[1]])) + + waypoint_rel_3 = np.dot(rt_mtx_gate2quad,waypoint_gate_3) + waypoint_rel_3 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_3[2], waypoint_rel_3[0], -waypoint_rel_3[1]])) + + waypoint_glob_1 = np.reshape(np.array([state.position.x_val + waypoint_rel_1[0], state.position.y_val + waypoint_rel_1[1], state.position.z_val - waypoint_rel_1[2]]),(1,3)) + waypoint_glob_2 = np.reshape(np.array([state.position.x_val + waypoint_rel_2[0], state.position.y_val + waypoint_rel_2[1], state.position.z_val - waypoint_rel_2[2]]),(1,3)) + waypoint_glob_3 = np.reshape(np.array([state.position.x_val + waypoint_rel_3[0], state.position.y_val + waypoint_rel_3[1], state.position.z_val - waypoint_rel_3[2]]),(1,3)) + + # check if your new measurement is far from your previous measurements + norm_dist_error = abs(np.linalg.norm(waypoint_glob_2) - np.linalg.norm(waypoint_ave_2)) + if norm_dist_error > 0.5: #reset if your new measurement is too far from average waypoint + measurement_count = 0 + + # calculate moving average of gate waypoints + measurement_count += 1 + print("Gate detected, Measurement Taken") + print(measurement_count) + if measurement_count == 1: + measurement_estimates_mtx_1 = np.reshape(waypoint_glob_1,(1,3)) + measurement_estimates_mtx_2 = np.reshape(waypoint_glob_2,(1,3)) + measurement_estimates_mtx_3 = np.reshape(waypoint_glob_3,(1,3)) + waypoint_ave_1 = np.reshape(waypoint_glob_1,(1,3)) + waypoint_ave_2 = np.reshape(waypoint_glob_2,(1,3)) + waypoint_ave_3 = np.reshape(waypoint_glob_3,(1,3)) + else: + measurement_estimates_mtx_1 = np.append(measurement_estimates_mtx_1, waypoint_glob_1,axis=0) + measurement_estimates_mtx_2 = np.append(measurement_estimates_mtx_2, waypoint_glob_2,axis=0) + measurement_estimates_mtx_3 = np.append(measurement_estimates_mtx_3, waypoint_glob_3,axis=0) + waypoint_ave_1 = np.reshape(np.mean(measurement_estimates_mtx_1,axis=0),(1,3)) + waypoint_ave_2 = np.reshape(np.mean(measurement_estimates_mtx_2,axis=0),(1,3)) + waypoint_ave_3 = np.reshape(np.mean(measurement_estimates_mtx_3,axis=0),(1,3)) + + # three waypoints + waypoint_1 = airsim.Vector3r(waypoint_ave_1[0,0],waypoint_ave_1[0,1], waypoint_ave_1[0,2]) + waypoint_2 = airsim.Vector3r(waypoint_ave_2[0,0],waypoint_ave_2[0,1], waypoint_ave_2[0,2]) + waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) + + no_gate_count = 0 + + client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 5.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=False, viz_traj=True, vehicle_name=drone_name).join() + last_good_waypoint = copy.deepcopy(waypoint_3) From d0ae0a32a109a87638cb463b66340bf58a8f6564 Mon Sep 17 00:00:00 2001 From: madratman Date: Tue, 13 Aug 2019 09:51:41 -0700 Subject: [PATCH 02/27] [baselines/perception] port code to a derived class of baseline racer --- baselines/baseline_racer_perception.py | 491 ++++++++++++------------- 1 file changed, 245 insertions(+), 246 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 06d207a..f8a338e 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -1,252 +1,251 @@ -from itertools import combinations +from argparse import ArgumentParser +from baseline_racer import BaselineRacer from imutils.perspective import order_points - +from itertools import combinations import airsimneurips as airsim -import numpy as np -import time -import cv2 import copy +import cv2 import math +import numpy as np +import time -# Find area of polygon -def find_poly_area(x,y): - return 0.5*np.abs(np.dot(x,np.roll(y,1))-np.dot(y,np.roll(x,1))) - -# Find four gate corners given more than four points -# use the four points that produce the largest area -def find_gate_corners(hull_comp): - hull_comp_new = hull_comp.reshape(len(hull_comp),2) - all_combs = list(combinations(hull_comp_new,4)) - comb_count_out = 0 - largest_area = 0.0 - for comb in all_combs: - comb_count_out += 1 - comb_count = 0 - for i in comb: - comb_count += 1 - if comb_count == 1: - comb_array = i +class BaselineRacerPerception(BaselineRacer): + def __init__(self, drone_name = "drone_1", plot_transform=True, viz_traj=True): + BaselineRacer.__init__(self, drone_name, plot_transform, viz_traj) + self.no_gate_count = 0 + self.eps = 0.01 + self.waypoint_ave_2 = np.zeros((1,3)) + self.last_good_waypoint = airsim.Vector3r(0,0,0) + self.kernel = np.ones((3,3),np.uint8) + self.dst_pts = np.float32([[[200, 132],[412, 132],[412, 346], [200, 346]]]) + + # Find area of polygon + def find_poly_area(self, x ,y): + return 0.5*np.abs(np.dot(x,np.roll(y,1))-np.dot(y,np.roll(x,1))) + + # Find four gate corners given more than four points + # use the four points that produce the largest area + def find_gate_corners(self, hull_comp): + hull_comp_new = hull_comp.reshape(len(hull_comp),2) + all_combs = list(combinations(hull_comp_new,4)) + comb_count_out = 0 + largest_area = 0.0 + for comb in all_combs: + comb_count_out += 1 + comb_count = 0 + for i in comb: + comb_count += 1 + if comb_count == 1: + comb_array = i + else: + comb_array = np.vstack((comb_array,i)) + poly_area = self.find_poly_area(comb_array[:,0], comb_array[:,1]) + if poly_area > largest_area: + largest_area = copy.deepcopy(poly_area) + comb_array_largest = copy.deepcopy(comb_array) + return comb_array_largest + + # Find the highest aspect ratio of the gate + # if aspect ratio is too high, a gate side may have been detected instead of a full gate + def find_aspect_ratio(self, hull_comp): + aspect_ratio_mtx = np.empty((1,4)) + aspect_ratio_mtx[0,0] = abs(np.float32(hull_comp[1][0][1] - hull_comp[2][0][1])/np.float32(hull_comp[0][0][0] - hull_comp[1][0][0]+self.eps)) + aspect_ratio_mtx[0,1] = abs(np.float32(hull_comp[0][0][1] - hull_comp[3][0][1])/np.float32(hull_comp[2][0][0] - hull_comp[3][0][0]+self.eps)) + aspect_ratio_mtx[0,2] = abs(np.float32(hull_comp[0][0][1] - hull_comp[3][0][1])/np.float32(hull_comp[0][0][0] - hull_comp[1][0][0]+self.eps)) + aspect_ratio_mtx[0,3] = abs(np.float32(hull_comp[1][0][1] - hull_comp[2][0][1])/np.float32(hull_comp[2][0][0] - hull_comp[3][0][0]+self.eps)) + large_aspect_ratio = 1.0 + for i in range(4): + if aspect_ratio_mtx[0,i] < 1.0: + aspect_ratio_mtx[0,i] = 1/aspect_ratio_mtx[0,i] + if aspect_ratio_mtx[0,i] > 1.2: + large_aspect_ratio = aspect_ratio_mtx[0,i] + return large_aspect_ratio + + + def run(self): + # Move through first gate + self.get_ground_truth_gate_poses() + self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) + self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max = 2.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() + + 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) # get numpy array + image_rgb = img1d.reshape(response[0].height, response[0].width, 3) + + # Get quad state when the image was taken + state = self.airsim_client.simGetVehiclePose() + q0 = state.orientation.w_val + q1 = state.orientation.x_val + q2 = state.orientation.y_val + q3 = state.orientation.z_val + + # rotation/translation matrix between quad body frame and global frame + rt_mtx_quad2global = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3-q0*q2)], + [2*(q1*q2 + q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], + [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) + + # Process image + # Find greenest pixels (pixels with gate) + lower_green = np.array([0, 210, 0]) + upper_green = np.array([200, 255, 200]) + # Find corners of the gate + mask = cv2.inRange(image_rgb,lower_green,upper_green) + dilated_comp = cv2.dilate(mask,self.kernel, iterations=8) + eroded_comp = cv2.erode(dilated_comp,self.kernel, iterations=8) + im_comp, contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + # contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + # im_comp, contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) + cv2.imshow("mask", mask) + cv2.imshow("dilated_comp", dilated_comp) + cv2.imshow("eroded_comp", eroded_comp) + #cv2.imshow("im_comp", im_comp) + cv2.waitKey(1) + largest_area = 0.0 + hull_comp = None + hull_comp_best = np.zeros((1,3)) + for c in contours_comp: + # print("area", cv2.contourArea(c)) + if cv2.contourArea(c) > largest_area: + self.epsilon_comp = 0.01 * cv2.arcLength(c, True) + approx_comp = cv2.approxPolyDP(c,self.epsilon_comp,True) + hull_comp = cv2.convexHull(approx_comp) + # check if there are more than four points given; if there are find four points that produce largest area polygon + if len(hull_comp)>4: + hull_comp = self.find_gate_corners(hull_comp) + hull_comp = hull_comp.astype(int).reshape(4,1,2) + if len(hull_comp)>=4: + aspect_ratio = self.find_aspect_ratio(hull_comp) + if aspect_ratio <= 1.2: + largest_area = cv2.contourArea(c) + gate_contour_comp = c + hull_comp_best = hull_comp + + # MOVE + if largest_area < 3500 or len(hull_comp_best)!=4: + self.no_gate_count += 1 + print("Gate NOT detected") + # If no gate has been detected for over 25 attempts, move to the last good waypoint + if self.no_gate_count > 25: + self.airsim_client.moveOnSplineAsync([self.last_good_waypoint + airsim.Vector3r(np.random.uniform(low=-0.25,high=0.25,size=1)[0], + np.random.uniform(low=-0.25,high=0.25,size=1)[0], + np.random.uniform(low=-0.25,high=0.25,size=1)[0])], + vel_max = 10.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() # move to last good waypoint + self.no_gate_count = 0 + # Rotate counterclockwise until gate has been found + else: + self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.5, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate else: - comb_array = np.vstack((comb_array,i)) - poly_area = find_poly_area(comb_array[:,0],comb_array[:,1]) - if poly_area > largest_area: - largest_area = copy.deepcopy(poly_area) - comb_array_largest = copy.deepcopy(comb_array) - return comb_array_largest - -# Find the highest aspect ratio of the gate -# if aspect ratio is too high, a gate side may have been detected instead of a full gate -def find_aspect_ratio(hull_comp): - aspect_ratio_mtx = np.empty((1,4)) - aspect_ratio_mtx[0,0] = abs(np.float32(hull_comp[1][0][1] - hull_comp[2][0][1])/np.float32(hull_comp[0][0][0] - hull_comp[1][0][0]+eps)) - aspect_ratio_mtx[0,1] = abs(np.float32(hull_comp[0][0][1] - hull_comp[3][0][1])/np.float32(hull_comp[2][0][0] - hull_comp[3][0][0]+eps)) - aspect_ratio_mtx[0,2] = abs(np.float32(hull_comp[0][0][1] - hull_comp[3][0][1])/np.float32(hull_comp[0][0][0] - hull_comp[1][0][0]+eps)) - aspect_ratio_mtx[0,3] = abs(np.float32(hull_comp[1][0][1] - hull_comp[2][0][1])/np.float32(hull_comp[2][0][0] - hull_comp[3][0][0]+eps)) - large_aspect_ratio = 1.0 - for i in range(4): - if aspect_ratio_mtx[0,i] < 1.0: - aspect_ratio_mtx[0,i] = 1/aspect_ratio_mtx[0,i] - if aspect_ratio_mtx[0,i] > 1.2: - large_aspect_ratio = aspect_ratio_mtx[0,i] - return large_aspect_ratio - -img_count = 1 -no_gate_count = 0 -eps = 0.01 -waypoint_ave_2 = np.zeros((1,3)) -last_good_waypoint = airsim.Vector3r(0,0,0) -kernel = np.ones((3,3),np.uint8) -#dst_pts = np.float32([[[240, 149],[399, 149],[399, 308], [240, 308]]]) -dst_pts = np.float32([[[200, 132],[412, 132],[412, 346], [200, 346]]]) - -# connect to airsim -drone_name = "drone_1" # should match settings.json -client = airsim.MultirotorClient() -client.confirmConnection() -client.simLoadLevel('Soccer_Field_Easy') -time.sleep(2.0) -client.enableApiControl(vehicle_name=drone_name) -client.arm(vehicle_name=drone_name) -client.setTrajectoryTrackerGains(airsim.TrajectoryTrackerGains().to_list(), vehicle_name=drone_name) -client.moveOnSplineAsync([airsim.Vector3r(0, 0, -0.3)], vel_max=5.0, acc_max=3.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=False, viz_traj=True, vehicle_name=drone_name).join() - -ctr = 0 -while client.isApiControlEnabled(vehicle_name=drone_name) == True: - img_count += 1 - - # Move through first gate - if img_count == 2: - gate_names_sorted_bad = sorted(client.simListSceneObjects("Gate.*")) - gate_indices_bad = [int(gate_name.split('_')[0][4:]) for gate_name in gate_names_sorted_bad] - gate_indices_correct = sorted(range(len(gate_indices_bad)), key=lambda k:gate_indices_bad[k]) - gate_names = [gate_names_sorted_bad[gate_idx] for gate_idx in gate_indices_correct] - gate_poses_rel = [client.simGetObjectPose(gate_name).position for gate_name in gate_names] - waypoint_next = gate_poses_rel[0] - # client.plot_tf([airsim.Pose(position_val=waypoint_next, orientation_val=airsim.Quaternionr())], duration=3.0, vehicle_name=drone_name) - client.moveOnSplineAsync([waypoint_next], vel_max = 2.0, acc_max = 5.0, viz_traj=True, vehicle_name=drone_name).join() - continue - - # Take image - response = client.simGetImages([airsim.ImageRequest("0", airsim.ImageType.Scene, False, False)]) - img1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8) # get numpy array - image_rgb = img1d.reshape(response[0].height, response[0].width, 3) - - # Get quad state when the image was taken - state = client.simGetVehiclePose() - q0 = state.orientation.w_val - q1 = state.orientation.x_val - q2 = state.orientation.y_val - q3 = state.orientation.z_val - - # rotation/translation matrix between quad body frame and global frame - rt_mtx_quad2global = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3-q0*q2)], - [2*(q1*q2 + q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], - [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) - - # Process image - # Find greenest pixels (pixels with gate) - lower_green = np.array([0, 210, 0]) - upper_green = np.array([200, 255, 200]) - # Find corners of the gate - mask = cv2.inRange(image_rgb,lower_green,upper_green) - dilated_comp = cv2.dilate(mask,kernel, iterations=8) - eroded_comp = cv2.erode(dilated_comp,kernel, iterations=8) - # im_comp, contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - # im_comp, contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) - cv2.imshow("mask", mask) - cv2.imshow("dilated_comp", dilated_comp) - cv2.imshow("eroded_comp", eroded_comp) - #cv2.imshow("im_comp", im_comp) - cv2.waitKey(1) - largest_area = 0.0 - hull_comp = None - hull_comp_best = np.zeros((1,3)) - for c in contours_comp: - # print("area", cv2.contourArea(c)) - if cv2.contourArea(c) > largest_area: - epsilon_comp = 0.01 * cv2.arcLength(c, True) - approx_comp = cv2.approxPolyDP(c,epsilon_comp,True) - hull_comp = cv2.convexHull(approx_comp) - # check if there are more than four points given; if there are find four points that produce largest area polygon - if len(hull_comp)>4: - hull_comp = find_gate_corners(hull_comp) - hull_comp = hull_comp.astype(int).reshape(4,1,2) - if len(hull_comp)>=4: - aspect_ratio = find_aspect_ratio(hull_comp) - if aspect_ratio <= 1.2: - largest_area = cv2.contourArea(c) - gate_contour_comp = c - hull_comp_best = hull_comp - - # MOVE - if largest_area < 3500 or len(hull_comp_best)!=4: - no_gate_count += 1 - print("Gate NOT detected") - # If no gate has been detected for over 25 attempts, move to the last good waypoint - if no_gate_count > 25: - client.moveOnSplineAsync([last_good_waypoint + airsim.Vector3r(np.random.uniform(low=-0.25,high=0.25,size=1)[0], - np.random.uniform(low=-0.25,high=0.25,size=1)[0], - np.random.uniform(low=-0.25,high=0.25,size=1)[0])], - vel_max = 10.0, acc_max = 5.0, viz_traj=True, vehicle_name=drone_name).join() # move to last good waypoint - no_gate_count = 0 - # Rotate counterclockwise until gate has been found - else: - client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.5, vehicle_name=drone_name).join() # rotate counter clockwise to look for next gate - else: - # gate corner points - src_pts = order_points(hull_comp_best.reshape(4,2)) - gate_corners_plot = src_pts - src_pts = np.float32(src_pts).reshape(-1,1,2) - - # find homography matrix between gate corner points in baseline image and corner points in image taken - M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0) - - # find middle of gate pixel coordinates - center_pixel_0 = np.array([306,239,1]).T - center_pixel_comp = np.dot(np.linalg.inv(M), center_pixel_0) - center_pixel_comp_norm = (int(center_pixel_comp[0]/(center_pixel_comp[2]+eps)), int(center_pixel_comp[1]/(center_pixel_comp[2]+eps))) - - print("center_pixel_comp", center_pixel_comp[0], center_pixel_comp[1], center_pixel_comp[2]) - print("center_pixel_comp_norm", center_pixel_comp_norm[0], center_pixel_comp_norm[1]) - cv2.circle(image_rgb, (int(center_pixel_comp_norm[0]), int(center_pixel_comp_norm[1])), 10, (255, 0, 0), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[0][0]), int(gate_corners_plot[0][1])), 10, (255, 100, 0), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[1][0]), int(gate_corners_plot[1][1])), 10, (255, 0, 100), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[2][0]), int(gate_corners_plot[2][1])), 10, (255, 200, 0), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[3][0]), int(gate_corners_plot[3][1])), 10, (255, 0, 200), -1) - cv2.imshow("image_rgb", image_rgb) - ctr+=1 - cv2.waitKey(1) - - # find 3D coordinate relative to quad - # camera matrix - mtx = np.array([[320.000, 0.000000, 320.000], - [0.000000, 320.000, 240.000], - [0.000000, 0.000000, 1.000000]]) - dist_coeff = np.zeros((1,5)) - - # coordinates of three waypoints in gate centered frame - waypoint_gate_1 = np.array([0.0,0.0,+1.0,1.0]).T.reshape(4,1) - waypoint_gate_2 = np.array([0.0,0.0,0.0,1.0]).T.reshape(4,1) - waypoint_gate_3 = np.array([0,0.0,-2.0,1.0]).T.reshape(4,1) - - # coordinates of gate corners in gate centered frame - objpoints = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) - - # coordinates of gate corners in image (pixel coordinates) - imgpoints = np.concatenate((np.array(center_pixel_comp_norm).reshape(-1,1,2),src_pts),axis=0) - imgpoints = np.float32(imgpoints) - - # find rotation/translation matrix between image to gate centered frame - ret, rvecs, tvecs = cv2.solvePnP(objpoints,imgpoints,mtx,dist_coeff) - rvec_full, jac1 = cv2.Rodrigues(rvecs) - rt_mtx_gate2quad = np.concatenate((rvec_full, tvecs), axis=1) - rt_mtx_gate2quad = np.concatenate((rt_mtx_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) - - # relative location of gate from quad to global location of gate - waypoint_rel_1 = np.dot(rt_mtx_gate2quad, waypoint_gate_1) - waypoint_rel_1 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_1[2], waypoint_rel_1[0], -waypoint_rel_1[1]])) - - waypoint_rel_2 = np.dot(rt_mtx_gate2quad, waypoint_gate_2) - waypoint_rel_2 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_2[2], waypoint_rel_2[0], -waypoint_rel_2[1]])) - - waypoint_rel_3 = np.dot(rt_mtx_gate2quad,waypoint_gate_3) - waypoint_rel_3 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_3[2], waypoint_rel_3[0], -waypoint_rel_3[1]])) - - waypoint_glob_1 = np.reshape(np.array([state.position.x_val + waypoint_rel_1[0], state.position.y_val + waypoint_rel_1[1], state.position.z_val - waypoint_rel_1[2]]),(1,3)) - waypoint_glob_2 = np.reshape(np.array([state.position.x_val + waypoint_rel_2[0], state.position.y_val + waypoint_rel_2[1], state.position.z_val - waypoint_rel_2[2]]),(1,3)) - waypoint_glob_3 = np.reshape(np.array([state.position.x_val + waypoint_rel_3[0], state.position.y_val + waypoint_rel_3[1], state.position.z_val - waypoint_rel_3[2]]),(1,3)) - - # check if your new measurement is far from your previous measurements - norm_dist_error = abs(np.linalg.norm(waypoint_glob_2) - np.linalg.norm(waypoint_ave_2)) - if norm_dist_error > 0.5: #reset if your new measurement is too far from average waypoint - measurement_count = 0 - - # calculate moving average of gate waypoints - measurement_count += 1 - print("Gate detected, Measurement Taken") - print(measurement_count) - if measurement_count == 1: - measurement_estimates_mtx_1 = np.reshape(waypoint_glob_1,(1,3)) - measurement_estimates_mtx_2 = np.reshape(waypoint_glob_2,(1,3)) - measurement_estimates_mtx_3 = np.reshape(waypoint_glob_3,(1,3)) - waypoint_ave_1 = np.reshape(waypoint_glob_1,(1,3)) - waypoint_ave_2 = np.reshape(waypoint_glob_2,(1,3)) - waypoint_ave_3 = np.reshape(waypoint_glob_3,(1,3)) - else: - measurement_estimates_mtx_1 = np.append(measurement_estimates_mtx_1, waypoint_glob_1,axis=0) - measurement_estimates_mtx_2 = np.append(measurement_estimates_mtx_2, waypoint_glob_2,axis=0) - measurement_estimates_mtx_3 = np.append(measurement_estimates_mtx_3, waypoint_glob_3,axis=0) - waypoint_ave_1 = np.reshape(np.mean(measurement_estimates_mtx_1,axis=0),(1,3)) - waypoint_ave_2 = np.reshape(np.mean(measurement_estimates_mtx_2,axis=0),(1,3)) - waypoint_ave_3 = np.reshape(np.mean(measurement_estimates_mtx_3,axis=0),(1,3)) - - # three waypoints - waypoint_1 = airsim.Vector3r(waypoint_ave_1[0,0],waypoint_ave_1[0,1], waypoint_ave_1[0,2]) - waypoint_2 = airsim.Vector3r(waypoint_ave_2[0,0],waypoint_ave_2[0,1], waypoint_ave_2[0,2]) - waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) - - no_gate_count = 0 - - client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 5.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=False, viz_traj=True, vehicle_name=drone_name).join() - last_good_waypoint = copy.deepcopy(waypoint_3) + # gate corner points + src_pts = order_points(hull_comp_best.reshape(4,2)) + gate_corners_plot = src_pts + src_pts = np.float32(src_pts).reshape(-1,1,2) + + # find homography matrix between gate corner points in baseline image and corner points in image taken + M, mask = cv2.findHomography(src_pts, self.dst_pts, cv2.RANSAC, 5.0) + + # find middle of gate pixel coordinates + center_pixel_0 = np.array([306,239,1]).T + center_pixel_comp = np.dot(np.linalg.inv(M), center_pixel_0) + center_pixel_comp_norm = (int(center_pixel_comp[0]/(center_pixel_comp[2]+self.eps)), int(center_pixel_comp[1]/(center_pixel_comp[2]+self.eps))) + + print("center_pixel_comp", center_pixel_comp[0], center_pixel_comp[1], center_pixel_comp[2]) + print("center_pixel_comp_norm", center_pixel_comp_norm[0], center_pixel_comp_norm[1]) + cv2.circle(image_rgb, (int(center_pixel_comp_norm[0]), int(center_pixel_comp_norm[1])), 10, (255, 0, 0), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[0][0]), int(gate_corners_plot[0][1])), 10, (255, 100, 0), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[1][0]), int(gate_corners_plot[1][1])), 10, (255, 0, 100), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[2][0]), int(gate_corners_plot[2][1])), 10, (255, 200, 0), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[3][0]), int(gate_corners_plot[3][1])), 10, (255, 0, 200), -1) + cv2.imshow("image_rgb", image_rgb) + cv2.waitKey(1) + + # find 3D coordinate relative to quad + # camera matrix + mtx = np.array([[320.000, 0.000000, 320.000], + [0.000000, 320.000, 240.000], + [0.000000, 0.000000, 1.000000]]) + dist_coeff = np.zeros((1,5)) + + # coordinates of three waypoints in gate centered frame + waypoint_gate_1 = np.array([0.0,0.0,+1.0,1.0]).T.reshape(4,1) + waypoint_gate_2 = np.array([0.0,0.0,0.0,1.0]).T.reshape(4,1) + waypoint_gate_3 = np.array([0,0.0,-2.0,1.0]).T.reshape(4,1) + + # coordinates of gate corners in gate centered frame + objpoints = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) + + # coordinates of gate corners in image (pixel coordinates) + imgpoints = np.concatenate((np.array(center_pixel_comp_norm).reshape(-1,1,2),src_pts),axis=0) + imgpoints = np.float32(imgpoints) + + # find rotation/translation matrix between image to gate centered frame + ret, rvecs, tvecs = cv2.solvePnP(objpoints,imgpoints,mtx,dist_coeff) + rvec_full, jac1 = cv2.Rodrigues(rvecs) + rt_mtx_gate2quad = np.concatenate((rvec_full, tvecs), axis=1) + rt_mtx_gate2quad = np.concatenate((rt_mtx_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) + + # relative location of gate from quad to global location of gate + waypoint_rel_1 = np.dot(rt_mtx_gate2quad, waypoint_gate_1) + waypoint_rel_1 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_1[2], waypoint_rel_1[0], -waypoint_rel_1[1]])) + + waypoint_rel_2 = np.dot(rt_mtx_gate2quad, waypoint_gate_2) + waypoint_rel_2 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_2[2], waypoint_rel_2[0], -waypoint_rel_2[1]])) + + waypoint_rel_3 = np.dot(rt_mtx_gate2quad,waypoint_gate_3) + waypoint_rel_3 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_3[2], waypoint_rel_3[0], -waypoint_rel_3[1]])) + + waypoint_glob_1 = np.reshape(np.array([state.position.x_val + waypoint_rel_1[0], state.position.y_val + waypoint_rel_1[1], state.position.z_val - waypoint_rel_1[2]]),(1,3)) + waypoint_glob_2 = np.reshape(np.array([state.position.x_val + waypoint_rel_2[0], state.position.y_val + waypoint_rel_2[1], state.position.z_val - waypoint_rel_2[2]]),(1,3)) + waypoint_glob_3 = np.reshape(np.array([state.position.x_val + waypoint_rel_3[0], state.position.y_val + waypoint_rel_3[1], state.position.z_val - waypoint_rel_3[2]]),(1,3)) + + # check if your new measurement is far from your previous measurements + norm_dist_error = abs(np.linalg.norm(waypoint_glob_2) - np.linalg.norm(self.waypoint_ave_2)) + if norm_dist_error > 0.5: #reset if your new measurement is too far from average waypoint + measurement_count = 0 + + # calculate moving average of gate waypoints + measurement_count += 1 + print("Gate detected, Measurement Taken") + print(measurement_count) + if measurement_count == 1: + measurement_estimates_mtx_1 = np.reshape(waypoint_glob_1,(1,3)) + measurement_estimates_mtx_2 = np.reshape(waypoint_glob_2,(1,3)) + measurement_estimates_mtx_3 = np.reshape(waypoint_glob_3,(1,3)) + waypoint_ave_1 = np.reshape(waypoint_glob_1,(1,3)) + self.waypoint_ave_2 = np.reshape(waypoint_glob_2,(1,3)) + waypoint_ave_3 = np.reshape(waypoint_glob_3,(1,3)) + else: + measurement_estimates_mtx_1 = np.append(measurement_estimates_mtx_1, waypoint_glob_1,axis=0) + measurement_estimates_mtx_2 = np.append(measurement_estimates_mtx_2, waypoint_glob_2,axis=0) + measurement_estimates_mtx_3 = np.append(measurement_estimates_mtx_3, waypoint_glob_3,axis=0) + waypoint_ave_1 = np.reshape(np.mean(measurement_estimates_mtx_1,axis=0),(1,3)) + self.waypoint_ave_2 = np.reshape(np.mean(measurement_estimates_mtx_2,axis=0),(1,3)) + waypoint_ave_3 = np.reshape(np.mean(measurement_estimates_mtx_3,axis=0),(1,3)) + + # three waypoints + waypoint_1 = airsim.Vector3r(waypoint_ave_1[0,0],waypoint_ave_1[0,1], waypoint_ave_1[0,2]) + waypoint_2 = airsim.Vector3r(self.waypoint_ave_2[0,0],self.waypoint_ave_2[0,1], self.waypoint_ave_2[0,2]) + waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) + + self.no_gate_count = 0 + + self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 5.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=False, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() + self.last_good_waypoint = copy.deepcopy(waypoint_3) + + +def main(args): + # ensure you have generated the neurips planning settings file by running python generate_settings_file.py + baseline_racer = BaselineRacerPerception(drone_name="drone_1", plot_transform=args.plot_transform, viz_traj=args.viz_traj) + baseline_racer.load_level(args.level_name) + baseline_racer.initialize_drone() + baseline_racer.takeoff_with_moveOnSpline() + baseline_racer.run() + +if __name__ == "__main__": + parser = ArgumentParser() + parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy"], default="Soccer_Field_Easy") + parser.add_argument('--plot_transform', dest='plot_transform', action='store_true', default=True) + parser.add_argument('--viz_traj', dest='viz_traj', action='store_true', default=True) + args = parser.parse_args() + main(args) \ No newline at end of file From b071f52a0b0ad1cc22b80e37d01101a0035f724d Mon Sep 17 00:00:00 2001 From: Keiko Date: Wed, 14 Aug 2019 03:59:58 -0700 Subject: [PATCH 03/27] 240x320 camera --- baselines/baseline_racer_perception.py | 260 +++++++++++++------------ 1 file changed, 132 insertions(+), 128 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index f8a338e..0d6ce86 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -10,14 +10,25 @@ import time class BaselineRacerPerception(BaselineRacer): - def __init__(self, drone_name = "drone_1", plot_transform=True, viz_traj=True): + def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False): BaselineRacer.__init__(self, drone_name, plot_transform, viz_traj) - self.no_gate_count = 0 self.eps = 0.01 self.waypoint_ave_2 = np.zeros((1,3)) - self.last_good_waypoint = airsim.Vector3r(0,0,0) self.kernel = np.ones((3,3),np.uint8) - self.dst_pts = np.float32([[[200, 132],[412, 132],[412, 346], [200, 346]]]) + self.dst_pts = np.float32([[[94, 64],[204, 64],[204, 174], [94, 174]]]) + self.camera_matrix = np.array([[160.000, 0.000000, 160.000], [0.000000, 160.000, 120.000], [0.000000, 0.000000, 1.000000]]) + self.gate_center_pixel_flat = np.array([149,119,1]).T + self.no_gate_count = 0 + self.measurement_count = 0 + self.close_count = 1 + self.wrong_gate_count = 0 + self.lower_green = np.array([0, 210, 0]) + self.upper_green = np.array([200, 255, 200]) + self.dist_coeff = np.zeros((1,5)) + self.waypoint_gate_1 = np.array([0.0,0.0,+1.0,1.0]).T.reshape(4,1) + self.waypoint_gate_2 = np.array([0.0,0.0,0.0,1.0]).T.reshape(4,1) + self.waypoint_gate_3 = np.array([0,0.0,-2.0,1.0]).T.reshape(4,1) + self.objpoints = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) # Find area of polygon def find_poly_area(self, x ,y): @@ -25,9 +36,9 @@ def find_poly_area(self, x ,y): # Find four gate corners given more than four points # use the four points that produce the largest area - def find_gate_corners(self, hull_comp): - hull_comp_new = hull_comp.reshape(len(hull_comp),2) - all_combs = list(combinations(hull_comp_new,4)) + def find_gate_corners(self, gate_corners): + gate_corners = gate_corners.reshape(len(gate_corners),2) + all_combs = list(combinations(gate_corners,4)) comb_count_out = 0 largest_area = 0.0 for comb in all_combs: @@ -43,31 +54,32 @@ def find_gate_corners(self, hull_comp): if poly_area > largest_area: largest_area = copy.deepcopy(poly_area) comb_array_largest = copy.deepcopy(comb_array) + comb_array_largest = comb_array_largest.astype(int).reshape(4,1,2) return comb_array_largest # Find the highest aspect ratio of the gate # if aspect ratio is too high, a gate side may have been detected instead of a full gate - def find_aspect_ratio(self, hull_comp): + def find_aspect_ratio(self, gate_corners): aspect_ratio_mtx = np.empty((1,4)) - aspect_ratio_mtx[0,0] = abs(np.float32(hull_comp[1][0][1] - hull_comp[2][0][1])/np.float32(hull_comp[0][0][0] - hull_comp[1][0][0]+self.eps)) - aspect_ratio_mtx[0,1] = abs(np.float32(hull_comp[0][0][1] - hull_comp[3][0][1])/np.float32(hull_comp[2][0][0] - hull_comp[3][0][0]+self.eps)) - aspect_ratio_mtx[0,2] = abs(np.float32(hull_comp[0][0][1] - hull_comp[3][0][1])/np.float32(hull_comp[0][0][0] - hull_comp[1][0][0]+self.eps)) - aspect_ratio_mtx[0,3] = abs(np.float32(hull_comp[1][0][1] - hull_comp[2][0][1])/np.float32(hull_comp[2][0][0] - hull_comp[3][0][0]+self.eps)) + aspect_ratio_mtx[0,0] = abs(np.float32(gate_corners[1][0][1] - gate_corners[2][0][1])/np.float32(gate_corners[0][0][0] - gate_corners[1][0][0]+self.eps)) + aspect_ratio_mtx[0,1] = abs(np.float32(gate_corners[0][0][1] - gate_corners[3][0][1])/np.float32(gate_corners[2][0][0] - gate_corners[3][0][0]+self.eps)) + aspect_ratio_mtx[0,2] = abs(np.float32(gate_corners[0][0][1] - gate_corners[3][0][1])/np.float32(gate_corners[0][0][0] - gate_corners[1][0][0]+self.eps)) + aspect_ratio_mtx[0,3] = abs(np.float32(gate_corners[1][0][1] - gate_corners[2][0][1])/np.float32(gate_corners[2][0][0] - gate_corners[3][0][0]+self.eps)) large_aspect_ratio = 1.0 for i in range(4): if aspect_ratio_mtx[0,i] < 1.0: aspect_ratio_mtx[0,i] = 1/aspect_ratio_mtx[0,i] - if aspect_ratio_mtx[0,i] > 1.2: + if aspect_ratio_mtx[0,i] > 1.4: large_aspect_ratio = aspect_ratio_mtx[0,i] return large_aspect_ratio - def run(self): # Move through first gate self.get_ground_truth_gate_poses() - self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) - self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max = 2.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() - + #self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) + self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max = 2.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + time.sleep(2) + print(self.gate_poses_ground_truth[1]) 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)]) @@ -81,158 +93,150 @@ def run(self): q2 = state.orientation.y_val q3 = state.orientation.z_val - # rotation/translation matrix between quad body frame and global frame + # rotation matrix between quad body frame and global frame rt_mtx_quad2global = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3-q0*q2)], [2*(q1*q2 + q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) - # Process image - # Find greenest pixels (pixels with gate) - lower_green = np.array([0, 210, 0]) - upper_green = np.array([200, 255, 200]) # Find corners of the gate - mask = cv2.inRange(image_rgb,lower_green,upper_green) - dilated_comp = cv2.dilate(mask,self.kernel, iterations=8) - eroded_comp = cv2.erode(dilated_comp,self.kernel, iterations=8) - im_comp, contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - # contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - # im_comp, contours_comp, hierarchy = cv2.findContours(dilated_comp, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) + mask = cv2.inRange(image_rgb,self.lower_green,self.upper_green) + dilated_gate = cv2.dilate(mask,self.kernel, iterations=8) + eroded_gate = cv2.erode(dilated_gate,self.kernel, iterations=8) + __, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + # gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + # __, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) cv2.imshow("mask", mask) - cv2.imshow("dilated_comp", dilated_comp) - cv2.imshow("eroded_comp", eroded_comp) - #cv2.imshow("im_comp", im_comp) + cv2.imshow("dilated_gate", dilated_gate) + cv2.imshow("eroded_gate", eroded_gate) cv2.waitKey(1) largest_area = 0.0 - hull_comp = None - hull_comp_best = np.zeros((1,3)) - for c in contours_comp: - # print("area", cv2.contourArea(c)) + gate_corners = None + gate_corners_best = np.zeros((1,3)) + for c in gate_contours: if cv2.contourArea(c) > largest_area: self.epsilon_comp = 0.01 * cv2.arcLength(c, True) - approx_comp = cv2.approxPolyDP(c,self.epsilon_comp,True) - hull_comp = cv2.convexHull(approx_comp) - # check if there are more than four points given; if there are find four points that produce largest area polygon - if len(hull_comp)>4: - hull_comp = self.find_gate_corners(hull_comp) - hull_comp = hull_comp.astype(int).reshape(4,1,2) - if len(hull_comp)>=4: - aspect_ratio = self.find_aspect_ratio(hull_comp) - if aspect_ratio <= 1.2: + gate_contour_approx = cv2.approxPolyDP(c,self.epsilon_comp,True) + gate_corners = cv2.convexHull(gate_contour_approx) + # check if there are more than four points given; if there are, find four points that produce largest area polygon + if len(gate_corners) > 4: + gate_corners = self.find_gate_corners(gate_corners) + if len(gate_corners) == 4: + aspect_ratio = self.find_aspect_ratio(gate_corners) + if aspect_ratio <= 1.4: largest_area = cv2.contourArea(c) gate_contour_comp = c - hull_comp_best = hull_comp + gate_corners_best = gate_corners # MOVE - if largest_area < 3500 or len(hull_comp_best)!=4: + if largest_area < 500 or len(gate_corners_best)!=4: self.no_gate_count += 1 + #self.wrong_gate_count = 0 print("Gate NOT detected") - # If no gate has been detected for over 25 attempts, move to the last good waypoint + # If no gate has been detected for over 25 attempts, rotate to look for gate if self.no_gate_count > 25: - self.airsim_client.moveOnSplineAsync([self.last_good_waypoint + airsim.Vector3r(np.random.uniform(low=-0.25,high=0.25,size=1)[0], - np.random.uniform(low=-0.25,high=0.25,size=1)[0], - np.random.uniform(low=-0.25,high=0.25,size=1)[0])], - vel_max = 10.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() # move to last good waypoint - self.no_gate_count = 0 - # Rotate counterclockwise until gate has been found - else: - self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.5, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate + self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=2, vehicle_name=self.drone_name) # rotate counter clockwise to look for next gate + else: - # gate corner points - src_pts = order_points(hull_comp_best.reshape(4,2)) + self.no_gate_count = 0 + src_pts = order_points(gate_corners_best.reshape(4,2)) gate_corners_plot = src_pts src_pts = np.float32(src_pts).reshape(-1,1,2) # find homography matrix between gate corner points in baseline image and corner points in image taken - M, mask = cv2.findHomography(src_pts, self.dst_pts, cv2.RANSAC, 5.0) + homography_matrix, mask = cv2.findHomography(src_pts, self.dst_pts, cv2.RANSAC, 5.0) # find middle of gate pixel coordinates - center_pixel_0 = np.array([306,239,1]).T - center_pixel_comp = np.dot(np.linalg.inv(M), center_pixel_0) - center_pixel_comp_norm = (int(center_pixel_comp[0]/(center_pixel_comp[2]+self.eps)), int(center_pixel_comp[1]/(center_pixel_comp[2]+self.eps))) - - print("center_pixel_comp", center_pixel_comp[0], center_pixel_comp[1], center_pixel_comp[2]) - print("center_pixel_comp_norm", center_pixel_comp_norm[0], center_pixel_comp_norm[1]) - cv2.circle(image_rgb, (int(center_pixel_comp_norm[0]), int(center_pixel_comp_norm[1])), 10, (255, 0, 0), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[0][0]), int(gate_corners_plot[0][1])), 10, (255, 100, 0), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[1][0]), int(gate_corners_plot[1][1])), 10, (255, 0, 100), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[2][0]), int(gate_corners_plot[2][1])), 10, (255, 200, 0), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[3][0]), int(gate_corners_plot[3][1])), 10, (255, 0, 200), -1) - cv2.imshow("image_rgb", image_rgb) - cv2.waitKey(1) - - # find 3D coordinate relative to quad - # camera matrix - mtx = np.array([[320.000, 0.000000, 320.000], - [0.000000, 320.000, 240.000], - [0.000000, 0.000000, 1.000000]]) - dist_coeff = np.zeros((1,5)) - - # coordinates of three waypoints in gate centered frame - waypoint_gate_1 = np.array([0.0,0.0,+1.0,1.0]).T.reshape(4,1) - waypoint_gate_2 = np.array([0.0,0.0,0.0,1.0]).T.reshape(4,1) - waypoint_gate_3 = np.array([0,0.0,-2.0,1.0]).T.reshape(4,1) - - # coordinates of gate corners in gate centered frame - objpoints = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) + gate_center_pixel = np.dot(np.linalg.inv(homography_matrix), self.gate_center_pixel_flat) + gate_center_pixel = (int(gate_center_pixel[0]/(gate_center_pixel[2]+self.eps)), int(gate_center_pixel[1]/(gate_center_pixel[2]+self.eps))) # coordinates of gate corners in image (pixel coordinates) - imgpoints = np.concatenate((np.array(center_pixel_comp_norm).reshape(-1,1,2),src_pts),axis=0) - imgpoints = np.float32(imgpoints) + imgpoints = np.float32(np.concatenate((np.array(gate_center_pixel).reshape(-1,1,2),src_pts),axis=0)) # find rotation/translation matrix between image to gate centered frame - ret, rvecs, tvecs = cv2.solvePnP(objpoints,imgpoints,mtx,dist_coeff) + ret, rvecs, tvecs = cv2.solvePnP(self.objpoints,imgpoints,self.camera_matrix,self.dist_coeff) rvec_full, jac1 = cv2.Rodrigues(rvecs) rt_mtx_gate2quad = np.concatenate((rvec_full, tvecs), axis=1) rt_mtx_gate2quad = np.concatenate((rt_mtx_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) # relative location of gate from quad to global location of gate - waypoint_rel_1 = np.dot(rt_mtx_gate2quad, waypoint_gate_1) + waypoint_rel_1 = np.dot(rt_mtx_gate2quad, self.waypoint_gate_1) waypoint_rel_1 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_1[2], waypoint_rel_1[0], -waypoint_rel_1[1]])) - waypoint_rel_2 = np.dot(rt_mtx_gate2quad, waypoint_gate_2) + waypoint_rel_2 = np.dot(rt_mtx_gate2quad, self.waypoint_gate_2) waypoint_rel_2 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_2[2], waypoint_rel_2[0], -waypoint_rel_2[1]])) - waypoint_rel_3 = np.dot(rt_mtx_gate2quad,waypoint_gate_3) + waypoint_rel_3 = np.dot(rt_mtx_gate2quad,self.waypoint_gate_3) waypoint_rel_3 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_3[2], waypoint_rel_3[0], -waypoint_rel_3[1]])) waypoint_glob_1 = np.reshape(np.array([state.position.x_val + waypoint_rel_1[0], state.position.y_val + waypoint_rel_1[1], state.position.z_val - waypoint_rel_1[2]]),(1,3)) waypoint_glob_2 = np.reshape(np.array([state.position.x_val + waypoint_rel_2[0], state.position.y_val + waypoint_rel_2[1], state.position.z_val - waypoint_rel_2[2]]),(1,3)) waypoint_glob_3 = np.reshape(np.array([state.position.x_val + waypoint_rel_3[0], state.position.y_val + waypoint_rel_3[1], state.position.z_val - waypoint_rel_3[2]]),(1,3)) - # check if your new measurement is far from your previous measurements - norm_dist_error = abs(np.linalg.norm(waypoint_glob_2) - np.linalg.norm(self.waypoint_ave_2)) - if norm_dist_error > 0.5: #reset if your new measurement is too far from average waypoint - measurement_count = 0 - - # calculate moving average of gate waypoints - measurement_count += 1 - print("Gate detected, Measurement Taken") - print(measurement_count) - if measurement_count == 1: - measurement_estimates_mtx_1 = np.reshape(waypoint_glob_1,(1,3)) - measurement_estimates_mtx_2 = np.reshape(waypoint_glob_2,(1,3)) - measurement_estimates_mtx_3 = np.reshape(waypoint_glob_3,(1,3)) - waypoint_ave_1 = np.reshape(waypoint_glob_1,(1,3)) - self.waypoint_ave_2 = np.reshape(waypoint_glob_2,(1,3)) - waypoint_ave_3 = np.reshape(waypoint_glob_3,(1,3)) + # if quad is too close to next waypoint, move through gate + self.quad_dist_error = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_2)) + if self.quad_dist_error < 3.0 and self.close_count == 0: + print("too close to gate") + self.airsim_client.moveOnSplineAsync([waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + time.sleep(3) + self.measurement_count = 0 + self.wrong_gate_count = 0 + self.close_count += 1 + self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) else: - measurement_estimates_mtx_1 = np.append(measurement_estimates_mtx_1, waypoint_glob_1,axis=0) - measurement_estimates_mtx_2 = np.append(measurement_estimates_mtx_2, waypoint_glob_2,axis=0) - measurement_estimates_mtx_3 = np.append(measurement_estimates_mtx_3, waypoint_glob_3,axis=0) - waypoint_ave_1 = np.reshape(np.mean(measurement_estimates_mtx_1,axis=0),(1,3)) - self.waypoint_ave_2 = np.reshape(np.mean(measurement_estimates_mtx_2,axis=0),(1,3)) - waypoint_ave_3 = np.reshape(np.mean(measurement_estimates_mtx_3,axis=0),(1,3)) - - # three waypoints - waypoint_1 = airsim.Vector3r(waypoint_ave_1[0,0],waypoint_ave_1[0,1], waypoint_ave_1[0,2]) - waypoint_2 = airsim.Vector3r(self.waypoint_ave_2[0,0],self.waypoint_ave_2[0,1], self.waypoint_ave_2[0,2]) - waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) - - self.no_gate_count = 0 - - self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 5.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=False, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() - self.last_good_waypoint = copy.deepcopy(waypoint_3) - + if self.close_count == 1: # right after passing through gate, reinitialize + self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) + self.measurement_count = 0 + self.close_count = 0 + norm_dist_error = abs(np.linalg.norm(waypoint_glob_2-self.waypoint_ave_2)) + quad_meas_error = abs(np.linalg.norm(waypoint_glob_2-np.array([state.position.x_val,state.position.y_val,state.position.z_val]))) + # if new measurement is very far from previous measurement, wrong gate is measured + if norm_dist_error > 1.0: + print(norm_dist_error, quad_meas_error) + print("wrong gate", self.wrong_gate_count) + # if wrong gate is measured over 10 times, reinitialize next waypoint + if self.wrong_gate_count > 10: + self.measurement_count = 0 + self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) + self.wrong_gate_count += 1 + # if new measurement is very far from quad, wrong gate is measured + elif quad_meas_error > 20: + print(norm_dist_error, quad_meas_error) + print("wrong gate", self.wrong_gate_count) + else: + cv2.circle(image_rgb, (int(gate_center_pixel[0]), int(gate_center_pixel[1])), 10, (255, 0, 0), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[0][0]), int(gate_corners_plot[0][1])), 10, (255, 100, 0), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[1][0]), int(gate_corners_plot[1][1])), 10, (255, 0, 100), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[2][0]), int(gate_corners_plot[2][1])), 10, (255, 200, 0), -1) + cv2.circle(image_rgb, (int(gate_corners_plot[3][0]), int(gate_corners_plot[3][1])), 10, (255, 0, 200), -1) + cv2.imshow("image_rgb", image_rgb) + cv2.waitKey(1) + self.wrong_gate_count = 0 + self.measurement_count += 1 + print("Gate detected, Measurement Taken") + print(self.measurement_count) + # calculate moving average of gate waypoints + if self.measurement_count == 1: + measurement_estimates_mtx_1 = np.reshape(waypoint_glob_1,(1,3)) + measurement_estimates_mtx_2 = np.reshape(waypoint_glob_2,(1,3)) + measurement_estimates_mtx_3 = np.reshape(waypoint_glob_3,(1,3)) + waypoint_ave_1 = np.reshape(waypoint_glob_1,(1,3)) + self.waypoint_ave_2 = np.reshape(waypoint_glob_2,(1,3)) + waypoint_ave_3 = np.reshape(waypoint_glob_3,(1,3)) + else: + measurement_estimates_mtx_1 = np.append(measurement_estimates_mtx_1, waypoint_glob_1,axis=0) + measurement_estimates_mtx_2 = np.append(measurement_estimates_mtx_2, waypoint_glob_2,axis=0) + measurement_estimates_mtx_3 = np.append(measurement_estimates_mtx_3, waypoint_glob_3,axis=0) + waypoint_ave_1 = np.reshape(np.mean(measurement_estimates_mtx_1,axis=0),(1,3)) + self.waypoint_ave_2 = np.reshape(np.mean(measurement_estimates_mtx_2,axis=0),(1,3)) + waypoint_ave_3 = np.reshape(np.mean(measurement_estimates_mtx_3,axis=0),(1,3)) + + # three waypoints + waypoint_1 = airsim.Vector3r(waypoint_ave_1[0,0],waypoint_ave_1[0,1], waypoint_ave_1[0,2]) + waypoint_2 = airsim.Vector3r(self.waypoint_ave_2[0,0],self.waypoint_ave_2[0,1], self.waypoint_ave_2[0,2]) + waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) + + #self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + self.airsim_client.moveOnSplineAsync([waypoint_2], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py @@ -245,7 +249,7 @@ def main(args): if __name__ == "__main__": parser = ArgumentParser() parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy"], default="Soccer_Field_Easy") - parser.add_argument('--plot_transform', dest='plot_transform', action='store_true', default=True) - parser.add_argument('--viz_traj', dest='viz_traj', action='store_true', default=True) + parser.add_argument('--plot_transform', dest='plot_transform', action='store_false', default=False) + parser.add_argument('--viz_traj', dest='viz_traj', action='store_false', default=False) args = parser.parse_args() - main(args) \ No newline at end of file + main(args) From e8be6432cfff236dcdb6f503975e6128cdbc1ead Mon Sep 17 00:00:00 2001 From: Keiko Date: Thu, 15 Aug 2019 02:20:50 -0700 Subject: [PATCH 04/27] take measurements while flying --- baselines/baseline_racer_perception.py | 133 +++++++++++++------------ 1 file changed, 70 insertions(+), 63 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 0d6ce86..93a4f31 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -28,6 +28,8 @@ def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False) self.waypoint_gate_1 = np.array([0.0,0.0,+1.0,1.0]).T.reshape(4,1) self.waypoint_gate_2 = np.array([0.0,0.0,0.0,1.0]).T.reshape(4,1) self.waypoint_gate_3 = np.array([0,0.0,-2.0,1.0]).T.reshape(4,1) + self.waypoint_gate_4 = np.array([0,0.0,-6.0,1.0]).T.reshape(4,1) + self.waypoint_gate_5 = np.array([0,0.0,-20.0,1.0]).T.reshape(4,1) self.objpoints = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) # Find area of polygon @@ -73,30 +75,40 @@ def find_aspect_ratio(self, gate_corners): large_aspect_ratio = aspect_ratio_mtx[0,i] return large_aspect_ratio + def get_rotation_matrix_quad_frame_to_global_frame(self,state): + q0 = state.orientation.w_val + q1 = state.orientation.x_val + q2 = state.orientation.y_val + q3 = state.orientation.z_val + + # rotation matrix between quad body frame and global frame + rt_mtx_quad2global = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3-q0*q2)], + [2*(q1*q2 + q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], + [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) + return rt_mtx_quad2global + + def gate_frame_waypoint_to_global_waypoint(self,state,rt_mtx_gate2quad,rt_mtx_quad2global,waypoint_gate): + waypoint_rel = np.dot(rt_mtx_gate2quad, waypoint_gate) + waypoint_rel = np.dot(rt_mtx_quad2global, np.array([waypoint_rel[2], waypoint_rel[0], -waypoint_rel[1]])) + waypoint_glob = np.reshape(np.array([state.position.x_val + waypoint_rel[0], state.position.y_val + waypoint_rel[1], state.position.z_val - waypoint_rel[2]]),(1,3)) + return waypoint_glob + def run(self): # Move through first gate self.get_ground_truth_gate_poses() #self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max = 2.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name) time.sleep(2) - print(self.gate_poses_ground_truth[1]) + waypoint_2 = airsim.Vector3r(0,10,self.gate_poses_ground_truth[0].position.z_val) 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) # get numpy array image_rgb = img1d.reshape(response[0].height, response[0].width, 3) - # Get quad state when the image was taken + # get rotation matrix from quad frame to global frame state = self.airsim_client.simGetVehiclePose() - q0 = state.orientation.w_val - q1 = state.orientation.x_val - q2 = state.orientation.y_val - q3 = state.orientation.z_val - - # rotation matrix between quad body frame and global frame - rt_mtx_quad2global = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3-q0*q2)], - [2*(q1*q2 + q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], - [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) + rt_mtx_quad2global = self.get_rotation_matrix_quad_frame_to_global_frame(state) # Find corners of the gate mask = cv2.inRange(image_rgb,self.lower_green,self.upper_green) @@ -109,6 +121,7 @@ def run(self): cv2.imshow("dilated_gate", dilated_gate) cv2.imshow("eroded_gate", eroded_gate) cv2.waitKey(1) + largest_area = 0.0 gate_corners = None gate_corners_best = np.zeros((1,3)) @@ -128,13 +141,12 @@ def run(self): gate_corners_best = gate_corners # MOVE - if largest_area < 500 or len(gate_corners_best)!=4: + if largest_area < 800 or len(gate_corners_best)!=4: self.no_gate_count += 1 - #self.wrong_gate_count = 0 print("Gate NOT detected") - # If no gate has been detected for over 25 attempts, rotate to look for gate - if self.no_gate_count > 25: - self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=2, vehicle_name=self.drone_name) # rotate counter clockwise to look for next gate + # If no gate has been detected for over 50 attempts, rotate to look for gate + if self.no_gate_count > 50: + self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.05, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate else: self.no_gate_count = 0 @@ -153,54 +165,45 @@ def run(self): imgpoints = np.float32(np.concatenate((np.array(gate_center_pixel).reshape(-1,1,2),src_pts),axis=0)) # find rotation/translation matrix between image to gate centered frame - ret, rvecs, tvecs = cv2.solvePnP(self.objpoints,imgpoints,self.camera_matrix,self.dist_coeff) - rvec_full, jac1 = cv2.Rodrigues(rvecs) - rt_mtx_gate2quad = np.concatenate((rvec_full, tvecs), axis=1) + ret, rvec, tvec = cv2.solvePnP(self.objpoints,imgpoints,self.camera_matrix,self.dist_coeff) + rvec_full, jac1 = cv2.Rodrigues(rvec) + rt_mtx_gate2quad = np.concatenate((rvec_full, tvec), axis=1) rt_mtx_gate2quad = np.concatenate((rt_mtx_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) - # relative location of gate from quad to global location of gate - waypoint_rel_1 = np.dot(rt_mtx_gate2quad, self.waypoint_gate_1) - waypoint_rel_1 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_1[2], waypoint_rel_1[0], -waypoint_rel_1[1]])) - - waypoint_rel_2 = np.dot(rt_mtx_gate2quad, self.waypoint_gate_2) - waypoint_rel_2 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_2[2], waypoint_rel_2[0], -waypoint_rel_2[1]])) + # gate frame waypoint to global frame waypoint + waypoint_glob_1 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_1) + waypoint_glob_2 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_2) + waypoint_glob_3 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_3) + waypoint_glob_4 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_4) + waypoint_glob_5 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_5) - waypoint_rel_3 = np.dot(rt_mtx_gate2quad,self.waypoint_gate_3) - waypoint_rel_3 = np.dot(rt_mtx_quad2global, np.array([waypoint_rel_3[2], waypoint_rel_3[0], -waypoint_rel_3[1]])) - - waypoint_glob_1 = np.reshape(np.array([state.position.x_val + waypoint_rel_1[0], state.position.y_val + waypoint_rel_1[1], state.position.z_val - waypoint_rel_1[2]]),(1,3)) - waypoint_glob_2 = np.reshape(np.array([state.position.x_val + waypoint_rel_2[0], state.position.y_val + waypoint_rel_2[1], state.position.z_val - waypoint_rel_2[2]]),(1,3)) - waypoint_glob_3 = np.reshape(np.array([state.position.x_val + waypoint_rel_3[0], state.position.y_val + waypoint_rel_3[1], state.position.z_val - waypoint_rel_3[2]]),(1,3)) - - # if quad is too close to next waypoint, move through gate - self.quad_dist_error = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_2)) - if self.quad_dist_error < 3.0 and self.close_count == 0: + # if quad is too close to next waypoint, move through gate without taking measurements + quad_to_next_gate_dist = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_2)) + if quad_to_next_gate_dist < 3.0 and self.close_count == 0: print("too close to gate") self.airsim_client.moveOnSplineAsync([waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) - time.sleep(3) + time.sleep(2) self.measurement_count = 0 self.wrong_gate_count = 0 - self.close_count += 1 + self.close_count = 1 self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) else: if self.close_count == 1: # right after passing through gate, reinitialize self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) self.measurement_count = 0 self.close_count = 0 - norm_dist_error = abs(np.linalg.norm(waypoint_glob_2-self.waypoint_ave_2)) - quad_meas_error = abs(np.linalg.norm(waypoint_glob_2-np.array([state.position.x_val,state.position.y_val,state.position.z_val]))) - # if new measurement is very far from previous measurement, wrong gate is measured - if norm_dist_error > 1.0: - print(norm_dist_error, quad_meas_error) + measurement_diff = abs(np.linalg.norm(waypoint_glob_2-self.waypoint_ave_2)) + quad_to_measurement_dist = abs(np.linalg.norm(waypoint_glob_2-np.array([state.position.x_val,state.position.y_val,state.position.z_val]))) + + if measurement_diff > 0.5: # if new measurement is very far from previous measurements, wrong gate is measured + print(measurement_diff, quad_to_measurement_dist) print("wrong gate", self.wrong_gate_count) - # if wrong gate is measured over 10 times, reinitialize next waypoint - if self.wrong_gate_count > 10: + if self.wrong_gate_count > 10: # if wrong gate is measured over 10 times, reinitialize next waypoint self.measurement_count = 0 self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) self.wrong_gate_count += 1 - # if new measurement is very far from quad, wrong gate is measured - elif quad_meas_error > 20: - print(norm_dist_error, quad_meas_error) + elif quad_to_measurement_dist > 20: # if new measurement is very far from quad, wrong gate is measured + print(measurement_diff, quad_to_measurement_dist) print("wrong gate", self.wrong_gate_count) else: cv2.circle(image_rgb, (int(gate_center_pixel[0]), int(gate_center_pixel[1])), 10, (255, 0, 0), -1) @@ -214,28 +217,32 @@ def run(self): self.measurement_count += 1 print("Gate detected, Measurement Taken") print(self.measurement_count) + # calculate moving average of gate waypoints if self.measurement_count == 1: - measurement_estimates_mtx_1 = np.reshape(waypoint_glob_1,(1,3)) - measurement_estimates_mtx_2 = np.reshape(waypoint_glob_2,(1,3)) - measurement_estimates_mtx_3 = np.reshape(waypoint_glob_3,(1,3)) - waypoint_ave_1 = np.reshape(waypoint_glob_1,(1,3)) - self.waypoint_ave_2 = np.reshape(waypoint_glob_2,(1,3)) - waypoint_ave_3 = np.reshape(waypoint_glob_3,(1,3)) - else: - measurement_estimates_mtx_1 = np.append(measurement_estimates_mtx_1, waypoint_glob_1,axis=0) - measurement_estimates_mtx_2 = np.append(measurement_estimates_mtx_2, waypoint_glob_2,axis=0) - measurement_estimates_mtx_3 = np.append(measurement_estimates_mtx_3, waypoint_glob_3,axis=0) - waypoint_ave_1 = np.reshape(np.mean(measurement_estimates_mtx_1,axis=0),(1,3)) - self.waypoint_ave_2 = np.reshape(np.mean(measurement_estimates_mtx_2,axis=0),(1,3)) - waypoint_ave_3 = np.reshape(np.mean(measurement_estimates_mtx_3,axis=0),(1,3)) - - # three waypoints + measurement_estimates_mtx_1 = np.empty((0,3)) + measurement_estimates_mtx_2 = np.empty((0,3)) + measurement_estimates_mtx_3 = np.empty((0,3)) + measurement_estimates_mtx_4 = np.empty((0,3)) + measurement_estimates_mtx_5 = np.empty((0,3)) + measurement_estimates_mtx_1 = np.append(measurement_estimates_mtx_1, waypoint_glob_1,axis=0) + measurement_estimates_mtx_2 = np.append(measurement_estimates_mtx_2, waypoint_glob_2,axis=0) + measurement_estimates_mtx_3 = np.append(measurement_estimates_mtx_3, waypoint_glob_3,axis=0) + measurement_estimates_mtx_4 = np.append(measurement_estimates_mtx_4, waypoint_glob_4,axis=0) + measurement_estimates_mtx_5 = np.append(measurement_estimates_mtx_5, waypoint_glob_5,axis=0) + waypoint_ave_1 = np.reshape(np.mean(measurement_estimates_mtx_1,axis=0),(1,3)) + self.waypoint_ave_2 = np.reshape(np.mean(measurement_estimates_mtx_2,axis=0),(1,3)) + waypoint_ave_3 = np.reshape(np.mean(measurement_estimates_mtx_3,axis=0),(1,3)) + waypoint_ave_4 = np.reshape(np.mean(measurement_estimates_mtx_4,axis=0),(1,3)) + waypoint_ave_5 = np.reshape(np.mean(measurement_estimates_mtx_5,axis=0),(1,3)) + + # waypoints waypoint_1 = airsim.Vector3r(waypoint_ave_1[0,0],waypoint_ave_1[0,1], waypoint_ave_1[0,2]) waypoint_2 = airsim.Vector3r(self.waypoint_ave_2[0,0],self.waypoint_ave_2[0,1], self.waypoint_ave_2[0,2]) waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) + waypoint_4 = airsim.Vector3r(waypoint_ave_4[0,0],waypoint_ave_4[0,1], waypoint_ave_4[0,2]) + waypoint_5 = airsim.Vector3r(waypoint_ave_5[0,0],waypoint_ave_5[0,1], waypoint_ave_5[0,2]) - #self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([waypoint_2], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) def main(args): From 9e2fc5de550fac3680afe29c29e5006f44b7c7e5 Mon Sep 17 00:00:00 2001 From: Keiko Date: Thu, 15 Aug 2019 14:07:30 -0700 Subject: [PATCH 05/27] adding functions --- baselines/baseline_racer_perception.py | 149 +++++++++++-------------- 1 file changed, 68 insertions(+), 81 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 93a4f31..1077744 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -15,7 +15,7 @@ def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False) self.eps = 0.01 self.waypoint_ave_2 = np.zeros((1,3)) self.kernel = np.ones((3,3),np.uint8) - self.dst_pts = np.float32([[[94, 64],[204, 64],[204, 174], [94, 174]]]) + self.gate_corners_flat = np.float32([[[94, 64],[204, 64],[204, 174], [94, 174]]]) self.camera_matrix = np.array([[160.000, 0.000000, 160.000], [0.000000, 160.000, 120.000], [0.000000, 0.000000, 1.000000]]) self.gate_center_pixel_flat = np.array([149,119,1]).T self.no_gate_count = 0 @@ -25,39 +25,39 @@ def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False) self.lower_green = np.array([0, 210, 0]) self.upper_green = np.array([200, 255, 200]) self.dist_coeff = np.zeros((1,5)) - self.waypoint_gate_1 = np.array([0.0,0.0,+1.0,1.0]).T.reshape(4,1) + self.waypoint_gate_1 = np.array([0,0.0,+1.0,1.0]).T.reshape(4,1) self.waypoint_gate_2 = np.array([0.0,0.0,0.0,1.0]).T.reshape(4,1) - self.waypoint_gate_3 = np.array([0,0.0,-2.0,1.0]).T.reshape(4,1) - self.waypoint_gate_4 = np.array([0,0.0,-6.0,1.0]).T.reshape(4,1) - self.waypoint_gate_5 = np.array([0,0.0,-20.0,1.0]).T.reshape(4,1) - self.objpoints = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) + self.waypoint_gate_3 = np.array([0,0.0,-1.0,1.0]).T.reshape(4,1) + self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) # Find area of polygon - def find_poly_area(self, x ,y): - return 0.5*np.abs(np.dot(x,np.roll(y,1))-np.dot(y,np.roll(x,1))) + def find_gate_area(self, x_coords ,y_coords): + area_numerator = 0 + for i in range(4): + if i == 3: + area_numerator += (x_coords[i]*y_coords[0]- y_coords[i]*x_coords[0]) + else: + area_numerator += (x_coords[i]*y_coords[i+1] - y_coords[i]*x_coords[i+1]) + gate_area = abs(area_numerator/2.0) + return gate_area # Find four gate corners given more than four points - # use the four points that produce the largest area - def find_gate_corners(self, gate_corners): + def find_four_gate_corners_largest_area(self, gate_corners): gate_corners = gate_corners.reshape(len(gate_corners),2) - all_combs = list(combinations(gate_corners,4)) - comb_count_out = 0 + gate_corner_combinations = list(combinations(gate_corners,4)) largest_area = 0.0 - for comb in all_combs: - comb_count_out += 1 - comb_count = 0 + for comb in gate_corner_combinations: + comb_array = np.empty((0,2)) for i in comb: - comb_count += 1 - if comb_count == 1: - comb_array = i - else: - comb_array = np.vstack((comb_array,i)) - poly_area = self.find_poly_area(comb_array[:,0], comb_array[:,1]) - if poly_area > largest_area: - largest_area = copy.deepcopy(poly_area) - comb_array_largest = copy.deepcopy(comb_array) - comb_array_largest = comb_array_largest.astype(int).reshape(4,1,2) - return comb_array_largest + i = np.reshape(i,(1,2)) + comb_array = np.append(comb_array,i,axis=0) + comb_array = order_points(comb_array) + gate_area = self.find_gate_area(comb_array[:,0], comb_array[:,1]) + if gate_area > largest_area: + largest_area = copy.deepcopy(gate_area) + gate_corners_largest_area = copy.deepcopy(comb_array) + gate_corners_largest_area = gate_corners_largest_area.astype(int).reshape(4,1,2) + return gate_corners_largest_area # Find the highest aspect ratio of the gate # if aspect ratio is too high, a gate side may have been detected instead of a full gate @@ -82,17 +82,22 @@ def get_rotation_matrix_quad_frame_to_global_frame(self,state): q3 = state.orientation.z_val # rotation matrix between quad body frame and global frame - rt_mtx_quad2global = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3-q0*q2)], + rot_matrix_quad2global = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3-q0*q2)], [2*(q1*q2 + q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) - return rt_mtx_quad2global + return rot_matrix_quad2global - def gate_frame_waypoint_to_global_waypoint(self,state,rt_mtx_gate2quad,rt_mtx_quad2global,waypoint_gate): - waypoint_rel = np.dot(rt_mtx_gate2quad, waypoint_gate) - waypoint_rel = np.dot(rt_mtx_quad2global, np.array([waypoint_rel[2], waypoint_rel[0], -waypoint_rel[1]])) + def gate_frame_waypoint_to_global_waypoint(self,state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,waypoint_gate): + waypoint_rel = np.dot(rot_translation_matrix_gate2quad, waypoint_gate) + waypoint_rel = np.dot(rot_matrix_quad2global, np.array([waypoint_rel[2], waypoint_rel[0], -waypoint_rel[1]])) waypoint_glob = np.reshape(np.array([state.position.x_val + waypoint_rel[0], state.position.y_val + waypoint_rel[1], state.position.z_val - waypoint_rel[2]]),(1,3)) return waypoint_glob + def get_average_waypoint(self,measurement_estimates_mtx,waypoint_glob): + measurement_estimates_mtx = np.append(measurement_estimates_mtx, waypoint_glob,axis=0) + waypoint_ave = np.reshape(np.mean(measurement_estimates_mtx,axis=0),(1,3)) + return waypoint_ave + def run(self): # Move through first gate self.get_ground_truth_gate_poses() @@ -108,7 +113,7 @@ def run(self): # get rotation matrix from quad frame to global frame state = self.airsim_client.simGetVehiclePose() - rt_mtx_quad2global = self.get_rotation_matrix_quad_frame_to_global_frame(state) + rot_matrix_quad2global = self.get_rotation_matrix_quad_frame_to_global_frame(state) # Find corners of the gate mask = cv2.inRange(image_rgb,self.lower_green,self.upper_green) @@ -116,7 +121,7 @@ def run(self): eroded_gate = cv2.erode(dilated_gate,self.kernel, iterations=8) __, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) # gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - # __, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) + cv2.imshow("mask", mask) cv2.imshow("dilated_gate", dilated_gate) cv2.imshow("eroded_gate", eroded_gate) @@ -125,62 +130,57 @@ def run(self): largest_area = 0.0 gate_corners = None gate_corners_best = np.zeros((1,3)) - for c in gate_contours: - if cv2.contourArea(c) > largest_area: - self.epsilon_comp = 0.01 * cv2.arcLength(c, True) - gate_contour_approx = cv2.approxPolyDP(c,self.epsilon_comp,True) + for contour in gate_contours: + if cv2.contourArea(contour) > largest_area: + 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) - # check if there are more than four points given; if there are, find four points that produce largest area polygon - if len(gate_corners) > 4: - gate_corners = self.find_gate_corners(gate_corners) + if len(gate_corners) > 4: # check if there are more than four points given, find four corners that produce largest area + gate_corners = self.find_four_gate_corners_largest_area(gate_corners) if len(gate_corners) == 4: aspect_ratio = self.find_aspect_ratio(gate_corners) if aspect_ratio <= 1.4: - largest_area = cv2.contourArea(c) - gate_contour_comp = c + largest_area = cv2.contourArea(contour) gate_corners_best = gate_corners # MOVE if largest_area < 800 or len(gate_corners_best)!=4: self.no_gate_count += 1 print("Gate NOT detected") - # If no gate has been detected for over 50 attempts, rotate to look for gate - if self.no_gate_count > 50: + if self.no_gate_count > 50: # If no gate has been detected for over 50 attempts, rotate to look for gate. self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.05, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate else: self.no_gate_count = 0 - src_pts = order_points(gate_corners_best.reshape(4,2)) - gate_corners_plot = src_pts - src_pts = np.float32(src_pts).reshape(-1,1,2) + gate_corners_best = order_points(gate_corners_best.reshape(4,2)) + gate_corners_plot = gate_corners_best + gate_corners_best = np.float32(gate_corners_best).reshape(-1,1,2) # find homography matrix between gate corner points in baseline image and corner points in image taken - homography_matrix, mask = cv2.findHomography(src_pts, self.dst_pts, cv2.RANSAC, 5.0) + homography_matrix, mask = cv2.findHomography(gate_corners_best, self.gate_corners_flat, cv2.RANSAC, 5.0) # find middle of gate pixel coordinates gate_center_pixel = np.dot(np.linalg.inv(homography_matrix), self.gate_center_pixel_flat) gate_center_pixel = (int(gate_center_pixel[0]/(gate_center_pixel[2]+self.eps)), int(gate_center_pixel[1]/(gate_center_pixel[2]+self.eps))) # coordinates of gate corners in image (pixel coordinates) - imgpoints = np.float32(np.concatenate((np.array(gate_center_pixel).reshape(-1,1,2),src_pts),axis=0)) + gate_points_2D = np.float32(np.concatenate((np.array(gate_center_pixel).reshape(-1,1,2),gate_corners_best),axis=0)) - # find rotation/translation matrix between image to gate centered frame - ret, rvec, tvec = cv2.solvePnP(self.objpoints,imgpoints,self.camera_matrix,self.dist_coeff) - rvec_full, jac1 = cv2.Rodrigues(rvec) - rt_mtx_gate2quad = np.concatenate((rvec_full, tvec), axis=1) - rt_mtx_gate2quad = np.concatenate((rt_mtx_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) + # find rotation and translation from gate frame to quad frame + __, rvec, tvec = cv2.solvePnP(self.gate_points_3D,gate_points_2D,self.camera_matrix,self.dist_coeff) + rvec_full, __ = cv2.Rodrigues(rvec) + rot_translation_matrix_gate2quad = np.concatenate((rvec_full, tvec), axis=1) + rot_translation_matrix_gate2quad = np.concatenate((rot_translation_matrix_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) # gate frame waypoint to global frame waypoint - waypoint_glob_1 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_1) - waypoint_glob_2 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_2) - waypoint_glob_3 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_3) - waypoint_glob_4 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_4) - waypoint_glob_5 = self.gate_frame_waypoint_to_global_waypoint(state,rt_mtx_gate2quad,rt_mtx_quad2global,self.waypoint_gate_5) + waypoint_glob_1 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_2) + waypoint_glob_2 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_2) + waypoint_glob_3 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_3) # if quad is too close to next waypoint, move through gate without taking measurements quad_to_next_gate_dist = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_2)) if quad_to_next_gate_dist < 3.0 and self.close_count == 0: - print("too close to gate") + print("Too close to gate") self.airsim_client.moveOnSplineAsync([waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) time.sleep(2) self.measurement_count = 0 @@ -188,22 +188,19 @@ def run(self): self.close_count = 1 self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) else: - if self.close_count == 1: # right after passing through gate, reinitialize + if self.close_count == 1: # right after passing through gate, reset the average self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) self.measurement_count = 0 self.close_count = 0 measurement_diff = abs(np.linalg.norm(waypoint_glob_2-self.waypoint_ave_2)) quad_to_measurement_dist = abs(np.linalg.norm(waypoint_glob_2-np.array([state.position.x_val,state.position.y_val,state.position.z_val]))) - if measurement_diff > 0.5: # if new measurement is very far from previous measurements, wrong gate is measured - print(measurement_diff, quad_to_measurement_dist) print("wrong gate", self.wrong_gate_count) - if self.wrong_gate_count > 10: # if wrong gate is measured over 10 times, reinitialize next waypoint + if self.wrong_gate_count > 10: # if wrong gate is measured over 10 times, reset the average self.measurement_count = 0 self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) self.wrong_gate_count += 1 elif quad_to_measurement_dist > 20: # if new measurement is very far from quad, wrong gate is measured - print(measurement_diff, quad_to_measurement_dist) print("wrong gate", self.wrong_gate_count) else: cv2.circle(image_rgb, (int(gate_center_pixel[0]), int(gate_center_pixel[1])), 10, (255, 0, 0), -1) @@ -218,32 +215,22 @@ def run(self): print("Gate detected, Measurement Taken") print(self.measurement_count) - # calculate moving average of gate waypoints - if self.measurement_count == 1: + # Calculate moving average of gate waypoints + if self.measurement_count == 1: # reset average measurement_estimates_mtx_1 = np.empty((0,3)) measurement_estimates_mtx_2 = np.empty((0,3)) measurement_estimates_mtx_3 = np.empty((0,3)) - measurement_estimates_mtx_4 = np.empty((0,3)) - measurement_estimates_mtx_5 = np.empty((0,3)) - measurement_estimates_mtx_1 = np.append(measurement_estimates_mtx_1, waypoint_glob_1,axis=0) - measurement_estimates_mtx_2 = np.append(measurement_estimates_mtx_2, waypoint_glob_2,axis=0) - measurement_estimates_mtx_3 = np.append(measurement_estimates_mtx_3, waypoint_glob_3,axis=0) - measurement_estimates_mtx_4 = np.append(measurement_estimates_mtx_4, waypoint_glob_4,axis=0) - measurement_estimates_mtx_5 = np.append(measurement_estimates_mtx_5, waypoint_glob_5,axis=0) - waypoint_ave_1 = np.reshape(np.mean(measurement_estimates_mtx_1,axis=0),(1,3)) - self.waypoint_ave_2 = np.reshape(np.mean(measurement_estimates_mtx_2,axis=0),(1,3)) - waypoint_ave_3 = np.reshape(np.mean(measurement_estimates_mtx_3,axis=0),(1,3)) - waypoint_ave_4 = np.reshape(np.mean(measurement_estimates_mtx_4,axis=0),(1,3)) - waypoint_ave_5 = np.reshape(np.mean(measurement_estimates_mtx_5,axis=0),(1,3)) + waypoint_ave_1 = self.get_average_waypoint(measurement_estimates_mtx_1,waypoint_glob_1) + self.waypoint_ave_2 = self.get_average_waypoint(measurement_estimates_mtx_2,waypoint_glob_2) + waypoint_ave_3 = self.get_average_waypoint(measurement_estimates_mtx_3,waypoint_glob_3) # waypoints waypoint_1 = airsim.Vector3r(waypoint_ave_1[0,0],waypoint_ave_1[0,1], waypoint_ave_1[0,2]) waypoint_2 = airsim.Vector3r(self.waypoint_ave_2[0,0],self.waypoint_ave_2[0,1], self.waypoint_ave_2[0,2]) waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) - waypoint_4 = airsim.Vector3r(waypoint_ave_4[0,0],waypoint_ave_4[0,1], waypoint_ave_4[0,2]) - waypoint_5 = airsim.Vector3r(waypoint_ave_5[0,0],waypoint_ave_5[0,1], waypoint_ave_5[0,2]) self.airsim_client.moveOnSplineAsync([waypoint_2], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + #self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py From 4cd20ace45a69e9ca25c30e1171a26fa00afadaf Mon Sep 17 00:00:00 2001 From: Keiko Date: Fri, 16 Aug 2019 12:44:31 -0700 Subject: [PATCH 06/27] small fixes --- baselines/baseline_racer_perception.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 1077744..fd31e18 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -13,6 +13,7 @@ class BaselineRacerPerception(BaselineRacer): def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False): BaselineRacer.__init__(self, drone_name, plot_transform, viz_traj) self.eps = 0.01 + self.aspect_ratio_max = 1.4 self.waypoint_ave_2 = np.zeros((1,3)) self.kernel = np.ones((3,3),np.uint8) self.gate_corners_flat = np.float32([[[94, 64],[204, 64],[204, 174], [94, 174]]]) @@ -30,7 +31,7 @@ def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False) self.waypoint_gate_3 = np.array([0,0.0,-1.0,1.0]).T.reshape(4,1) self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) - # Find area of polygon + # Find area of gate def find_gate_area(self, x_coords ,y_coords): area_numerator = 0 for i in range(4): @@ -41,7 +42,7 @@ def find_gate_area(self, x_coords ,y_coords): gate_area = abs(area_numerator/2.0) return gate_area - # Find four gate corners given more than four points + # Find four gate corners with largest area given more than four points def find_four_gate_corners_largest_area(self, gate_corners): gate_corners = gate_corners.reshape(len(gate_corners),2) gate_corner_combinations = list(combinations(gate_corners,4)) @@ -71,7 +72,7 @@ def find_aspect_ratio(self, gate_corners): for i in range(4): if aspect_ratio_mtx[0,i] < 1.0: aspect_ratio_mtx[0,i] = 1/aspect_ratio_mtx[0,i] - if aspect_ratio_mtx[0,i] > 1.4: + if aspect_ratio_mtx[0,i] > self.aspect_ratio_max: large_aspect_ratio = aspect_ratio_mtx[0,i] return large_aspect_ratio @@ -80,8 +81,6 @@ def get_rotation_matrix_quad_frame_to_global_frame(self,state): q1 = state.orientation.x_val q2 = state.orientation.y_val q3 = state.orientation.z_val - - # rotation matrix between quad body frame and global frame rot_matrix_quad2global = np.array([[1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3-q0*q2)], [2*(q1*q2 + q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) @@ -104,7 +103,6 @@ def run(self): #self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max = 2.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name) time.sleep(2) - waypoint_2 = airsim.Vector3r(0,10,self.gate_poses_ground_truth[0].position.z_val) 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)]) @@ -139,7 +137,7 @@ def run(self): gate_corners = self.find_four_gate_corners_largest_area(gate_corners) if len(gate_corners) == 4: aspect_ratio = self.find_aspect_ratio(gate_corners) - if aspect_ratio <= 1.4: + if aspect_ratio <= self.aspect_ratio_max: largest_area = cv2.contourArea(contour) gate_corners_best = gate_corners From 7d1329ff746ec2277d6d5f77971cfef32e5633fb Mon Sep 17 00:00:00 2001 From: Keiko Date: Fri, 16 Aug 2019 14:32:12 -0700 Subject: [PATCH 07/27] small fix --- baselines/baseline_racer_perception.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index fd31e18..b60b867 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -213,7 +213,7 @@ def run(self): print("Gate detected, Measurement Taken") print(self.measurement_count) - # Calculate moving average of gate waypoints + # Calculate average of gate waypoints if self.measurement_count == 1: # reset average measurement_estimates_mtx_1 = np.empty((0,3)) measurement_estimates_mtx_2 = np.empty((0,3)) From 02073e0680acfeca9c6b7034b55cb58c2758992f Mon Sep 17 00:00:00 2001 From: madratman Date: Tue, 20 Aug 2019 16:43:12 -0700 Subject: [PATCH 08/27] allow viz option --- baselines/baseline_racer_perception.py | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index b60b867..56624fd 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -100,9 +100,11 @@ def get_average_waypoint(self,measurement_estimates_mtx,waypoint_glob): def run(self): # Move through first gate self.get_ground_truth_gate_poses() - #self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) + if self.plot_transform: + self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max = 2.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name) time.sleep(2) + 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)]) @@ -120,9 +122,11 @@ def run(self): __, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) # gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - cv2.imshow("mask", mask) - cv2.imshow("dilated_gate", dilated_gate) - cv2.imshow("eroded_gate", eroded_gate) + print("got image") + cv2.imshow("image_rgb", image_rgb) + # cv2.imshow("mask", mask) + # cv2.imshow("dilated_gate", dilated_gate) + # cv2.imshow("eroded_gate", eroded_gate) cv2.waitKey(1) largest_area = 0.0 @@ -179,6 +183,8 @@ def run(self): quad_to_next_gate_dist = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_2)) if quad_to_next_gate_dist < 3.0 and self.close_count == 0: print("Too close to gate") + if self.plot_transform: + self.airsim_client.plot_transform([airsim.Pose(waypoint_3, airsim.Quaternionr())], vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) time.sleep(2) self.measurement_count = 0 @@ -227,9 +233,13 @@ def run(self): waypoint_2 = airsim.Vector3r(self.waypoint_ave_2[0,0],self.waypoint_ave_2[0,1], self.waypoint_ave_2[0,2]) waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) + if self.plot_transform: + self.airsim_client.plot_transform([airsim.Pose(waypoint_2, airsim.Quaternionr())], vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([waypoint_2], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) #self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + #time.sleep(0.5) + def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py baseline_racer = BaselineRacerPerception(drone_name="drone_1", plot_transform=args.plot_transform, viz_traj=args.viz_traj) @@ -241,7 +251,7 @@ def main(args): if __name__ == "__main__": parser = ArgumentParser() parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy"], default="Soccer_Field_Easy") - parser.add_argument('--plot_transform', dest='plot_transform', action='store_false', default=False) - parser.add_argument('--viz_traj', dest='viz_traj', action='store_false', default=False) + parser.add_argument('--plot_transform', dest='plot_transform', action='store_true', default=False) + parser.add_argument('--viz_traj', dest='viz_traj', action='store_true', default=False) args = parser.parse_args() main(args) From 1c136d346a2ff59c857062024b3bdf29d9cd5a3d Mon Sep 17 00:00:00 2001 From: madratman Date: Tue, 20 Aug 2019 16:47:12 -0700 Subject: [PATCH 09/27] to squash commit --- baselines/baseline_racer_perception.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 56624fd..b39fcab 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -123,10 +123,9 @@ def run(self): # gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) print("got image") - cv2.imshow("image_rgb", image_rgb) - # cv2.imshow("mask", mask) - # cv2.imshow("dilated_gate", dilated_gate) - # cv2.imshow("eroded_gate", eroded_gate) + cv2.imshow("mask", mask) + cv2.imshow("dilated_gate", dilated_gate) + cv2.imshow("eroded_gate", eroded_gate) cv2.waitKey(1) largest_area = 0.0 From a09f6bb11ab18354bb26ca8f0d21838671c2ef57 Mon Sep 17 00:00:00 2001 From: Keiko Date: Wed, 21 Aug 2019 14:26:19 -0700 Subject: [PATCH 10/27] waypoint_1 fix --- baselines/baseline_racer_perception.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index b39fcab..9e268b4 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -26,9 +26,9 @@ def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False) self.lower_green = np.array([0, 210, 0]) self.upper_green = np.array([200, 255, 200]) self.dist_coeff = np.zeros((1,5)) - self.waypoint_gate_1 = np.array([0,0.0,+1.0,1.0]).T.reshape(4,1) - self.waypoint_gate_2 = np.array([0.0,0.0,0.0,1.0]).T.reshape(4,1) - self.waypoint_gate_3 = np.array([0,0.0,-1.0,1.0]).T.reshape(4,1) + self.waypoint_gate_1 = np.array([0.0, 0.0, +1.0, 1.0]).T.reshape(4,1) + self.waypoint_gate_2 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) + self.waypoint_gate_3 = np.array([0.0, 0.0, -1.0, 1.0]).T.reshape(4,1) self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) # Find area of gate @@ -174,7 +174,7 @@ def run(self): rot_translation_matrix_gate2quad = np.concatenate((rot_translation_matrix_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) # gate frame waypoint to global frame waypoint - waypoint_glob_1 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_2) + waypoint_glob_1 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_1) waypoint_glob_2 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_2) waypoint_glob_3 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_3) From d46ded037e39408f1e333cc8fa5b039b0c6d1cfb Mon Sep 17 00:00:00 2001 From: Keiko Date: Tue, 27 Aug 2019 11:50:30 -0700 Subject: [PATCH 11/27] move waypoint 3 further away --- baselines/baseline_racer_perception.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 9e268b4..7df77c8 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -28,7 +28,7 @@ def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False) self.dist_coeff = np.zeros((1,5)) self.waypoint_gate_1 = np.array([0.0, 0.0, +1.0, 1.0]).T.reshape(4,1) self.waypoint_gate_2 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) - self.waypoint_gate_3 = np.array([0.0, 0.0, -1.0, 1.0]).T.reshape(4,1) + self.waypoint_gate_3 = np.array([0.0, 0.0, -2.0, 1.0]).T.reshape(4,1) self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) # Find area of gate @@ -236,7 +236,7 @@ def run(self): self.airsim_client.plot_transform([airsim.Pose(waypoint_2, airsim.Quaternionr())], vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([waypoint_2], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) #self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) - + #time.sleep(0.5) #time.sleep(0.5) def main(args): From 310550b6a6f631adb7a4be6aa255a469ae4b9ae5 Mon Sep 17 00:00:00 2001 From: Keiko Date: Wed, 4 Sep 2019 17:59:13 -0700 Subject: [PATCH 12/27] airsimneurips v0.1.1 --- baselines/baseline_racer_perception.py | 57 ++++++++++++++++++-------- 1 file changed, 41 insertions(+), 16 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 7df77c8..033e5fa 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -10,8 +10,10 @@ import time class BaselineRacerPerception(BaselineRacer): - def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False): - BaselineRacer.__init__(self, drone_name, plot_transform, viz_traj) + #def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False): + def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0], viz_image_cv2=True): + #BaselineRacer.__init__(self, drone_name, plot_transform, viz_traj) + BaselineRacer.__init__(self, drone_name, viz_traj, viz_traj_color_rgba, viz_image_cv2) self.eps = 0.01 self.aspect_ratio_max = 1.4 self.waypoint_ave_2 = np.zeros((1,3)) @@ -23,13 +25,19 @@ def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False) self.measurement_count = 0 self.close_count = 1 self.wrong_gate_count = 0 - self.lower_green = np.array([0, 210, 0]) - self.upper_green = np.array([200, 255, 200]) + #self.lower_green = np.array([0, 210, 0]) + #self.upper_green = np.array([200, 255, 200]) + self.lower_green = np.array([0, 150, 0]) + self.upper_green = np.array([255, 255, 110]) self.dist_coeff = np.zeros((1,5)) self.waypoint_gate_1 = np.array([0.0, 0.0, +1.0, 1.0]).T.reshape(4,1) self.waypoint_gate_2 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) self.waypoint_gate_3 = np.array([0.0, 0.0, -2.0, 1.0]).T.reshape(4,1) self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) + self.viz_traj = viz_traj + self.viz_traj_color_rgba = viz_traj_color_rgba + self.drone_name = drone_name + self.viz_image_cv2 = viz_image_cv2 # Find area of gate def find_gate_area(self, x_coords ,y_coords): @@ -100,9 +108,12 @@ def get_average_waypoint(self,measurement_estimates_mtx,waypoint_glob): def run(self): # Move through first gate self.get_ground_truth_gate_poses() - if self.plot_transform: - self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) - self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max = 2.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + # if self.plot_transform: + # self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) + # self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max = 2.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max=2.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, + add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) + time.sleep(2) while self.airsim_client.isApiControlEnabled(vehicle_name=self.drone_name): @@ -117,6 +128,7 @@ def run(self): # Find corners of the gate mask = cv2.inRange(image_rgb,self.lower_green,self.upper_green) + #print(mask) dilated_gate = cv2.dilate(mask,self.kernel, iterations=8) eroded_gate = cv2.erode(dilated_gate,self.kernel, iterations=8) __, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) @@ -182,10 +194,13 @@ def run(self): quad_to_next_gate_dist = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_2)) if quad_to_next_gate_dist < 3.0 and self.close_count == 0: print("Too close to gate") - if self.plot_transform: - self.airsim_client.plot_transform([airsim.Pose(waypoint_3, airsim.Quaternionr())], vehicle_name=self.drone_name) - self.airsim_client.moveOnSplineAsync([waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) - time.sleep(2) + #if self.plot_transform: + # self.airsim_client.plot_transform([airsim.Pose(waypoint_3, airsim.Quaternionr())], vehicle_name=self.drone_name) + #self.airsim_client.moveOnSplineAsync([waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + # self.airsim_client.moveOnSplineAsync([waypoint_3], vel_max=2.0, acc_max=2.0, add_position_constraint=True, add_velocity_constraint=False, + # add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) + + time.sleep(3) self.measurement_count = 0 self.wrong_gate_count = 0 self.close_count = 1 @@ -205,6 +220,11 @@ def run(self): self.wrong_gate_count += 1 elif quad_to_measurement_dist > 20: # if new measurement is very far from quad, wrong gate is measured print("wrong gate", self.wrong_gate_count) + print(quad_to_measurement_dist) + if self.wrong_gate_count > 10: # if wrong gate is measured over 10 times, reset the average + self.measurement_count = 0 + #self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.05, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate + #self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) else: cv2.circle(image_rgb, (int(gate_center_pixel[0]), int(gate_center_pixel[1])), 10, (255, 0, 0), -1) cv2.circle(image_rgb, (int(gate_corners_plot[0][0]), int(gate_corners_plot[0][1])), 10, (255, 100, 0), -1) @@ -232,16 +252,20 @@ def run(self): waypoint_2 = airsim.Vector3r(self.waypoint_ave_2[0,0],self.waypoint_ave_2[0,1], self.waypoint_ave_2[0,2]) waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) - if self.plot_transform: - self.airsim_client.plot_transform([airsim.Pose(waypoint_2, airsim.Quaternionr())], vehicle_name=self.drone_name) - self.airsim_client.moveOnSplineAsync([waypoint_2], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + #if self.plot_transform: + # self.airsim_client.plot_transform([airsim.Pose(waypoint_2, airsim.Quaternionr())], vehicle_name=self.drone_name) + #self.airsim_client.moveOnSplineAsync([waypoint_2], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) + self.airsim_client.moveOnSplineAsync([waypoint_1, waypoint_2, waypoint_3], vel_max=2.0, acc_max=2.0, add_position_constraint=True, add_velocity_constraint=False, + add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) + #self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) #time.sleep(0.5) #time.sleep(0.5) def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py - baseline_racer = BaselineRacerPerception(drone_name="drone_1", plot_transform=args.plot_transform, viz_traj=args.viz_traj) + #baseline_racer = BaselineRacerPerception(drone_name="drone_1", plot_transform=args.plot_transform, viz_traj=args.viz_traj) + baseline_racer = BaselineRacerPerception(drone_name="drone_1", viz_traj=args.viz_traj, viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0], viz_image_cv2=args.viz_image_cv2) baseline_racer.load_level(args.level_name) baseline_racer.initialize_drone() baseline_racer.takeoff_with_moveOnSpline() @@ -250,7 +274,8 @@ def main(args): if __name__ == "__main__": parser = ArgumentParser() parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy"], default="Soccer_Field_Easy") - parser.add_argument('--plot_transform', dest='plot_transform', action='store_true', default=False) + #parser.add_argument('--plot_transform', dest='plot_transform', action='store_true', default=False) parser.add_argument('--viz_traj', dest='viz_traj', action='store_true', default=False) + parser.add_argument('--enable_viz_image_cv2', dest='viz_image_cv2', action='store_true', default=False) args = parser.parse_args() main(args) From fc6e1d3b9f66dd3a95df5fb3450c0c09abbb56f4 Mon Sep 17 00:00:00 2001 From: Keiko Date: Wed, 4 Sep 2019 18:08:19 -0700 Subject: [PATCH 13/27] decrease max wrong gate count --- baselines/baseline_racer_perception.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 033e5fa..2843467 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -214,14 +214,14 @@ def run(self): quad_to_measurement_dist = abs(np.linalg.norm(waypoint_glob_2-np.array([state.position.x_val,state.position.y_val,state.position.z_val]))) if measurement_diff > 0.5: # if new measurement is very far from previous measurements, wrong gate is measured print("wrong gate", self.wrong_gate_count) - if self.wrong_gate_count > 10: # if wrong gate is measured over 10 times, reset the average + if self.wrong_gate_count > 5: # if wrong gate is measured over 10 times, reset the average self.measurement_count = 0 self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) self.wrong_gate_count += 1 elif quad_to_measurement_dist > 20: # if new measurement is very far from quad, wrong gate is measured print("wrong gate", self.wrong_gate_count) print(quad_to_measurement_dist) - if self.wrong_gate_count > 10: # if wrong gate is measured over 10 times, reset the average + if self.wrong_gate_count > 5: # if wrong gate is measured over 10 times, reset the average self.measurement_count = 0 #self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.05, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate #self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) From b1627daa9b1fd36a80df91eb5cdcc09bf1c6fbf7 Mon Sep 17 00:00:00 2001 From: Keiko Date: Thu, 5 Sep 2019 13:12:35 -0700 Subject: [PATCH 14/27] multiple masks --- baselines/baseline_racer_perception.py | 48 +++++++------------------- 1 file changed, 12 insertions(+), 36 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 2843467..42a712c 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -10,9 +10,7 @@ import time class BaselineRacerPerception(BaselineRacer): - #def __init__(self, drone_name = "drone_1", plot_transform=False, viz_traj=False): def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0], viz_image_cv2=True): - #BaselineRacer.__init__(self, drone_name, plot_transform, viz_traj) BaselineRacer.__init__(self, drone_name, viz_traj, viz_traj_color_rgba, viz_image_cv2) self.eps = 0.01 self.aspect_ratio_max = 1.4 @@ -25,19 +23,17 @@ def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[ self.measurement_count = 0 self.close_count = 1 self.wrong_gate_count = 0 - #self.lower_green = np.array([0, 210, 0]) - #self.upper_green = np.array([200, 255, 200]) - self.lower_green = np.array([0, 150, 0]) - self.upper_green = np.array([255, 255, 110]) + + self.lower_green1 = np.array([0, 120, 0]) + self.upper_green1 = np.array([50, 255, 170]) + self.lower_green2 = np.array([0, 175, 0]) + self.upper_green2 = np.array([150, 255, 200]) + self.dist_coeff = np.zeros((1,5)) self.waypoint_gate_1 = np.array([0.0, 0.0, +1.0, 1.0]).T.reshape(4,1) self.waypoint_gate_2 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) - self.waypoint_gate_3 = np.array([0.0, 0.0, -2.0, 1.0]).T.reshape(4,1) + self.waypoint_gate_3 = np.array([0.0, 0.0, -5.0, 1.0]).T.reshape(4,1) self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) - self.viz_traj = viz_traj - self.viz_traj_color_rgba = viz_traj_color_rgba - self.drone_name = drone_name - self.viz_image_cv2 = viz_image_cv2 # Find area of gate def find_gate_area(self, x_coords ,y_coords): @@ -108,27 +104,23 @@ def get_average_waypoint(self,measurement_estimates_mtx,waypoint_glob): def run(self): # Move through first gate self.get_ground_truth_gate_poses() - # if self.plot_transform: - # self.airsim_client.plot_transform([self.gate_poses_ground_truth[0]], vehicle_name=self.drone_name) - # self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max = 2.0, acc_max = 5.0, viz_traj=self.viz_traj, vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max=2.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) time.sleep(2) - 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) # get numpy array image_rgb = img1d.reshape(response[0].height, response[0].width, 3) - # get rotation matrix from quad frame to global frame state = self.airsim_client.simGetVehiclePose() rot_matrix_quad2global = self.get_rotation_matrix_quad_frame_to_global_frame(state) # Find corners of the gate - mask = cv2.inRange(image_rgb,self.lower_green,self.upper_green) - #print(mask) + mask1 = cv2.inRange(image_rgb,self.lower_green1,self.upper_green1) + mask2 = cv2.inRange(image_rgb,self.lower_green2,self.upper_green2) + mask = mask1 + mask2 dilated_gate = cv2.dilate(mask,self.kernel, iterations=8) eroded_gate = cv2.erode(dilated_gate,self.kernel, iterations=8) __, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) @@ -194,13 +186,7 @@ def run(self): quad_to_next_gate_dist = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_2)) if quad_to_next_gate_dist < 3.0 and self.close_count == 0: print("Too close to gate") - #if self.plot_transform: - # self.airsim_client.plot_transform([airsim.Pose(waypoint_3, airsim.Quaternionr())], vehicle_name=self.drone_name) - #self.airsim_client.moveOnSplineAsync([waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) - # self.airsim_client.moveOnSplineAsync([waypoint_3], vel_max=2.0, acc_max=2.0, add_position_constraint=True, add_velocity_constraint=False, - # add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) - - time.sleep(3) + time.sleep(5) self.measurement_count = 0 self.wrong_gate_count = 0 self.close_count = 1 @@ -223,8 +209,7 @@ def run(self): print(quad_to_measurement_dist) if self.wrong_gate_count > 5: # if wrong gate is measured over 10 times, reset the average self.measurement_count = 0 - #self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.05, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate - #self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) + self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.05, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate else: cv2.circle(image_rgb, (int(gate_center_pixel[0]), int(gate_center_pixel[1])), 10, (255, 0, 0), -1) cv2.circle(image_rgb, (int(gate_corners_plot[0][0]), int(gate_corners_plot[0][1])), 10, (255, 100, 0), -1) @@ -252,19 +237,11 @@ def run(self): waypoint_2 = airsim.Vector3r(self.waypoint_ave_2[0,0],self.waypoint_ave_2[0,1], self.waypoint_ave_2[0,2]) waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) - #if self.plot_transform: - # self.airsim_client.plot_transform([airsim.Pose(waypoint_2, airsim.Quaternionr())], vehicle_name=self.drone_name) - #self.airsim_client.moveOnSplineAsync([waypoint_2], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) self.airsim_client.moveOnSplineAsync([waypoint_1, waypoint_2, waypoint_3], vel_max=2.0, acc_max=2.0, add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) - #self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,waypoint_3], vel_max = 2.0, acc_max = 2.0, add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, viz_traj=self.viz_traj, vehicle_name=self.drone_name) - #time.sleep(0.5) - #time.sleep(0.5) - def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py - #baseline_racer = BaselineRacerPerception(drone_name="drone_1", plot_transform=args.plot_transform, viz_traj=args.viz_traj) baseline_racer = BaselineRacerPerception(drone_name="drone_1", viz_traj=args.viz_traj, viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0], viz_image_cv2=args.viz_image_cv2) baseline_racer.load_level(args.level_name) baseline_racer.initialize_drone() @@ -274,7 +251,6 @@ def main(args): if __name__ == "__main__": parser = ArgumentParser() parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy"], default="Soccer_Field_Easy") - #parser.add_argument('--plot_transform', dest='plot_transform', action='store_true', default=False) parser.add_argument('--viz_traj', dest='viz_traj', action='store_true', default=False) parser.add_argument('--enable_viz_image_cv2', dest='viz_image_cv2', action='store_true', default=False) args = parser.parse_args() From 5137b3e028d616ac3b314c76061d31510dd0cb87 Mon Sep 17 00:00:00 2001 From: Keiko Date: Thu, 5 Sep 2019 15:49:30 -0700 Subject: [PATCH 15/27] better thresholds --- baselines/baseline_racer_perception.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 42a712c..2b7f80a 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -25,7 +25,7 @@ def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[ self.wrong_gate_count = 0 self.lower_green1 = np.array([0, 120, 0]) - self.upper_green1 = np.array([50, 255, 170]) + self.upper_green1 = np.array([50, 255, 50]) self.lower_green2 = np.array([0, 175, 0]) self.upper_green2 = np.array([150, 255, 200]) @@ -105,8 +105,7 @@ def run(self): # Move through first gate self.get_ground_truth_gate_poses() self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max=2.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, - add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) - + add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) time.sleep(2) while self.airsim_client.isApiControlEnabled(vehicle_name=self.drone_name): # Take image From c35a46d732973a2355002977c27949885ae41dbe Mon Sep 17 00:00:00 2001 From: Keiko Date: Thu, 5 Sep 2019 17:45:26 -0700 Subject: [PATCH 16/27] higher acc_max --- baselines/baseline_racer_perception.py | 44 +++++++++++--------------- 1 file changed, 18 insertions(+), 26 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 2b7f80a..0b434aa 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -14,7 +14,7 @@ def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[ BaselineRacer.__init__(self, drone_name, viz_traj, viz_traj_color_rgba, viz_image_cv2) self.eps = 0.01 self.aspect_ratio_max = 1.4 - self.waypoint_ave_2 = np.zeros((1,3)) + self.waypoint_ave_1 = np.zeros((1,3)) self.kernel = np.ones((3,3),np.uint8) self.gate_corners_flat = np.float32([[[94, 64],[204, 64],[204, 174], [94, 174]]]) self.camera_matrix = np.array([[160.000, 0.000000, 160.000], [0.000000, 160.000, 120.000], [0.000000, 0.000000, 1.000000]]) @@ -23,16 +23,13 @@ def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[ self.measurement_count = 0 self.close_count = 1 self.wrong_gate_count = 0 - self.lower_green1 = np.array([0, 120, 0]) self.upper_green1 = np.array([50, 255, 50]) self.lower_green2 = np.array([0, 175, 0]) self.upper_green2 = np.array([150, 255, 200]) - self.dist_coeff = np.zeros((1,5)) - self.waypoint_gate_1 = np.array([0.0, 0.0, +1.0, 1.0]).T.reshape(4,1) - self.waypoint_gate_2 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) - self.waypoint_gate_3 = np.array([0.0, 0.0, -5.0, 1.0]).T.reshape(4,1) + self.waypoint_gate_1 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) + self.waypoint_gate_2 = np.array([0.0, 0.0, -4.0, 1.0]).T.reshape(4,1) self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) # Find area of gate @@ -112,6 +109,7 @@ def run(self): 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) # get numpy array image_rgb = img1d.reshape(response[0].height, response[0].width, 3) + # get rotation matrix from quad frame to global frame state = self.airsim_client.simGetVehiclePose() rot_matrix_quad2global = self.get_rotation_matrix_quad_frame_to_global_frame(state) @@ -153,7 +151,6 @@ def run(self): print("Gate NOT detected") if self.no_gate_count > 50: # If no gate has been detected for over 50 attempts, rotate to look for gate. self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.05, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate - else: self.no_gate_count = 0 gate_corners_best = order_points(gate_corners_best.reshape(4,2)) @@ -179,36 +176,34 @@ def run(self): # gate frame waypoint to global frame waypoint waypoint_glob_1 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_1) waypoint_glob_2 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_2) - waypoint_glob_3 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_3) # if quad is too close to next waypoint, move through gate without taking measurements - quad_to_next_gate_dist = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_2)) + quad_to_next_gate_dist = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_1)) if quad_to_next_gate_dist < 3.0 and self.close_count == 0: print("Too close to gate") - time.sleep(5) + time.sleep(3) self.measurement_count = 0 self.wrong_gate_count = 0 self.close_count = 1 - self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) + self.waypoint_ave_1 = copy.deepcopy(waypoint_glob_1) else: if self.close_count == 1: # right after passing through gate, reset the average - self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) + self.waypoint_ave_1 = copy.deepcopy(waypoint_glob_1) self.measurement_count = 0 self.close_count = 0 - measurement_diff = abs(np.linalg.norm(waypoint_glob_2-self.waypoint_ave_2)) - quad_to_measurement_dist = abs(np.linalg.norm(waypoint_glob_2-np.array([state.position.x_val,state.position.y_val,state.position.z_val]))) + measurement_diff = abs(np.linalg.norm(waypoint_glob_1-self.waypoint_ave_1)) + quad_to_measurement_dist = abs(np.linalg.norm(waypoint_glob_1-np.array([state.position.x_val,state.position.y_val,state.position.z_val]))) if measurement_diff > 0.5: # if new measurement is very far from previous measurements, wrong gate is measured print("wrong gate", self.wrong_gate_count) - if self.wrong_gate_count > 5: # if wrong gate is measured over 10 times, reset the average + if self.wrong_gate_count > 5: # if wrong gate is measured over 5 times, reset the average self.measurement_count = 0 - self.waypoint_ave_2 = copy.deepcopy(waypoint_glob_2) + self.waypoint_ave_1 = copy.deepcopy(waypoint_glob_1) self.wrong_gate_count += 1 elif quad_to_measurement_dist > 20: # if new measurement is very far from quad, wrong gate is measured print("wrong gate", self.wrong_gate_count) print(quad_to_measurement_dist) - if self.wrong_gate_count > 5: # if wrong gate is measured over 10 times, reset the average + if self.wrong_gate_count > 5: # if wrong gate is measured over 5 times, reset the average self.measurement_count = 0 - self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.05, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate else: cv2.circle(image_rgb, (int(gate_center_pixel[0]), int(gate_center_pixel[1])), 10, (255, 0, 0), -1) cv2.circle(image_rgb, (int(gate_corners_plot[0][0]), int(gate_corners_plot[0][1])), 10, (255, 100, 0), -1) @@ -226,17 +221,14 @@ def run(self): if self.measurement_count == 1: # reset average measurement_estimates_mtx_1 = np.empty((0,3)) measurement_estimates_mtx_2 = np.empty((0,3)) - measurement_estimates_mtx_3 = np.empty((0,3)) - waypoint_ave_1 = self.get_average_waypoint(measurement_estimates_mtx_1,waypoint_glob_1) - self.waypoint_ave_2 = self.get_average_waypoint(measurement_estimates_mtx_2,waypoint_glob_2) - waypoint_ave_3 = self.get_average_waypoint(measurement_estimates_mtx_3,waypoint_glob_3) + self.waypoint_ave_1 = self.get_average_waypoint(measurement_estimates_mtx_1,waypoint_glob_1) + waypoint_ave_2 = self.get_average_waypoint(measurement_estimates_mtx_2,waypoint_glob_2) # waypoints - waypoint_1 = airsim.Vector3r(waypoint_ave_1[0,0],waypoint_ave_1[0,1], waypoint_ave_1[0,2]) - waypoint_2 = airsim.Vector3r(self.waypoint_ave_2[0,0],self.waypoint_ave_2[0,1], self.waypoint_ave_2[0,2]) - waypoint_3 = airsim.Vector3r(waypoint_ave_3[0,0],waypoint_ave_3[0,1], waypoint_ave_3[0,2]) + waypoint_1 = airsim.Vector3r(self.waypoint_ave_1[0,0],self.waypoint_ave_1[0,1], self.waypoint_ave_1[0,2]) + waypoint_2 = airsim.Vector3r(waypoint_ave_2[0,0],waypoint_ave_2[0,1], waypoint_ave_2[0,2]) - self.airsim_client.moveOnSplineAsync([waypoint_1, waypoint_2, waypoint_3], vel_max=2.0, acc_max=2.0, add_position_constraint=True, add_velocity_constraint=False, + self.airsim_client.moveOnSplineAsync([waypoint_1, waypoint_2], vel_max=2.0, acc_max=4.0, add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) def main(args): From 02d58c8eb66873121a0b3412fb8a39e085586745 Mon Sep 17 00:00:00 2001 From: Keiko Date: Fri, 11 Oct 2019 18:48:22 -0700 Subject: [PATCH 17/27] add start_race --- baselines/baseline_racer_perception.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 0b434aa..db2ccd2 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -120,8 +120,8 @@ def run(self): mask = mask1 + mask2 dilated_gate = cv2.dilate(mask,self.kernel, iterations=8) eroded_gate = cv2.erode(dilated_gate,self.kernel, iterations=8) - __, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - # gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + #__, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) print("got image") cv2.imshow("mask", mask) @@ -235,6 +235,7 @@ def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py baseline_racer = BaselineRacerPerception(drone_name="drone_1", viz_traj=args.viz_traj, viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0], viz_image_cv2=args.viz_image_cv2) baseline_racer.load_level(args.level_name) + baseline_racer.start_race(args.race_tier) baseline_racer.initialize_drone() baseline_racer.takeoff_with_moveOnSpline() baseline_racer.run() @@ -244,5 +245,6 @@ def main(args): parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy"], default="Soccer_Field_Easy") parser.add_argument('--viz_traj', dest='viz_traj', action='store_true', default=False) parser.add_argument('--enable_viz_image_cv2', dest='viz_image_cv2', action='store_true', default=False) + parser.add_argument('--race_tier', type=int, choices=[1,2,3], default=2) args = parser.parse_args() main(args) From 48ec5c842ce2c9f323c40553854bf994abc669ec Mon Sep 17 00:00:00 2001 From: Keiko Date: Fri, 1 Nov 2019 16:22:35 -0700 Subject: [PATCH 18/27] use priors and kalman filter --- baselines/baseline_racer_perception.py | 305 ++++++++++++------------- 1 file changed, 148 insertions(+), 157 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index db2ccd2..9227772 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -2,6 +2,7 @@ from baseline_racer import BaselineRacer from imutils.perspective import order_points from itertools import combinations +from numpy.linalg import multi_dot import airsimneurips as airsim import copy import cv2 @@ -13,69 +14,25 @@ class BaselineRacerPerception(BaselineRacer): def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0], viz_image_cv2=True): BaselineRacer.__init__(self, drone_name, viz_traj, viz_traj_color_rgba, viz_image_cv2) self.eps = 0.01 - self.aspect_ratio_max = 1.4 - self.waypoint_ave_1 = np.zeros((1,3)) self.kernel = np.ones((3,3),np.uint8) self.gate_corners_flat = np.float32([[[94, 64],[204, 64],[204, 174], [94, 174]]]) self.camera_matrix = np.array([[160.000, 0.000000, 160.000], [0.000000, 160.000, 120.000], [0.000000, 0.000000, 1.000000]]) self.gate_center_pixel_flat = np.array([149,119,1]).T self.no_gate_count = 0 self.measurement_count = 0 - self.close_count = 1 - self.wrong_gate_count = 0 self.lower_green1 = np.array([0, 120, 0]) self.upper_green1 = np.array([50, 255, 50]) self.lower_green2 = np.array([0, 175, 0]) - self.upper_green2 = np.array([150, 255, 200]) + self.upper_green2 = np.array([170, 255, 150]) self.dist_coeff = np.zeros((1,5)) self.waypoint_gate_1 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) self.waypoint_gate_2 = np.array([0.0, 0.0, -4.0, 1.0]).T.reshape(4,1) self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) - - # Find area of gate - def find_gate_area(self, x_coords ,y_coords): - area_numerator = 0 - for i in range(4): - if i == 3: - area_numerator += (x_coords[i]*y_coords[0]- y_coords[i]*x_coords[0]) - else: - area_numerator += (x_coords[i]*y_coords[i+1] - y_coords[i]*x_coords[i+1]) - gate_area = abs(area_numerator/2.0) - return gate_area - - # Find four gate corners with largest area given more than four points - def find_four_gate_corners_largest_area(self, gate_corners): - gate_corners = gate_corners.reshape(len(gate_corners),2) - gate_corner_combinations = list(combinations(gate_corners,4)) - largest_area = 0.0 - for comb in gate_corner_combinations: - comb_array = np.empty((0,2)) - for i in comb: - i = np.reshape(i,(1,2)) - comb_array = np.append(comb_array,i,axis=0) - comb_array = order_points(comb_array) - gate_area = self.find_gate_area(comb_array[:,0], comb_array[:,1]) - if gate_area > largest_area: - largest_area = copy.deepcopy(gate_area) - gate_corners_largest_area = copy.deepcopy(comb_array) - gate_corners_largest_area = gate_corners_largest_area.astype(int).reshape(4,1,2) - return gate_corners_largest_area - - # Find the highest aspect ratio of the gate - # if aspect ratio is too high, a gate side may have been detected instead of a full gate - def find_aspect_ratio(self, gate_corners): - aspect_ratio_mtx = np.empty((1,4)) - aspect_ratio_mtx[0,0] = abs(np.float32(gate_corners[1][0][1] - gate_corners[2][0][1])/np.float32(gate_corners[0][0][0] - gate_corners[1][0][0]+self.eps)) - aspect_ratio_mtx[0,1] = abs(np.float32(gate_corners[0][0][1] - gate_corners[3][0][1])/np.float32(gate_corners[2][0][0] - gate_corners[3][0][0]+self.eps)) - aspect_ratio_mtx[0,2] = abs(np.float32(gate_corners[0][0][1] - gate_corners[3][0][1])/np.float32(gate_corners[0][0][0] - gate_corners[1][0][0]+self.eps)) - aspect_ratio_mtx[0,3] = abs(np.float32(gate_corners[1][0][1] - gate_corners[2][0][1])/np.float32(gate_corners[2][0][0] - gate_corners[3][0][0]+self.eps)) - large_aspect_ratio = 1.0 - for i in range(4): - if aspect_ratio_mtx[0,i] < 1.0: - aspect_ratio_mtx[0,i] = 1/aspect_ratio_mtx[0,i] - if aspect_ratio_mtx[0,i] > self.aspect_ratio_max: - large_aspect_ratio = aspect_ratio_mtx[0,i] - return large_aspect_ratio + self.C = np.identity(3) + self.Q = 50*np.identity(3) + self.too_close_count = 0 + self.largest_next_gate_position_dist = 1.7 + self.aspect_ratio_max = 2.0 def get_rotation_matrix_quad_frame_to_global_frame(self,state): q0 = state.orientation.w_val @@ -87,149 +44,183 @@ def get_rotation_matrix_quad_frame_to_global_frame(self,state): [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) return rot_matrix_quad2global + def find_aspect_ratio(self, gate_corners): + aspect_ratio_mtx = np.empty((1,4)) + aspect_ratio_mtx[0,0] = abs(np.float32(gate_corners[1][1] - gate_corners[2][1])/np.float32(gate_corners[0][0] - gate_corners[1][0]+self.eps)) + aspect_ratio_mtx[0,1] = abs(np.float32(gate_corners[0][1] - gate_corners[3][1])/np.float32(gate_corners[2][0] - gate_corners[3][0]+self.eps)) + aspect_ratio_mtx[0,2] = abs(np.float32(gate_corners[0][1] - gate_corners[3][1])/np.float32(gate_corners[0][0] - gate_corners[1][0]+self.eps)) + aspect_ratio_mtx[0,3] = abs(np.float32(gate_corners[1][1] - gate_corners[2][1])/np.float32(gate_corners[2][0] - gate_corners[3][0]+self.eps)) + large_aspect_ratio = 1.0 + for i in range(4): + if aspect_ratio_mtx[0,i] < 1.0: + aspect_ratio_mtx[0,i] = 1/aspect_ratio_mtx[0,i] + if aspect_ratio_mtx[0,i] > self.aspect_ratio_max: + large_aspect_ratio = aspect_ratio_mtx[0,i] + return large_aspect_ratio + def gate_frame_waypoint_to_global_waypoint(self,state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,waypoint_gate): waypoint_rel = np.dot(rot_translation_matrix_gate2quad, waypoint_gate) waypoint_rel = np.dot(rot_matrix_quad2global, np.array([waypoint_rel[2], waypoint_rel[0], -waypoint_rel[1]])) waypoint_glob = np.reshape(np.array([state.position.x_val + waypoint_rel[0], state.position.y_val + waypoint_rel[1], state.position.z_val - waypoint_rel[2]]),(1,3)) return waypoint_glob - def get_average_waypoint(self,measurement_estimates_mtx,waypoint_glob): - measurement_estimates_mtx = np.append(measurement_estimates_mtx, waypoint_glob,axis=0) - waypoint_ave = np.reshape(np.mean(measurement_estimates_mtx,axis=0),(1,3)) - return waypoint_ave + def find_measurement_error(self,four_gate_corners, mu_1, state, rot_matrix_quad2global): + four_gate_corners = order_points(four_gate_corners.reshape(4,2)) + gate_corners_plot = copy.deepcopy(four_gate_corners) + four_gate_corners = np.float32(four_gate_corners).reshape(-1,1,2) + + # find homography matrix between gate corner points in baseline image and corner points in image taken + homography_matrix, __ = cv2.findHomography(four_gate_corners, self.gate_corners_flat, cv2.RANSAC, 5.0) + + # find middle of gate pixel coordinates + gate_center_pixel = np.dot(np.linalg.inv(homography_matrix), self.gate_center_pixel_flat) + gate_center_pixel = (int(gate_center_pixel[0]/(gate_center_pixel[2]+self.eps)), int(gate_center_pixel[1]/(gate_center_pixel[2]+self.eps))) + + # coordinates of gate corners in image (pixel coordinates) + gate_points_2D = np.float32(np.concatenate((np.array(gate_center_pixel).reshape(-1,1,2),four_gate_corners),axis=0)) + + # find rotation and translation from gate frame to quad frame + __, rvec, tvec = cv2.solvePnP(self.gate_points_3D,gate_points_2D,self.camera_matrix,self.dist_coeff) + rvec_full, __ = cv2.Rodrigues(rvec) + rot_translation_matrix_gate2quad = np.concatenate((rvec_full, tvec), axis=1) + rot_translation_matrix_gate2quad = np.concatenate((rot_translation_matrix_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) + + # gate frame waypoint to global frame waypoint + waypoint_glob_1 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_1) + waypoint_glob_2 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_2) + waypoint_1_error = mu_1 - np.reshape(waypoint_glob_1,(3,1)) + measurement_error = np.linalg.norm(waypoint_1_error) + return measurement_error, waypoint_glob_1, waypoint_glob_2, gate_center_pixel, gate_corners_plot + + def kalman_filter(self, mu, sigma, y): + K = multi_dot([sigma,self.C.T,np.linalg.inv(multi_dot([self.C,sigma,self.C.T])+self.Q)]) + mu = mu + np.dot(K,(y-np.dot(self.C,mu))) + sigma = np.dot((np.identity(3)-np.dot(K,self.C)),sigma) + return mu, sigma def run(self): # Move through first gate - self.get_ground_truth_gate_poses() - self.airsim_client.moveOnSplineAsync([self.gate_poses_ground_truth[0].position], vel_max=2.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, - add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) - time.sleep(2) + self.get_ground_truth_gate_poses() # noisy since tier 2 is default + next_gate_idx = 0 + 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 + # 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) # get numpy array image_rgb = img1d.reshape(response[0].height, response[0].width, 3) - # get rotation matrix from quad frame to global frame + # 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) - - # Find corners of the gate mask1 = cv2.inRange(image_rgb,self.lower_green1,self.upper_green1) mask2 = cv2.inRange(image_rgb,self.lower_green2,self.upper_green2) mask = mask1 + mask2 dilated_gate = cv2.dilate(mask,self.kernel, iterations=8) - eroded_gate = cv2.erode(dilated_gate,self.kernel, iterations=8) - #__, gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + #eroded_gate = cv2.erode(dilated_gate,self.kernel, iterations=8) + gate_contours = None gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - print("got image") cv2.imshow("mask", mask) cv2.imshow("dilated_gate", dilated_gate) - cv2.imshow("eroded_gate", eroded_gate) + #cv2.imshow("eroded_gate", eroded_gate) cv2.waitKey(1) largest_area = 0.0 gate_corners = None - gate_corners_best = np.zeros((1,3)) + smallest_measurement_error = 100 + #check for each contour for contour in gate_contours: - if cv2.contourArea(contour) > largest_area: - 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) - if len(gate_corners) > 4: # check if there are more than four points given, find four corners that produce largest area - gate_corners = self.find_four_gate_corners_largest_area(gate_corners) - if len(gate_corners) == 4: - aspect_ratio = self.find_aspect_ratio(gate_corners) - if aspect_ratio <= self.aspect_ratio_max: - largest_area = cv2.contourArea(contour) - gate_corners_best = gate_corners + 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) + #now find the measurement error + 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 = self.find_aspect_ratio(four_gate_corners) + if measurement_error < smallest_measurement_error and aspect_ratio < self.aspect_ratio_max: + 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)) + #print(smallest_measurement_error, next_gate_position_dist, next_gate_idx) # MOVE - if largest_area < 800 or len(gate_corners_best)!=4: + if smallest_measurement_error > 5.0: self.no_gate_count += 1 print("Gate NOT detected") - if self.no_gate_count > 50: # If no gate has been detected for over 50 attempts, rotate to look for gate. - self.airsim_client.moveByYawRateAsync(yaw_rate=-15.0, duration=0.05, vehicle_name=self.drone_name).join() # rotate counter clockwise to look for next gate + if self.no_gate_count == 10 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") + self.airsim_client.moveOnSplineAsync([airsim.Vector3r(mu_1[0,0],mu_1[1,0], mu_1[2,0])], vel_max=15.0, acc_max=4.0, add_position_constraint=True, add_velocity_constraint=False, + add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=True) + elif next_gate_position_dist < self.largest_next_gate_position_dist: #don't collect measurements + print("Gate too close") + self.no_gate_count = 0 + self.too_close_count += 1 + if self.too_close_count == 1: + print("reset gate too close") + next_gate_idx += 1 + if next_gate_idx >= len(self.gate_poses_ground_truth): + next_gate_idx = 0 + print("race end") + mu_1_airsimvector = self.gate_poses_ground_truth[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 + self.too_close_count = 0 else: self.no_gate_count = 0 - gate_corners_best = order_points(gate_corners_best.reshape(4,2)) - gate_corners_plot = gate_corners_best - gate_corners_best = np.float32(gate_corners_best).reshape(-1,1,2) - - # find homography matrix between gate corner points in baseline image and corner points in image taken - homography_matrix, mask = cv2.findHomography(gate_corners_best, self.gate_corners_flat, cv2.RANSAC, 5.0) - - # find middle of gate pixel coordinates - gate_center_pixel = np.dot(np.linalg.inv(homography_matrix), self.gate_center_pixel_flat) - gate_center_pixel = (int(gate_center_pixel[0]/(gate_center_pixel[2]+self.eps)), int(gate_center_pixel[1]/(gate_center_pixel[2]+self.eps))) - - # coordinates of gate corners in image (pixel coordinates) - gate_points_2D = np.float32(np.concatenate((np.array(gate_center_pixel).reshape(-1,1,2),gate_corners_best),axis=0)) - - # find rotation and translation from gate frame to quad frame - __, rvec, tvec = cv2.solvePnP(self.gate_points_3D,gate_points_2D,self.camera_matrix,self.dist_coeff) - rvec_full, __ = cv2.Rodrigues(rvec) - rot_translation_matrix_gate2quad = np.concatenate((rvec_full, tvec), axis=1) - rot_translation_matrix_gate2quad = np.concatenate((rot_translation_matrix_gate2quad, np.array([0,0,0,1]).reshape(1,4)), axis=0) - - # gate frame waypoint to global frame waypoint - waypoint_glob_1 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_1) - waypoint_glob_2 = self.gate_frame_waypoint_to_global_waypoint(state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,self.waypoint_gate_2) - - # if quad is too close to next waypoint, move through gate without taking measurements - quad_to_next_gate_dist = abs(np.linalg.norm(np.array([state.position.x_val,state.position.y_val,state.position.z_val]) - self.waypoint_ave_1)) - if quad_to_next_gate_dist < 3.0 and self.close_count == 0: - print("Too close to gate") + 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: #don't collect measurements, continue to waypoint, reset filter with new mu and sigmas + print("Gate too close") time.sleep(3) - self.measurement_count = 0 - self.wrong_gate_count = 0 - self.close_count = 1 - self.waypoint_ave_1 = copy.deepcopy(waypoint_glob_1) - else: - if self.close_count == 1: # right after passing through gate, reset the average - self.waypoint_ave_1 = copy.deepcopy(waypoint_glob_1) + self.too_close_count += 1 + if self.too_close_count == 1: + print("reset gate too close") + next_gate_idx += 1 + if next_gate_idx >= len(self.gate_poses_ground_truth): + next_gate_idx = 0 + print("race end") + mu_1_airsimvector = self.gate_poses_ground_truth[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 - self.close_count = 0 - measurement_diff = abs(np.linalg.norm(waypoint_glob_1-self.waypoint_ave_1)) - quad_to_measurement_dist = abs(np.linalg.norm(waypoint_glob_1-np.array([state.position.x_val,state.position.y_val,state.position.z_val]))) - if measurement_diff > 0.5: # if new measurement is very far from previous measurements, wrong gate is measured - print("wrong gate", self.wrong_gate_count) - if self.wrong_gate_count > 5: # if wrong gate is measured over 5 times, reset the average - self.measurement_count = 0 - self.waypoint_ave_1 = copy.deepcopy(waypoint_glob_1) - self.wrong_gate_count += 1 - elif quad_to_measurement_dist > 20: # if new measurement is very far from quad, wrong gate is measured - print("wrong gate", self.wrong_gate_count) - print(quad_to_measurement_dist) - if self.wrong_gate_count > 5: # if wrong gate is measured over 5 times, reset the average - self.measurement_count = 0 - else: - cv2.circle(image_rgb, (int(gate_center_pixel[0]), int(gate_center_pixel[1])), 10, (255, 0, 0), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[0][0]), int(gate_corners_plot[0][1])), 10, (255, 100, 0), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[1][0]), int(gate_corners_plot[1][1])), 10, (255, 0, 100), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[2][0]), int(gate_corners_plot[2][1])), 10, (255, 200, 0), -1) - cv2.circle(image_rgb, (int(gate_corners_plot[3][0]), int(gate_corners_plot[3][1])), 10, (255, 0, 200), -1) - cv2.imshow("image_rgb", image_rgb) - cv2.waitKey(1) - self.wrong_gate_count = 0 - self.measurement_count += 1 - print("Gate detected, Measurement Taken") - print(self.measurement_count) - - # Calculate average of gate waypoints - if self.measurement_count == 1: # reset average - measurement_estimates_mtx_1 = np.empty((0,3)) - measurement_estimates_mtx_2 = np.empty((0,3)) - self.waypoint_ave_1 = self.get_average_waypoint(measurement_estimates_mtx_1,waypoint_glob_1) - waypoint_ave_2 = self.get_average_waypoint(measurement_estimates_mtx_2,waypoint_glob_2) - - # waypoints - waypoint_1 = airsim.Vector3r(self.waypoint_ave_1[0,0],self.waypoint_ave_1[0,1], self.waypoint_ave_1[0,2]) - waypoint_2 = airsim.Vector3r(waypoint_ave_2[0,0],waypoint_ave_2[0,1], waypoint_ave_2[0,2]) - - self.airsim_client.moveOnSplineAsync([waypoint_1, waypoint_2], vel_max=2.0, acc_max=4.0, add_position_constraint=True, add_velocity_constraint=False, - add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) + else: + 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) + self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2], vel_max=5.0, acc_max=3.0, add_position_constraint=True, add_velocity_constraint=False, + add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=True) def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py @@ -242,7 +233,7 @@ def main(args): if __name__ == "__main__": parser = ArgumentParser() - parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy"], default="Soccer_Field_Easy") + parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy", "Soccer_Field_Medium", "ZhangJiaJie_Medium", "Building99_Hard", "Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3"], default="Qualifier_Tier_2") parser.add_argument('--viz_traj', dest='viz_traj', action='store_true', default=False) parser.add_argument('--enable_viz_image_cv2', dest='viz_image_cv2', action='store_true', default=False) parser.add_argument('--race_tier', type=int, choices=[1,2,3], default=2) From 182c9e52339e0c5bfd16c19aec7918b3c7b3b625 Mon Sep 17 00:00:00 2001 From: Keiko Date: Fri, 1 Nov 2019 17:44:34 -0700 Subject: [PATCH 19/27] wait for moveonspline --- baselines/baseline_racer_perception.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 9227772..8dae103 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -33,6 +33,7 @@ def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[ self.too_close_count = 0 self.largest_next_gate_position_dist = 1.7 self.aspect_ratio_max = 2.0 + self.moveonspline_count = 0 def get_rotation_matrix_quad_frame_to_global_frame(self,state): q0 = state.orientation.w_val @@ -164,8 +165,8 @@ def run(self): self.no_gate_count = 0 self.too_close_count = 0 print("Move toward best estimate") - self.airsim_client.moveOnSplineAsync([airsim.Vector3r(mu_1[0,0],mu_1[1,0], mu_1[2,0])], vel_max=15.0, acc_max=4.0, add_position_constraint=True, add_velocity_constraint=False, - add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=True) + self.airsim_client.moveOnSplineAsync([airsim.Vector3r(mu_1[0,0],mu_1[1,0], mu_1[2,0])], vel_max=10.0, acc_max=3.0, add_position_constraint=True, add_velocity_constraint=False, + add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) elif next_gate_position_dist < self.largest_next_gate_position_dist: #don't collect measurements print("Gate too close") self.no_gate_count = 0 @@ -212,6 +213,7 @@ def run(self): print("Gate detected, Measurement Taken") print(self.measurement_count) self.too_close_count = 0 + self.moveonspline_count += 1 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) @@ -219,8 +221,10 @@ def run(self): 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) - self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2], vel_max=5.0, acc_max=3.0, add_position_constraint=True, add_velocity_constraint=False, - add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=True) + if self.moveonspline_count == 5: + self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2], vel_max=2.0, acc_max=4.0, add_position_constraint=True, add_velocity_constraint=False, + add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=True) + self.moveonspline_count = 0 def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py From 30e05c48ed46966d1672e32de266fdc129e8870f Mon Sep 17 00:00:00 2001 From: Keiko Date: Wed, 6 Nov 2019 14:47:56 -0800 Subject: [PATCH 20/27] vary vmax and amax --- baselines/baseline_racer_perception.py | 73 +++++++++++--------------- 1 file changed, 32 insertions(+), 41 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 8dae103..61f853c 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -29,9 +29,9 @@ def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[ self.waypoint_gate_2 = np.array([0.0, 0.0, -4.0, 1.0]).T.reshape(4,1) self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) self.C = np.identity(3) - self.Q = 50*np.identity(3) + self.Q = 5*np.identity(3) self.too_close_count = 0 - self.largest_next_gate_position_dist = 1.7 + self.largest_next_gate_position_dist = 2.5 self.aspect_ratio_max = 2.0 self.moveonspline_count = 0 @@ -158,31 +158,22 @@ def run(self): #print(smallest_measurement_error, next_gate_position_dist, next_gate_idx) # MOVE + self.moveonspline_count += 1 if smallest_measurement_error > 5.0: self.no_gate_count += 1 print("Gate NOT detected") - if self.no_gate_count == 10 and next_gate_position_dist >= self.largest_next_gate_position_dist: + if self.no_gate_count == 15 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") - self.airsim_client.moveOnSplineAsync([airsim.Vector3r(mu_1[0,0],mu_1[1,0], mu_1[2,0])], vel_max=10.0, acc_max=3.0, add_position_constraint=True, add_velocity_constraint=False, + print("Move toward best estimate", next_gate_position_dist) + if next_gate_position_dist > 20.0: + vmax = 7.0 + amax = 5.0 + else: + vmax = 3.0 + amax = 2.0 + 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=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) - elif next_gate_position_dist < self.largest_next_gate_position_dist: #don't collect measurements - print("Gate too close") - self.no_gate_count = 0 - self.too_close_count += 1 - if self.too_close_count == 1: - print("reset gate too close") - next_gate_idx += 1 - if next_gate_idx >= len(self.gate_poses_ground_truth): - next_gate_idx = 0 - print("race end") - mu_1_airsimvector = self.gate_poses_ground_truth[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 - self.too_close_count = 0 else: self.no_gate_count = 0 self.measurement_count += 1 @@ -193,27 +184,11 @@ def run(self): 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: #don't collect measurements, continue to waypoint, reset filter with new mu and sigmas - print("Gate too close") - time.sleep(3) - self.too_close_count += 1 - if self.too_close_count == 1: - print("reset gate too close") - next_gate_idx += 1 - if next_gate_idx >= len(self.gate_poses_ground_truth): - next_gate_idx = 0 - print("race end") - mu_1_airsimvector = self.gate_poses_ground_truth[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 - else: + 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 - self.moveonspline_count += 1 + 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) @@ -221,10 +196,26 @@ def run(self): 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: - self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2], vel_max=2.0, acc_max=4.0, add_position_constraint=True, add_velocity_constraint=False, + if self.moveonspline_count >= 5: + self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2], vel_max=7.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=True) self.moveonspline_count = 0 + if next_gate_position_dist < self.largest_next_gate_position_dist: #don't collect measurements, continue to waypoint, reset filter with new mu and sigmas + print("Gate too close") + time.sleep(3) + self.no_gate_count = 0 + self.too_close_count += 1 + if self.too_close_count == 1: + print("reset gate too close") + next_gate_idx += 1 + if next_gate_idx >= len(self.gate_poses_ground_truth): + next_gate_idx = 0 + print("race end") + mu_1_airsimvector = self.gate_poses_ground_truth[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 def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py From a6c14507fc709f5d1703c30661303d1b08b2f15a Mon Sep 17 00:00:00 2001 From: Keiko Date: Wed, 6 Nov 2019 14:54:13 -0800 Subject: [PATCH 21/27] update baseline_racer --- baselines/baseline_racer.py | 207 ++++++++++++++++++++++++++---------- 1 file changed, 152 insertions(+), 55 deletions(-) diff --git a/baselines/baseline_racer.py b/baselines/baseline_racer.py index 54c97c8..02257f5 100644 --- a/baselines/baseline_racer.py +++ b/baselines/baseline_racer.py @@ -1,25 +1,55 @@ from argparse import ArgumentParser import airsimneurips as airsim +import cv2 +import threading import time import utils +import numpy as np +import math # drone_name should match the name in ~/Document/AirSim/settings.json class BaselineRacer(object): - def __init__(self, drone_name = "drone_1", plot_transform=True, viz_traj=True): + def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0], viz_image_cv2=True): self.drone_name = drone_name self.gate_poses_ground_truth = None - self.plot_transform = plot_transform + self.viz_image_cv2 = viz_image_cv2 self.viz_traj = viz_traj + self.viz_traj_color_rgba = viz_traj_color_rgba + self.airsim_client = airsim.MultirotorClient() self.airsim_client.confirmConnection() + # we need two airsim MultirotorClient objects because the comm lib we use (rpclib) is not thread safe + # so we poll images in a thread using one airsim MultirotorClient object + # and use another airsim MultirotorClient for querying state commands + self.airsim_client_images = airsim.MultirotorClient() + self.airsim_client_images.confirmConnection() + self.airsim_client_odom = airsim.MultirotorClient() + self.airsim_client_odom.confirmConnection() self.level_name = None - + + self.image_callback_thread = threading.Thread(target=self.repeat_timer_image_callback, args=(self.image_callback, 0.03)) + self.odometry_callback_thread = threading.Thread(target=self.repeat_timer_odometry_callback, args=(self.odometry_callback, 0.02)) + self.is_image_thread_active = False + self.is_odometry_thread_active = False + + self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS = 10 # see https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/38 + + # loads desired level def load_level(self, level_name, sleep_sec = 2.0): self.level_name = level_name self.airsim_client.simLoadLevel(self.level_name) self.airsim_client.confirmConnection() # failsafe time.sleep(sleep_sec) # let the environment load completely + # Starts an instance of a race in your given level, if valid + def start_race(self, tier=3): + self.airsim_client.simStartRace(tier) + + # Resets a current race: moves players to start positions, timer and penalties reset + def reset_race(self): + self.airsim_client.simResetRace() + + # arms drone, enable APIs, set default traj tracker gains def initialize_drone(self): self.airsim_client.enableApiControl(vehicle_name=self.drone_name) self.airsim_client.arm(vehicle_name=self.drone_name) @@ -33,33 +63,44 @@ def initialize_drone(self): kp_vel_z = 0.4, kd_vel_z = 0.0, kp_yaw = 3.0, kd_yaw = 0.1) - self.airsim_client.setTrajectoryTrackerGains(traj_tracker_gains.to_list(), vehicle_name=self.drone_name) + self.airsim_client.setTrajectoryTrackerGains(traj_tracker_gains, vehicle_name=self.drone_name) time.sleep(0.2) def takeoffAsync(self): self.airsim_client.takeoffAsync().join() - def takeoff_with_moveOnSpline(self, takeoff_height = 0.1): - if self.level_name == "ZhangJiaJie_Medium": - takeoff_height = 0.3 - takeoff_waypoint = airsim.Vector3r(0, 0, -takeoff_height) - if(self.plot_transform): - self.airsim_client.plot_transform([airsim.Pose(takeoff_waypoint, airsim.Quaternionr())], vehicle_name=self.drone_name) + # like takeoffAsync(), but with moveOnSpline() + def takeoff_with_moveOnSpline(self, takeoff_height = 1.0): + start_position = self.airsim_client.simGetVehiclePose(vehicle_name=self.drone_name).position + takeoff_waypoint = airsim.Vector3r(start_position.x_val, start_position.y_val, start_position.z_val-takeoff_height) - self.airsim_client.moveOnSplineAsync([takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_curr_odom_position_constraint=True, - add_curr_odom_velocity_constraint=False, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() + self.airsim_client.moveOnSplineAsync([takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, + add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name).join() + # stores gate ground truth poses as a list of airsim.Pose() objects in self.gate_poses_ground_truth def get_ground_truth_gate_poses(self): gate_names_sorted_bad = sorted(self.airsim_client.simListSceneObjects("Gate.*")) # gate_names_sorted_bad is of the form `GateN_GARBAGE`. for example: # ['Gate0', 'Gate10_21', 'Gate11_23', 'Gate1_3', 'Gate2_5', 'Gate3_7', 'Gate4_9', 'Gate5_11', 'Gate6_13', 'Gate7_15', 'Gate8_17', 'Gate9_19'] # we sort them by their ibdex of occurence along the race track(N), and ignore the unreal garbage number after the underscore(GARBAGE) gate_indices_bad = [int(gate_name.split('_')[0][4:]) for gate_name in gate_names_sorted_bad] - gate_indices_correct = sorted(range(len(gate_indices_bad)), key=lambda k:gate_indices_bad[k]) + gate_indices_correct = sorted(range(len(gate_indices_bad)), key=lambda k: gate_indices_bad[k]) gate_names_sorted = [gate_names_sorted_bad[gate_idx] for gate_idx in gate_indices_correct] - self.gate_poses_ground_truth = [self.airsim_client.simGetObjectPose(gate_name) for gate_name in gate_names_sorted] + self.gate_poses_ground_truth = [] + for gate_name in gate_names_sorted: + curr_pose = self.airsim_client.simGetObjectPose(gate_name) + counter = 0 + while (math.isnan(curr_pose.position.x_val) or math.isnan(curr_pose.position.y_val) or math.isnan(curr_pose.position.z_val)) and (counter < self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS): + print(f"DEBUG: {gate_name} position is nan, retrying...") + counter += 1 + curr_pose = self.airsim_client.simGetObjectPose(gate_name) + assert not math.isnan(curr_pose.position.x_val), f"ERROR: {gate_name} curr_pose.position.x_val is still {curr_pose.position.x_val} after {counter} trials" + assert not math.isnan(curr_pose.position.y_val), f"ERROR: {gate_name} curr_pose.position.y_val is still {curr_pose.position.y_val} after {counter} trials" + assert not math.isnan(curr_pose.position.z_val), f"ERROR: {gate_name} curr_pose.position.z_val is still {curr_pose.position.z_val} after {counter} trials" + self.gate_poses_ground_truth.append(curr_pose) - # scale of the vector dictates speed of the velocity constraint + # this is utility function to get a velocity constraint which can be passed to moveOnSplineVelConstraints() + # the "scale" parameter scales the gate facing vector accordingly, thereby dictating the speed of the velocity constraint def get_gate_facing_vector_from_quaternion(self, airsim_quat, scale = 1.0): import numpy as np # convert gate quaternion to rotation matrix. @@ -85,15 +126,11 @@ def fly_through_all_gates_one_by_one_with_moveOnSpline(self): vel_max = 10.0 acc_max = 5.0 - for gate_pose in self.gate_poses_ground_truth: - if(self.plot_transform): - self.airsim_client.plot_transform([gate_pose], vehicle_name=self.drone_name) - - self.airsim_client.moveOnSplineAsync([gate_pose.position], vel_max=vel_max, acc_max=acc_max, - add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=False, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() + return self.airsim_client.moveOnSplineAsync([gate_pose.position], vel_max=vel_max, acc_max=acc_max, + add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) def fly_through_all_gates_at_once_with_moveOnSpline(self): - if self.level_name in ["Soccer_Field_Medium", "Soccer_Field_Easy", "ZhangJiaJie_Medium"] : + if self.level_name in ["Soccer_Field_Medium", "Soccer_Field_Easy", "ZhangJiaJie_Medium", "Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3"] : vel_max = 30.0 acc_max = 15.0 @@ -101,14 +138,12 @@ def fly_through_all_gates_at_once_with_moveOnSpline(self): vel_max = 4.0 acc_max = 1.0 - if(self.plot_transform): - self.airsim_client.plot_transform(self.gate_poses_ground_truth, vehicle_name=self.drone_name) - - self.airsim_client.moveOnSplineAsync([gate_pose.position for gate_pose in self.gate_poses_ground_truth], vel_max=30.0, acc_max=15.0, - add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=False, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() + return self.airsim_client.moveOnSplineAsync([gate_pose.position for gate_pose in self.gate_poses_ground_truth], vel_max=vel_max, acc_max=acc_max, + add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) def fly_through_all_gates_one_by_one_with_moveOnSplineVelConstraints(self): - add_curr_odom_velocity_constraint = True + add_velocity_constraint = True + add_acceleration_constraint = False if self.level_name in ["Soccer_Field_Medium", "Soccer_Field_Easy"] : vel_max = 15.0 @@ -124,18 +159,14 @@ def fly_through_all_gates_one_by_one_with_moveOnSplineVelConstraints(self): vel_max = 2.0 acc_max = 0.5 speed_through_gate = 0.5 - add_curr_odom_velocity_constraint = False - - for gate_pose in self.gate_poses_ground_truth: - if(self.plot_transform): - self.airsim_client.plot_transform([gate_pose], vehicle_name=self.drone_name) + add_velocity_constraint = False - # scale param scales the gate facing vector by desired speed. - self.airsim_client.moveOnSplineVelConstraintsAsync([gate_pose.position], - [self.get_gate_facing_vector_from_quaternion(gate_pose.orientation, scale = speed_through_gate)], - vel_max=vel_max, acc_max=acc_max, - add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=add_curr_odom_velocity_constraint, - viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() + # scale param scales the gate facing vector by desired speed. + return self.airsim_client.moveOnSplineVelConstraintsAsync([gate_pose.position], + [self.get_gate_facing_vector_from_quaternion(gate_pose.orientation, scale = speed_through_gate)], + vel_max=vel_max, acc_max=acc_max, + add_position_constraint=True, add_velocity_constraint=add_velocity_constraint, add_acceleration_constraint=add_acceleration_constraint, + viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) def fly_through_all_gates_at_once_with_moveOnSplineVelConstraints(self): if self.level_name in ["Soccer_Field_Easy", "Soccer_Field_Medium", "ZhangJiaJie_Medium"]: @@ -148,41 +179,107 @@ def fly_through_all_gates_at_once_with_moveOnSplineVelConstraints(self): acc_max = 2.0 speed_through_gate = 1.0 - if(self.plot_transform): - self.airsim_client.plot_transform(self.gate_poses_ground_truth, vehicle_name=self.drone_name) - - self.airsim_client.moveOnSplineVelConstraintsAsync([gate_pose.position for gate_pose in self.gate_poses_ground_truth], + return self.airsim_client.moveOnSplineVelConstraintsAsync([gate_pose.position for gate_pose in self.gate_poses_ground_truth], [self.get_gate_facing_vector_from_quaternion(gate_pose.orientation, scale = speed_through_gate) for gate_pose in self.gate_poses_ground_truth], - vel_max=15.0, acc_max=7.5, - add_curr_odom_position_constraint=True, add_curr_odom_velocity_constraint=True, - viz_traj=self.viz_traj, vehicle_name=self.drone_name).join() + vel_max=vel_max, acc_max=acc_max, + add_position_constraint=True, add_velocity_constraint=True, add_acceleration_constraint=False, + viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) + + 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 odometry_callback(self): + # get uncompressed fpv cam image + drone_state = self.airsim_client_odom.getMultirotorState() + # in world frame: + position = drone_state.kinematics_estimated.position + orientation = drone_state.kinematics_estimated.orientation + linear_velocity = drone_state.kinematics_estimated.linear_velocity + angular_velocity = drone_state.kinematics_estimated.angular_velocity + + # call task() method every "period" seconds. + def repeat_timer_image_callback(self, task, period): + while self.is_image_thread_active: + task() + time.sleep(period) + + def repeat_timer_odometry_callback(self, task, period): + while self.is_odometry_thread_active: + task() + time.sleep(period) + + def start_image_callback_thread(self): + if not self.is_image_thread_active: + self.is_image_thread_active = True + self.image_callback_thread.start() + print("Started image callback thread") + + def stop_image_callback_thread(self): + if self.is_image_thread_active: + self.is_image_thread_active = False + self.image_callback_thread.join() + print("Stopped image callback thread.") + + def start_odometry_callback_thread(self): + if not self.is_odometry_thread_active: + self.is_odometry_thread_active = True + self.odometry_callback_thread.start() + print("Started odometry callback thread") + + def stop_odometry_callback_thread(self): + if self.is_odometry_thread_active: + self.is_odometry_thread_active = False + self.odometry_callback_thread.join() + print("Stopped odometry callback thread.") def main(args): # ensure you have generated the neurips planning settings file by running python generate_settings_file.py - baseline_racer = BaselineRacer(drone_name="drone_1", plot_transform=args.plot_transform, viz_traj=args.viz_traj) + baseline_racer = BaselineRacer(drone_name="drone_1", viz_traj=args.viz_traj, viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0], viz_image_cv2=args.viz_image_cv2) baseline_racer.load_level(args.level_name) + if args.level_name == "Qualifier_Tier_1": + args.race_tier = 1 + if args.level_name == "Qualifier_Tier_2": + args.race_tier = 2 + if args.level_name == "Qualifier_Tier_3": + args.race_tier = 3 + baseline_racer.start_race(args.race_tier) baseline_racer.initialize_drone() baseline_racer.takeoff_with_moveOnSpline() baseline_racer.get_ground_truth_gate_poses() + baseline_racer.start_image_callback_thread() + baseline_racer.start_odometry_callback_thread() if args.planning_baseline_type == "all_gates_at_once" : if args.planning_and_control_api == "moveOnSpline": - baseline_racer.fly_through_all_gates_at_once_with_moveOnSpline() + baseline_racer.fly_through_all_gates_at_once_with_moveOnSpline().join() if args.planning_and_control_api == "moveOnSplineVelConstraints": - baseline_racer.fly_through_all_gates_at_once_with_moveOnSplineVelConstraints() + baseline_racer.fly_through_all_gates_at_once_with_moveOnSplineVelConstraints().join() if args.planning_baseline_type == "all_gates_one_by_one": if args.planning_and_control_api == "moveOnSpline": - baseline_racer.fly_through_all_gates_one_by_one_with_moveOnSpline() + baseline_racer.fly_through_all_gates_one_by_one_with_moveOnSpline().join() if args.planning_and_control_api == "moveOnSplineVelConstraints": - baseline_racer.fly_through_all_gates_one_by_one_with_moveOnSplineVelConstraints() + baseline_racer.fly_through_all_gates_one_by_one_with_moveOnSplineVelConstraints().join() + + baseline_racer.stop_image_callback_thread() + baseline_racer.stop_odometry_callback_thread() + baseline_racer.reset_race() if __name__ == "__main__": parser = ArgumentParser() - parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy", "Soccer_Field_Medium", "ZhangJiaJie_Medium", "Building99_Hard"], default="ZhangJiaJie_Medium") + parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy", "Soccer_Field_Medium", "ZhangJiaJie_Medium", "Building99_Hard", + "Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3"], default="ZhangJiaJie_Medium") parser.add_argument('--planning_baseline_type', type=str, choices=["all_gates_at_once","all_gates_one_by_one"], default="all_gates_at_once") parser.add_argument('--planning_and_control_api', type=str, choices=["moveOnSpline", "moveOnSplineVelConstraints"], default="moveOnSpline") - parser.add_argument('--plot_transform', dest='plot_transform', action='store_true', default=False) - parser.add_argument('--viz_traj', dest='viz_traj', action='store_true', default=False) + parser.add_argument('--enable_viz_traj', dest='viz_traj', action='store_true', default=False) + parser.add_argument('--enable_viz_image_cv2', dest='viz_image_cv2', action='store_true', default=False) + parser.add_argument('--race_tier', type=int, choices=[1,2,3], default=1) args = parser.parse_args() - main(args) \ No newline at end of file + main(args) From 9443d08fde75ec168e09dc335667fcb037cf3df4 Mon Sep 17 00:00:00 2001 From: Keiko Date: Wed, 6 Nov 2019 16:09:27 -0800 Subject: [PATCH 22/27] faster further from gate --- baselines/baseline_racer_perception.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 61f853c..5748637 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -162,16 +162,18 @@ def run(self): if smallest_measurement_error > 5.0: self.no_gate_count += 1 print("Gate NOT detected") - if self.no_gate_count == 15 and next_gate_position_dist >= self.largest_next_gate_position_dist: + if next_gate_position_dist > 30.0: + vmax = 10.0 + amax = 6.0 + no_gate_count_max = 15 + else: + vmax = 3.0 + amax = 2.0 + no_gate_count_max = 5 + 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) - if next_gate_position_dist > 20.0: - vmax = 7.0 - amax = 5.0 - else: - vmax = 3.0 - amax = 2.0 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=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) else: @@ -188,7 +190,6 @@ def run(self): 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) From be99df4923c93d07d2df4d1fc268dfd791ba9b54 Mon Sep 17 00:00:00 2001 From: Keiko Date: Mon, 18 Nov 2019 18:32:08 -0800 Subject: [PATCH 23/27] add mask3 and mask4 --- baselines/baseline_racer_perception.py | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 5748637..652bb75 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -24,6 +24,10 @@ def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[ self.upper_green1 = np.array([50, 255, 50]) self.lower_green2 = np.array([0, 175, 0]) self.upper_green2 = np.array([170, 255, 150]) + self.lower_green3 = np.array([50, 130, 50]) + self.upper_green3 = np.array([120, 255, 100]) + self.lower_green4 = np.array([215, 250, 239]) + self.upper_green4 = np.array([230, 252, 248]) self.dist_coeff = np.zeros((1,5)) self.waypoint_gate_1 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) self.waypoint_gate_2 = np.array([0.0, 0.0, -4.0, 1.0]).T.reshape(4,1) @@ -118,7 +122,9 @@ def run(self): rot_matrix_quad2global = self.get_rotation_matrix_quad_frame_to_global_frame(state) mask1 = cv2.inRange(image_rgb,self.lower_green1,self.upper_green1) mask2 = cv2.inRange(image_rgb,self.lower_green2,self.upper_green2) - mask = mask1 + mask2 + 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) #eroded_gate = cv2.erode(dilated_gate,self.kernel, iterations=8) gate_contours = None @@ -167,15 +173,15 @@ def run(self): amax = 6.0 no_gate_count_max = 15 else: - vmax = 3.0 - amax = 2.0 - no_gate_count_max = 5 + 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=False, - add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) + 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: self.no_gate_count = 0 self.measurement_count += 1 @@ -198,12 +204,14 @@ def run(self): cv2.imshow("image_rgb", image_rgb) cv2.waitKey(1) if self.moveonspline_count >= 5: - self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2], vel_max=7.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False, - add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name, replan_from_lookahead=True) + if next_gate_idx >= len(self.gate_poses_ground_truth)-1: + next_gate_idx = -1 + self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,self.gate_poses_ground_truth[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 next_gate_position_dist < self.largest_next_gate_position_dist: #don't collect measurements, continue to waypoint, reset filter with new mu and sigmas print("Gate too close") - time.sleep(3) + time.sleep(2) self.no_gate_count = 0 self.too_close_count += 1 if self.too_close_count == 1: From cf0748f485b91d37eee284f58bf35982a2b86a61 Mon Sep 17 00:00:00 2001 From: Keiko Date: Thu, 21 Nov 2019 11:47:16 -0800 Subject: [PATCH 24/27] adding comments --- baselines/baseline_racer_perception.py | 69 ++++++++++++++------------ 1 file changed, 38 insertions(+), 31 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index 652bb75..cc9397c 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -6,7 +6,7 @@ import airsimneurips as airsim import copy import cv2 -import math +#import math import numpy as np import time @@ -18,8 +18,6 @@ def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[ self.gate_corners_flat = np.float32([[[94, 64],[204, 64],[204, 174], [94, 174]]]) self.camera_matrix = np.array([[160.000, 0.000000, 160.000], [0.000000, 160.000, 120.000], [0.000000, 0.000000, 1.000000]]) self.gate_center_pixel_flat = np.array([149,119,1]).T - self.no_gate_count = 0 - self.measurement_count = 0 self.lower_green1 = np.array([0, 120, 0]) self.upper_green1 = np.array([50, 255, 50]) self.lower_green2 = np.array([0, 175, 0]) @@ -29,16 +27,20 @@ def __init__(self, drone_name = "drone_1", viz_traj=True, viz_traj_color_rgba=[ self.lower_green4 = np.array([215, 250, 239]) self.upper_green4 = np.array([230, 252, 248]) self.dist_coeff = np.zeros((1,5)) - self.waypoint_gate_1 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) - self.waypoint_gate_2 = np.array([0.0, 0.0, -4.0, 1.0]).T.reshape(4,1) + self.waypoint_gate_1 = np.array([0.0, 0.0, 0.0, 1.0]).T.reshape(4,1) #waypoint at gate center + self.waypoint_gate_2 = np.array([0.0, 0.0, -4.0, 1.0]).T.reshape(4,1) #waypoint behind the gate self.gate_points_3D = 1.5*np.array([[0,0,0],[-1.0,1.0,0],[1.0,1.0,0],[1.0,-1.0,0],[-1.0,-1.0,0]]) self.C = np.identity(3) self.Q = 5*np.identity(3) - self.too_close_count = 0 self.largest_next_gate_position_dist = 2.5 self.aspect_ratio_max = 2.0 + self.no_gate_count = 0 + self.measurement_count = 0 + self.too_close_count = 0 self.moveonspline_count = 0 + self.next_gate_idx = 0 + # gets rotation matrix from quad frame to global frame def get_rotation_matrix_quad_frame_to_global_frame(self,state): q0 = state.orientation.w_val q1 = state.orientation.x_val @@ -49,26 +51,37 @@ def get_rotation_matrix_quad_frame_to_global_frame(self,state): [2*(q1*q3-q0*q2), 2*(q1*q0+q2*q3), 1-2*(q1**2+q2**2)]]) return rot_matrix_quad2global + # returns true if the aspect ratio of the gate is too large, returns false if the aspect ratio of the gate is under self.aspect_ratio_max def find_aspect_ratio(self, gate_corners): aspect_ratio_mtx = np.empty((1,4)) aspect_ratio_mtx[0,0] = abs(np.float32(gate_corners[1][1] - gate_corners[2][1])/np.float32(gate_corners[0][0] - gate_corners[1][0]+self.eps)) aspect_ratio_mtx[0,1] = abs(np.float32(gate_corners[0][1] - gate_corners[3][1])/np.float32(gate_corners[2][0] - gate_corners[3][0]+self.eps)) aspect_ratio_mtx[0,2] = abs(np.float32(gate_corners[0][1] - gate_corners[3][1])/np.float32(gate_corners[0][0] - gate_corners[1][0]+self.eps)) aspect_ratio_mtx[0,3] = abs(np.float32(gate_corners[1][1] - gate_corners[2][1])/np.float32(gate_corners[2][0] - gate_corners[3][0]+self.eps)) - large_aspect_ratio = 1.0 + aspect_ratio_too_large = False for i in range(4): if aspect_ratio_mtx[0,i] < 1.0: aspect_ratio_mtx[0,i] = 1/aspect_ratio_mtx[0,i] if aspect_ratio_mtx[0,i] > self.aspect_ratio_max: - large_aspect_ratio = aspect_ratio_mtx[0,i] - return large_aspect_ratio + aspect_ratio_too_large = True + return aspect_ratio_too_large + # get waypoint in global coordinate frame from waypoint in gate centered frame def gate_frame_waypoint_to_global_waypoint(self,state,rot_translation_matrix_gate2quad,rot_matrix_quad2global,waypoint_gate): waypoint_rel = np.dot(rot_translation_matrix_gate2quad, waypoint_gate) waypoint_rel = np.dot(rot_matrix_quad2global, np.array([waypoint_rel[2], waypoint_rel[0], -waypoint_rel[1]])) waypoint_glob = np.reshape(np.array([state.position.x_val + waypoint_rel[0], state.position.y_val + waypoint_rel[1], state.position.z_val - waypoint_rel[2]]),(1,3)) return waypoint_glob + # Use a Kalman Filter to estimate waypoints in global coordinate frame + def kalman_filter(self, mu, sigma, y): + K = multi_dot([sigma,self.C.T,np.linalg.inv(multi_dot([self.C,sigma,self.C.T])+self.Q)]) + mu = mu + np.dot(K,(y-np.dot(self.C,mu))) + sigma = np.dot((np.identity(3)-np.dot(K,self.C)),sigma) + return mu, sigma + + # find the error between the measured gate center and mean of the estimate + # measured gate center found through homography transformation from four gate corner points def find_measurement_error(self,four_gate_corners, mu_1, state, rot_matrix_quad2global): four_gate_corners = order_points(four_gate_corners.reshape(4,2)) gate_corners_plot = copy.deepcopy(four_gate_corners) @@ -97,16 +110,9 @@ def find_measurement_error(self,four_gate_corners, mu_1, state, rot_matrix_quad2 measurement_error = np.linalg.norm(waypoint_1_error) return measurement_error, waypoint_glob_1, waypoint_glob_2, gate_center_pixel, gate_corners_plot - def kalman_filter(self, mu, sigma, y): - K = multi_dot([sigma,self.C.T,np.linalg.inv(multi_dot([self.C,sigma,self.C.T])+self.Q)]) - mu = mu + np.dot(K,(y-np.dot(self.C,mu))) - sigma = np.dot((np.identity(3)-np.dot(K,self.C)),sigma) - return mu, sigma - def run(self): # Move through first gate - self.get_ground_truth_gate_poses() # noisy since tier 2 is default - next_gate_idx = 0 + 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) @@ -114,12 +120,14 @@ def run(self): 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) # get numpy array + 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) @@ -128,17 +136,16 @@ def run(self): dilated_gate = cv2.dilate(mask,self.kernel, iterations=8) #eroded_gate = cv2.erode(dilated_gate,self.kernel, iterations=8) gate_contours = None - gate_contours, hierarchy = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + gate_contours, ____ = cv2.findContours(dilated_gate, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) cv2.imshow("mask", mask) cv2.imshow("dilated_gate", dilated_gate) #cv2.imshow("eroded_gate", eroded_gate) cv2.waitKey(1) - largest_area = 0.0 gate_corners = None smallest_measurement_error = 100 - #check for each contour + # 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) @@ -153,15 +160,15 @@ def run(self): four_gate_corners = np.append(four_gate_corners,i,axis=0) #now find the measurement error 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 = self.find_aspect_ratio(four_gate_corners) - if measurement_error < smallest_measurement_error and aspect_ratio < self.aspect_ratio_max: + 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)) - #print(smallest_measurement_error, next_gate_position_dist, next_gate_idx) + #print(smallest_measurement_error, next_gate_position_dist, self.next_gate_idx) # MOVE self.moveonspline_count += 1 @@ -204,9 +211,9 @@ def run(self): cv2.imshow("image_rgb", image_rgb) cv2.waitKey(1) if self.moveonspline_count >= 5: - if next_gate_idx >= len(self.gate_poses_ground_truth)-1: - next_gate_idx = -1 - self.airsim_client.moveOnSplineAsync([waypoint_1,waypoint_2,self.gate_poses_ground_truth[next_gate_idx+1].position], vel_max=7.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=True, + 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 next_gate_position_dist < self.largest_next_gate_position_dist: #don't collect measurements, continue to waypoint, reset filter with new mu and sigmas @@ -216,11 +223,11 @@ def run(self): self.too_close_count += 1 if self.too_close_count == 1: print("reset gate too close") - next_gate_idx += 1 - if next_gate_idx >= len(self.gate_poses_ground_truth): - next_gate_idx = 0 + 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[next_gate_idx].position + 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) From f7b6974d5dfda22f79bdb4a709a64736a404b696 Mon Sep 17 00:00:00 2001 From: Keiko Date: Thu, 21 Nov 2019 17:40:28 -0800 Subject: [PATCH 25/27] adding comments --- baselines/baseline_racer_perception.py | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/baselines/baseline_racer_perception.py b/baselines/baseline_racer_perception.py index cc9397c..296ea47 100644 --- a/baselines/baseline_racer_perception.py +++ b/baselines/baseline_racer_perception.py @@ -6,7 +6,6 @@ import airsimneurips as airsim import copy import cv2 -#import math import numpy as np import time @@ -92,7 +91,7 @@ def find_measurement_error(self,four_gate_corners, mu_1, state, rot_matrix_quad2 # find middle of gate pixel coordinates gate_center_pixel = np.dot(np.linalg.inv(homography_matrix), self.gate_center_pixel_flat) - gate_center_pixel = (int(gate_center_pixel[0]/(gate_center_pixel[2]+self.eps)), int(gate_center_pixel[1]/(gate_center_pixel[2]+self.eps))) + gate_center_pixel = ((gate_center_pixel[0]/(gate_center_pixel[2])), (gate_center_pixel[1]/(gate_center_pixel[2]))) # coordinates of gate corners in image (pixel coordinates) gate_points_2D = np.float32(np.concatenate((np.array(gate_center_pixel).reshape(-1,1,2),four_gate_corners),axis=0)) @@ -111,7 +110,6 @@ def find_measurement_error(self,four_gate_corners, mu_1, state, rot_matrix_quad2 return measurement_error, waypoint_glob_1, waypoint_glob_2, gate_center_pixel, gate_corners_plot def run(self): - # Move through first gate 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)) @@ -134,17 +132,17 @@ def run(self): 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) - #eroded_gate = cv2.erode(dilated_gate,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.imshow("eroded_gate", eroded_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) @@ -158,7 +156,6 @@ def run(self): for i in comb: i = np.reshape(i,(1,2)) four_gate_corners = np.append(four_gate_corners,i,axis=0) - #now find the measurement error 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: @@ -168,16 +165,15 @@ def run(self): 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)) - #print(smallest_measurement_error, next_gate_position_dist, self.next_gate_idx) - - # MOVE 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 = 6.0 + amax = 3.0 no_gate_count_max = 15 else: vmax = 5.0 @@ -189,6 +185,7 @@ def run(self): 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 @@ -203,7 +200,7 @@ def run(self): 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_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) @@ -216,7 +213,8 @@ def run(self): 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 next_gate_position_dist < self.largest_next_gate_position_dist: #don't collect measurements, continue to waypoint, reset filter with new mu and sigmas + # 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 From 7f6690183c6302f362db41974df8164a6b0cb3d7 Mon Sep 17 00:00:00 2001 From: Keiko Date: Fri, 3 Apr 2020 17:17:58 -0700 Subject: [PATCH 26/27] add perception baseline section in readme --- README.md | 419 +++++++++++++++++++++++++++++++----- baselines/baseline_racer.py | 5 +- 2 files changed, 367 insertions(+), 57 deletions(-) diff --git a/README.md b/README.md index 2ecc71d..197fc10 100644 --- a/README.md +++ b/README.md @@ -1,95 +1,404 @@ # Game of Drones: A NeurIPS 2019 Competition + ## Quickstart -- [Competition website](https://www.microsoft.com/en-us/research/academic-program/game-of-drones-competition-at-neurips-2019/) +- [Website](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/) +- [Register](https://www.microsoft.com/en-us/research/academic-program/game-of-drones-competition-at-neurips-2019/) - [Competition guidelines](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/blob/master/docs/competition_guidelines.md) - [Linux and Windows Binaries](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases) -- Python API - - [airsimneurips @PyPI](https://pypi.org/project/airsimneurips/) - - [airsimneurips API doc](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/) - +- [Python API](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html), [airsimneurips PyPI package](https://pypi.org/project/airsimneurips/) -## Unreal Environments -This competition marks the advent of a new release process for AirSim, in which we have separated out the AirSim plugin from environment content. Instead of having a series of individual executables for each environment, we have compacted all of the relevant AirSim content and API into a single binary (`AirSimExe`). -Unreal environments containing race courses are released as separate downloadable content (DLC) packages, in the form of `.pak` files, which can be loaded and unloaded into the main binary as needed. +### Downloading and running AirSim Binaries +#### Downloading +- Final round binaries and environments (v1.1) + - tl;dr: + - [Linux] Use the [download_final_round_binaries.sh](download_final_round_binaries.sh) script + - Long version: + - Download the v1.1 [Linux](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v1.1-linux) or [Windows](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v1.1-windows) `AirSim.zip`, and unzip it. + - Download your qualifier environments (shipped in pakfiles) - `Final_Tier_1_and_Tier_2.pak` and ` Final_Tier_3.pak`. + - Move the environment pakfiles into `AirSim/AirSimExe/Content/Paks`. + - Download and move the `settings.json` file to `~/Documents/AirSim/settings.json`. + - Use `airsimneurips` >= 1.2.0 + +- Qualifier binaries and environments (v1.0) + - tl;dr: + - [Linux] Use the [download_qualification_binaries.sh](download_qualification_binaries.sh) script + - Long version: + - Download the v1.0 [Linux](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v1.0-linux) or [Windows](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v1.0-windows) `AirSim.zip`, and unzip it. + - Download your qualifier environments (shipped in pakfiles) - `Qual_Tier_1_and_Tier_3.pak` and ` Qual_Tier_2.pak`. + - Move the environment pakfiles into `AirSim/AirSimExe/Content/Paks`. + - Download and move the `settings.json` file to `~/Documents/AirSim/settings.json`. + +- Training binaries and environments (v0.3): + - tl;dr: + - [Linux] Use the [download_training_binaries.sh](download_training_binaries.sh) script + - Long version: + - Download the v0.3 [Linux](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v0.3.0-linux) or [Windows](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v0.3.0) `AirSim.zip`, and unzip it. + - Download training environments (shipped in pakfiles) - `Soccer_Field.pak`, `ZhangJiaJie.pak`, and `Building99.pak`. + - Move the environment pakfiles into `AirSim/AirSimExe/Content/Paks`. + - Download and move the `settings.json` file to `~/Documents/AirSim/settings.json`. + +Notes: +- `Source code (zip)` or `Source code (tar.gz)` might not be up-to-date with the master branch of this repository. It can be lagging by `n commits to master since this release`, specified on the released page. + For the code on this repository, it's best to just `git clone`. -### Downloading AirSimExe and Unreal Environments -- Navigate to [the releases page](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases). -- Download the latest Windows or Linux `AirSim.zip`. -- Download your desired environments (pakfiles) - `Building99.pak` / ` SoccerField.pak` / `ZhangJiaJie.pak`. -- Move the environment pakfile into `AirSim/AirSimExe/Content/Paks`. -- The contents in the environment pakfile will now be linked to your AirSim binary! +- List of disabled APIs in qualification binaries: The following APIs on the server side in the qualification binaries. You should see an error message pop up in the terminal message when you call these. They do work in the training binaries: + - `simSetObjectPose` + - `simSetVehiclePose` + - `simSetObjectScale` + - `simGetObjectScale` + - `simSetSegmentationObjectID` + - `simGetSegmentationObjectID` + - `simPause` + - `simContinueForTime` -### Running the Executable +#### Running - Linux - - Open a terminal window, `cd` to `AirSim/` directory, and enter the following command: + - Open a terminal window, `cd` to `AirSim_Training/` or `AirSim_Qualification` directory, and enter the following command: + ``` + ./AirSimExe.sh -windowed -opengl4 + ``` + - Running headless (with rendering of images enabled): + ``` + DISPLAY= ./AirSimExe.sh -opengl4 + ``` + - To disable rendering completely for training planning and / or control policies, you can use: + ``` + -./AirSimExe.sh -nullrhi ``` - ./AirSimExe.sh -windowed + Note that `simGetImages` will not work with this option. + - To increase speed of `simGetImages` / increase speed of Unreal Engine's game thread; + - Add the `"ViewMode": "NoDisplay"` to your settings.json file, or use [this file](https://gist.github.com/madratman/5fadbb08f65e9c0187ccc1f5090fc086) directly. + This disables rendering in the main viewport camera. + Then run the binary with the following options. ``` + ./AirSimExe.sh -windowed -NoVSync -BENCHMARK + ``` + You can also use the Unreal console commands `Stat FPS`, `Stat UnitGraph`, `r.VSync`, `t.maxFPS`. See [Issue #111](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/111) for more details. - Windows - - Navigate to the `AirSim/` directory, and double-click `run.bat` + - Navigate to the `AirSim/` directory, and double-click `run.bat` (or `AirSimExe.exe -windowed`) + +## Docker +- Prerequisites: + - Install [docker-ce](https://docs.docker.com/install/linux/docker-ce/ubuntu/). + - Complete the desired [post-installation steps for linux](https://docs.docker.com/install/linux/linux-postinstall/) after installing docker. + At the minimum, the page tells you how torun docker without root, and other useful setup options. + - Install [nvidia-docker2](https://github.com/NVIDIA/nvidia-docker/wiki/Installation-(version-2.0)). + +- Dockerfile: + We provide a sample [dockerfile](docker/Dockerfile) you can modify. + It downloads the training and qualification binaries automatically, and installs the python client. + By default, it uses Ubuntu 18.04 and CUDA 10.0 with OpenGL, and is build on top of [nvidia/cudagl:10.0-devel-ubuntu18.04](https://hub.docker.com/r/nvidia/cudagl). + This can be changed of course, as explained in the following section. + +- Building the docker image: + You can use [build_docker_image.py](docker/build_docker_image.py) to build the dockerfile above (or your own custom one) + **Usage** (with default arguments) + ```shell + cd docker/; + python3 build_docker_image.py \ + --dockerfile Dockerfile \ + --base_image nvidia/cudagl:10.0-devel-ubuntu18.04 \ + -- target_image airsim_neurips:10.0-devel-ubuntu18.04 + ``` +- Running the docker image: + See [docker/run_docker_image.sh](docker/run_docker_image.sh) to run the docker image: + **Usage** + - for running default image, training binaries, in windowed mode: + `$ ./run_docker_image.sh "" training` + - for running default image, qualification binaries, in windowed mode: + `$ ./run_docker_image.sh "" qualification` + - for running default image, training binaries, in headless mode: + `$ ./run_docker_image.sh "" training headless` + - for running default image, qualification binaries, in headless mode: + `$ ./run_docker_image.sh "" qualification headless` + - for running a custom image in windowed mode, pass in you image name and tag: + `$ ./run_docker_image.sh DOCKER_IMAGE_NAME:TAG` + - for running a custom image in headless mode, pass in you image name and tag, followed by "headless": + `$ ./run_docker_image.sh DOCKER_IMAGE_NAME:TAG headless` ## AirSim API -To control your drone and get information from the environment, you will need the `airsimneurips` API, which is accessible via Python. +- To control your drone and get information from the environment, you will need the `airsimneurips` API, which is accessible via Python. +We recommend you used python >= 3.6. Python 2.7 will go [out of support soon](https://pythonclock.org/) -### Installation -- To install the Python API for the Neurips competition, please use: +- To install the Python API, do a : ``` pip install airsimneurips ``` - Corollary: Do not do a `pip install airsim`, as we will have a few custom APIs specific to this competition. + +- See [quick overview of the API](#quick-api-overview) below + +- The API is documented at [airsimneurips API doc](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html) + - Resources - - [airsimneurips API doc](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/) - - [AirSim upstream API](https://microsoft.github.io/AirSim/docs/apis/) and [examples](https://github.com/microsoft/AirSim/tree/master/PythonClient) - (Note that this is not used in the competition, however is a good learning resource) + - Going through both open and closed issues in this repository might answer some of your questions. The search bar on top left can prove useful. + - [AirSim upstream API](https://microsoft.github.io/AirSim/docs/apis/) and [examples](https://github.com/microsoft/AirSim/tree/master/PythonClient) can also be of use. However, please note that the main AirSim repo's API is not used in the competition (there's some overlap and some differences), however is a good learning resource. + +## Submitting Results and Leaderboard - Qualification Round +- For the qualification round, we have one race track for each tier. The relevant binaries (v1.0) are available for [linux](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v1.0-linux) and [windows](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v1.0-windows) + - Tier 1: This is in the Soccer Field environment. + THe race track is in the `Qual_Tier_1_and_Tier_3.pak` pakfile + - Tier 2: This is in the ZhangJiaJie environment. + The race track is in the `Qual_Tier_2.pak` pakfile. + - Tier 3: This is again in the Soccer Field environment. + The race track is in the `Qual_Tier_1_and_Tier_3.pak` pakfile. -## Changing Environments -There are two ways to swap between levels, either via AirSIm API or by the UI menu. -- API - - We have added a new API `simLoadLevel(level_name="MainMenu")` to change Unreal environments on the fly. - Possible values for `level_name` are : `"Soccer_Field_Easy"`, `"Soccer_Field_Medium"`, `"ZhangJiaJie_Medium"`, `"Building99_Hard"`. - Here's a quick snippet to iterate throught them all. Before trying this, please ensure you've downloaded the corresponding pak files from [our releases page](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases). +- How to generate logfiles for each tier: + - Loading level and starting race: + - Please update your airsimneurips pythonclient (should be >=1.0.0). + - Calling `simStartRace(race_tier=1, 2, or 3)` generates the appropriate log files. + - Tier 1: + ```python + airsim_client.simLoadLevel('Qualifier_Tier_1') + airsim_client.simStartRace(1) + ``` + - Tier 2: + ```python + airsim_client.simLoadLevel('Qualifier_Tier_2') + airsim_client.simStartRace(2) + ``` + + - Tier 3: + ```python + airsim_client.simLoadLevel('Qualifier_Tier_3') + airsim_client.simStartRace(3) + ``` + - As Tier 2 focuses on perception and Tier 3 focuses on both perception and planning, note that `simGetObjectPose` returns noisy gate poses, after `simStartRace(2)` and `simStartRace(3)` is called. + + - As soon as `simStartRace(1)` or `simStartRace(3)` is called, `drone_2` (MSR opponent racer) will start flying. + + - See `baseline_racer.py` for sample code. The previous bullet points are being called in wrapper functions in the following snippet in `baseline_racer.py`: ```python - import airsimneurips as airsim - client = airsim.MultirotorClient() - client.confirmConnection() - client.simLoadLevel('Soccer_Field_Easy') - client.simLoadLevel('Soccer_Field_Medium') - client.simLoadLevel('ZhangJiaJie_Medium') - client.simLoadLevel('Building99_Hard') + baseline_racer.load_level(args.level_name) + if args.level_name == "Qualifier_Tier_1": + args.race_tier = 1 + if args.level_name == "Qualifier_Tier_2": + args.race_tier = 2 + if args.level_name == "Qualifier_Tier_3": + args.race_tier = 3 + baseline_racer.start_race(args.race_tier) ``` -- UI Menu - - Press `F10` to toggle the level menu - - Click your desired level. (Note: the UI lists all the pakfiles in the `AirSim/AirSimExe/Content/Paks` directory. Ensure you downloaded the pakfile, if you are not able to see a particular environment) -## Baselines - - Plan and move on minimum jerk trajectory using gate ground truth poses: - - Generate an AirSim settings.json file +- To submit your results to the leaderboard: + - Navigate to the [submission site](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/upload.html), enter your team name in the proper field, and upload any number of [race logs](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/blob/master/docs/competition_guidelines.md#race-monitoring). +It's ok to make a submission for as little as a single track and/or a single tier. +You can find race logs inside of `AirSimExe/Saved/Logs/RaceLogs` in your downloaded binary folder. +Please read [the race monitoring section](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/blob/master/docs/competition_guidelines.md#race-monitoring) in the competition guidelines for more details. + - The leaderboard will publish the results of a drone that is named `drone_1` (call [`generate_settings_file.py`](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/blob/master/baselines/generate_settings_file.py) to generate an AirSim settings file, as done for the `baseline_racer` below. + - Please submit a PDF file in the `report` section to help us verify the honesty of your submission for the Nov 21st deadline. Please summarize your approach for all tiers you make a submission for, with appropriate citations. The report PDF size should not exceed 10 MB, and should be a maximum of 4 pages in length. We leave the exact format of the report to your descrition, but the [IEEE template](https://ras.papercept.net/conferences/support/tex.php) is a good choice. + - We have emailed you a private key, which should be entered in the `Team ID` field. This helps us verify it was your team who indeed made the submission. + - The [leaderboard](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/leaderboard.html) is updated once per day at 2100 PST. + If you do not see your results after 24 hours, please [email us](mailto:neuripsdronecontestinfo@gmail.com) with your team name and submitted log files. + +## Submitting Results and Leaderboard - Final Round +- For the final round, we have one race track for each tier. The relevant binaries (v1.1) are available for [linux](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v1.1-linux) and [windows](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/releases/tag/v1.1-windows) + - Tier 1: This is in the Soccer Field environment. + THe race track is in the `Final_Tier_1_and_Tier_2.pak` pakfile + - Tier 2: This is in the Soccer Field environment. + The race track is in the `Final_Tier_1_and_Tier_2.pak` pakfile. + - Tier 3: This is again in the ZhangJiaJie environment. + The race track is in the `Final_Tier_3.pak` pakfile. + +- How to generate logfiles for each tier: + - Loading level and starting race: + - Please update your airsimneurips pythonclient (should be >=1.2.0). + - Calling `simStartRace(race_tier=1, 2, or 3)` generates the appropriate log files. You can only run `tier N` races in `Final_Tier_N` levels. + - Tier 1: + ```python + airsim_client.simLoadLevel('Final_Tier_1') + airsim_client.simStartRace(tier=1) + ``` + + - Tier 2: + ```python + airsim_client.simLoadLevel('Final_Tier_2') + airsim_client.simStartRace(tier=2) + ``` + + - Tier 3: + ```python + airsim_client.simLoadLevel('Final_Tier_3') + airsim_client.simStartRace(tier=3) + ``` + - As Tier 2 focuses on perception and Tier 3 focuses on both perception and planning, note that `simGetObjectPose` returns noisy gate poses. + + - As soon as `simStartRace(tier=1)` or `simStartRace(tier=3)` is called, `drone_2` (MSR opponent racer) will start flying. + + - See `baseline_racer.py` for sample code. The previous bullet points are being called in wrapper functions in the following snippet in `baseline_racer.py`: + ```python + baseline_racer.load_level(args.level_name) + baseline_racer.start_race(args.race_tier) + ``` + +- To submit your results to the final leaderboard: + - Navigate to the [submission site](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/upload.html), enter your team name in the proper field, and upload any number of [race logs](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/blob/master/docs/competition_guidelines.md#race-monitoring). +It's ok to make a submission for as little as a single track and/or a single tier. +You can find race logs inside of `AirSimExe/Saved/Logs/RaceLogs` in your downloaded binary folder. +Please read [the race monitoring section](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/blob/master/docs/competition_guidelines.md#race-monitoring) in the competition guidelines for more details. + - The leaderboard will publish the results of a drone that is named `drone_1` (call [`generate_settings_file.py`](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/blob/master/baselines/generate_settings_file.py) to generate an AirSim settings file, as done for the `baseline_racer` below. + - Please submit a PDF file in the `report` section to help us verify the honesty of your submission by the Dec 5th, 2359 PST deadline. Please summarize your approach for all tiers you make a submission for, with appropriate citations. The report PDF size should not exceed 10 MB, and should be a maximum of 6 pages in length. We leave the exact format of the report to your descrition, but the [IEEE template](https://ras.papercept.net/conferences/support/tex.php) is a good choice. + - We have emailed you a private key, which should be entered in the `Team ID` field. This helps us verify it was your team who indeed made the submission. + - The [final leaderboard](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/leaderboard_final.html) is updated once per day at 2100 PST. + If you do not see your results after 24 hours, please [email us](mailto:neuripsdronecontestinfo@gmail.com) with your team name and submitted log files. + +## Sample code + - Plan and move on a minimum jerk trajectory using ground truth poses of gates: + - Generate an AirSim settings.json file (same as the one provided in releases) ```shell $ cd baselines; $ python generate_settings_file.py ``` - - Start the AirSim Neurips binary, [as explained above](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing#running-the-executable) - - Run the code! - See all the [baseline arguments here](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/blob/master/baselines/baseline_racer.py#L184-#L188) + - Start the AirSim Neurips binary, [as explained above](#running) + - Run the code! ```shell $ python baseline_racer.py \ - --viz_traj \ - --plot_transform \ + --enable_viz_traj \ + --enable_viz_image_cv2 \ --planning_baseline_type all_gates_at_once \ --planning_and_control_api moveOnSpline \ - --level_name ZhangJiaJie_Medium + --level_name ZhangJiaJie_Medium \ + --race_tier 1 + ``` + + Usage is: + ```shell + $ python baselines/baseline_racer.py -h + usage: baseline_racer.py [-h] + [--level_name {Soccer_Field_Easy,Soccer_Field_Medium,ZhangJiaJie_Medium,Building99_Hard,Qualifier_Tier_1,Qualifier_Tier_2,Qualifier_Tier_3,Final_Tier_1,Final_Tier_2,Final_Tier_3}] + [--planning_baseline_type {all_gates_at_once,all_gates_one_by_one}] + [--planning_and_control_api {moveOnSpline,moveOnSplineVelConstraints}] + [--enable_viz_traj] [--enable_viz_image_cv2] + [--race_tier {1,2,3}] ``` - -## List of Environments in Increasing Order of Difficulty -- Soccer Field: A simple outdoors environment with few obstacles, and an easy to follow course. -- ZhangJiaJie: A mountainous landscape based on a national park in the Hunan province of China. -- Building99: A tight race course designed inside one of Microsoft's very own buildings. + - Plan a Game Theoretic Plan (GTP) trajectory for an ego drone based on an estimate of the opponent drone's behavior. + - Generate an AirSim settings.json file + ```shell + $ cd baselines; + $ python generate_settings_file.py + ``` + - Start the AirSim Neurips binary, [as explained above](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing#running) + - Run the GTP code! + ```shell + $ python baseline_racer_gtp.py \ + --blocking_behavior \ + --plot_gtp \ + --enable_viz_traj \ + --level_name Qualifier_Tier_1 + ``` + - This method is an Iterative Best Response (IBR) trajectory planning technique. In IBR, first the trajectories of both drones are initialized as straight down the track at maximum speed (to win the game!). The opponent trajectory is then held constant while we solve for the ego trajectory via Model Predictive Control (MPC) optimization (details in [gtp.py](baselines/gtp.py)). Then, we hold the ego trajectory constant and solve for a guess of the opponent's trajectory in the same fashion. If after some iterations, the solution convereges (i.e., the resulting trajectories stop changing), we have reached a Nash equilibrium over the space of trajectories. That is to say, either agents can not unilaterally change their trajectory to increase their own performance. This implementation is a heuristic based on the original method proposed in the paper below ([PDF here](https://arxiv.org/abs/1801.02302)). + - R. Spica, D. Falanga, E. Cristofalo, E. Montijano, D. Scaramuzza, and M. Schwager, "A Real-Time Game Theoretic Planner for Autonomous Two-Player Drone Racing", in the Proccedings of Robotics: Science and Systems (RSS), 2018. + + - Race with a perception method that estimates the next gate's location and moves using moveOnSplineAsync. + - Generate an AirSim settings.json file + ```shell + $ cd baselines; + $ python generate_settings_file.py + ``` + - Start the AirSim Neurips binary, [as explained above](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing#running) + - Run the perception code! + ```shell + $ python baseline_racer_perception.py \ + --level_name Qualifier_Tier_2 + ``` + - This perception method leverages that all gates are planar, rectangular, and have similar coloration. In each image frame, color thresholding is done to create a mask of the gate. This mask is then compared to that of a baseline image that is taken from a known distance from a gate with known dimensions. A homography transformation between the two masks can be retrieved to find the image coordinate of the center of the gate in the image taken during flight, and a point to point correspondance can be used to find the 3D coordinate of the oncoming gate center. For each gate, a Kalman Filter is used to estimate of the gate center location based on the measurements. The coordinate is then used as a waypoint in the moveOnSplineAsync() API. + +## Quick API overview +We added some new APIs (marked with 💚) to [AirSim](https://github.com/Microsoft/Airsim) for the NeurIPS competition binaries. + +#### Loading Unreal Engine environments +- [`simLoadLevel(level_name)`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simLoadLevel) 💚 +Possible values for `level_name` are: + - `"Soccer_Field_Easy"`, `"Soccer_Field_Medium"`, `"ZhangJiaJie_Medium"`, `"Building99_Hard"` in the training binaries (`v0.3`). + - `"Qualification_Tier_1"`, `"Qualification_Tier_2"`, `"Qualification_Tier_3"` in the qualification binaries (`v1.0`). + - `"Final_Tier_1"`, `"Final_Tier_2"`, `"Final_Tier_3"` in the final round binaries (`v1.1`). +Before trying this, please ensure you've downloaded the corresponding training (`v0.3`) / qualifier (`v1.0`) / final round (`v1.0`) binaries, [as described above](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing#downloading-airsimexe-and-unreal-environments) + +- UI Menu + - Press `F10` to toggle the level menu + - Click your desired level. (Note: the UI lists all the pakfiles in the `AirSim/AirSimExe/Content/Paks` directory. Ensure you downloaded the pakfile, if you are not able to see a particular environment) + +#### Race APIs: +- Start a race: + [`simStartRace(tier=1/2/3)`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simStartRace) 💚 + +- Reset race: + [`simResetRace()`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simResetRace) 💚 + +- Check if racer is disqualified: + [`simIsRacerDisqualified()`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simIsRacerDisqualified) 💚 + +- Get index of last gate passed: + [`simGetLastGatePassed()`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simGetLastGatePassed) 💚 + +- Disable generation of logfiles by race APIs: + [`simDisableRaceLog`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simDisableRaceLog) 💚 + +#### Lower level control APIs: +- FPV like Angle rate setpoint APIs: + - [`moveByAngleRatesThrottleAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveByAngleRatesThrottleAsync) 💚 + - [`moveByAngleRatesZAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveByAngleRatesZAsync) 💚 (stabilizes altitude) + +- Angle setpoint APIs: + - [`moveByRollPitchYawThrottleAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveByRollPitchYawThrottleAsync) 💚 + - [`moveByRollPitchYawZAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveByRollPitchYawZAsync) 💚 (stabilizes altitude) + +- RollPitchYawrate setpoint APIs: + - [`moveByRollPitchYawrateThrottleAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveByRollPitchYawrateThrottleAsync) 💚 + - [`moveByRollPitchYawrateZAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveByRollPitchYawrateZAsync) 💚 (stabilizes altitude) + +#### Medium level control APIs: +- Velocity setpoints + - [`moveByVelocityAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveByVelocityAsync) + - [`moveByVelocityZAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveByVelocityZAsync) (stabilizes altitude) + +- Position setpoints + - [`moveToPosition`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveToPositionAsync) + - [`moveOnPath`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveOnPathAsync) + - [`moveToZAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveToZAsync) + +#### High level control APIs: +- Minimum jerk trajectory planning (using [ethz-asl/mav_trajectory_generation](https://github.com/ethz-asl/mav_trajectory_generation)), and trajectory tracking (using a pure pursuit like controller minimizing position and velocity errors), with position setpoints. + Optionally use the `*lookahead*` parameters to start new trajectory from a point sampled `n` seconds ahead for trajectory being tracked currently. + - [`moveOnSplineAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveOnSplineAsync) 💚 + +- Minimum jerk trajectory planning (using [ethz-asl/mav_trajectory_generation](https://github.com/ethz-asl/mav_trajectory_generation)), and trajectory tracking (using a pure pursuit like controller minimizing position and velocity errors), with position setpoints and corresponding velocity constraints. Useful for making a drone go through a gate waypoint, while obeying speed and direction constraints. + Optionally use the `*lookahead*` parameters to start new trajectory from a point sampled `n` seconds ahead for trajectory being tracked currently. + - [`moveOnSplineVelConstraintsAsync`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.moveOnSplineVelConstraintsAsync) 💚 + +- Clear and stop following current trajectory. + - [`clearTrajectory`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.clearTrajectory) 💚 + +#### Gain setter APIs: +- [`setAngleRateControllerGains`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.setAngleRateControllerGains) 💚 +- [`setAngleLevelControllerGains`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.setAngleLevelControllerGains) 💚 +- [`setVelocityControllerGains`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.setVelocityControllerGains) 💚 +- [`setPositionControllerGains`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.setPositionControllerGains) 💚 +- [`setTrajectoryTrackerGains`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.setTrajectoryTrackerGains) 💚 + +#### APIs to help generate gate detection datasets: +- Object pose setter and getter: + - [`simSetObjectPose`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simSetObjectPose) + - [`simGetObjectPose`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simGetObjectPose) + +- Object scale setter and getter: + - [`simSetObjectScale`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simSetObjectScale) 💚 + - [`simGetObjectScale`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simGetObjectScale) 💚 + +- Object segmentation ID setter and getter: + - [`simGetSegmentationObjectID`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simGetSegmentationObjectID) + - [`simSetSegmentationObjectID`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simSetSegmentationObjectID) + +- Listing all the objects in the scene: + - [`simListSceneObjects`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simListSceneObjects) 💚 + +- Gate specific APIs: + - [`simGetNominalGateInnerDimensions`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simGetNominalGateInnerDimensions) 💚 + - [`simGetNominalGateOuterDimensions`](https://microsoft.github.io/AirSim-NeurIPS2019-Drone-Racing/api.html#airsimneurips.client.MultirotorClient.simGetNominalGateOuterDimensions) 💚 ## Questions Please open a Github Issue on **this** repository (not [AirSim](https://github.com/microsoft/AirSim)) for any technical questions w.r.t. the Neurips competition. diff --git a/baselines/baseline_racer.py b/baselines/baseline_racer.py index 02257f5..683da06 100644 --- a/baselines/baseline_racer.py +++ b/baselines/baseline_racer.py @@ -130,7 +130,7 @@ def fly_through_all_gates_one_by_one_with_moveOnSpline(self): add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name) def fly_through_all_gates_at_once_with_moveOnSpline(self): - if self.level_name in ["Soccer_Field_Medium", "Soccer_Field_Easy", "ZhangJiaJie_Medium", "Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3"] : + if self.level_name in ["Soccer_Field_Medium", "Soccer_Field_Easy", "ZhangJiaJie_Medium", "Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3", "Final_Tier_1", "Final_Tier_2", "Final_Tier_3"] : vel_max = 30.0 acc_max = 15.0 @@ -268,6 +268,7 @@ def main(args): if args.planning_and_control_api == "moveOnSplineVelConstraints": baseline_racer.fly_through_all_gates_one_by_one_with_moveOnSplineVelConstraints().join() + # Comment out the following if you observe the python script exiting prematurely, and resetting the race baseline_racer.stop_image_callback_thread() baseline_racer.stop_odometry_callback_thread() baseline_racer.reset_race() @@ -275,7 +276,7 @@ def main(args): if __name__ == "__main__": parser = ArgumentParser() parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy", "Soccer_Field_Medium", "ZhangJiaJie_Medium", "Building99_Hard", - "Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3"], default="ZhangJiaJie_Medium") + "Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3", "Final_Tier_1", "Final_Tier_2", "Final_Tier_3"], default="ZhangJiaJie_Medium") parser.add_argument('--planning_baseline_type', type=str, choices=["all_gates_at_once","all_gates_one_by_one"], default="all_gates_at_once") parser.add_argument('--planning_and_control_api', type=str, choices=["moveOnSpline", "moveOnSplineVelConstraints"], default="moveOnSpline") parser.add_argument('--enable_viz_traj', dest='viz_traj', action='store_true', default=False) From 2fbe28c778532abf72c6c0cc96305b3809d83811 Mon Sep 17 00:00:00 2001 From: kayn329 Date: Fri, 3 Apr 2020 17:22:03 -0700 Subject: [PATCH 27/27] fix readme format --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 197fc10..997cc45 100644 --- a/README.md +++ b/README.md @@ -294,18 +294,18 @@ Please read [the race monitoring section](https://github.com/microsoft/AirSim-Ne - R. Spica, D. Falanga, E. Cristofalo, E. Montijano, D. Scaramuzza, and M. Schwager, "A Real-Time Game Theoretic Planner for Autonomous Two-Player Drone Racing", in the Proccedings of Robotics: Science and Systems (RSS), 2018. - Race with a perception method that estimates the next gate's location and moves using moveOnSplineAsync. - - Generate an AirSim settings.json file + - Generate an AirSim settings.json file ```shell $ cd baselines; $ python generate_settings_file.py ``` - Start the AirSim Neurips binary, [as explained above](https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing#running) - - Run the perception code! + - Run the perception code! ```shell $ python baseline_racer_perception.py \ --level_name Qualifier_Tier_2 ``` - - This perception method leverages that all gates are planar, rectangular, and have similar coloration. In each image frame, color thresholding is done to create a mask of the gate. This mask is then compared to that of a baseline image that is taken from a known distance from a gate with known dimensions. A homography transformation between the two masks can be retrieved to find the image coordinate of the center of the gate in the image taken during flight, and a point to point correspondance can be used to find the 3D coordinate of the oncoming gate center. For each gate, a Kalman Filter is used to estimate of the gate center location based on the measurements. The coordinate is then used as a waypoint in the moveOnSplineAsync() API. + - This perception method leverages that all gates are planar, rectangular, and have similar coloration. In each image frame, color thresholding is done to create a mask of the gate. This mask is then compared to that of a baseline image that is taken from a known distance from a gate with known dimensions. A homography transformation between the two masks can be retrieved to find the image coordinate of the center of the gate in the image taken during flight, and a point to point correspondance can be used to find the 3D coordinate of the oncoming gate center. For each gate, a Kalman Filter is used to estimate of the gate center location based on the measurements. The coordinate is then used as a waypoint in the moveOnSplineAsync() API. ## Quick API overview We added some new APIs (marked with 💚) to [AirSim](https://github.com/Microsoft/Airsim) for the NeurIPS competition binaries.