def findPath(self, edges, start, goal):
"""
Finds a path between start and goal using A*. The search is done in the
graph self.edges.
"""
openSet = []
heappush(openSet, (0, start))
path = {}
cost = {}
path[start] = 0
cost[start] = np.asarray([0, 0])
# A value that a real path should not have.
cost[goal] = np.asarray([float('Inf'), float('Inf')])
if start == goal:
cost[goal] = np.asarray([0, 0])
openSet = []
# As long as there are paths to be explored
while not (len(openSet) == 0):
current = heappop(openSet)[1]
# We found the goal, stop searching, we are done.
if current == goal:
break
# For all nodes connected to the one we are looking at for the
# moment.
for nextNode, speed, roadInt, _ in edges[current]:
# How fast you can go on a road matters on the type of the road
# It can be seen as a penalty for "smaller" roads.
speedDecrease = (1 - (float(roadInt) / 50))
#fromCoordinate = nodes[current]
#toCoordinate = nodes[nextNode]
roadLength = coordinate.measure(current, nextNode)
timeOnRoad = (roadLength /
(speedDecrease * (float(speed) * 1000 / 3600)))
newCost = cost[current] + [timeOnRoad, roadLength]
if nextNode not in cost or (newCost[0] < cost[nextNode][0]):
cost[nextNode] = newCost
weight = (newCost[0] + (roadInt ** 1) +
(heuristic(nextNode, goal) /
(float(self.standardSpeed) * 1000 / 3600)))
heappush(openSet, (weight, nextNode))
path[nextNode] = current
# Is there a shortest path
if cost[goal][0] is float('Inf'):
shortestpath = []
else:
shortestpath = reconstruct_path(path, start, goal)
return shortestpath, cost
def heuristic(node, goal):
"""
The heuristic used by A*. It measures the length between node and goal
in meters.
:param node a Coordinate object
:param goal a Coordinate object
:return the distance in meters
"""
return coordinate.measure(node, goal)
def findClosestBusStopFromCoordinates(self, lon, lat):
"""
Finds the closest bus stop to the position of (lon, lat).
:param lon: longitude
:param lat: latitude
:return: BusStop object
"""
stop = self.busStopList[0]
position = Coordinate(latitude=lat, longitude=lon)
dist = coordinate.measure(stop, position)
for _stop in self.busStopList:
_dist = coordinate.measure(_stop, position)
if _dist < dist:
stop = _stop
dist = _dist
return stop
def findBusStopsFromCoordinates(self, lon, lat, distance):
"""
Find the bus stops to the position of (lon, lat) and that is in the
radius of distance.
:param lon: longitude float
:param lat: latitude float
:param distance: meters float
:return: list of tuples [(name, coordinates, distance)]
"""
position = Coordinate(longitude=lon, latitude=lat)
busStops = []
for _stop in self.busStopList:
_dist = coordinate.measure(_stop, position)
if _dist <= distance:
busStops.append((_stop.name, _stop.coordinates, _dist))
if not busStops:
_closest = self.findClosestBusStopFromCoordinates(lon, lat)
_cdist = coordinate.measure(_closest, position)
busStops.append((_closest.name, _closest.coordinates, _cdist))
return busStops
def heuristic(self, node, goal):
return coordinate.measure(node, goal)
def findPath(self, edges, startBusStop, goalBusStop):
"""
A*, with the added search in the self.graph to speed up the process
of creating the graph.
"""
start = startBusStop.coordinates
goal = goalBusStop.coordinates
standardSpeed = 50
openSet = []
heappush(openSet, (0, start))
path = {}
cost = {}
# A value that a real path does not have.
cost[goal] = np.asarray([float('Inf'), float('Inf')])
path[start] = 0
cost[start] = np.asarray([0, 0])
# As long as there are paths to be explored
while not (len(openSet) == 0):
current = heappop(openSet)[1]
# We found the goal, stop searching, we are done.
if current == goal:
break
if current in self.busStopDict and self.busStopDict[current] in self.graph:
if self.busStopDict[goal] in self.graph[self.busStopDict[current]]:
openSet = []
if not self.graph[self.busStopDict[current]][self.busStopDict[goal]][0]:
sub_station = self.graph[self.busStopDict[current]][goalBusStop]
sub_station = sub_station[1]
sub_path = self.graph[self.busStopDict[current]][sub_station[0]]
sub_path = sub_path[1]
else:
sub_path = self.graph[self.busStopDict[current]][self.busStopDict[goal]][1]
previous_node = sub_path[0]
for node in sub_path:
if node not in path:
path[node] = previous_node
newCost = cost[previous_node] + [1, 1]
cost[node] = newCost
previous_node = node
heappush(openSet, (0, previous_node))
continue
# For all nodes connected to the one we are looking at for the
# moment.
for nextCoord, speed, roadInt, _ in edges[current]:
# How fast you can go on a road matters on the type of the road
# It can be seen as a penalty for "smaller" roads.
speedDecrease = (1 - (float(roadInt) / 50))
roadLength = coordinate.measure(current, nextCoord)
timeOnRoad = (roadLength /
(speedDecrease * (float(speed) * 1000 / 3600)))
newCost = cost[current] + [timeOnRoad, roadLength]
if nextCoord not in cost or newCost[0] < cost[nextCoord][0]:
cost[nextCoord] = newCost
weight = (newCost[0] + (roadInt ** 2) +
(self.heuristic(nextCoord, goal)*2 /
(float(standardSpeed) * 1000 / 3600)))
heappush(openSet, (weight, nextCoord))
path[nextCoord] = current
_path = aStar.reconstruct_path(path, start, goal)
_keys = []
for k in cost:
if k not in _path:
_keys.append(k)
for k in _keys:
cost.pop(k)
return _path, self.reconstruct_cost(_path, cost)
def heuristic(self, node, goal):
return coordinate.measure(node, goal)
def findPath(self, edges, startBusStop, goalBusStop):
"""
A*, with the added search in the self.graph to speed up the process
of creating the graph.
"""
start = startBusStop.coordinates
goal = goalBusStop.coordinates
standardSpeed = 50
openSet = []
heappush(openSet, (0, start))
path = {}
cost = {}
# A value that a real path does not have.
cost[goal] = np.asarray([float('Inf'), float('Inf')])
path[start] = 0
cost[start] = np.asarray([0, 0])
# As long as there are paths to be explored
while not (len(openSet) == 0):
current = heappop(openSet)[1]
# We found the goal, stop searching, we are done.
if current == goal:
break
if current in self.busStopDict and self.busStopDict[
current] in self.graph:
if self.busStopDict[goal] in self.graph[
self.busStopDict[current]]:
openSet = []
if not self.graph[self.busStopDict[current]][
self.busStopDict[goal]][0]:
sub_station = self.graph[
self.busStopDict[current]][goalBusStop]
sub_station = sub_station[1]
sub_path = self.graph[self.busStopDict[current]][
sub_station[0]]
sub_path = sub_path[1]
else:
sub_path = self.graph[self.busStopDict[current]][
self.busStopDict[goal]][1]
previous_node = sub_path[0]
for node in sub_path:
if node not in path:
path[node] = previous_node
newCost = cost[previous_node] + [1, 1]
cost[node] = newCost
previous_node = node
heappush(openSet, (0, previous_node))
continue
# For all nodes connected to the one we are looking at for the
# moment.
for nextCoord, speed, roadInt, _ in edges[current]:
# How fast you can go on a road matters on the type of the road
# It can be seen as a penalty for "smaller" roads.
speedDecrease = (1 - (float(roadInt) / 50))
roadLength = coordinate.measure(current, nextCoord)
timeOnRoad = (roadLength / (speedDecrease *
(float(speed) * 1000 / 3600)))
newCost = cost[current] + [timeOnRoad, roadLength]
if nextCoord not in cost or newCost[0] < cost[nextCoord][0]:
cost[nextCoord] = newCost
weight = (newCost[0] + (roadInt**2) +
(self.heuristic(nextCoord, goal) * 2 /
(float(standardSpeed) * 1000 / 3600)))
heappush(openSet, (weight, nextCoord))
path[nextCoord] = current
_path = aStar.reconstruct_path(path, start, goal)
_keys = []
for k in cost:
if k not in _path:
_keys.append(k)
for k in _keys:
cost.pop(k)
return _path, self.reconstruct_cost(_path, cost)
def pathLenght(path):
l = 0
for n in range(len(path) - 2):
l += coordinate.measure(path[n], path[n+1])
return l
