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