def get_depth(self, cfg):
if cfg.env_type == 'indoor' or cfg.env_type == 'Indoor':
responses = self.client.simGetImages(
[
airsim.ImageRequest(2, airsim.ImageType.DepthVis, False,
False)
],
vehicle_name=self.vehicle_name)
img1d = np.fromstring(responses[0].image_data_uint8,
dtype=np.uint8)
depth = img1d.reshape(responses[0].height, responses[0].width,
3)[:, :, 0]
thresh = 50
elif cfg.env_type == 'outdoor' or cfg.env_type == 'Outdoor':
responses = self.client.simGetImages(
[airsim.ImageRequest(1, airsim.ImageType.DepthPlanner, True)],
vehicle_name=self.vehicle_name)
depth = airsim.list_to_2d_float_array(
responses[0].image_data_float, responses[0].width,
responses[0].height)
# img1d = np.asarray(responses[0].image_data_float)
# depth = img1d.reshape(responses[0].height, responses[0].width)
thresh = 50
# To make sure the wall leaks in the unreal environment doesn't mess up with the reward function
super_threshold_indices = depth > thresh
depth[super_threshold_indices] = thresh
depth = depth / thresh
# plt.imshow(depth)
# # plt.gray()
# plt.show()
return depth, thresh
def getVisualResponse(self):
"""
Returns visual responses stacked on depth axis
"""
responses = self._client.simGetImages(
[
airsim.ImageRequest("0", airsim.ImageType.Infrared,
False, False),
airsim.ImageRequest("0", airsim.ImageType.Scene,
False, False),
airsim.ImageRequest("0", airsim.ImageType.Segmentation,
False, False),
airsim.ImageRequest("0", airsim.ImageType.DepthPerspective, True, False)
]
)
im_reshaped = []
for response in responses[:-1] :
img1d = np.fromstring(response.image_data_uint8, dtype=np.uint8)
im = img1d.reshape(response.height, response.width, 4)
im_reshaped.append(im)
response = responses[-1]
img = airsim.list_to_2d_float_array(response.image_data_float, response.width, response.height)
img = np.expand_dims(img,axis=-1)
im_reshaped.append(img)
vis_response = np.vstack(im_reshaped)
return vis_response
def getGroundTruthImg(count=0, time=Time.now(), need_compress=False):
responds = client.simGetImages([
airsim.ImageRequest("RGB_Left",
airsim.ImageType.DepthPlanar,
pixels_as_float=True,
compress=need_compress),
airsim.ImageRequest("RGB_Left",
airsim.ImageType.Segmentation,
compress=need_compress)
])
# get numpy array
img_depth = airsim.list_to_2d_float_array(responds[0].image_data_float,
responds[0].width,
responds[0].height)
img_buf = np.frombuffer(responds[1].image_data_uint8, dtype=np.uint8)
imgR_seg = img_buf.reshape(responds[1].height, responds[1].width, 3)
msg_depth = bridge.cv2_to_imgmsg(cvim=img_depth, encoding="passthrough")
msg_depth.header.seq = count
msg_depth.header.stamp.set(time.secs, time.nsecs)
msg_seg = bridge.cv2_to_imgmsg(cvim=imgR_seg, encoding="bgr8")
msg_seg.header.seq = count
msg_seg.header.stamp.set(time.secs, time.nsecs)
return msg_depth, msg_seg
def calc_depth_vis_image():
responses = client.simGetImages(
[airsim.ImageRequest("0", airsim.ImageType.DepthVis, True, False)])
response = responses[0]
img2d = airsim.list_to_2d_float_array(response.image_data_float,
response.width, response.height)
# original image is fliped vertically
img2d = np.flipud(img2d)
return img2d
def transform_input(responses, img_size=[84, 84]):
# list returned by airsim api
#
response = responses[0]
# get numpy array
img = airsim.list_to_2d_float_array(response.image_data_float,
response.width, response.height)
img = cv2.resize(img, dsize=tuple(img_size), interpolation=cv2.INTER_CUBIC)
img = np.expand_dims(img, axis=-1)
return img
def get_depth(client):
requests = []
requests.append(
airsim.ImageRequest('front_center',
airsim.ImageType.DepthPlanner,
pixels_as_float=True,
compress=False))
responses = client.simGetImages(requests)
depth = airsim.list_to_2d_float_array(responses[0].image_data_float,
responses[0].width,
responses[0].height)
depth = np.expand_dims(depth, axis=2)
depth = depth.squeeze()
return depth, responses[0].width, responses[0].height
def getDepth(self, save=False):
response = self.client.simGetImages(
[airsim.ImageRequest("0", airsim.ImageType.DepthPlanner, True)])
response = response[0]
# get numpy array
img1d = airsim.list_to_2d_float_array(response.image_data_float,
response.width, response.height)
if save:
cv2.imwrite(
f'../data/imagens/Depth/voo/frame_{self.cont_depth}_{self.date_str}.jpg',
img1d)
self.cont_depth += 1
if len(img1d) < 1:
return None
return img1d
def capture_depth(self):
requests = [
airsim.ImageRequest('front_center',
airsim.ImageType.DepthPlanner,
pixels_as_float=True,
compress=False)
]
responses = self.env.client.simGetImages(requests)
response = responses[0]
depth = airsim.list_to_2d_float_array(response.image_data_float,
response.width, response.height)
plt.imshow(depth, cmap='gray', vmin=-5, vmax=30)
plt.savefig(self.save_path + "/depth/image{}.png".format(self.img_idx),
dpi=300)
print("Image {} captured!".format(self.img_idx))
self.img_idx += 1
def get_image_messages(client):
# get camera images from the car
responses = client.simGetImages([
airsim.ImageRequest("1", airsim.ImageType.Scene, False, False),
airsim.ImageRequest("0", airsim.ImageType.DepthPlanner, True)
])
# convert scene uint8 array to NumPy 2D array using
scene_img1d = np.fromstring(responses[0].image_data_uint8, dtype=np.uint8) # get numpy array
scene_img_rgb = scene_img1d.reshape(responses[0].height, responses[0].width, 3) # reshape array to image array H X W
# convert depth float array to NumPy 2D array using
depth_img = airsim.list_to_2d_float_array(responses[1].image_data_float, responses[1].width, responses[1].height)
# Populate image message
rgb_msg = numpy_to_image(scene_img_rgb, "rgb8")
depth_msg = numpy_to_image(depth_img, "32FC1")
return rgb_msg, depth_msg
def getSensorsData(self, sensor, **kargs):
if sensor is "cam":
response = self.client.simGetImages([
airsim.ImageRequest("0", airsim.ImageType.Scene, False, False)
])
response = response[0]
img1d = np.fromstring(response.image_data_uint8, dtype=np.uint8)
img_rgba = img1d.reshape(response.height, response.width, 4)
return img_rgba
elif sensor is "dcam":
response = self.client.simGetImages([
airsim.ImageRequest("0", airsim.ImageType.DepthPlanner, True)
])
response = response[0]
img1d = airsim.list_to_2d_float_array(response.image_data_float,
response.width,
response.height)
return img1d
else:
return None
def get_CustomDepth(self, cfg):
camera_name = 2
if cfg.env_type == 'indoor' or cfg.env_type == 'Indoor':
max_tries = 5
tries = 0
correct = False
while not correct and tries < max_tries:
tries += 1
responses = self.client.simGetImages(
[
airsim.ImageRequest(camera_name,
airsim.ImageType.DepthVis, False,
False)
],
vehicle_name=self.vehicle_name)
img1d = np.fromstring(responses[0].image_data_uint8,
dtype=np.uint8)
# AirSim bug: Sometimes it returns invalid depth map with a few 255 and all 0s
if np.max(img1d) == 255 and np.mean(img1d) < 0.05:
correct = False
else:
correct = True
depth = img1d.reshape(responses[0].height, responses[0].width,
3)[:, :, 0]
thresh = 50
elif cfg.env_type == 'outdoor' or cfg.env_type == 'Outdoor':
responses = self.client.simGetImages(
[airsim.ImageRequest(1, airsim.ImageType.DepthPlanner, True)],
vehicle_name=self.vehicle_name)
depth = airsim.list_to_2d_float_array(
responses[0].image_data_float, responses[0].width,
responses[0].height)
thresh = 50
# To make sure the wall leaks in the unreal environment doesn't mess up with the reward function
super_threshold_indices = depth > thresh
depth[super_threshold_indices] = thresh
depth = depth / thresh
return depth, thresh
def _extract_depth_planner_image(self, sim_img_response_list):
"""
Extract depth planner img from response obj in ^ and apply self.PHI to a depth planner image.
:param sim...: list of image responses w/ 1 ImageResponse object for depth_planner
that has its data that can be reshaped into a 2D (i.e., 1 channel) array / an image
:returns: 2D numpy array of float32; preprocessed depth planner image
"""
depth_planner_img_responses = []
for idx in range(0, len(sim_img_response_list)):
if sim_img_response_list[
idx].image_type == airsim.ImageType.DepthPlanner:
depth_planner_img_responses.append(sim_img_response_list[idx])
# originally, the image is in 3 1D python lists of strings; want to turn them into a 2D numpy arrays
img = np.concatenate([airsim.list_to_2d_float_array(depth_planner_img_response_obj.image_data_float,
depth_planner_img_response_obj.width,
depth_planner_img_response_obj.height) \
for depth_planner_img_response_obj in depth_planner_img_responses],
axis=1)
return img
def get_camera_observation(client,
sensor_types=['rgb', 'depth'],
max_dist=10,
height=64,
width=64):
"""
Makes call to AirSim and extract depth and/or RGB images from the UAV's camera. Due to a bug in AirSim
an empty array is sometimes returned which is caught in this method.
:param client: AirsimEnv object
:param sensor_types: List of the image types to extract. 'rgb' and 'depth' available.
:param max_dist: Max depth at which the depth map is capped.
:param height: height of the images
:param width: width of the images
:return: dict containing the 'rgb' and/or 'depth' images
"""
requests = []
sensor_idx = {}
idx_counter = 0
if 'rgb' in sensor_types:
requests.append(
airsim.ImageRequest('front_center',
airsim.ImageType.Scene,
pixels_as_float=False,
compress=False))
sensor_idx.update({'rgb': idx_counter})
idx_counter += 1
if 'depth' in sensor_types:
requests.append(
airsim.ImageRequest('front_center',
airsim.ImageType.DepthPlanner,
pixels_as_float=True,
compress=False))
sensor_idx.update({'depth': idx_counter})
idx_counter += 1
responses = client.simGetImages(requests)
images = {}
if 'rgb' in sensor_types:
idx = sensor_idx['rgb']
# convert to uint and reshape to matrix with 3 color channels
try:
bgr = np.reshape(
airsim.string_to_uint8_array(responses[idx].image_data_uint8),
(height, width, 3))
# move color channels around
rgb = np.array(bgr[:, :, [2, 1, 0]], dtype=np.float32)
except ValueError as err:
print('========================================================')
print('Value err when reshaping RGB image: {0}'.format(err))
print('Replacing rgb with all zeros')
print('========================================================')
rgb = np.zeros((height, width, 3), dtype=np.float32)
images.update({'rgb': rgb})
if 'depth' in sensor_types:
idx = sensor_idx['depth']
# convert to 2D numpy array. Had unexpected exception here. Try: Catch
try:
depth = airsim.list_to_2d_float_array(
responses[idx].image_data_float, width, height)
except ValueError as err:
print('========================================================')
print('Value err when reshaping depth image: {0}'.format(err))
print('Replacing depth map with all max dist values')
print('========================================================')
depth = np.ones((height, width), dtype=np.float32) * max_dist
depth = np.expand_dims(depth, axis=2)
images.update({'depth': depth})
return images
pixels_as_float=True,
compress=False),
airsim.ImageRequest(camera_name=forward_cam_name,
image_type=airsim.ImageType.DepthPlanner,
pixels_as_float=True,
compress=False),
airsim.ImageRequest(camera_name=right_cam_name,
image_type=airsim.ImageType.DepthPlanner,
pixels_as_float=True,
compress=False)
])
print(len(sim_img_responses))
img = np.concatenate([
airsim.list_to_2d_float_array(
img_response_obj.image_data_float,
img_response_obj.width,
img_response_obj.height,
) for img_response_obj in sim_img_responses
],
axis=1)
fraction_of_top_of_scene_to_drop = 0.3
fraction_of_bottom_of_scene_to_drop = 0.45
first_scene_row_idx = int(img.shape[0] * fraction_of_top_of_scene_to_drop)
last_scene_row_idx = int(img.shape[0] *
(1 - fraction_of_bottom_of_scene_to_drop))
img = img[first_scene_row_idx:last_scene_row_idx]
# 255 * 2 = 510
PHI = lambda pixel: 0.0 if pixel < 0.575 else 724.0 / pixel
# this function is about 40% faster than the previous min/max func (below)
def main(args):
client = airsim.VehicleClient()
client.confirmConnection()
ts = datetime.datetime.now().isoformat()[:-7].replace(':', '')
tmp_dir = os.path.join(args.out_path, args.env, ts)
print("Saving images to %s" % tmp_dir)
try:
os.makedirs(tmp_dir)
except OSError:
if not os.path.isdir(tmp_dir):
raise
nseqs = 3600
h5file = tables.open_file(os.path.join(tmp_dir, 'output.h5'),
mode="w",
title="Flythroughs")
seq_len = 40
short_seq_len = 10
nominal_fps = 30
labels_table = h5file.create_table('/',
'labels',
Particle,
expectedrows=nseqs)
video_table = h5file.create_earray('/',
'videos',
tables.UInt8Atom(),
shape=(0, seq_len, 3, 112, 112),
expectedrows=nseqs)
short_video_table = h5file.create_earray('/',
'short_videos',
tables.UInt8Atom(),
shape=(0, short_seq_len, 3, 112,
112),
expectedrows=nseqs)
depth_table = h5file.create_earray('/',
'depth',
tables.Float64Atom(),
shape=(0, 112, 112),
expectedrows=nseqs)
for i in tqdm(range(nseqs)): # do few times
# For flat environments, start ground plane localization not too high.
ground_from = 5
# 3 meters/s -> jogging speed
MAX_SPEED = 3
collisions = True
pause = 0
# Manually define areas of interest. Note that inside one of the airsim
# environments, you can pull up coordinates using `;`. However, the
# coordinates listed are multiplied by 100 (i.e. the numbers are in cm
# rather than meters); divide by 100 to define reasonable boundaries
# for the capture area.
if args.env == 'blocks':
x = np.random.uniform(-50, 50)
y = np.random.uniform(-50, 50)
pause = .05 # blocks is too fast sometimes
elif args.env.startswith('nh'):
x = np.random.uniform(-150, 150)
y = np.random.uniform(-150, 150)
client.simEnableWeather(True)
for k in range(8):
client.simSetWeatherParameter(k, 0.0)
if args.env == 'nh_fall':
client.simSetWeatherParameter(
airsim.WeatherParameter.MapleLeaf, 1.0)
client.simSetWeatherParameter(airsim.WeatherParameter.RoadLeaf,
1.0)
if args.env == 'nh_winter':
client.simSetWeatherParameter(airsim.WeatherParameter.Snow,
1.0)
client.simSetWeatherParameter(airsim.WeatherParameter.RoadSnow,
1.0)
elif args.env == 'mountains':
# Most of the interesting stuff (e.g. the lake, roads) is on the
# diagonal of this very large environment (several kilometers!).
xy = np.random.uniform(0, 2500)
xmy = np.random.uniform(-100, 100)
x = xy + xmy
y = xy - xmy
# This environment varies a lot in height, start from higher
ground_from = 100
elif args.env == 'trap':
x = np.random.uniform(-10, 10)
y = np.random.uniform(-10, 10)
# This one has lots of branches, keep sequences that have collisions
collisions = False
else:
raise NotImplementedError(args.env)
# Time of day effects works in blocks and trap only.
time_of_day = np.random.randint(5, 21)
client.simSetTimeOfDay(
True,
start_datetime=f"2020-03-21 {time_of_day:02}:00:00",
is_start_datetime_dst=True,
celestial_clock_speed=0,
update_interval_secs=60,
move_sun=True)
if pause > 0:
time.sleep(pause)
pitch = np.random.uniform(
-.25,
.25) # Sometimes we look a little up, sometimes a little down
roll = 0 # Should be 0 unless something is wrong
# 360 degrees lookaround
yaw = np.random.uniform(-np.pi, np.pi)
heading_yaw = draw_von_mises(2.5)
heading_pitch = draw_von_mises(16)
# Max ~90 degrees per second head rotation
rotation_yaw = 30 * np.pi / 180 * np.random.randn()
rotation_pitch = 10 * np.pi / 180 * np.random.randn()
speed = MAX_SPEED * np.random.rand()
# Figure out the height of the ground. Move the camera way far above
# ground, and get the distance to the ground via the depth buffer
# from the bottom camera.
client.simSetVehiclePose(
airsim.Pose(airsim.Vector3r(x, y, -ground_from),
airsim.to_quaternion(0, 0, 0)), True)
if pause > 0:
time.sleep(pause)
responses = client.simGetImages([
airsim.ImageRequest("bottom_center",
airsim.ImageType.DepthPlanner,
pixels_as_float=True)
])
response = responses[0]
the_arr = airsim.list_to_2d_float_array(response.image_data_float,
response.width,
response.height)
# Then move to ~5.5 feet above the ground
rgy = range(int(.4 * the_arr.shape[0]), int(.6 * the_arr.shape[0]))
rgx = range(int(.4 * the_arr.shape[0]), int(.6 * the_arr.shape[0]))
the_min = np.median(the_arr[rgy, rgx])
z = the_min - ground_from - np.random.uniform(1.4, 2)
if z > 50:
# More than 50 meters below sea level, bad draw.
continue
#client.startRecording()
z = z.item()
depths = []
frames = []
booped = False
for k in range(seq_len):
if booped:
continue
# In nominal seconds
t = (k - (seq_len - 1) / 2) / nominal_fps
d = t * speed
client.simSetVehiclePose(
airsim.Pose(
airsim.Vector3r(
x + d * np.cos(yaw + heading_yaw) *
np.cos(pitch + heading_pitch),
y + d * np.sin(yaw + heading_yaw) *
np.cos(pitch + heading_pitch),
z + d * np.sin(pitch + heading_pitch)),
airsim.to_quaternion(pitch + t * rotation_pitch, roll,
yaw + t * rotation_yaw)), True)
responses = client.simGetImages([
airsim.ImageRequest("front_center", airsim.ImageType.Scene,
False, False),
airsim.ImageRequest("front_center",
airsim.ImageType.DepthPlanner, True,
False),
])
for j, response in enumerate(responses):
if j == 0:
frames.append(
airsim.string_to_uint8_array(
response.image_data_uint8).reshape(
response.height, response.width,
3)[:, :, ::-1])
if j == 1:
zbuff = airsim.list_to_2d_float_array(
response.image_data_float, response.width,
response.height)
depths.append(zbuff)
# Did we bump into something?
if collisions:
closest = zbuff[zbuff.shape[0] // 4:-zbuff.shape[0] //
4, zbuff.shape[1] //
4:-zbuff.shape[1] // 4].min()
if closest < .5:
# oops I booped it again.
booped = True
if j == 0 and args.save_images:
filename = os.path.join(tmp_dir, f"{i:02}_{k:02}.png")
matplotlib.image.imsave(filename, frames[-1])
if pause > 0:
time.sleep(pause)
row = labels_table.row
if not booped and not args.save_images:
# Let's save this!
row['x'] = x
row['y'] = y
row['z'] = z
row['yaw'] = yaw
row['pitch'] = pitch
row['speed'] = speed
row['heading_yaw'] = heading_yaw
row['heading_pitch'] = heading_pitch
row['rotation_yaw'] = rotation_yaw
row['rotation_pitch'] = rotation_pitch
row.append()
V = np.stack(frames, axis=0).transpose((0, 3, 1, 2))
V = V.reshape((1, ) + V.shape)
video_table.append(V)
# Take a subset of the data.
mid_seq = range((seq_len - short_seq_len) // 2,
(seq_len + short_seq_len) // 2)
assert V.shape[1] == seq_len
short_video_table.append(V[:, mid_seq, :, :, :])
# Only record the mid-point z buffer
n = seq_len // 2
D = depths[n]
D = D.reshape((1, ) + D.shape)
depth_table.append(D)
h5file.close()
# currently reset() doesn't work in CV mode. Below is the workaround
client.simSetVehiclePose(
airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)),
True)
def image_views(survey_path,
reward_detection,
x,
y,
z,
breaker,
z_min=-30,
z_max=-60,
quad=31,
camera='3'):
# print ('Starting segmentation:',reward_detection)
# print ('Starting segmentation:',survey_path)
#print ('Array:',reward_detection[:])
#import pdb;pdb.set_trace();
#reward = np.zeros((10,10),dtype=int)
client = airsim.MultirotorClient()
# client.confirmConnection()
# client.enableApiControl(True)
# client.armDisarm(True)
#Initialize windows
cv2.namedWindow('Depth')
cv2.namedWindow('Segmentation')
cv2.namedWindow('Scene')
#Set the segmentation id for the object to be detected
segment_id = client.simSetSegmentationObjectID('OrangeBall[\w]*', 6, True)
#object_global_info = client.simGetObjectPose('OrangeBall[\w]*')
#print (object_global_info)
scale_x = quad * (10 / x)
scale_y = quad * (10 / y)
scale_z = (z_max - z_min) / z
# path = [(p[0]*quad,p[1]*quad) for p in survey_path]
#path = [(p[0]*scale_x+scale_x/2,p[1]*scale_y+scale_y/2,z_min+p[2]*(z_max-z_min)/z) for p in survey_path]
lower = np.array([100, 60, 150], dtype="uint8")
upper = np.array([110, 70, 160], dtype="uint8")
curr_count, prev_count, tot_count, local_count, index = 0, 0, 0, 0, 0
tolerance = 500
locations = {'x': [], 'y': []}
label, reward_dict = {}, {}
curr_drone_pos, prev_drone_pos = (0, 0, 0), (0, 0, 0)
curr_survey_path, prev_survey_path = [], []
#ll.display(nodes)
# nodes = initiliaze_nodes(survey_path,x,y,z)
# curr_node = nodes.data
# next_node = nodes.next
# print ('Nodes:',curr_node,'->',next_node.data)
while True:
try:
curr_survey_path = survey_path[:]
nodes = initiliaze_nodes(survey_path, x, y, z)
curr_node = nodes.data
next_node = nodes.next
#print ('Nodes:',curr_node,'->',next_node.data)
boxes = []
responses = client.simGetImages([
airsim.ImageRequest(camera, airsim.ImageType.DepthPerspective,
True),
airsim.ImageRequest(camera, airsim.ImageType.Segmentation,
False, False),
airsim.ImageRequest(camera, airsim.ImageType.Scene, False,
False)
])
drone_position = client.simGetVehiclePose().position
#rounded_coordinates = nearest_multiple(drone_position,scale_x,scale_y,scale_z,z_min)
curr_drone_pos = nearest_multiple(drone_position, scale_x, scale_y,
scale_z, z_min)
for resp in responses:
if not resp.pixels_as_float:
img = np.fromstring(resp.image_data_uint8, dtype=np.uint8)
img = img.reshape(resp.height, resp.width, 3)
if resp.image_type == 5:
win_name = 'Segmentation'
mask = cv2.inRange(img, lower, upper)
number, labels, stats, centroids = cv2.connectedComponentsWithStats(
mask, 8, cv2.CV_32S)
curr_count = number - 1
for ind in range(1, number):
i = stats[ind]
cen = centroids[ind]
bb = [(i[0], i[1]), (i[0] + i[2], i[1] + i[3])]
# finding the global position (0-63000) of the topleft corner of the bounding box with camera focal length of 134.48
#global y location of the object in UE coordinates
object_global_pose_y = drone_position.y_val * 100 + (
cen[0] - 127.5) * (-drone_position.z_val *
100) / 129.31
#global x location of the object in UE coordinates
object_global_pose_x = drone_position.x_val * 100 - (
cen[1] - 71.5) * (-drone_position.z_val *
100) / 129.31
#print ('Drone Pos:',drone_position.y_val*100)
#print (ind,':',bb,':', i[0], 'y:', object_global_pose_y, 'x:', object_global_pose_x)
locations['x'].append(object_global_pose_x)
locations['y'].append(object_global_pose_y)
#print (locations)
boxes.append(bb)
if prev_count > curr_count:
#print ('Checking if the object is already detected')
if len(locations['x']) > 0 and len(
locations['y']) > 0:
X_avg = sum(locations['x']) // len(
locations['x'])
Y_avg = sum(locations['y']) // len(
locations['y'])
check = check_locations(
label, X_avg, Y_avg, tolerance)
#print ('X average:',X_avg,'Y average:',Y_avg)
if check:
#print (locations)
local_count += 1
tot_count += 1
# label[tot_count] = np.round([np.min(locations),np.max(locations)],).astype(int)
label[tot_count] = (int(X_avg), int(Y_avg))
locations = {'x': [], 'y': []}
#print (label)
prev_count = curr_count
#boxes = object_detection(img, client)
elif resp.image_type == 0:
win_name = 'Scene'
for i in boxes:
cv2.rectangle(img, i[0], i[1], (0, 255, 0), 2)
else:
win_name = 'Depth'
img = airsim.list_to_2d_float_array(
resp.image_data_float, resp.width, resp.height)
img = cv2.convertScaleAbs(img)
cv2.imshow(win_name, img)
if breaker.value == 0:
# index = i_j_to_state(int(curr_node[0]/quad),int(curr_node[1]/quad),size)
# reward_detection[index] = local_count
#print ('Reward:',reward_detection)
print('Completed Episode. Exiting segmentation.py...')
break
# print ('Actual Drone location:',drone_position.z_val)
# print ('Current Node:',curr_node,'Next node:',next_node.data)
# print ('Drone Position:',rounded_coordinates)
#if (next_node != None) and (rounded_coordinates == next_node.data):
if curr_survey_path != prev_survey_path:
#if curr_node not in reward_dict:
# print ('Drone moved from ',curr_node,' to ',next_node.data)
# print ('Number of Detections: ',local_count)
# print ('Award assigned to: ',curr_node)
reward_detection.value = local_count
# print ('Reward:',reward_detection.value)
reward_dict[curr_node] = local_count
local_count = 0
# print ('Reward Dictionary:',reward_dict)
# print ('Locations of detected objects:',label)
#else:
# continue
# curr_node = next_node.data
# next_node = next_node.next
# index += 1
prev_drone_pos = curr_drone_pos
prev_survey_path = curr_survey_path
key = cv2.waitKey(1) & 0xFF
if (key == 27 or key == ord('q') or key == ord('x')):
break
except Exception as e:
print('Exception in segmentation.py', e)
traceback.print_exc()
img_rgb = img1d.reshape(img.height, img.width, 3)
cv2.imshow('Scene', img_rgb)
img = imgs[1]
# if img.pixels_as_float == 0.0:
# img1d = np.frombuffer(img.image_data_uint8, np.uint8)
# img2d = img1d.reshape(img.height, img.width, 3)
# else:
# data_float = list(map(lambda x: x / 50, img.image_data_float))
# img2d = airsim.list_to_2d_float_array(data_float, img.width, img.height)
# cv2.imshow('DepthPlanner', img2d)
# img = imgs[2]
# data_float = list(map(lambda x: x / 50, img.image_data_float))
# img2d = airsim.list_to_2d_float_array(data_float, img.width, img.height)
# cv2.imshow('DepthPerspective', img2d)
# img = imgs[3]
img1d = airsim.list_to_2d_float_array(img.image_data_float, img.width,
img.height)
cv2.imshow('DepthVis', img1d)
# img = imgs[4]
# img1d = np.frombuffer(img.image_data_uint8, dtype=np.uint8)
# img_rgb = img1d.reshape(img.height, img.width, 3)
# cv2.imshow('Segmentation', img_rgb)
# Esc的ASCII码为27, 0xFF可以防止某些系统出bug
if cv2.waitKey(1) & 0xFF == 27:
break
client.simPause(True)
client.enableApiControl(False)
client.armDisarm(False)
cv2.destroyAllWindows()
