diff --git a/routeplanning/convexDecomp.py b/routeplanning/convexDecomp.py
index 5d678ff0c96a02cbe6342b4c0aeea407a526351a..17c467038e3ad3b86ed5afedcb9966c058b54bb2 100644
--- a/routeplanning/convexDecomp.py
+++ b/routeplanning/convexDecomp.py
@@ -8,7 +8,7 @@ class ConvexDecomp:
#Loops through all polygons and attempts to connect ones that have an edge in common
- def combine(self,polygons):
+ def __combine(self,polygons):
new_polys = []
old_polys = []
@@ -34,7 +34,7 @@ class ConvexDecomp:
#If the polygons share two and only two points then union. If they share less than two then they are not adjacent
#and if they share more than two then they must have a reflex angle between them so we don't want them to be unioned
if count == 2:
- if ((merged := self.union(list_vers1,list_vers2,vs)) != None):
+ if ((merged := self.__union(list_vers1,list_vers2,vs)) != None):
#Adds the union shape to a list and the old shapes so they are not merged more than once
new_polys.append(merged)
old_polys.append(p1)
@@ -49,7 +49,7 @@ class ConvexDecomp:
#Performs the unioning of two polygons
- def union(self,vs1,vs2,shared_edge):
+ def __union(self,vs1,vs2,shared_edge):
#Creates a list of unique vertecies of the union shape
union_vertices = [*set(vs1 + vs2)]
@@ -88,8 +88,8 @@ class ConvexDecomp:
ps = Polygon.convex_decompose(self.P)
#Check to see if any resulting polygons can be combined as this sometimes
#happens when using this library.
- ps = self.combine(ps)
+ ps = self.__combine(ps)
for p in ps:
result.append(Polygon.as_tuple_list(p))
- return result
\ No newline at end of file
+ return result
diff --git a/routeplanning/coordinateProjection.py b/routeplanning/coordinateProjection.py
index f1f55dce21baed573d65388a1d3a74f7df40253e..ce45a906439696a4317aefa62237b289b9da240c 100644
--- a/routeplanning/coordinateProjection.py
+++ b/routeplanning/coordinateProjection.py
@@ -116,4 +116,4 @@ class GeoConv:
def getCandAlt(self):
- return self.cAndAlt
\ No newline at end of file
+ return self.cAndAlt
diff --git a/routeplanning/energyCost.py b/routeplanning/energyCost.py
index 022bdcb82b56a5cc18598360772011dfc909923d..046c6955bc35aaca6fee1fbf91bee01ff808f6ae 100644
--- a/routeplanning/energyCost.py
+++ b/routeplanning/energyCost.py
@@ -5,7 +5,7 @@ from sympy import sin,atan,Add
class EnergyCost:
- def __init__(self,dists,els,C,W,rho,m):
+ def __init__(self,C,W,rho,m):
#C = spray uniformity, lm^-1
#W = Width of the drone
@@ -13,20 +13,59 @@ class EnergyCost:
self.C = C
self.W = W
self.rho = rho
+ self.m = m
+ #s = displacement
+ #vi = initial velocity
+ #t = time
+ def accel1(self,s,vi,t):
+ return 2*(s-vi*t)/t**2
+
+ #s = displacement
+ #vi = initial velocity
+ #t = time
+ def accel2(self,vf,vi,t):
+ return (vf-vi)/t
+
+ #Implementation of the energy function 14 found in the documentation.
+ def const_energy(self,ah,av,m,t):
+ if not (ah == 0):
+ fh = ah*m
+ fv = av*m
+ p1 = fh/(sin(atan(fh/(m*9.81+fv))))
+ p2 = (5000*p1/(9*9.81))**1.341549002
+ p3 = p2 * ((21*t)/2500)
+ return p3
+ else:
+ return ((5000*m*9.81)/(9*9.81))**1.341549002*(21*t)/2500
+
+
+ #The algorithm creates an outline of a movement and then calculates the energy for this movement.
+ #The energy is added to a running total. If the total is larger than a minimum supplied by the user
+ #then the calculations stop early to save time.
+ def energyHelp(self,dists,els,min=100000000000000):
+
+ total = 0
+
#To calculate the energy needed to fly, the acceleration, deceleration, mass and time spent
#in these states by the drone are precalculated
- haccels = []
- vaccels = []
- times = []
- masses = []
- mids = []
count = 0
+ prev_m = 0
for i in range(len(dists)):
+ #If the route can not be the cheapest then stop early
+ if total > min:
+ break
+
+ haccels = []
+ vaccels = []
+ times = []
+ masses = []
+ mids = []
+
#Every second movement between waypoints the drone is not spraying
- if i % 2 == 0:
+ if i % 2 == 0 and self.C != 0:
spray = True
else:
spray = False
@@ -37,9 +76,11 @@ class EnergyCost:
#Sets the mass for a maneuver
if count == 0:
- m1 = m
- else:
- m1 = masses[count -1]
+ m1 = self.m
+ elif prev_m <= 41:
+ m1 = self.m
+ else:
+ m1 = prev_m
#The vertical acceleration is calculated by multiplying the horizontal acceleration
#so that they both finish at the same time.
@@ -50,7 +91,7 @@ class EnergyCost:
#situations
tmin = abs(2*ratio)/(m1*9.81)
- #0.21 is a preset optimum value so if a maneuver requires more time than 0.21s then tmin is
+ #0.21 is a preset optimum value so if a maneuver requires less time than 0.21s then tmin is
#used instead.
if tmin < 0.21:
t = 0.21
@@ -102,15 +143,17 @@ class EnergyCost:
mids.append(False)
if spray:
- m2 = m1 - abs(C*rho*W*ah*t**2)
- m3 = m2 - abs(C*rho*W*2*((d-2*s)/2))
+ m2 = m1 - abs(self.C*self.rho*self.W*ah*t**2)
+ m3 = m2 - abs(self.C*self.rho*self.W*2*((d-2*s)/2))
masses.append(m1)
masses.append(m2)
masses.append(m3)
+ prev_m = m3
else:
masses.append(m1)
masses.append(m1)
masses.append(m1)
+ prev_m = m1
count += 3
@@ -129,60 +172,29 @@ class EnergyCost:
mids.append(False)
if spray:
- m2 = m1 - abs(C*rho*W*ah*t**2)
+ m2 = m1 - abs(self.C*self.rho*self.W*ah*t**2)
masses.append(m1)
masses.append(m2)
+ prev_m = m2
else:
masses.append(m1)
masses.append(m1)
+ prev_m = m1
count += 2
- self.dists = dists
- self.els = els
- self.haccels = haccels
- self.vaccels = vaccels
- self.times = times
- self.masses = masses
- self.mids = mids
-
-
- #s = displacement
- #vi = initial velocity
- #t = time
- def accel1(self,s,vi,t):
- return 2*(s-vi*t)/t**2
-
- #s = displacement
- #vi = initial velocity
- #t = time
- def accel2(self,vf,vi,t):
- return (vf-vi)/t
-
- #Implementation of the energy function 14 found in the documentation.
- def const_energy(self,ah,av,m,t):
- if not (ah == 0):
- fh = ah*m
- fv = av*m
- p1 = fh/(sin(atan(fh/(m*9.81+fv))))
- p2 = (5000*p1/(9*9.81))**1.341549002
- p3 = p2 * ((21*t)/2500)
- return p3
- else:
- return ((5000*m*9.81)/(9*9.81))**1.341549002*(21*t)/2500
-
-
- #Calculates the energy required
- def energyHelp(self):
- total = 0
- for i in range(len(self.haccels)):
- if self.mids[i]:
- total += self.const_energy(self.haccels[i],self.vaccels[i],(self.masses[i] + self.masses[i-1]) / 2,self.times[i])
- else:
- total += self.const_energy(self.haccels[i],self.vaccels[i],self.masses[i],self.times[i])
-
- #If an impossible situation does occur then we dismiss the route
- if type(total) == Add:
- total = 100000000
+ #Calculates the energy required to fly the specific motion between two waypoints
+ for i in range(len(haccels)):
+ if mids[i]:
+ #Adds the energy consumption to the total for the entire route
+ total += self.const_energy(haccels[i],vaccels[i],(masses[i] + masses[i-1]) / 2,times[i])
+ else:
+ total += self.const_energy(haccels[i],vaccels[i],masses[i],times[i])
- return total
\ No newline at end of file
+ #If an impossible situation does occur (denoted by an imaginary number)
+ #then we dismiss the route and stop early
+ if type(total) == Add:
+ total = 100000000000
+ break
+
+ return total
diff --git a/routeplanning/mathUtility.py b/routeplanning/mathUtility.py
index 7b39b52885b525e91487a3ab59d0f4031fe0e948..975c1a47878df9010c9bff114a6d597719bcd520 100644
--- a/routeplanning/mathUtility.py
+++ b/routeplanning/mathUtility.py
@@ -207,4 +207,4 @@ def find_antipodal_pairs(points, point_names):
pairs.append((point_names[i], point_names[jj], width(points[i], points[jj])))
j = jj
- return min(pairs, key = lambda t: t[2])
\ No newline at end of file
+ return min(pairs, key = lambda t: t[2])
diff --git a/routeplanning/routePlan.py b/routeplanning/routePlan.py
index f970bff87ac157622442e64cd76760fbb5ef9ff5..7ef54e00f60180d4f7f3b3530a71e5ad2490b0cc 100644
--- a/routeplanning/routePlan.py
+++ b/routeplanning/routePlan.py
@@ -11,14 +11,15 @@ import numpy as np
#it can give the highest and lowest points to the waygen class
class RoutePlan:
- def __init__(self,coords,dia,C,W,key):
-
- #User inputs for spray uniformity and drone width. Room for alternative spray liquid
- #density and drone initial mass to be added
- self.C = C
- self.W = W
- self.rho = 1
- self.mass = 61
+ def __init__(self,coords,dia,key,rho=1,mass=61,C=0.0):
+
+ #User inputs for spray uniformity. Room for alternative spray liquid
+ #density, drone width and drone initial mass to be added
+ self.W = dia
+ self.coords = coords
+
+ #Creates a model of the drone to be used for energy calculations
+ self.e = cost.EnergyCost(C,dia,rho,mass)
#If a field has less than 2 meters of elevation then the minimum turns algorithm will be used
self.maxHeightDiff = 2
@@ -26,25 +27,6 @@ class RoutePlan:
#Creates a GoogleMapsAPI client
self.client = googlemaps.Client(key)
- #Creates a GeoConv object to translate coordinates
- self.p = proj.GeoConv(coords,self.client)
-
- #Gets local and global coordinates with elevation
- self.localAndEl = self.p.locals # ((x,y),elevation)
- self.coordsAndEl = self.p.cAndAlt # ((x,y),elevation)
-
- #Takes just the coordinates from the above tuples
- self.locals = np.array([x for (x,y) in self.localAndEl]) # gets just coords
- self.coords = np.array([x for (x,y) in self.coordsAndEl]) # gets just coords
-
- #Length of max elevation line in long and lat
- fieldLen = ut.get_distance(coords[0],coords[1])
- fieldVec = ut.getVector(self.locals[0],self.locals[1])
- fieldVecMag = ut.magOfVec(fieldVec)
-
- #The radius in m converted to local coordinate system magnitude
- self.dia_conv = (fieldVecMag/fieldLen) * dia
-
#Returns the maximum height difference over the field
def __heightDiff(self,els):
@@ -55,41 +37,52 @@ class RoutePlan:
if not p == n:
diff = p-n
if diff > heightDiff:
- heightDiff = 0
-
+ heightDiff = diff
+
return heightDiff
- def __waysAndEnergy(self,pro,r):
+ def __waysAndEnergy(self,pro,r,least):
#Converts the points back to global
pro.clearWays()
pro.addWayPoint(r)
ways = pro.wayToGlobal()
+
#Uses the waypoints to get a better estimation of the change in distance and elevation of the route
dists = np.array([])
els = np.array([])
+
+ alts = []
if ways != None:
#Finds the elevations for the new waypoints
alts = pro.getAlt(ways)
+
for i in range(1,len(ways)):
#Adds the changes in distance/elevation for each leg of the route so an energy estimation can be calculated
- dists = np.append(dists,ut.get_distance(ways[i-1],ways[i]))
- els = np.append(els,alts[i] - alts[i-1])
+ d = ut.get_distance(ways[i-1],ways[i])
+ e = alts[i] - alts[i-1]
+ dists = np.append(dists,d)
+ els = np.append(els,e)
+
+ ways = list(zip(ways,alts))
#Calculates the energy cost for the motions
- e = cost.EnergyCost(dists,els,self.C,self.W,self.rho,self.mass)
- energy = e.energyHelp()
+ energy = self.e.energyHelp(dists,els,least)
return (ways,energy)
- def getCheapest(self,routes,pro):
+ def __getCheapest(self,routes,pro):
+
#Compares the minimum energy cost of the two routes and returns the cheapest
least = 100000000000
+
way = None
+
for r in routes:
- w = self.__waysAndEnergy(pro,r)
+ w = self.__waysAndEnergy(pro,r,least)
+
if (w[1] < least):
least = w[1]
way = w[0]
@@ -104,10 +97,6 @@ class RoutePlan:
decs = dec.decompose()
decs = np.array(decs)
- #Plots the decomposed shapes
- #for w in range(len(decs)):
- # plt.plot(np.append(np.array(decs[w])[:,0],decs[w][0][0]),np.append(np.array(decs[w])[:,1],decs[w][0][1]),alpha = 0.5)
-
wayList = []
#Loops through the decomposed shapes and...
@@ -115,6 +104,7 @@ class RoutePlan:
#Creates a new class to translate the coordinates
pro = proj.GeoConv(decs[d],self.client)
+
ls = np.array([x for (x,y) in pro.getLocals()]) # gets just local coords
#Finds the maximum height difference of a polygon to decide whether the faster
@@ -125,11 +115,11 @@ class RoutePlan:
if self.__heightDiff(diffs) > self.maxHeightDiff:
#Finds the routes to possibly fly
- g = gen.WayGen(ls,self.dia_conv)
+ g = gen.WayGen(ls,self.W)
routes = g.getWays()
#Stores the most efficient route for the polygon
- wayList.append(self.getCheapest(routes,pro))
+ wayList.append(self.__getCheapest(routes,pro))
else:
@@ -138,9 +128,20 @@ class RoutePlan:
indices = [(0 + i) for i in range(len(ls))]
antis = ut.find_antipodal_pairs(ls,indices)
- g = gen.WayGen(ls,self.dia_conv)
+ g = gen.WayGen(ls,self.W)
routes = g.getWaysFlat([ls[antis[0]],ls[antis[1]]])
- wayList.append(self.getCheapest(routes,pro))
+ wayList.append(self.__getCheapest(routes,pro))
+
+ #Temporarily uses polygons in order of appearance rather than efficiency
+ flat = [x for sub in wayList for x in sub]
+
+ output = []
+
+ for x in flat:
+ dic = {"lat":x[0][0],
+ "long":x[0][1],
+ "altitude":x[1]}
+ output.append(dic)
- return wayList
+ return output
diff --git a/routeplanning/run.py b/routeplanning/run.py
deleted file mode 100644
index 3d914a0b1c8f236fa47fecae79352a38c22e701f..0000000000000000000000000000000000000000
--- a/routeplanning/run.py
+++ /dev/null
@@ -1,40 +0,0 @@
-import routePlan as route
-from matplotlib import pyplot as plt
-import numpy as np
-
-cs = np.array([[4.895262, -75.062352],[4.894979, -75.0625],[4.894869, -75.062396],[4.895042, -75.062399]])
-#cs = np.array([[4.895024, -75.062171],[4.894938, -75.062084],[4.894902, -75.061988],[4.894367, -75.061860],[4.894464, -75.062215],[4.894774, -75.062396],[4.895037, -75.062439]])
-#cs = np.array([[4.895939, -75.063709],[4.895047, -75.062834],[4.894930, -75.062803],[4.894681, -75.062873],[4.894420, -75.063014],[4.894576, -75.063873],[4.894650, -75.064115],[4.894922, -75.064298],[4.895126, -75.064088],[4.895379, -75.063958],[4.895682, -75.063828]])
-#cs = np.array([[4.895024, -75.062171],[4.894938, -75.062084],[4.894367, -75.061860],[4.894464, -75.062215],[4.894774, -75.062396],[4.895037, -75.062439]])
-#cs = np.array([[4.895962021003516,-75.06371959616334],[4.895830074899626,-75.06376251025222],[4.895713625421434,-75.06380536831608],[4.895621409758562,-75.06385637286699],[4.895507188130711,-75.06390835537293],[4.895369612417832,-75.06398587615841],[4.895253968447753,-75.06406043548989],[4.895143450318733,-75.06410879618066],[4.895045309803765,-75.06419303501585],[4.894956106786798,-75.06429858946146],[4.894769321177979,-75.06418478543124],[4.894626594003233,-75.0640608955089],[4.894564941692598,-75.06392167488916],[4.89464895211454,-75.06377105119107],[4.89478266472092,-75.0636903289406],[4.894903713939073,-75.06359729527684],[4.895039558292459,-75.06353398712551],[4.895125903665662,-75.06349831223994],[4.895236147704252,-75.06346655329092],[4.895372761334865,-75.06343322700772],[4.895496508441547,-75.06341712079953],[4.895665183262881,-75.06339523713802],[4.895802969372566,-75.0635077763]])
-
-key = 'AIzaSyDzvNlPMcan4qvybkqQkWyMMJAIE4q37Vg'
-
-r = route.RoutePlan(cs,2,0.05,2,key)
-plt.axes().set_aspect('equal')
-ways = r.main()
-print(ways)
-for w in range(0,len(ways)):
-
- plt.plot(np.array(ways[w])[:,0],np.array(ways[w])[:,1],alpha = 0.5)
- #print('Way: ' + str(w))
- #print(ways[w][0])
- #print()
- #print(ways[w+1])
- #print(ways[w][1])
- #print()
- #print()
-
-
-#cs = r.locals
-plt.plot(np.append(cs[:,0],cs[0,0]),np.append(cs[:,1],cs[0,1]))
-
-
-
-#for i in range(len(ways)):
-# r1 = ways[i][0]
- #Plots the route
-# plt.plot(np.array(r1)[:,0],np.array(r1)[:,1])
-# plt.plot(np.append(cs[:,0],cs[0,0]),np.append(cs[:,1],cs[0,1]))
-#plt.savefig('C:/GDP/gdp/concave.png',transparent = True)
-plt.show()
\ No newline at end of file
diff --git a/routeplanning/waypointGen.py b/routeplanning/waypointGen.py
index e231c81d5788fc5c15a8c2d5dde7ca90d90fca99..aed2cff059ff43d07d9b5fda92d4172bae103472 100644
--- a/routeplanning/waypointGen.py
+++ b/routeplanning/waypointGen.py
@@ -53,27 +53,6 @@ class WayGen:
route2.append(ways[0])
else:
end = True
-
- '''if(end == True):
- p1 = route1[-1]
- p2 = route1[-2]
-
- m = ut.getMidpoint([p1,p2])
-
- #If there are no possible waypoints then check if the end of the field has been sprayed
- #The left hand side calculates the distance from the out of bounds contour to the lowest point of the polygon
- #If this distance is less than half a spray width then some of the field has been missed
- if abs(contour[0]*low[0] + low[1] + contour[1]/ut.width(contour[0],contour[1]) < ut.magOfVec(toMove)/2:
-
- #Moves the contour to include the end of the field(and some that has already been sprayed)
- #The 1.5 is so that when 1 is added back on below, it equals a move of -0.5 spray widths
- startCont = startCont - 1.5*toMove
-
- #End the loop next time around
- next = True
- else:
- #Just end if all the field was sprayed
- end = True'''
#Update the contour line values
startCont = startCont + toMove
@@ -86,6 +65,7 @@ class WayGen:
contL = -1/contM
lC = mid[1] - mid[0] * contL
elLine = np.array([contL,lC])
+
#Unit vector down the slope
unit = ut.getUnitVec(np.array((self.points[0][0],elLine[0]*self.points[0][0] + elLine[1])),np.array((self.points[1][0],elLine[0]*self.points[1][0] + elLine[1])))
@@ -108,17 +88,17 @@ class WayGen:
edges = ut.getEdges(self.points)
#Used to calculate gradients
- i = 1
+ m = 1
#20 loops allows gradients from 1 to -1 in the x and y axis i.e. a full rotation
- for i in range(20):
+ for i in range(21):
- #When i == 0 there is a divide by zero error
- if not i == 0:
+ #When m == 0 there is a divide by zero error
+ if not m == 0:
#Sets the gradients of two contours to better explore all routes
- contM1 = i
- contM2 = 1/i
+ contM1 = m
+ contM2 = -1/m
toMove1,contour1 = self.__prep(contM1,mid)
toMove2,contour2 = self.__prep(contM2,mid)
@@ -132,18 +112,18 @@ class WayGen:
#Combines the half routes in the correct way to make them as expected
r1 = np.append(np.flip(r1,axis = 0),r4[2:],axis = 0)
- r2 = np.append(np.flip(r2,axis = 0),r3[2:],axis = 0)
+ r3 = np.append(np.flip(r5,axis = 0),r8[2:],axis = 0)
- r5 = np.append(np.flip(r5,axis = 0),r8[2:],axis = 0)
- r6 = np.append(np.flip(r6,axis = 0),r7[2:],axis = 0)
+ r5 = np.flip(r1,axis = 0)
+ r7 = np.flip(r3,axis = 0)
all.append(r1)
- all.append(r2)
+ all.append(r3)
all.append(r5)
- all.append(r6)
+ all.append(r7)
#Indirectly alters the gradient
- i -= 0.1
+ m -= 0.1
return all