davegimo

dronepath

Dec 28th, 2020 (edited)
619
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import folium
  2. import json
  3. from geopy.distance import geodesic
  4. import datetime
  5. import decimal
  6.  
  7.  
  8. #create map object
  9. #different tiles (map styles) can be used, like 'Stamen Toner', 'Stamen Terrain', ...
  10.  
  11. #SETUP MAPPA
  12. m = folium.Map(location=[43.81839701513138, 10.874407867954424], tiles='OpenStreetMap', zoom_start=10)
  13.  
  14. #CONFIGURAZIONE DRONE
  15. v = 30 #km/h
  16. v_ms = v/3.6 #m/s
  17.  
  18. battery_capacity = 30*60 #%s
  19. current_capacity = battery_capacity
  20. low_battery_percentage = 0.25
  21.  
  22. takeoff_time = 10
  23. charging_capacity = 3 #s/s
  24.  
  25. delta = 1
  26.  
  27. #SETUP GRAFO
  28. file = "CP.json"
  29. km = 20
  30.  
  31.  
  32. path = []
  33.  
  34.  
  35.  
  36. A = [10.868582820343955,43.943903280475745]
  37. B = [11.3556223267028783,43.776762971467626]
  38. pisa = [10.53406560359761, 43.827866455929644]
  39. firenze = [11.31860339831112,43.768645934352094]
  40. spezia = [10.059203674430062, 44.21233580919796]
  41. agliata = [11.040129230323007,43.904717805526865]
  42. viareggio = [10.411424635775173, 43.93842229074731]
  43.  
  44.  
  45. def inserisciMarker(mappa, lat,lng, popup, icon, color):
  46.     folium.Marker(location=[lat, lng], tooltip=popup, icon=folium.Icon(icon=icon, color=color)).add_to(mappa)
  47.  
  48.  
  49. def inserisciLinea(mappa, points, color, w, opacity, tooltip):
  50.     folium.PolyLine(points, tooltip= tooltip , color=color, weight=w, opacity=opacity).add_to(mappa)
  51.  
  52.  
  53. def inserisciCerchio(mappa, lat, lng, radius):
  54.     folium.CircleMarker([lat, lng],radius=radius*10/2, fill=True,opacity= 0.1).add_to(mappa)
  55.  
  56. def inserisciPunto(mappa, lat, lng, txt):
  57.     folium.CircleMarker([lat, lng],radius=5,fill=True, color="black", popup=txt).add_to(mappa)
  58.  
  59. def properTime(t):
  60.     return str(datetime.timedelta(seconds=t))[:7]
  61.  
  62. def computeDistance(p1,p2):
  63.     p1tuple = (p1[1], p1[0])
  64.     p2tuple = (p2[1], p2[0])
  65.     return geodesic(p1tuple, p2tuple)
  66.  
  67.  
  68. def lowBattery(battery_left):
  69.     return battery_left <= low_battery_percentage*battery_capacity
  70.  
  71.  
  72. def createGraph(file,dist):
  73.     with open(file) as json_file:
  74.         json_data = json.load(json_file)
  75.         grafo = {}
  76.  
  77.         print(len(json_data['features']))
  78.  
  79.         for p in json_data['features']:
  80.             coord = p["geometry"]["coordinates"]
  81.             s = p["properties"]["code"]
  82.             inserisciMarker(m, coord[1], coord[0], s, "", "blue")
  83.             #inserisciCerchio(m, coord[1], coord[0], km)
  84.             #print(s)
  85.             grafo[s] = {}
  86.             grafo[s]["coords"] = coord
  87.             grafo[s]["adjacent"] = []
  88.             #print(coord)
  89.             coordtuple = (coord[1],coord[0])
  90.             for pp in json_data['features']:
  91.                 if s != pp["properties"]["code"]:
  92.                     pptuple = (pp["geometry"]["coordinates"][1], pp["geometry"]["coordinates"][0])
  93.                     d = geodesic(coordtuple, pptuple).km
  94.                     if d < dist:
  95.                         l = []
  96.                         l.append(coordtuple)
  97.                         l.append(pptuple)
  98.                         #inserisciLinea(m,l,"blue",3,0.2, d)
  99.                         grafo[s]["adjacent"].append(pp["properties"]["code"])
  100.  
  101.         with open('data.json', 'w') as fp:
  102.             json.dump(grafo, fp, indent=4)
  103.  
  104.         return grafo
  105.  
  106.  
  107. def reachable(point, destination, battery_left):
  108.     dist = computeDistance(point, destination).m
  109.     t = dist / v_ms
  110.     if t < battery_left:
  111.         return True
  112.     return False
  113.  
  114. def goToCharge(point, destination, battery_left):
  115.     dist = computeDistance(point, destination).m
  116.     t = dist / v_ms
  117.     charging_time = (battery_capacity - battery_left - t) / charging_capacity
  118.     #uso charging time per il marker da mettere con il tempo impiegato
  119.     return [battery_capacity - t, charging_time]
  120.  
  121. def move(source, destination, x):  #returns new point, x km closer to destination
  122.     dist = computeDistance(source,destination).km
  123.     t = x/dist  #t is not time, it's an angle
  124.     #t = decimal.Decimal.from_float(x/dist)
  125.     #val = decimal.Decimal.from_float(1-t)
  126.     # (((1βˆ’π‘‘)π‘₯0+𝑑π‘₯1),((1βˆ’π‘‘)𝑦0+𝑑𝑦1))
  127.     return [(1-t)*source[0] + t*destination[0], (1-t)*source[1] + t*destination[1]]
  128.     #return [(val)*source[0] + t*destination[0], (val)*source[1] + t*destination[1]]
  129.  
  130. def fromLocGetClosestStation(location,file):
  131.     with open(file) as json_file:
  132.         json_data = json.load(json_file)
  133.         minimo = 10000000000
  134.         closest = None
  135.         finaltuple = None
  136.         locationtuple = (location[1], location[0])
  137.         for p in json_data['features']:
  138.             coord = p["geometry"]["coordinates"]
  139.             coordtuple = (coord[1], coord[0])
  140.             s = p["properties"]["code"]
  141.             dist = geodesic(coordtuple, locationtuple).km
  142.             if dist < minimo:
  143.                 closest = s
  144.                 minimo = dist
  145.                 finaltuple = coordtuple
  146.  
  147.         l = []
  148.         l.append(locationtuple)
  149.         l.append(finaltuple)
  150.         #inserisciLinea(m, l, "red", 4, 1, "")
  151.         return [finaltuple[1],finaltuple[0]]
  152.  
  153. def fromLocGetClosestStationOld(location,file):
  154.     with open(file) as json_file:
  155.         json_data = json.load(json_file)
  156.         minimo = 10000000000
  157.         closest = None
  158.         finaltuple = None
  159.         locationtuple = (location[1], location[0])
  160.         for p in json_data['features']:
  161.             coord = p["geometry"]["coordinates"]
  162.             coordtuple = (coord[1], coord[0])
  163.             s = p["properties"]["code"]
  164.             dist = geodesic(coordtuple, locationtuple).km
  165.             if dist < minimo:
  166.                 closest = s
  167.                 minimo = dist
  168.                 finaltuple = coordtuple
  169.  
  170.         l = []
  171.         l.append(locationtuple)
  172.         l.append(finaltuple)
  173.         #inserisciLinea(m, l, "red", 4, 1, "")
  174.         return closest
  175.  
  176. def getClosestReachableStation(location,file, time_left):
  177.     station = fromLocGetClosestStation(location,file)
  178.     print("stazione: " + str(station))
  179.     flag = reachable(location,station,time_left)
  180.     if flag:
  181.         return station
  182.     return None
  183.  
  184. def fromStationsGetClosestStation(sourceStation,destStation,grafo):
  185.     minimo = 10000000000
  186.     closest = None
  187.  
  188.     print(grafo[sourceStation]["adjacent"])
  189.     for node in grafo[sourceStation]["adjacent"]:
  190.         coord = grafo[node]["coords"]
  191.         coordtuple = (coord[1], coord[0])
  192.  
  193.         locationtuple = (grafo[destStation]["coords"][1], grafo[destStation]["coords"][0])
  194.         dist = geodesic(coordtuple, locationtuple).km
  195.         if dist < minimo:
  196.             minimo = dist
  197.             closest = node
  198.  
  199.     return closest
  200.  
  201.  
  202. def recursivePath(source,destination, punti,battery_left, delta):
  203.  
  204.     t = delta*1000 / v_ms
  205.     battery_left -= t
  206.     #print(battery_left)
  207.     dist = computeDistance(source,destination).km
  208.     print(dist)
  209.     if dist < delta and reachable(source, destination, battery_left) and len(punti) == 0:
  210.         print("raggiunto")
  211.         return True
  212.  
  213.     #print(lowBattery(battery_left))
  214.  
  215.     if lowBattery(battery_left):
  216.         s = getClosestReachableStation(source,file,battery_left)
  217.         #print("stazione: " + s)
  218.         if s == None:
  219.             return False
  220.         else:
  221.             gtc = goToCharge(source, s, battery_left)  # variabile di comodo!!!!!
  222.             battery_left = gtc[0]
  223.             path.append([[[source[1],source[0]],[s[1],s[0]]], 2, gtc[1]])
  224.  
  225.     if dist < delta and reachable(source, destination, battery_left) and len(punti) > 0:
  226.         print("ciaooooooo")
  227.         path.append([destination, 1]) #deve diventare 4
  228.         new_source = destination
  229.         new_destination = punti.pop(0)
  230.         flag = recursivePath(new_source, new_destination, punti, battery_left, delta)
  231.         if flag:
  232.             return True
  233.         else:
  234.             path.pop()
  235.             punti.insert(0,new_destination)
  236.             s = getClosestReachableStation(source, file, battery_left)
  237.             if s == None:
  238.                 return False
  239.             else:
  240.                 gtc = goToCharge(source, s, battery_left) #variabile di comodo!!!!!
  241.                 battery_left = gtc[0]
  242.                 path.append([[[source[1],source[0]],[s[1],s[0]]], 3, gtc[1]])
  243.                 path.append([destination, 1]) #diventerΓ  4!!!!!!!!!!!
  244.                 new_source = destination
  245.                 new_destination = punti.pop(0)
  246.                 flag = recursivePath(new_source,new_destination,punti,battery_left,delta)
  247.                 if flag:
  248.                     return True
  249.                 else:
  250.                     path.pop()
  251.                     path.pop()
  252.                     punti.insert(0, new_destination)
  253.                     return False
  254.  
  255.     else:
  256.         new_point = move(source, destination, delta)
  257.  
  258.         path.append([new_point, 1])
  259.         flag = recursivePath(new_point,destination,punti,battery_left,delta)
  260.         if flag:
  261.             return True
  262.         else:
  263.             print("falsooooooo")
  264.             path.pop()
  265.             s = getClosestReachableStation(source, file, battery_left)
  266.             if s == None:
  267.                 return False
  268.             else:
  269.                 gtc = goToCharge(source, s, battery_left) #variabile di comodo!!!!!
  270.                 battery_left = gtc[0]
  271.                 path.append([[[source[1],source[0]],[s[1],s[0]]], 3, gtc[1]])
  272.                 path.append([new_point, 1])
  273.                 flag = recursivePath(new_point,destination,punti,battery_left,delta)
  274.                 if flag:
  275.                     return True
  276.                 else:
  277.                     path.pop() #doppio backtracking!!!!!!
  278.                     path.pop()
  279.                     return False
  280.  
  281.  
  282. def computePath(punti, battery_left, delta):
  283.     x = len(punti)
  284.     if x < 2:
  285.         print("ERRORE")
  286.         return False
  287.     source = punti.pop(0)
  288.     first_destination = punti.pop(0)
  289.     print(x)
  290.     if x == 2:
  291.         return recursivePath(source,first_destination,[],battery_left, delta)
  292.  
  293.     return recursivePath(source,first_destination,punti,battery_left, delta)
  294.  
  295.  
  296.  
  297. def disegnaLinee(lista):
  298.     for i in range(len(lista)-1):
  299.         l = []
  300.         l.append((lista[i][1], lista[i][0]))
  301.         l.append((lista[i+1][1], lista[i+1][0]))
  302.         inserisciLinea(m, l, "blue", 6, 0.8, "")
  303.  
  304.  
  305. def disegnaPercorso(path):
  306.     total_time = 0
  307.     total_dist = 0
  308.     t = delta * 1000 / v_ms
  309.  
  310.     for el in path:
  311.         print(el)
  312.         if el[1] == 1:
  313.             total_time += t
  314.             total_dist += delta
  315.             inserisciPunto(m,el[0][1],el[0][0],str(datetime.timedelta(seconds=total_time))[:7])
  316.         else:
  317.             distance_to_reach = computeDistance(el[0][0],el[0][1])
  318.             time_to_reach = distance_to_reach.m / v_ms
  319.             time_to_charge = el[2]
  320.             total_dist += (distance_to_reach.km*2)
  321.             total_time += (time_to_reach*2) + el[2]
  322.             inserisciMarker(m,el[0][1][0],el[0][1][1],str(datetime.timedelta(seconds=time_to_charge))[:7],"","purple")
  323.  
  324.             if el[1] == 2:
  325.                 inserisciLinea(m, el[0],"red",4,1,"andata e ritorno: " + str(datetime.timedelta(seconds=time_to_reach*2))[:7] + ", " + str(distance_to_reach.km*2)[:5] + "km")
  326.             if el[1] == 3:
  327.                 inserisciLinea(m, el[0],"green",4,1,"andata e ritorno: " + str(datetime.timedelta(seconds=time_to_reach*2))[:7] + ", " + str(distance_to_reach.km*2)[:5] + "km")
  328.  
  329.     return [total_time,total_dist]
  330.  
  331. source = pisa
  332. destination = firenze
  333. punti = [A,B,firenze,pisa, viareggio]
  334. disegnaLinee(punti)
  335. dist = computeDistance(source,destination).km
  336. grafo = createGraph(file,km)
  337.  
  338.  
  339. l = []
  340. l.append((source[1],source[0]))
  341. l.append((destination[1],destination[0]))
  342. #inserisciLinea(m, l, "blue", 6, 0.8, "")
  343.  
  344.  
  345.  
  346.  
  347. print(dist)
  348. np = source
  349.  
  350. # while dist > 10:
  351. #     np = move(np,destination,10)
  352. #     dist -= 10
  353. #     inserisciMarker(m, np[1], np[0], "source", "", "purple")
  354.  
  355.  
  356. #print(recursivePath(source,destination,[],battery_capacity,delta))
  357. print(computePath(punti,battery_capacity,delta))
  358. print(path)
  359. res = disegnaPercorso(path)
  360.  
  361. inserisciMarker(m, source[1], source[0], "source: " + str(dist)[:5] + "km", "", "red")
  362. inserisciMarker(m, destination[1], destination[0], "destination reached in " + properTime(res[0]) + " doing " + str(res[1])[:5] + "km in total", "", "green")
  363.  
  364.  
  365. m.save('inspection.html')
  366.  
RAW Paste Data