def createMap(self):
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# plt.imshow(np.ones((self.height, self.width)), cmap = "binary")
for zone in self.noFlyZones:
# create a tetrahedron from the no fly zone and display it as a polycollection
zonePoly = zone.polygonObj
baseHeight = zone.bottom
topHeight = zone.top
vertices = []
topFace = []
bottomFace = []
for coord in zone.coords:
bottomFace.append(list(coord) + [baseHeight])
topFace.append(list(coord) + [topHeight])
sideFaces = []
for i in range(0, len(zone.coords)):
face = [
list(zone.coords[i]) + [baseHeight],
list(zone.coords[i]) + [topHeight],
list(zone.coords[i - 1]) + [topHeight],
list(zone.coords[i - 1]) + [baseHeight]
]
sideFaces.append(face)
collection = Poly3DCollection([topFace, bottomFace] + sideFaces,
linewidths=1,
alpha=0.2)
collection.set_edgecolor("orange")
collection.set_facecolor("black")
ax.add_collection3d(collection)
i = 0
for waypoint in self.waypoints:
plotStyle = None
if i == 0: # display the first and second waypoints differently
color = 'r'
elif i == 1:
color = 'b'
else:
color = 'k'
ax.scatter(waypoint.x, waypoint.y, waypoint.z, s=30, c=color)
i = i + 1
# show area which we CAN fly in with green
flyZonePatch = PolygonPatch(self.flyZone)
flyZonePatch.set_facecolor("green")
flyZonePatch.set_alpha(0.2)
#ax.add_patch(flyZonePatch)
# now lets show the path
ax.scatter(self.finalPath[0][0],
self.finalPath[0][1],
self.finalPath[0][2],
s=50,
c='g')
ax.scatter(self.finalPath[-1][0],
self.finalPath[-1][1],
self.finalPath[-1][2],
s=50,
c='g')
xvals = [row[0] for row in self.finalPath]
yvals = [row[1] for row in self.finalPath]
zvals = [row[2] for row in self.finalPath]
plt.plot(xvals, yvals, zvals, '.-')
plt.show()
def main():
j = 1
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
mng = plt.get_current_fig_manager()
mng.full_screen_toggle()
cleo = True
listpiw = []
dancing = 0
freeze = 0
downwind_leeway, crosswind_leeway = leeway()
errord = 0.12
errorc = 0.094
comprised = 0
fov = 20
plt.ion()
plt.grid()
soph = [] #holds all search areas - for all timestamps
foundline = False
eliminate_bug = True
elly = False
###INITIAL POSITION###
op = 50
gg = 1000
errors = []
for error in range(200):
errors.append([
normalfunction(0.12, False),
normalfunction(0.094, False)
]) ###### compute errors to simulate PIW uncertainty
tin = 0
ship_vel = 10.8 #m/s
mob_signal_time = 180 #seconds
distancetolkp = ship_vel * mob_signal_time
ax.set_facecolor('#e6f5ff')
locationuav = [50, 1000 + distancetolkp]
locationuav_no2 = [50, 1000 + distancetolkp]
start_point_uav = [50, 1000 + distancetolkp]
goals = 1
uavspeed = 15.0 #make sure that the loops align - search for "LOOP_PART" - there should be 3: 0, 1, 2 LOOP_PART0
sum_path = 0
theirgoal = 1
p = [0, 0]
piwlkp = [50, 1000]
locationstoplotplsnohate = []
piw_atmob = [
piwlkp[0] + downwind_leeway * (mob_signal_time) - errord * 2 *
(mob_signal_time), piwlkp[1]
]
center_piw_atmob = [
piwlkp[0] + downwind_leeway * (mob_signal_time), piwlkp[1]
]
timestamp = 10 #1 timestamp = 10 seconds
for i in range(
0, 2000,
timestamp): ##########Caution: TIMESTAMPS############## LOOP_PART1
t = i
timothy = (time.time() - person_down) #*10 + 100
ax = plt.gca()
ax.cla() # optionally clear axes
downwind_leeway, crosswind_leeway = leeway()
gg = 50 + downwind_leeway * t
op = 1000 + crosswind_leeway * t
exp1 = 0.12 * 2 * 2 * t #**2 #############EXPAND THE AREA every second##### on X
exp2 = 0.094 * 2 * 2 * t #**2 #### ON Y ####
theirupdown = 0
theirlist = []
thier_sum_path = 0
ells = Ellipse((gg, op), exp1, exp2, el_ang, color="#66c2ff")
plt.plot(gg, op, color='g', marker='o')
ells.set_alpha(0.4)
ells.set_clip_box(ax.bbox)
#ells.set_alpha(0.1)
ax.add_artist(ells)
if t >= mob_signal_time:
polygon1 = []
if foundline == False: ###FIND the intercept - where the UAV should fly to intersect with the last position where the PIW may be
for totot in range(t, 20000):
p = [
piw_atmob[0] + downwind_leeway *
(totot - mob_signal_time) - errord * 2 *
(totot - mob_signal_time), piw_atmob[1]
]
cant = dist(locationuav[0], locationuav[1], p[0], p[1])
auxil = p[0] - piw_atmob[0]
if int(
dist(locationuav[0], locationuav[1], p[0], p[1]) /
uavspeed) <= int(auxil /
(downwind_leeway - 2 * errord)):
firstone = totot - mob_signal_time
foundline = True
break
pathuav = []
pathuav.append([50, start_point_uav[1]])
pathuav.append(p)
plt.plot((50, p[0]), (start_point_uav[1], p[1]), 'b-')
plt.plot(piw_atmob[0], piw_atmob[1], color='m', marker='D')
plt.plot(p[0], p[1], color='m', marker='D')
if t <= mob_signal_time + int(
dist(start_point_uav[0], start_point_uav[1], p[0], p[1]) /
uavspeed):
errmm = (mob_signal_time + int(
dist(start_point_uav[0], start_point_uav[1], p[0], p[1]) /
uavspeed))
deliminator = 0.1
gg = 50 + downwind_leeway * errmm
op = 1000 + crosswind_leeway * errmm
exp1 = ((0.12) * 2) * 2 * errmm
exp2 = ((0.094) * 2) * 2 * errmm
####PLOT the search area####
ells = Ellipse((gg, op), exp1, exp2, el_ang, color="#ff0066")
plt.plot(gg, op, color='g', marker='o')
ells.set_alpha(0.3)
ells.set_clip_box(ax.bbox)
#ells.set_alpha(0.1)
ax.add_artist(ells)
if retursoph(
exp1, exp2, deliminator, gg, op
) not in soph: #merge search areas over different timestamps
soph.append(
retursoph(exp1, exp2, deliminator, gg,
op)) ####add polygons for each area(t)
xs, ys = [], []
for poop in range(0, len(soph[len(soph) - 1])):
xs.append(round(soph[len(soph) - 1][poop][0], 2))
ys.append(round(soph[len(soph) - 1][poop][1], 2))
polygon1 = shape_pol(
list(zip(xs, ys))
) ### polygon in shapely (library) format ### --- so that we can merge them
i += 1
else:
for i in range(
len(soph) + mob_signal_time + int(
dist(start_point_uav[0], start_point_uav[1], p[0],
p[1]) / uavspeed), t):
deliminator = 0.1
gg = 50 + downwind_leeway * i ###FIXED LEEWAY SPEED
op = 1000 + crosswind_leeway * i
exp1 = ((0.12) * 2) * 2 * i #**2
exp2 = (
(0.094) * 2
) * 2 * i ###############EXPAND THE search area #############
if i == t: ####PLOT the latest search area####
ells = Ellipse((gg, op),
exp1,
exp2,
el_ang,
color="#ff0066")
plt.plot(gg, op, color='g', marker='o')
ells.set_alpha(0.3)
ells.set_clip_box(ax.bbox)
#ells.set_alpha(0.1)
ax.add_artist(ells)
if i > 0:
if retursoph(exp1, exp2, deliminator, gg,
op) not in soph:
soph.append(
retursoph(
exp1, exp2, deliminator, gg,
op)) ####add polygons for each area(t)
xs, ys = [], []
for poop in range(0, len(soph[len(soph) - 1])):
xs.append(round(soph[len(soph) - 1][poop][0], 2))
ys.append(round(soph[len(soph) - 1][poop][1], 2))
polygon1 = shape_pol(
list(zip(xs, ys))
) ### polygon in shapely (library) format ### --- so that we can merge them
i += 1
if polygon1 != []: # > 0:
if elly == True: ####PUT POLYGONS TOGETHER
boundarii = cascaded_union([boundarii, polygon1])
else:
boundarii = cascaded_union([polygon1])
elly = True
patch2b = PolygonPatch(boundarii,
fc='#3399ff',
ec='#ff9900',
alpha=1,
zorder=2)
patch2b.set_alpha(0.3)
ax.add_patch(patch2b)
plt.grid(color='#ffcce0', linestyle=':', linewidth=2)
#####################STARTING TO PLOT THE PATH#######################################
boing = list(
boundarii.exterior.coords
) ###ASSIGN ALL POINTS IN FRAME POLYGON = list containing all points belonging to the polygon margins
murs = len(boing)
mid = boundarii.bounds[1] + (boundarii.bounds[3] -
boundarii.bounds[1]) / 2
upper = []
lower = []
########SORT ABOUT MID
for y in range(0, len(boing)):
if boing[y][1] > mid:
upper.append(boing[y])
#plt.plot(boing[y][0], boing[y][1], color = 'r', marker = 'o')
elif boing[y][1] <= mid:
lower.append(boing[y])
#plt.plot(boing[y][0], boing[y][1], color = 'b', marker = 'o')
#sort this??
lower = sorted(lower)
fov = 20
lolo = []
sum_path = 0
upup = []
########################### KEEP POINTS AT FOV DISTANCE ####################
lolo.append(lower[0])
for y in range(0, len(lower)):
keep = True
if y == 0:
auxx = 50 #
auxy = 1000
upper_x = 50 + downwind_leeway * t + (0.12) * 2 * t #**2
upper_y = 1000 + crosswind_leeway * t #+ (0.094)*t
else:
if dist(auxx, auxy, lower[y][0], lower[y][1]) > fov:
auxx = lower[y][0]
auxy = lower[y][1]
lolo.append(lower[y])
if dist(upper_x, upper_y, lower[y][0],
lower[y][1]) < fov - fov / 3:
break
upup.append(upper[0])
for y in range(0, len(upper)):
keep = True
if y == 0:
auxx = 50
auxy = 1000
upper_x = 50 + downwind_leeway * t + (0.12) * 2 * t #**2
upper_y = 1000 + crosswind_leeway * t #+ (0.094)*t
else:
if dist(auxx, auxy, upper[y][0], upper[y][1]) > fov:
auxx = upper[y][0]
auxy = upper[y][1]
if int(upper[y][0]) != 50 and int(upper[y][1]) != 50:
upup.append(upper[y])
if dist(upper_x, upper_y, upper[y][0],
upper[y][1]) < fov - fov / 3:
break
updown = 0
pathuav = []
pathuav.append([50, start_point_uav[1]])
pathuav.append(p)
pathuav.append([upup[0][0], upup[0][1]])
pathuav.append(
[upup[0][0], upup[0][1], 1000 + crosswind_leeway * 0])
sum_path += dist(upup[0][0], upup[0][1], 50, 1000)
upup.append([
50 + downwind_leeway * t + (0.12) * 2 * t,
1000 + crosswind_leeway * t
])
lolo.append([
50 + downwind_leeway * t + (0.12) * 2 * t,
1000 + crosswind_leeway * t
])
if len(lolo) < len(upup):
memes = len(lolo) - 1
else:
memes = len(upup) - 1
for mem in range(memes):
plt.plot((upup[mem][0], lolo[mem][0]),
(upup[mem][1], lolo[mem][1]), 'r-')
sum_path += dist(upup[mem][0], upup[mem][1], lolo[mem][0],
lolo[mem][1])
if updown == 0:
pathuav.append([upup[mem][0], upup[mem][1]])
pathuav.append([lolo[mem][0], lolo[mem][1]])
pathuav.append([lolo[mem + 1][0], lolo[mem + 1][1]])
updown = 1
sum_path += dist(lolo[mem + 1][0], lolo[mem + 1][1],
lolo[mem][0], lolo[mem][1])
else:
updown = 0
pathuav.append([lolo[mem][0], lolo[mem][1]])
pathuav.append([upup[mem][0], upup[mem][1]])
pathuav.append([upup[mem + 1][0], upup[mem + 1][1]])
ey = 0
for point in pathuav:
if ey < len(pathuav) - 1:
plt.plot((pathuav[ey + 1][0], pathuav[ey][0]),
(pathuav[ey + 1][1], pathuav[ey][1]),
color='#339966',
marker='_')
ey += 1
if t >= mob_signal_time:
for i in range(int(uavspeed) * timestamp +
1): ######## LOOP_PART2
if goals < len(pathuav):
p1 = locationuav
p2 = pathuav[goals]
x = p2[0] - p1[0]
y = p2[1] - p1[1]
#print (x, y)
angle = math.atan2(y, x)
locationuav = [p1[0] + cos(angle), p1[1] + sin(angle)]
if int(pathuav[goals][0]) in np.arange(
int(locationuav[0]) - 2.0,
int(locationuav[0]) + 2.0) and int(
pathuav[goals][1]) in np.arange(
int(locationuav[1]) - 2.0,
int(locationuav[1]) + 2.0):
goals += 1
plt.plot(locationuav[0],
locationuav[1],
color='k',
marker='o') #339966
if cleo == False:
for point in listpiw:
if point[0] < locationuav[
0] + fov / 2 and point[
0] > locationuav[0] - fov / 2:
if point[1] < locationuav[
1] + fov / 2 and point[
1] > locationuav[1] - fov / 2:
print('seen')
listpiw.remove(point)
# ################ PIW ################
if cleo and t == 0: ###########GENERATE POINTS, only once
ggp = 50 + downwind_leeway * 1 ###FIXED LEEWAY SPEED
opp = 1000 + crosswind_leeway * 1
exp1p = ((0.12) * 2) * t #**2
exp2p = ((0.094) * 2) * t
listpiw = genpoint(gg, op, exp1p, exp2p, el_ang, errors, t)
cleo = False
print('points generated')
for point in listpiw:
plt.plot(point[0], point[1], color='r', marker='o')
###################MOVE POINTS #########################
elif t > 0: #and tin < t:
dancing = 0
freeze = 0
comprised = 0
for point in range(len(listpiw)):
if point / 2.0 == 0:
insteadofcrosswind_leeway = 0.1
else:
insteadofcrosswind_leeway = -0.1
add = piw_movement(listpiw[point][0], listpiw[point][1],
downwind_leeway * (t - tin),
crosswind_leeway * (t - tin),
t) #HOLD INITIAL POSITION OF POINT
listpiw[point][0] += add[0] + errors[point][0] * (
t - tin
) ## set list of errors created before the while loop ##
listpiw[point][1] += add[1] + errors[point][1] * (t - tin)
freeze, dancing = insidecheck(
listpiw[point][0], listpiw[point][1], gg, op, exp1, exp2,
el_ang, freeze,
dancing) #Display PIWs: red = not within the search area
tin = t
prev_t = t
if goals > 1 or t < mob_signal_time:
plt.axis([0, 1000, 700, 1300])
else:
plt.axis([0, 1000, 700, 1100 + ship_vel * mob_signal_time])
ax.set_xlabel((t, str('s'), 'PIW spawned', 200, 'PIW remaining',
len(listpiw), 'UAV takeoff (s)', mob_signal_time,
'PIW speed (cm/s)', round(downwind_leeway, 2)),
fontsize=14)
plt.draw()
plt.pause(0.1)
plt.show()
time.sleep(1)
