def find_route(start_lat, start_long, end_lat, end_long, transport_type='car'):
'''
This function converts the lat/long distance into
a meters distance.
Parameters
----------
start_lat: float
The start latitude
start_long: float
The end latitude
end_lat: float
The start longitude
end_long: float
The end longitude
transport_type: string
The type of transport (default is car)
Returns
-------
route_latlongs: arraylike
array of Points representing the vertices of the route.
'''
router = Router(transport_type)
start = router.findNode(start_lat, start_long)
end = router.findNode(end_lat, end_long)
try:
status, route = router.doRoute(start, end)
except:
return None
if status == 'success':
route_latlongs = list(map(router.nodeLatLon, route)) # Get actual route coordinates
return route_latlongs
def find_route(names: list,
pos: (float, float),
file_name: str = 'names.csv') -> dict:
'''
creates a file with distance matrix among chosen places
:param names: names of the places you want to visit
:param pos: your position at the moment (starting point) ((yet has no use))
:param file_name: name of a file with name,lat,lon lines
:return: return a dictionary {1: 'first place', 2: 'second place', ...} (for later in code)
'''
places_number_name = dict()
places_name_coor = dict()
name_in = 'SampleIn_1.txt'
name_out = 'SampleOut.txt'
for i in range(len(names)):
places_number_name[i + 1] = names[i]
file = open(file_name, 'r')
for line in file.readlines():
if line.split(sep=',')[0] in names:
places_name_coor[line.split(sep=',')[0]] = (float(
line.split(sep=',')[1]), float(line.split(sep=',')[2]))
file.close()
file_m = open(name_in, 'w', encoding='UTF-8')
file_m.write(str(len(names)) + '\n')
router = Router('car')
for name_start in places_name_coor.keys():
start = router.findNode(places_name_coor[name_start][0],
places_name_coor[name_start][1])
for name_end in places_name_coor.keys():
end = router.findNode(places_name_coor[name_end][0],
places_name_coor[name_end][1])
if name_start == name_end:
file_m.write('-1 ')
continue
status, route = router.doRoute(start, end)
print(status)
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route))
sum_ = 0
for i in range(len(routeLatLons) - 1):
sum_ += router.distance(routeLatLons[i],
routeLatLons[i + 1])
file_m.write(' ' + str(sum_)[:5] + ' ')
file_m.write('\n')
file_m.close()
return places_number_name
def Itinéraire(loc1,loc2,iti):
router = Router("car")
print(router)
depart=router.findNode(loc1[0],loc1[1])
arrivee=router.findNode(loc2[0],loc2[1])
status, route = router.doRoute(depart, arrivee)
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route))
f.PolyLine(routeLatLons, color="blue", weight=7.5, opacity=8).add_to(iti)
def Itinéraire_Supérette_la_plus_proche(loc1,loc2,iti_sup):
router = Router("car")
print(router)
depart=router.findNode(loc1[0],loc1[1])
arrivee=router.findNode(loc2[0],loc2[1])
status, route = router.doRoute(depart, arrivee)
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route))
f.PolyLine(routeLatLons, color="orange", weight=7.5, opacity=8).add_to(iti_sup)
return iti_sup
def locateUserRests(request,restname):
customer = Customer.objects.get(email=request.session['id'])
print(customer.email)
restaurant =Restaurant.objects.get(name=restname)
print(restaurant.name)
add1=Locuser.objects.get(custid=request.session['id'])
add_user=eval(add1.address_user)
add2=Locrest.objects.get(restid=restaurant.email)
add_rest=eval(add2.address_rest)
distance=geodesic(add_user,add_rest).kilometers
print(distance)
router = Router("car") # Initialise it
start = router.findNode(add_user[0], add_user[1]) # Find start and end nodes
end = router.findNode(add_rest[0], add_rest[1])
status, route = router.doRoute(start, end) # Find the route - a list of OSM nodes
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route)) # Get actual route coordinates
latitude_list = []
longitude_list = []
for i in range(len(routeLatLons)):
latitude_list.append(routeLatLons[i][0])
longitude_list.append(routeLatLons[i][1])
gmap3 = gmplot.GoogleMapPlotter(23.0, 72.53, 13)
# scatter method of map object
# scatter points on the google map
#gmap3.scatter( latitude_list, longitude_list, '# FF0000', size = 40, marker = False )
# Plot method Draw a line in
# between given coordinates
gmap3.plot(latitude_list, longitude_list,
'blue', edge_width = 5)
gmap3.draw( "map13.html" )
webbrowser.open_new_tab('http://127.0.0.1:8000/map13.html')
return render(request,"foodspark/restview.html")
def find_route(start_lat,start_lon,end_lat,end_lon):
router = Router("car") # Initialise it
start = router.findNode(start_lat, start_lon) # Find start and end nodes
end = router.findNode(end_lat, end_lon)
status, route = router.doRoute(start, end) # Find the route - a list of OSM nodes
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route)) # Get actual route coordinates
# print(routeLatLons)
Y = np.array([i[0] for i in routeLatLons])
X = np.array([i[1] for i in routeLatLons])
start_Y = Y[0]
start_X = X[0]
Y -= Y[0]
Y *= 111392.84
Y = np.dstack((Y[:-1],Y[:-1] + np.diff(Y)/2.0)).ravel()
Y = np.dstack((Y[:-1],Y[:-1] + np.diff(Y)/2.0)).ravel()
Y = gaussian_filter(Y,sigma=2)
X -= X[0]
X *= 111392.84
X = np.dstack((X[:-1],X[:-1] + np.diff(X)/2.0)).ravel()
X = np.dstack((X[:-1],X[:-1] + np.diff(X)/2.0)).ravel()
X = gaussian_filter(X,sigma=2)
set_track_width(10)
slope = generate_slopes(X,Y)
bx,by = get_bezier_track(X,Y,slope)
# doing the following because working directly with GPS coordinates is not a great idea tbh. Also, these equations will only work near the equator (great for us as we live in india :P)
bx /= 111392.84
by /= 111392.84
bx += start_X
by += start_Y
print(len(bx))
plt.scatter(bx,by)
# plt.scatter(X,Y)
plt.axis('equal')
plt.show()
def route(mode, osm_file, in_start_end, out_json):
try:
print('s', time.strftime('%H:%M:%S', time.localtime(time.time())))
router = Router(mode, osm_file) # Initialise it
with open(out_json, 'w') as w:
f = open(in_start_end, 'r')
features = loads(f.read())
x1, y1, x2, y2 = 0, 0, 0, 0
for feat in features['features']:
# print(feat['geometry']['coordinates'])
if feat['properties']['s_e'] == 's':
x1, y1 = feat['geometry']['coordinates']
if feat['properties']['s_e'] == 'e':
x2, y2 = feat['geometry']['coordinates']
start = router.findNode(y1, x1)
end = router.findNode(y2, x2)
status, route = router.doRoute(
start, end) # Find the route - a list of OSM nodes
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route))
my_feature = Feature(
geometry=LineString(tuple([i[::-1]
for i in routeLatLons])))
feature_collection = FeatureCollection([my_feature])
# print(feature_collection)
w.write('{}'.format(dumps(feature_collection, indent=4)))
print('e', time.strftime('%H:%M:%S', time.localtime(time.time())))
print('-' * 40)
except Exception:
pass
finally:
del router
def _task_runner(task):
(src, dt, (node_time, routing_source, routing_destination)) = task
logger.debug(
f"Received message at {dt} from {src} <node_time={node_time}, "
f"routing_source={routing_source}, "
f"routing_destination={routing_destination}>")
start_timer = time.perf_counter()
router = Router("car")
start = router.findNode(routing_source[0], routing_source[1])
end = router.findNode(routing_destination[0], routing_destination[1])
status, route = router.doRoute(start, end)
if status == "success":
route_coords = [router.nodeLatLon(x) for x in route]
encoded_route = (0, _format_route(route_coords))
elif status == "no_route":
encoded_route = (1, None)
elif status == "gave_up":
encoded_route = (2, None)
else:
logger.error(f"Unknown result '{status}'")
encoded_route = (3, None)
end_timer = time.perf_counter()
duration = end_timer - start_timer
encoded_route_len = 0 if encoded_route[1] is None else len(
encoded_route[1])
logger.debug(
f"Job {task} took {duration} seconds with status {status} route length = {encoded_route_len}"
)
return (src, encoded_route, duration)
def setWayWidth(lat, longit, mode, fileRead, width):
router = Router(mode)
node = router.findNode(lat, longit)
#print(node)
tree = ET.parse(fileRead)
root = tree.getroot()
findedNodeWay = ".//*[@ref='" + str(node) + "'].."
data = root.findall(findedNodeWay)
xmlString = "<tag k=\"width\" v=\"" + str(width) + "\"></tag>"
xml = ET.fromstring(xmlString)
data[0].append(xml)
tree.write(fileRead)
print("Done")
from pyroutelib3 import Router # Import the router
router = Router("car") # Initialise it
start = router.findNode(lat=-15.80423,
lon=-47.9526007) # Find start and end nodes
end = router.findNode(lat=-15.8055607, lon=-47.9515105)
status, route = router.doRoute(start,
end) # Find the route - a list of OSM nodes
print(status)
print(route)
if status == 'success':
routeLatLons = list(map(router.nodeLatLon,
route)) # Get actual route coordinates
from pyroutelib3 import Router # Import the router
router = Router("car") # Initialise it
print("Finding")
start = router.findNode(51.53495526869262,
-0.20454431828209635) # Find start and end nodes
end = router.findNode(51.5305246465236, -0.18548599498011692)
status, route = router.doRoute(start,
end) # Find the route - a list of OSM nodes
if status == 'success':
routeLatLons = list(map(router.nodeLatLon,
route)) # Get actual route coordinates
print(routeLatLons)
def find_route(names: list, pos: (float, float), transport_type: str):
"""
creates a file with distance matrix among chosen places
:param names: names of the places you want to visit
:param pos: your position at the moment (starting point)
:param transport_type: obvious
:return: None
"""
global after_dot, routes, places_number_name, places_name_coor, name_in, city
places_number_name[1] = 'curr_pos'
for i in range(1, len(names) + 1):
places_number_name[i + 1] = names[i - 1]
places_name_coor['curr_pos'] = pos
data = pd.read_csv(city + ".csv")
all_names = data['Название']
all_lon = data['Долгота']
all_lat = data['Широта']
wh = list()
types = [
'Памятники', 'Дома и дворцы', 'Башни и ворота', 'Современные здания',
'Московское центральное кольцо (МЦК)'
]
for name in names:
places_name_coor[name] = (all_lon[pd.Index(all_names).get_loc(
name.replace(',', '+++'))], all_lat[pd.Index(all_names).get_loc(
name.replace(',', '+++'))])
file_m = open(name_in, 'w', encoding='UTF-8')
file_m.write(str(len(names) + 1) + '\n')
router = Router(transport_type) # , 'static/Moscow_test.osm')
key = 0
for name_start in places_name_coor.keys():
if name_start != 'curr_pos':
idx = pd.Index(all_names).get_loc(name_start.replace(',', '+++'))
type = data['Тип Постройки'][pd.Index(all_names).get_loc(
name_start.replace(',', '+++'))]
week = [
data['Mon'][idx], data['Tue'][idx], data['Wed'][idx],
data['Thu'][idx], data['Fri'][idx], data['Sat'][idx],
data['Sun'][idx]
]
for d in week:
if d != 'None':
wh.append(' 1 ')
elif type not in types:
wh.append('-1 ')
else:
wh.append(' 1 ')
wh.append('\n')
to_write = str()
start = router.findNode(places_name_coor[name_start][0],
places_name_coor[name_start][1])
for name_end in places_name_coor.keys():
key += 1
end = router.findNode(places_name_coor[name_end][0],
places_name_coor[name_end][1])
status, route = router.doRoute(start, end)
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route))
routes[key] = routeLatLons
sum_ = 0
for i in range(len(routeLatLons) - 1):
sum_ += router.distance(routeLatLons[i],
routeLatLons[i + 1])
sum_ *= after_dot
to_write += ' ' + str(sum_)[:str(sum_).index('.')] + ' '
elif status == 'no_route':
routes[key] = route
to_write += '-1 '
to_write = to_write.rstrip()
file_m.write(to_write + '\n')
for i in wh:
file_m.write(i)
file_m.close()
# On utilise la librairie de M. Kuranowski appelée pyroutelib3
# https://github.com/MKuranowski/pyroutelib3
# Cette librairie se base sur les données d'OpenStreetMap
from pyroutelib3 import Router
import sys
# Ici, à l'aide du constructeur (Router), on initialise un objet de type router
# Lors de l'instantiation, on lui spécifie via le paramètre "car" que l'on souhaite se déplacer en voiture.
# Mode de transport disponibles: car, cycle, foot, horse, tram, train
router = Router("car")
# On utilise la méthode findNode de l'objet router afin de récupérer le node le plus proche du couple latitude longitude spécifié
# https://www.openstreetmap.fr/ pour comprendre et https://www.openstreetmap.org pour trouver lat et lon
debutTrajet = router.findNode(50.198153, 3.220213)
finTrajet = router.findNode(50.17601, 3.22928)
# debutTrajet et finTrajet sont deux "Nodes" (=noeuds de navigation)
#Nous allonrs maintenant demander à l'objet router de trouver un ensemble de nodes à partir afin d'arriver du node de début au node de fin.
status, listeNodes = router.doRoute(debutTrajet, finTrajet)
print("Statut du calcul de l'itinéraire: ", status)
if status != 'success':
print(" Fin du programme. Revoyez vos coordonnées GPS")
sys.exit(1)
print()
print()
print("Voici les nodes à suivre: ")
print()
print()
def post_list(request):
posts = Post.objects.filter(
published_date__lte=timezone.now()).order_by('published_date')
ret_code = 'none'
timing = ''
routeLatLons = list()
routeLatLonsAdd = list()
#center_phi = (56.8874 + 56.8843) * 0.5
#center_lambda = (35.8652 + 35.8819) * 0.5
bound_min_phi = 56.8874
bound_max_phi = 56.8843
bound_min_la = 35.8652
bound_max_la = 35.8819
token = get_token()
if request.method == 'POST':
form = NameForm(request.POST)
if form.is_valid():
bound_min_phi = form.cleaned_data['begin_phi']
bound_max_phi = form.cleaned_data['end_phi']
transport = form.cleaned_data['wheel_val']
ar_time_h = form.cleaned_data['arrival_time_hour']
ar_time_m = form.cleaned_data['arrival_time_minutes']
scl_time_h = form.cleaned_data['sclad_time_hour']
scl_time_m = form.cleaned_data['sclad_time_minutes']
print('before')
print(ar_time_h, ar_time_m)
print('scl_time_h', scl_time_h)
ar_time_h, ar_time_m = add_delay(ar_time_h, ar_time_m, scl_time_h,
scl_time_m)
print('after')
print(ar_time_h, ar_time_m)
router = Router(transport)
if bound_min_phi > bound_max_phi:
temp = bound_min_phi
bound_min_phi = bound_max_phi
bound_max_phi = temp
bound_min_la = form.cleaned_data['begin_lambda']
bound_max_la = form.cleaned_data['end_lambda']
if bound_min_la > bound_max_la:
temp = bound_min_la
bound_min_la = bound_max_la
bound_max_la = temp
start = router.findNode(form.cleaned_data['begin_phi'],
form.cleaned_data['begin_lambda'])
end = router.findNode(form.cleaned_data['end_phi'],
form.cleaned_data['end_lambda'])
med = router.findNode(form.cleaned_data['med_phi'],
form.cleaned_data['med_lambda'])
start_time = time.time()
status, route = router.doRoute(start, med)
sum_length = 0
time_str_1 = ''
if status == 'success':
# Get actual route coordinates
routeLatLons = list(map(router.nodeLatLon, route))
temp_phi = 0
temp_la = 0
for point in routeLatLons:
if point == routeLatLons[0]:
temp_phi = point[0]
temp_la = point[1]
else:
slat = radians(temp_phi)
slon = radians(temp_la)
elat = radians(point[0])
elon = radians(point[1])
dist = 6371.01 * acos(
sin(slat) * sin(elat) +
cos(slat) * cos(elat) * cos(slon - elon))
sum_length = sum_length + dist
temp_phi = point[0]
temp_la = point[1]
if point[0] > bound_max_phi:
bound_max_phi = point[0]
if point[0] < bound_min_phi:
bound_min_phi = point[0]
if point[1] > bound_max_la:
bound_max_la = point[1]
if point[1] < bound_min_la:
bound_min_la = point[1]
#ret_code = 'success'
#ret_code = '1-й маршрут построен, ' + 'время в пути ' + "{0:.2f}".format(sum_length / speed_list[transport]) + ' ч, '
ret_code = 'Результаты расчетов: время в пути до склада '
time_str_1 = hours_to_time_str(sum_length /
speed_list[transport])
ret_code = ret_code + time_str_1
else:
if ret_code == 'none':
ret_code = 'Результаты расчетов: маршрут до склада отсутствует, '
else:
ret_code = 'Результаты расчетов: маршрут до склада не построен, '
status, route = router.doRoute(med, end)
sum_length2 = 0
time_str_2 = ''
if status == 'success':
# Get actual route coordinates
routeLatLonsAdd = list(map(router.nodeLatLon, route))
temp_phi = 0
temp_la = 0
for point in routeLatLonsAdd:
if point == routeLatLonsAdd[0]:
temp_phi = point[0]
temp_la = point[1]
else:
slat = radians(temp_phi)
slon = radians(temp_la)
elat = radians(point[0])
elon = radians(point[1])
dist = 6371.01 * acos(
sin(slat) * sin(elat) +
cos(slat) * cos(elat) * cos(slon - elon))
sum_length2 = sum_length2 + dist
temp_phi = point[0]
temp_la = point[1]
if point[0] > bound_max_phi:
bound_max_phi = point[0]
if point[0] < bound_min_phi:
bound_min_phi = point[0]
if point[1] > bound_max_la:
bound_max_la = point[1]
if point[1] < bound_min_la:
bound_min_la = point[1]
#ret_code = 'success'
ret_code = ret_code + '; время в пути до точки встречи '
time_str_2 = hours_to_time_str(sum_length2 /
speed_list[transport])
ret_code = ret_code + time_str_2
else:
if ret_code == 'none':
ret_code = 'маршрут до точки встречи отсутствует, '
else:
ret_code = 'маршрут до точки встречи не построен, '
ret_code = ret_code + '; общая длина маршрута ' + "{0:.2f}".format(
sum_length + sum_length2) + ' км'
ret_code = ret_code + ', время движения ' + hours_to_time_str(
(sum_length + sum_length2) / speed_list[transport])
#ret_code = ret_code + ' ( ' + time_str_1 + ' до склада, ' + time_str_2 + ' до точки встречи)'
ret_code = ret_code + time_to_start_str(
(sum_length + sum_length2) / speed_list[transport], ar_time_h,
ar_time_m)
timing = 'Время выполнения расчётов: ' + "{0:.2f}".format(
time.time() - start_time) + ' сек '
elif request.method == 'GET':
form = NameForm()
ret_code = 'Введите данные'
else:
form = NameForm()
ret_code = 'Неправильный http-запрос'
return render(request, 'blog/post_list.html', {'ret_code': ret_code, \
'form': form, 'route': routeLatLons, 'routeAdd': routeLatLonsAdd,\
'bound_min_phi': bound_min_phi - 0.001,\
'bound_max_phi': bound_max_phi + 0.001, \
'bound_min_la': bound_min_la - 0.001,\
'bound_max_la': bound_max_la + 0.001, \
'timing': timing, \
'mapbox_access_token':token })
def gpp2lpp(velocity, current_lat, current_lon, light, ALL_STOP):
# Initialize---------------------------------------------------------------
global Out_of_speed
global Out_of_accel
global Out_of_curvature
global Warning_obstacle
global MAX_LEFT_WIDTH
global MAX_RIGHT_WIDTH
global show_animation
matplotlib.use('TkAgg')
location = './'
Directory = os.listdir(location) # 경로에 있는 모든 파일들을 불러온다
channel_num = find_channel(2) # For Sending information to Control System
Channel = setUpChannel(channel_num)
Coordination = []
stop_line = []
HD_LIST = []
# HD Map 정보가 담겨져 있는 csv 파일을 가져온다
for name in Directory:
file_name = name.split('.')
if len(file_name) > 1:
if file_name[1] == 'csv':
HD_LIST.append(name)
for name in HD_LIST:
with open(name, 'r', encoding='UTF8') as f:
# Necessary----------------------------------------------
if name == "A2_LINK.csv": # 이 파일은 HD Map의 모든 노드 데이터들이 있는 파일이다. (필수 데이터)
all_data = csv.reader(f)
coord = []
for line in all_data:
if not line == []:
temp = [float(x) for x in line]
coord.append(temp)
Coordination = np.array(coord)
# Necessary----------------------------------------------
# OPTIONS-------------------------------------------------
elif name == "A2_STOP.csv": # 이 파일은 HD Map의 Stop Line 데이터들이 있는 파일이다. (옵션 데이터)
all_data = csv.reader(f)
tt = []
for line in all_data:
if not line == []:
temp = [float(x) for x in line]
tt.append(temp)
stop_line = np.array(tt)
# OPTIONS-------------------------------------------------
router = Router("car", "A2_LINK_OUT.osm") # HD Map이 있는 모든 데이터들을 가져온다. Readme.md 설명 참조
# Initialize---------------------------------------------------------------
# 차량의 위도 및 경도를 받았을 때 while문을 나온다
while True:
if current_lat.value > 0 and current_lon.value > 0:
lat = current_lat.value
lon = current_lon.value
break
# Start -----------------------------------------------------------------
start = router.findNode(lat, lon) # 나의 좌표
end = router.findNode(37.1111111, 126.11111111) # 목표 지점 좌표
status, route = router.doRoute(start, end) # Find the route - a list of OSM nodes (Global Path Planning)
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route)) # Get actual route coordinates
routeLatLons = np.array(routeLatLons)
# Essential (Change list)
result_path = []
for latlon in routeLatLons:
result_path.append([float(latlon[0]), float(latlon[1])])
result_path = np.array(result_path)
# plotting Coordination------------------------------------------ option
plt.title("Global Path Planning")
plt.plot(Coordination[:, 1], Coordination[:, 0], '*b', label="HD Map")
plt.plot(result_path[:, 1], result_path[:, 0], '--r', linewidth=3, label='Generated Path Trajectory')
plt.plot(result_path[0, 1], result_path[0, 0], 'Xm', markersize=10, label="Start Point")
plt.plot(result_path[-1, 1], result_path[-1, 0], 'Xg', markersize=10, label="End Point")
plt.plot(stop_line[:, 1], stop_line[:, 0], 'ok', markersize=5, label="Stop Lines")
plt.legend()
plt.show()
# plotting Coordination------------------------------------------ option
center = sum(Coordination) / len(Coordination) # Calculate ENU Center For Convert other LLH Data to ENU
# Create all lane------------------------- option
ENU_all = []
for llh in Coordination:
e, n, u = pm.geodetic2enu(llh[0], llh[1], llh[2], center[0], center[1], center[2])
ENU_all.append([e, n])
ENU_all = np.array(ENU_all)
# ------------------------------------------ option
# Create enu pqosition of the GPP result
temp_result = []
for llh in result_path:
e, n, u = pm.geodetic2enu(llh[0], llh[1], center[2], center[0], center[1], center[2])
temp_result.append([e, n])
result_path = np.array(temp_result)
# Create Stop Line by me
traffic_all = []
for llh in stop_line:
e, n, u = pm.geodetic2enu(llh[0], llh[1], llh[2], center[0], center[1], center[2])
traffic_all.append([e, n])
traffic_all = np.array(traffic_all)
# This is Local Path Planning----------------------------------
dx, dy, dyaw, dk, s = get_course(result_path[:, 0], result_path[:, 1]) # X, Y Yaw, Curvature, Index
# Initial Parameters ------------------------------------
# For LPP
c_d = 0.0 # current lateral position [m]
c_d_d = 0.0 # current lateral speed [m/s]
c_d_dd = 0.0 # current lateral acceleration [m/s]
# s0 = 0.0 # current course position
area = 50.0 # animation area length [m]
# For LPP
# For Comparing
heading = 0
time = 0.0
toggle = False
temp_light = 0
# For Comparing
# ----------------------------------------------
lists_dict = {}
for index, lists in enumerate(result_path):
lists_dict[index] = lists
start_time = datetime.now() # For Compare Times
obstacle = [[0, 0], [1, 1]] #example Obstacle
# LOCAL PATH PLANNING Start!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
while True:
if ALL_STOP.value == 1: # IF all stop toggle on, All Codes Stop
break
# Change my Coordinate to ENU
comp_x, comp_y, comp_z = pm.geodetic2enu(current_lat.value, current_lon.value, center[2], center[0], center[1], center[2])
# Compare my Coordination--------------------------
temp_dict = {}
for key, value in lists_dict.items():
point_position = np.hypot(value[0] - comp_x, value[1] - comp_y)
temp_dict[key] = point_position
POINT = min(temp_dict.keys(), key=lambda k: temp_dict[k])
s0 = s.s[POINT]
# For Car's left right light----------------------
# 차선 바꾸고 나서 가고 있는데 장애물 회피해야 됨, 그러면 이거
if Warning_obstacle == True:
temp_light = 0
# 깜빡이 처리============================================================
end_time = datetime.now()
SEC = (end_time - start_time).seconds # Second
if SEC > 3: # 만약 3초 동안 깜빡이 없으면
toggle = False
if temp_light == 0: # 이전에 켜진 깜박이가 없거나, 옆으로 갔다가 다시 돌아올 때
# 정밀지도 상에서 헤딩에 따라 피하고 난 다음 위치 선정 -> 헤딩이 일정 구간 안에 있으면 왼쪽 or 오른쪽
if heading > 150 and heading < 250: # 왼쪽으로 피함
MAX_LEFT_WIDTH = -LARGE
MAX_RIGHT_WIDTH = 1
elif heading > 300 or heading < 50: # 오른쪽으로 피함
MAX_LEFT_WIDTH = -1
MAX_RIGHT_WIDTH = LARGE
elif temp_light < 0: # 오른쪽으로만 계속 주행할 수 있도록 설정
MAX_LEFT_WIDTH = -LARGE * abs(temp_light)
MAX_RIGHT_WIDTH = -SHORTE * abs(temp_light)
elif temp_light > 0: # 왼쪽으로만 계속 주행할 수 있도록 설정
MAX_LEFT_WIDTH = SHORTE * abs(temp_light)
MAX_RIGHT_WIDTH = LARGE * abs(temp_light)
if light.value == -1: # 오른쪽 신호를 받았을 때
start_time = datetime.now()
if toggle == False: # 한번만 실행되게 만드는것
idx = np.abs(np.array(dx) - path.x[-1]).argmin() # 맨 앞의 LPP Position 값이 전체 구역에서 몇번째 index 가지고 있는지 추출 (X)
idy = np.abs(np.array(dy) - path.y[-1]).argmin() # 맨 앞의 LPP Position 값이 전체 구역에서 몇번째 index 가지고 있는지 추출 (Y)
obstacle.append([dx[idx], dy[idy]]) # 자리 이동할 수 있도록 하나의 장애물을 위에 추가
temp_light += light.value # 만약 오른쪽 신호를 받고 그 쪽으로 갔는데, 또 오른쪽 신호를 받으면 피할 ROI를 더 증가하도록 하기 위한 것
toggle = True # 한번 신호 받으면 그 다음 이 if 문으로 안 들어 오게 하는 toggle
if temp_light == 0:
MAX_LEFT_WIDTH = -SHORTE
MAX_RIGHT_WIDTH = SHORTE
elif temp_light < 0:
MAX_LEFT_WIDTH = LARGE * temp_light # 오른쪽으로 피함
MAX_RIGHT_WIDTH = SHORTE * temp_light
else:
MAX_LEFT_WIDTH = SHORTE * temp_light # 왼쪽으로 피함
MAX_RIGHT_WIDTH = LARGE * temp_light
if light.value == 1: # 왼쪽 신호를 받았을 때
start_time = datetime.now()
if toggle == False:
idx = np.abs(np.array(dx) - path.x[-1]).argmin() # 맨 앞의 LPP Position 값이 전체 구역에서 몇번째 index 가지고 있는지 추출 (X)
idy = np.abs(np.array(dy) - path.y[-1]).argmin() # 맨 앞의 LPP Position 값이 전체 구역에서 몇번째 index 가지고 있는지 추출 (Y)
obstacle.append([dx[idx], dy[idy]]) # 자리 이동할 수 있도록 하나의 장애물을 위에 추가
temp_light += light.value
toggle = True # 한번 신호 받으면 그 다음 이 if 문으로 안 들어 오게 하는 toggle
if temp_light == 0:
MAX_LEFT_WIDTH = -SHORTE
MAX_RIGHT_WIDTH = SHORTE
elif temp_light < 0:
MAX_LEFT_WIDTH = LARGE * temp_light # 오른쪽으로 피함
MAX_RIGHT_WIDTH = SHORTE * temp_light
else:
MAX_LEFT_WIDTH = SHORTE * temp_light # 왼쪽으로 피함
MAX_RIGHT_WIDTH = LARGE * temp_light
# 깜빡이 처리============================================================
# For Car's left right light----------------------
obstacle = np.array(obstacle) # Relate Coordination
path = frenet_optimal_planning(s, s0, velocity.value, c_d, c_d_d, c_d_dd, obstacle)
c_d = path.d[1]
c_d_d = path.d_d[1]
c_d_dd = path.d_dd[1]
time = time + DT
heading = math.atan2(path.x[1] - path.x[0], path.y[1] - path.y[0]) * 180 / math.pi + 180 # 현재 값과 다음 값의 방향을 비교해서 Heading값 추출하기
# update my coordination ---------------------------------
# stop line 찾는 법은 간단하게 코사인 유사도 거리 방법 및 유클라디안 거리 검출 사용함====================
INDEXS = {}
point = np.array([path.x[0], path.y[0]])
last_point = np.array([path.x[int(len(path.x) / 2)], path.y[int(len(path.y) / 2)]])
comp_heading = math.atan2(last_point[0] - point[0], last_point[1] - point[1]) * 180 / math.pi + 180
for i in range(len(traffic_all)):
comp = np.array(traffic_all[i])
point_position = np.array(comp - point)
# Heading ----------->
heading_traffic = math.atan2(point_position[0], point_position[1]) * 180 / math.pi + 180
point_pos = np.sqrt((point_position[0] ** 2) + (point_position[1] ** 2)) # L2 norm vector -> 유클라디안 거리 검출임
if abs(abs(heading_traffic) - abs(comp_heading)) < 10: # 대충 Cosine 유사도 거리 검출임
INDEXS[i] = point_pos
if len(INDEXS) > 0:
POINT = min(INDEXS.keys(), key=lambda k: INDEXS[k])
else:
POINT = 0
stop_lat, stop_lon, stop_h = pm.enu2geodetic(traffic_all[POINT][0], traffic_all[POINT][1], center[2],
center[0], center[1], center[2])
# stop line 찾는 법은 간단하게 코사인 유사도 거리 방법 및 유클라디안 거리 검출 사용함====================
# Find next Coordination based on Lookahead=====================================================================
lookahead = velocity.value * 0.2 + 4.5
num = 0
for i in range(len(path.x)):
distance = np.hypot(path.x[i] - path.x[0], path.y[i] - path.y[0])
dists = distance - lookahead
if dists > 0:
num = i
break
# ENU to LLH
next_lat, next_lon, next_alt = pm.enu2geodetic(path.x[num], path.y[num], center[2], center[0], center[1], center[2])
# Find next Coordination based on Lookahead=====================================================================
# tx버퍼 클리어 구문
# 실제 실험할 때 키는 구문 (제어 프로세스에 송신)==============================
canlib.IOControl(Channel).flush_tx_buffer()
Wave_Path_Next_Lat_sig.Wave_Path_Next_Lat.phys = round(float(next_lat), 8)
Channel.write(Wave_Path_Next_Lat_sig._frame)
Wave_Path_Next_Long_sig.Wave_Path_Next_Long.phys = round(float(next_lon), 7)
Channel.write(Wave_Path_Next_Long_sig._frame)
Wave_Path_Next_Alt_Yaw_sig.Wave_Path_Next_Alt.phys = round(float(next_alt), 2)
Channel.write(Wave_Path_Next_Alt_Yaw_sig._frame)
Wave_StopLine_Lat_sig.Wave_StopLine_Lat.phys = float(stop_lat)
Channel.write(Wave_StopLine_Lat_sig._frame)
Wave_StopLine_Long_sig.Wave_StopLine_Long.phys = float(stop_lon)
Channel.write(Wave_StopLine_Long_sig._frame)
# 실제 실험할 때 키는 구문 (제어 프로세스에 송신)==============================
# Warning Alert=================================
if Out_of_speed == True:
print("[Speed] -> Out of the Max")
Out_of_speed = False
if Out_of_accel == True:
print("[Accel] -> Out of the Max")
Out_of_accel = False
if Out_of_curvature == True:
print("[Curvature] -> Out of the Max")
Out_of_curvature = False
if Warning_obstacle == True:
print("[WARNING] -> Obstacle")
Warning_obstacle = False
# Warning Alert=================================
# 자동차 제동 거리 공식
break_distance = round(0.0005 * math.pow(velocity.value * 3.6, 2) + 0.2 * (velocity * 3.6), 3)
comp_end = np.hypot(path.x[1] - dx[-1], path.y[1] - dy[-1])
# 자동차 제동 거리와 비교해서 다달으면 모든 코드 break
if comp_end <= break_distance:
print("Goal!")
ALL_STOP.value = 1
break
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
# line of the world
plt.plot(ENU_all[:, 0], ENU_all[:, 1], '*b')
# line of the GPP
plt.plot(dx, dy, '--k', linewidth=3)
if len(obstacle) > 0:
plt.plot(obstacle[:, 0], obstacle[:, 1], "or", markersize=6)
# line of the LPP
plt.plot(path.x[1:], path.y[1:], "-og", linewidth=2)
plot_car(path.x[1], path.y[1], radian)
plt.plot(traffic_all[:, 0], traffic_all[:, 1], '*y')
plt.plot(traffic_all[POINT][0], traffic_all[POINT][1], 'Xm', markersize=10)
# ROI
plt.xlim(path.x[1] - area, path.x[1] + area)
plt.ylim(path.y[1] - area, path.y[1] + area)
text = "Time: " + str(round(time, 2)) + " / Velocity: " + str(round(velocity * 3.6, 2)) + "km/h"
plt.title(text)
plt.grid(True)
plt.pause(0.0001)
print("Finish")
else:
print("Position not found. Tray again...")
print("GPP=LPP FINISH")
def generate_route_main(ap_id, ts1, ts2, lat1, lon1, lat2, lon2,
transport_mode, current_route):
start_time = datetime.now() # start time
'''
lat2 = 26.65317
lon2 = 128.090794
lat1 = 26.66539
lon1 = 128.103902
'''
#print ap_id,ts1, ts2, lat1,lon1, lat2, lon2, transport_mode, current_route
arr_generated_rows = []
router = None
if (osm_data_source != ''):
router = Router(transport_mode, osm_data_source) # use local osm data
else:
router = Router(transport_mode) # use osm data from Internet
start = None
end = None
try:
start = router.findNode(lat1, lon1) # Find start and end nodes
end = router.findNode(lat2, lon2)
except:
print('cannot resolve start/end OSM node:', current_route)
return []
status = None
route = None
try:
#result, route = router.doRoute(node1, node2)
status, route = router.doRoute(
start, end) # Find the route - a list of OSM nodes
except:
print('XML parse error:', current_route)
return []
# ill formated osm gives errors like Unclosed Token Error is raised by the XML ETree parser, not by pyroutelib.
n_points = len(route)
ts_interval = (ts2 - ts1) / (n_points - 1)
if status == 'success':
routeLatLons = list(map(router.nodeLatLon,
route)) # Get actual route coordinates
counter = 0
for (lat, lon) in routeLatLons:
ts = ts1 + ts_interval * counter
#node = data.rnodes[node_id]
print_line = ("%s,%f,%f,%s" % (ap_id, lat, lon, ts))
counter += 1
arr_generated_rows.insert(0, {
'ap_id': ap_id,
'timestamp': ts,
'latitude': lat,
'longitude': lon
})
print("points count:", counter)
else:
print(("\t (%s)" % status), current_route, ' Skipping...', end="")
msg = '\n %s %f,%f,%s,%f,%f,%s,--%s--' % (current_route, lat1, lon1,
ts1, lat2, lon2, ts2, status)
log_error(msg)
#log the time and memory consumed for current transaction
current_memory_mb = get_memory_usage()
end_time = datetime.now()
time_taken = (end_time - start_time).total_seconds()
time_msg = '\n' + current_route + ',' + str(
len(arr_generated_rows)) + ',' + status + ',' + str(
current_memory_mb) + ',' + str(time_taken) + ',' + str(
start_time) + ',' + str(end_time)
#print (current_memory_mb, time_msg)
log_error(msg=time_msg, log_file="log_txn_time.txt")
return arr_generated_rows # number of points got
class GetRoutInfo(object):
def __init__(self, localFile):
# https://wiki.openstreetmap.org/wiki/Routing
pyroutelib3.TYPES["car"]['weights']['motorway'] = 20
pyroutelib3.TYPES["car"]['weights']['trunk'] = 10
pyroutelib3.TYPES["car"]['weights']['primary'] = 1
pyroutelib3.TYPES["car"]['weights']['secondary'] = 1
pyroutelib3.TYPES["car"]['weights']['tertiary'] = 1
pyroutelib3.TYPES["car"]['weights']['unclassified'] = 1
pyroutelib3.TYPES["car"]['weights']['residential'] = 0.5
pyroutelib3.TYPES["car"]['weights']['track'] = 0
pyroutelib3.TYPES["car"]['weights']['service'] = 0
if localFile:
self.router = Router("car", localFile)
else:
self.router = Router("car")
"""
This methode is setting the weights for the used router
Check out the follwoing web page for further details
# https://wiki.openstreetmap.org/wiki/Routing
"""
def setRouteweights(self, motorway, trunk, primary, secondary, tertiary,
unclassified, residential, track, service):
pyroutelib3.TYPES["car"]['weights']['motorway'] = motorway
pyroutelib3.TYPES["car"]['weights']['trunk'] = trunk
pyroutelib3.TYPES["car"]['weights']['primary'] = primary
pyroutelib3.TYPES["car"]['weights']['secondary'] = secondary
pyroutelib3.TYPES["car"]['weights']['tertiary'] = tertiary
pyroutelib3.TYPES["car"]['weights']['unclassified'] = unclassified
pyroutelib3.TYPES["car"]['weights']['residential'] = residential
pyroutelib3.TYPES["car"]['weights']['track'] = track
pyroutelib3.TYPES["car"]['weights']['service'] = service
"""
This methode findes a route between two points defined by coordinates
"""
def routeF(self, p1Lag, p1Long, p2Lag, p2Long):
self.s = (p1Lag, p1Long)
self.e = (p2Lag, p2Long)
start = self.router.findNode(self.s[0], self.s[1])
end = self.router.findNode(self.e[0], self.e[1])
self.filesName = "{}_{}".format(start, end)
routeFile = self.filesName + "_route.json"
#if file already available load it
if os.path.isfile(routeFile):
with open(routeFile, 'r') as f:
(self.route, self.routeLatLons) = json.load(f)
#self.routeLatLons = list(map(self.router.nodeLatLon, self.route))
#if no file is available calcualte route and store it
else:
status, self.route = self.router.doRoute(start, end)
if status == 'success':
self.routeLatLons = list(
map(self.router.nodeLatLon,
self.route)) # Get actual route coordinates
with open(routeFile, 'w') as f:
json.dump([self.route, self.routeLatLons], f)
else:
raise Exception(
"could not find a route from two points p1: ({}) p2: ({}). Status:{}"
.format(start, end, status))
"""
This methode prints the route into a map
"""
def printRoute(self, dpi, width):
tilemapbase.start_logging()
tilemapbase.init(create=True)
t = tilemapbase.tiles.build_OSM()
if self.s[0] < self.e[0]:
south = self.s[0]
north = self.e[0]
else:
south = self.e[0]
north = self.s[0]
if self.s[1] < self.e[1]:
east = self.s[1]
west = self.e[1]
else:
east = self.e[1]
west = self.s[1]
degree_range = 0.1
extent = tilemapbase.Extent.from_lonlat(east - degree_range,
west + degree_range,
south - degree_range,
north + degree_range)
fig, ax = plt.subplots(figsize=(8, 8), dpi=dpi)
plotter = tilemapbase.Plotter(extent, t, width=width)
plotter.plot(ax, t)
for i in self.routeLatLons:
x, y = tilemapbase.project(i[1], i[0])
ax.scatter(x, y, marker=".", color="black", linewidth=2)
plt.show()
"""
This methode is used to find ways onto which the nodes are placed
This is done via Overpass API
Due to the fact a node can be a member of several ways a simple algorithi is used which
search for the correct way.
"""
def getWay(self):
#https://www.openstreetmap.org/node/34817889 -> To see node in osm.org
#http://overpass-api.de/api/interpreter?data=[out:json];node(34817889);way(bn);out;
#-> what we are doing with request.
wayfile = self.filesName + "_way.json"
if os.path.isfile(wayfile):
with open(wayfile, 'r') as f:
self.way = json.load(f)
else:
data = []
overpass_url = "http://overpass-api.de/api/interpreter"
for i in self.route:
overpass_query = """
[out:json];
(node({});
way(bn);
);
out center;
""".format(i)
while True:
try:
response = requests.get(
overpass_url, params={'data': overpass_query})
data.append(response.json())
break
except:
print("error {}".format(i))
#set_trace()
with open(wayfile + "temp.json", 'w') as f:
json.dump(data, f)
#remove not needed information
elements = []
for i in range(0, len(data)):
elements.append(data[i]['elements'])
#filter ways a bit
ways = []
for i in elements:
ways.append([])
for j in i:
if j['type'] == 'way':
if 'tags' in j:
if 'highway' in j['tags']:
if j['tags']['highway'] != 'footway' \
and j['tags']['highway'] != 'raceway' \
and j['tags']['highway'] != 'bridleway' \
and j['tags']['highway'] != 'steps' \
and j['tags']['highway'] != 'path' \
and j['tags']['highway'] != 'service':
ways[-1].append(j)
#algorithm to detect correct way out of multible ways of singel point
#initail point
way = []
for i in range(0, len(ways[0])):
for j in range(0, len(ways[1])):
if ways[0][i]['id'] == ways[1][j]['id']:
way.append(ways[0][i])
break
#following points
cnt = 0
for i in range(1, len(ways)):
if cnt > 1:
#set_trace()
print("{} duplicate found".format(cnt))
for i in range(0, cnt - 1):
del (way[-1])
#raise Exception("can't detect correct way point!")
cnt = 0
for j in range(0, len(ways[i])):
for k in range(0, len(ways[i - 1])):
if ways[i][j]['id'] == ways[i - 1][k]['id']:
way.append(ways[i][j])
cnt += 1
self.way = way
with open(wayfile, 'w') as f:
json.dump(self.way, f)
"""
This methode is used to extract the maxspeed data for the ways.
If no maxspeed is specifired a assumption depending on the
road classification is met
"""
def getMaxSpeed(self):
speed = []
for i in self.way:
if 'maxspeed' in i['tags'] and i['tags']['maxspeed'] != 'signals':
speed.append(int(i['tags']['maxspeed']))
else:
if i['tags']['highway'] == 'motorway':
if 'tunnel' in i['tags'] and i['tags']['tunnel'] == 'yes':
speed.append(100)
else:
speed.append(130)
elif i['tags']['highway'] == 'motorway_link':
speed.append(100)
elif i['tags']['highway'] == 'trunk':
speed.append(100)
elif i['tags']['highway'] == 'trunk_link':
speed.append(100)
elif i['tags']['highway'] == 'primary':
speed.append(100)
elif i['tags']['highway'] == 'primary_link':
speed.append(80)
elif i['tags']['highway'] == 'secondary':
speed.append(100)
elif i['tags']['highway'] == 'secondary_link':
speed.append(80)
elif i['tags']['highway'] == 'tertiary':
speed.append(70)
elif i['tags']['highway'] == 'tertiary_link':
speed.append(50)
elif i['tags']['highway'] == 'unclassified':
speed.append(70)
elif i['tags']['highway'] == 'residential':
speed.append(50)
else:
raise Exception(
"can't find max speed of route:{}".format(i))
self.maxSpeed = speed
"""
This methode is used to create a list of distances between each node
"""
def getDist(self):
# list of distance between nodes
self.distance = []
#for i in range(0, len(self.routeLatLons) - 1):
# self.distance.append(self.router.distance(self.routeLatLons[i], self.routeLatLons[i + 1]))
for i in range(0, len(self.way) - 1):
self.distance.append(
self.router.distance([
self.way[i]['center']['lat'], self.way[i]['center']['lon']
], [
self.way[i + 1]['center']['lat'],
self.way[i + 1]['center']['lon']
]))
from pyroutelib3 import Router
import folium
import webbrowser
# creation de la carte
latD, lonD=48.58626, 7.75246 # Depart : pont du Theatre
latA, lonA=48.58167, 7.75048 # Arrivee : Cathedrale
c = folium.Map(location = [(latA+latD)/2, (lonA+lonD)/2], zoom_start=17) #carte centree
# position depart et arrivee
folium.Marker((latD, lonD),popup = "Depart", icon=folium.Icon(
color = 'green', icon ='home')).add_to(c)
folium.Marker((latA,lonA),popup = "Arrivee", icon=folium.Icon(
color = 'blue', icon = 'eye-open')).add_to(c)
# itineraire "optimal" apec pyroutelib3
router = Router("foot") # cycle, foot, horse, tram, train , car
depart = router.findNode(latD, lonD)
arrivee = router.findNode(latA, lonA)
status, route = router.doRoute(depart, arrivee)
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route))
folium.PolyLine(routeLatLons, color="red", weight=2.5, opacity=1).add_to(c)
# sauvegarde et affichage de la carte
c.save('cartev0.42.html')
webbrowser.open('cartev0.42.html')
class RoutingUsage:
def __init__(self,Mode):
self.router = Router(Mode) # Initialise it
def node(self,lat,longit):
return [lat,longit]
def getRouteMultiple(self,nodesNew):
queueNodesNewRight = []
for index in range(0,len(nodesNew)-1):
nodeStart = nodesNew[index]
nodeEnd = nodesNew[index+1]
route = self.getTheRouteBetweenTwoNodes(nodeStart[0],nodeStart[1],nodeEnd[0],nodeEnd[1])
if len(route[0]) == 0 and route[1] > 0:
return None
queueNodesNewRight.append(route)
return queueNodesNewRight
def arrangeNodesDependsOnLength(self,nodes):
for nodeStartIndex in range(0,len(nodes)-1):
lastDistance = self.router.distance(nodes[nodeStartIndex],nodes[nodeStartIndex+1])
for nodeNowIndex in range(nodeStartIndex+1,len(nodes)):
theReturnedNodeWhichisNearst=nodes[nodeStartIndex+1]
nowLength = self.router.distance(nodes[nodeStartIndex],nodes[nodeNowIndex])
#print(nowLength)
theReturnedNodeIndex = nodeStartIndex
if nowLength < lastDistance :
theReturnedNodeWhichisNearst = nodes[nodeNowIndex]
theReturnedNodeIndex = nodeNowIndex
lastDistance = self.router.distance(nodes[nodeStartIndex],nodes[nodeNowIndex])
ReserveNode = nodes[nodeStartIndex+1]
nodes[nodeStartIndex+1] = nodes[theReturnedNodeIndex]
nodes[theReturnedNodeIndex] = ReserveNode
#print("length: %f"%lastDistance)
return nodes
def getTheRouteBetweenTwoNodes(self,lat1,long1,lat2,long2):
start = self.router.findNode(lat1,long1) # Find start and end nodes
end = self.router.findNode(lat2,long2)
## print("start : %s, Lat: %s, Lon: %s "% (start,lat1,long1))
## print("end : %s, Lat: %s, Lon: %s "% (end,lat2,long2))
status, route = self.router.doRoute(start, end) # Find the route - a list of OSM nodes
if status == 'success':
routeLatLons = list(map(self.router.nodeLatLon, route)) # Get actual route coordinates
# list the lat/long
queueNodes = []
sumPath = 0
l = len(route)
for index, obj in enumerate(route):
thisElement = route[index]
newDistance = 0
if index < l-1 :
nextElement = route[index+1]
thisElementD = [self.router.nodeLatLon(thisElement)[0],self.router.nodeLatLon(thisElement)[1]]
nextElementD = [self.router.nodeLatLon(nextElement)[0],self.router.nodeLatLon(nextElement)[1]]
newDistance = self.router.distance(nextElementD,thisElementD)
sumPath = sumPath + newDistance
elif index == l -1:
nextElement = route[index-1]
typeData = self.router.getNodeWay(thisElement,nextElement)
#get width Depends on the Category
width = self.router.getRouteWidth(typeData["tag"]["highway"])
#get width Depends on the way lanes numbers
NumberOfLanes = typeData["tag"].get("lanes")
#Const Lanes Width it will be 3 meter
laneWidth = 3/12742/6*1.1 #Meter
if NumberOfLanes != None:
width = int(NumberOfLanes)*laneWidth
#get width Depends on the way width
widthUnCalibrated = typeData["tag"].get("width")
if widthUnCalibrated != None:
width = float(widthUnCalibrated)/12742/6*1.1
nodeNow=self.router.nodeLatLon(thisElement)
nodeNext=self.router.nodeLatLon(nextElement)
queueNodes.append([route[index], nodeNow[0],nodeNow[1],width])
if newDistance > 0.009:
newNodesBetween = self.router.getNumberOfNodesBetweenThose(7, nodeNow,nodeNext)
for nodeBet in newNodesBetween:
queueNodes.append([str(index)+str(nodeBet[0])+"975", nodeBet[1],nodeBet[2],width])
#/////////////////////////////////////////////////Shift the Nodes
queueNodesNewRight = []
for index, obj in enumerate(queueNodes):
lV = len(queueNodes)
if index < lV-1 :
nextElement = [queueNodes[index+1][1],queueNodes[index+1][2]]
nextElementId = queueNodes[index+1][0]
thisElement = [queueNodes[index][1],queueNodes[index][2]]
thisElementId = queueNodes[index][0]
newDistance = self.router.distance(thisElement,nextElement)
elif index == lV -1:
nextElement = [queueNodes[index][1],queueNodes[index][2]]
thisElement = [queueNodes[index][1],queueNodes[index][2]]
newNode = self.router.getLatLongWithNewWidth(queueNodes[index][3],newDistance, thisElement,nextElement)
queueNodesNewRight.append([queueNodes[index][0], newNode[0],newNode[1],queueNodes[index][3]])
return [queueNodesNewRight,sumPath]
else:
node1=self.node(lat1,long1)
node2=self.node(lat2,long2)
return [[],self.router.distance(node1,node2)]
def get_distance_between_two_addresses(self, first_address,
second_address):
# check address in db
query_first = Address.select().where(Address.name == first_address)
query_second = Address.select().where(Address.name == second_address)
if query_first.exists() and query_second.exists():
query_distance = DistanceBetweenAddress.select().where(
DistanceBetweenAddress.address_id == query_first.get().id,
DistanceBetweenAddress.next_address_id ==
query_second.get().id)
if query_distance.exists():
return query_distance.get().distance
router = Router(self.calculate_method)
# Initialise it
coord_s_t = time.time()
first_lat, first_lng = (query_first.get().lat, query_first.get().lng) if query_first.exists() else \
self.get_coordinates_from_yandex(first_address)
second_lat, second_lng = (query_second.get().lat, query_second.get().lng) if query_second.exists() else \
self.get_coordinates_from_yandex(second_address)
coord_e_t = time.time()
start = router.findNode(first_lng,
first_lat) # Find start and end nodes
end = router.findNode(second_lng, second_lat)
rout_s_t = time.time()
status, route = router.doRoute(
start, end) # Find the route - a list of OSM nodes
route_e_t = time.time()
if status == 'success':
routeLatLons = list(map(router.nodeLatLon,
route)) # Get actual route coordinates
total_distance = 0
# calculate total distance from route coordinates
for index in range(1, len(routeLatLons)):
total_distance += geopy.distance.vincenty(
routeLatLons[index - 1], routeLatLons[index]).km
else:
total_distance = 0
# это случается, когда 2 точки с одинаковым адресом, надо перепроверить
print(f'{route}')
first = query_first.get().id if query_first.exists(
) else Address.create(name=first_address, lat=first_lat, lng=first_lng)
second = query_second.get().id if query_second.exists(
) else Address.create(
name=second_address, lat=second_lat, lng=second_lng)
DistanceBetweenAddress.bulk_create([
DistanceBetweenAddress(address_id=first,
next_address_id=second,
distance=total_distance),
DistanceBetweenAddress(address_id=second,
next_address_id=first,
distance=total_distance)
])
print(f'COORDINATES TIME = {coord_e_t - coord_s_t} sec')
print(f'ROUTE TIME = {route_e_t - rout_s_t} sec')
print(total_distance, 'Total distance ==============================')
return total_distance