googlemapsastar/convertJSON.py at master · vraj152/googlemapsastar · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
import xmltodict
from haversine import haversine
import time
import numpy as np
from sklearn.neighbors import KDTree

s = time.time()
doc = {}
with open('data/map.graphml') as fd:
    doc = xmltodict.parse(fd.read())
print(time.time()-s)

def getLatLon(OSMId):
    lat, lon = 0, 0
    nodes = doc['graphml']['graph']['node']
    for eachNode in range(len(nodes)):
        if(nodes[eachNode]["@id"] == str(OSMId)):
            lat = float(nodes[eachNode]["data"][0]["#text"])
            lon = float(nodes[eachNode]["data"][1]["#text"])
    return (lat, lon)

def getOSMId(lat, lon):
    OSMId = 0
    nodes = doc['graphml']['graph']['node']
    for eachNode in range(len(nodes)):
        if(nodes[eachNode]["data"][0]["#text"]==str(lat)):
            if(nodes[eachNode]["data"][1]["#text"]==str(lon)):
                OSMId = nodes[eachNode]["data"][2]["#text"]

    return OSMId

def calculateHeuristic(curr,destination):
    return (haversine(curr,destination))

def getNeighbours(OSMId, destinationLetLon):
    neighbourDict = {}
    tempList = []
    edges = doc['graphml']['graph']['edge']
    for eachEdge in range(len(edges)):
        if(edges[eachEdge]["@source"]==str(OSMId)):
            temp_nbr = {}

            neighbourCost = 0
            neighbourId = edges[eachEdge]["@target"]
            neighbourLatLon = getLatLon(neighbourId)

            dataPoints = edges[eachEdge]["data"]
            for eachData in range(len(dataPoints)):
                if(dataPoints[eachData]["@key"]=="d12"):
                    neighbourCost = dataPoints[eachData]["#text"]

            neighborHeuristic = calculateHeuristic(neighbourLatLon, destinationLetLon)

            temp_nbr[neighbourId] = [neighbourLatLon, neighbourCost, neighborHeuristic]
            tempList.append(temp_nbr)

    neighbourDict[OSMId] = tempList
    return (neighbourDict)

def getNeighbourInfo(neighbourDict):
    neighbourId = 0
    neighbourHeuristic = 0
    neighbourCost = 0
    for key, value in neighbourDict.items():

        neighbourId = key
        neighbourHeuristic = float(value[2])
        neighbourCost = float(value[1])/1000
        neighbourLatLon = value[0]

    return neighbourId, neighbourHeuristic, neighbourCost, neighbourLatLon

#Argument should be tuple

def getKNN(pointLocation):
    nodes = doc["graphml"]["graph"]["node"]
    locations = []
    for eachNode in range(len(nodes)):
        locations.append((nodes[eachNode]["data"][0]["#text"],nodes[eachNode]["data"][1]["#text"]))

    locations_arr = np.asarray(locations, dtype=np.float32)
    point = np.asarray(pointLocation, dtype=np.float32)

    tree = KDTree(locations_arr, leaf_size=2)
    dist, ind = tree.query(point.reshape(1,-1), k=3)

    nearestNeighbourLoc = (float(locations[ind[0][0]][0]), float(locations[ind[0][0]][1]))

    return nearestNeighbourLoc

def getResponsePathDict(paths, source, destination):
    finalPath = []
    child = destination
    parent = ()
    cost = 0
    while(parent!=source):
        tempDict = {}
        cost = cost + float(paths[str(child)]["cost"])
        parent = paths[str(child)]["parent"]
        parent = tuple(float(x) for x in parent.strip('()').split(','))

        tempDict["lat"] = parent[0]
        tempDict["lng"] = parent[1]

        finalPath.append(tempDict)
        child = parent

    return finalPath, cost