def replanRoutes(self):
"""
Checks the replan conditions and recalculate drivers' routes, if needed
"""
for d in self.drivers.getDriverList():
# does not try replanning if it is disabled, if drv is not on trip
# or if driver just exited a link
if not d.canReplan() or not d.onTrip() or d.changedLink():
continue
td = d.currentTravelTime()
remaining = d.remainingTripLinks()
#print remaining
if td + sum([self.knownTT[d.getId()][j] for j in remaining]) >\
d.acceptableDelay() * d.estimatedTT():
print d.getId(), 'will replan'
route = dijkstra(
self.roadNetwork,
self.roadNetwork.getEdge(d.currentEdge()),
self.roadNetwork.getEdge(d.getDestination()),
lambda edge: self.knownTT[d.getId()][edge.getID()] #need to make sure lambda is OK
)
#need to add the vehicles before, right?
edges = [edge.getID().encode('utf-8') for edge in route]
#print 'recalc.ed route:', edges
d.updateETT(edges, self.knownTT[d.getId()])
#update ESTIMATED TT
d.setRoute(d.traversedTripLinks() + edges)
print d.getId(), '\'s new route ', d.getRoute()
def __addRoutes(self, emitters):
"""Adds routes to the simulation for each edge found in the emitters' list
"""
routes = {}
for e in emitters:
#uses edgeId as key to ensure that no route will be inserted twice
#if emitter is for only one edge, uses this edge as the route
#otherwise, get the route via Dijkstra
if not hasattr(e, "arrivalEdge"):
routes[e.getRouteId()] = [e.departEdge]
else:
#tests whether a network was parsed
if self.network is None:
raise Exception("No network file supplied. Use -n or --net-file to inform the network file")
#obtains the departure and arrival edges from traci and uses them to route
dijkstraRoute = dijkstra(
self.network,
self.network.getEdge(e.departEdge),
self.network.getEdge(e.arrivalEdge), None, True
)
routes[e.getRouteId()] = [edge.getID().encode('utf-8') for edge in dijkstraRoute]
#now adds the one-edge routes via traci
for rid,route in routes.items():
traci.route.add(rid,route)
def is_optimistic(h, trans, goal):
"""Checks if heuristic is optimistic.
Function that performs optimistic check on given heuristic function.
Heuristic is optimistic if it never overestimates the real cost from the
current state to the end state. Real costs are calculated using dijkstra
algorithm. Also prints out the check and errors if optimistic property is
violated.
Args:
h: Heuristic function that is being checked.
trans: A dictionary containing list of possible transitions and their
costs for each key that represents the state name.
goal: List of goal states represented by string name of the state.
Returns:
Boolean indicating whether the heuristic is optimistic or not.
"""
print('Checking if heuristic is optimistic.')
costs = dijkstra(goal, flip_transitions(trans))
errors = []
for s in trans:
hs = h(s)
if hs > costs[s]:
errors.append((s, hs, costs[s]))
print_optimistic_check(errors)
return not errors
def act(self):
'''
Must be called at each timestep. Stores the drivers that departed for
writing them later in an output file
'''
thisTs = traci.simulation.getCurrentTime() / 1000
#does nothing if we have more vehicles than the desired number
if len(traci.vehicle.getIDList()) > self._num_veh:
return
for vehId in traci.simulation.getArrivedIDList():
if self._exclude_prefix is not None and self._exclude_prefix in vehId:
continue
(orig_taz, dest_taz) = self._od_matrix.select_od_taz()
orig_edg = self._road_net.getEdge(orig_taz.select_source()['id'])
dest_edg = self._road_net.getEdge(dest_taz.select_sink()['id'])
theRoute = search.dijkstra(self._road_net, orig_edg, dest_edg,
None, True)
#tries again if dest is not reachable from orig
if theRoute is None:
continue
edges = [edge.getID().encode('utf-8') for edge in theRoute]
vehId = str(thisTs) + '-' + vehId
traci.route.add(vehId, edges)
traci.vehicle.add(vehId, vehId, traci.vehicle.DEPART_NOW, 5.10, 0)
def calculateRoutes(self, tripNo):
'''
Calculates routes and adds vehicles in SUMO for each driver.
Also, records the links used by the self-interested fleet
'''
currentTs = traci.simulation.getCurrentTime() / 1000
self.fleetLinks = {} #resets the list of links used by the fleet
for d in self.drivers.getDriverList():
if d.isFromFleet():
self.fleetLinks[d.fleetID()] = []
#print '%s fleet ID is %s' % (d.getId(), d.fleetID())
#print 'Fleet links init: %s' % self.fleetLinks
for d in self.drivers.getDriverList():
weight_function = lambda edge: self.knownTT[d.getId()][edge.getID(
)]
if d.isFromFleet() and self.mal_strategy == 'shortest-path':
weight_function = lambda edge: 1.0 / edge.getLaneNumber()
routeID = d.getId() + '_' + str(
tripNo
) #cannot use d.getTraciId() because it isn't updated yet
try:
route = dijkstra(
self.roadNetwork,
self.roadNetwork.getEdge(d.getOrigin()),
self.roadNetwork.getEdge(d.getDestination()),
weight_function #need to make sure lambda is OK
)
except KeyError as k:
self.logger.error('Tried to route on non-existing edge:' +
str(k))
self.logger.info('Exiting on error...')
traci.close()
exit()
#need to add the vehicles before, right?
edges = [edge.getID().encode('utf-8') for edge in route]
# if d.isFromFleet():
# print d.getId(), edges, [weight_function(self.roadNetwork.getEdge(e)) for e in edges]
traci.route.add(routeID, edges)
#traci.vehicle.setRoute(d.getId(), edges)
traci.vehicle.add(d.getId() + '_' + str(tripNo), routeID,
d.getDepartTime() + currentTs,
StandardDriver.DEPART_POS, 0)
#adds the route links to the list of fleet links if the driver belongs to the fleet
if d.isFromFleet():
self.fleetLinks[d.fleetID()] += [l.getID() for l in route]
#removes duplicates
self.fleetLinks[d.fleetID()] = list(
set(self.fleetLinks[d.fleetID()]))
def act(self):
'''Must be called at each timestep. Stores the drivers that departed for
writing them later in an output file
'''
thisTs = traci.simulation.getCurrentTime() / 1000
for drvid in traci.simulation.getDepartedIDList():
if self.exclude is not None and self.exclude in drvid:
continue
drivers[drvid] = {
'depart': thisTs,
'route': traci.vehicle.getRoute(drvid)
}
#if len(traci.vehicle.getIDList()) >= options.numveh:
# continue
# print len(traci.vehicle.getIDList()), options.numveh
#numInserted = 0
for vehId in traci.simulation.getArrivedIDList():
if self.exclude is not None and self.exclude in vehId:
#print vehId, ' not replaced.'
continue
#assign new route to the vehicle
congestedRoute = True
numTries = 0
while (congestedRoute and numTries < 100): #try to distribute load
#if numTries > 0:
# print 'Congested! Retries: ', numTries
orig = random.choice(net._edges)
dest = random.choice(net._edges)
#print '%.2f\t%.2f' % (origOcc, destOcc)
#will consider unique weight
theRoute = dijkstra(net, orig, dest, lambda e: 300, True)
edges = [edge.getID().encode('utf-8') for edge in theRoute]
#TODO: parametrize min length
if len(edges) > 4: #at least 4 hops per trip
congestedRoute = False
for e in edges:
if traci.edge.getLastStepOccupancy(e) > 0.7:
congestedRoute = True
break #the inner loop
numTries += 1
vehId = str(thisTs) + '-' + vehId
traci.route.add(vehId, edges)
traci.vehicle.add(vehId, vehId, traci.vehicle.DEPART_NOW, 5.10, 0)
def on_depart(self):
"""Calculates the route upon departure."""
curr_edge = self.net.getEdge(
traci.vehicle.getRoadID(self.veh_id.encode('utf-8')))
route = dijkstra(self.net, curr_edge, self.dest, self.evaluate_edge)
if route is None:
raise Exception(
'Cannot find a route for %s, from %s to %s.' %
(self.veh_id, curr_edge.getID(), self.dest.getID()))
edges = [edge.getID().encode('utf-8') for edge in route]
traci.vehicle.setRoute(self.veh_id.encode('utf-8'), edges)
log.debug('Calculated route for vehicle %s.', self.veh_id)
def main():
f = open("p1_graph.txt", "r")
Lines = f.readlines()
f.close()
vlist, source, dest = readFile(Lines)
print("Path from ", source, " to ", dest)
# uninformed search: dijkstra
start = timer()
path_d = dijkstra(vlist, source, dest)
end = timer()
d_time = end - start
print("\nDijkstra Algorithm")
print("Path Distance: ", path_d, "\nTime Taken: ", d_time)
# informed search: a* heuristic search
start = timer()
path_h = a_heuristic(vlist, source, dest)
end = timer()
a_time = end - start
print("\nA* Heuristic Search Algorithm")
print("Path Distance: ", path_h, "\nTime Taken: ", a_time)
def act(self):
'''Must be called at each timestep. Stores the drivers that departed for
writing them later in an output file
'''
thisTs = traci.simulation.getCurrentTime() / 1000
for vehId in traci.simulation.getArrivedIDList():
if self.exclude is not None and self.exclude in vehId:
#print vehId, ' not replaced.'
continue
congestedRoute = True
numTries = 0
while (congestedRoute and numTries < 100): #try to distribute load
#if numTries > 0:
# print 'Congested! Retries: ', numTries
orig = random.choice(self.roadNetwork._edges)
dest = random.choice(self.roadNetwork._edges)
#print '%.2f\t%.2f' % (origOcc, destOcc)
#will consider unique weight
theRoute = dijkstra(self.roadNetwork, orig, dest, lambda e: 300, True)
edges = [edge.getID().encode('utf-8') for edge in theRoute]
#TODO: parametrize min length
congestedRoute = False
for e in edges:
if traci.edge.getLastStepOccupancy(e) > 0.7:
congestedRoute = True
break #the inner loop
numTries += 1
vehId = str(thisTs) + '-' + vehId
traci.route.add(vehId, edges)
traci.vehicle.add(vehId, vehId, traci.vehicle.DEPART_NOW, 5.10, 0)
def calculateRoutes(self,tripNo):
"""
Calculates routes and adds vehicles in SUMO for each driver.
Also, records the links used by the self-interested fleet
"""
currentTs = traci.simulation.getCurrentTime() / 1000
self.fleetLinks = [] #resets the list of links used by the fleet
for d in self.drivers.getDriverList():
routeID = d.getId() + '_' + str(tripNo)
try:
route = dijkstra(
self.roadNetwork,
self.roadNetwork.getEdge(d.getOrigin()),
self.roadNetwork.getEdge(d.getDestination()),
lambda edge: self.knownTT[d.getId()][edge.getID()] #need to make sure lambda is OK
)
except KeyError as k:
self.logger.error('Tried to route on non-existing edge:' + str(k))
self.logger.info('Exiting...')
traci.close()
exit()
#need to add the vehicles before, right?
edges = [edge.getID().encode('utf-8') for edge in route]
traci.route.add(routeID, edges)
#traci.vehicle.setRoute(d.getId(), edges)
traci.vehicle.add(
d.getId() + '_' + str(tripNo), routeID, d.getDepartTime() + currentTs,
StandardDriver.DEPART_POS, 0
)
self.logger.info(d.getId() + "'s route: [" + ", ".join(edges) + ']')
#adds the route links to the list of fleet links if the driver belongs to the fleet
if d.isFromFleet():
self.fleetLinks += [l.getID() for l in route]
#removes duplicates
self.fleetLinks = list(set(self.fleetLinks))
def act(self):
'''
Must be called every timestep (or in regular intervals of time)
to ensure that the load on the network will be steady
'''
if self.is_steady_duration_finished():
print 'Warning: steady duration of network has finished'
return
#saves data of vehicles that just departed
thisTs = traci.simulation.getCurrentTime() / 1000
for drvid in traci.simulation.getDepartedIDList():
if self._exclude_prefix is not None and self._exclude_prefix in drvid:
continue
self._launched_vehicles[drvid] = {
'depart': thisTs,
'route': traci.vehicle.getRoute(drvid)
}
#counts the vehicles without the exclude prefix that are in the network
num_veh = 0
for veh_id in traci.vehicle.getIDList():
if self._exclude_prefix is None or self._exclude_prefix not in veh_id:
num_veh += 1
inserted_this_ts = 0
if self._count_complete_trips == -1 and num_veh >= self._num_veh:
self._count_complete_trips = traci.simulation.getCurrentTime() / 1000
self._steady_timesteps = 0
if self._count_complete_trips > 0:
self._started_trips += traci.simulation.getDepartedIDList()
self._steady_timesteps += 1
for arvd in traci.simulation.getArrivedIDList():
if arvd in self._started_trips:
self._started_trips.remove(arvd)
self._complete_trips += 1
self._complete_trips_per_ts.append(
((traci.simulation.getCurrentTime() / 1000), self._complete_trips)
)
#inserts vehicles until the maximum number is reached
#or the maximum insertions per timestep is reached
while num_veh < self._num_veh:
if self._max_per_action != 0 and inserted_this_ts >= self._max_per_action:
break
(orig_taz, dest_taz) = self._od_matrix.select_od_taz()
orig_edg = self._road_net.getEdge(orig_taz.select_source()['id'])
dest_edg = self._road_net.getEdge(dest_taz.select_sink()['id'])
theRoute = search.dijkstra(
self._road_net,
orig_edg,
dest_edg, None, True
)
#tries again if dest is not reachable from orig
if theRoute is None:
continue
edges = [edge.getID().encode('utf-8') for edge in theRoute]
veh_id = self._aux_prefix + str(self._insertions)
traci.route.add(veh_id, edges)
traci.vehicle.add(veh_id, veh_id,
traci.vehicle.DEPART_NOW, 5.10, 13)
self._insertions += 1
inserted_this_ts += 1
num_veh += 1
def act(self):
thisTs = traci.simulation.getCurrentTime() / 1000
num_veh = 0
for veh_id in traci.vehicle.getIDList():
if self._exclude_prefix is None or self._exclude_prefix not in veh_id:
num_veh += 1
inserted_this_ts = 0
if self._count_complete_trips == -1 and num_veh >= self._num_veh:
self._count_complete_trips = traci.simulation.getCurrentTime() / 1000
self._steady_timesteps = 0
if self._count_complete_trips > 0:
self._started_trips += traci.simulation.getDepartedIDList()
self._steady_timesteps += 1
for drvid in traci.simulation.getDepartedIDList():
if self._exclude_prefix is not None and self._exclude_prefix in drvid:
continue
self._launched_vehicles[drvid] = {
'depart': thisTs,
'route': traci.vehicle.getRoute(drvid)
}
#inserts vehicles until the maximum number is reached
#or the maximum insertions per timestep is reached
while num_veh < self._num_veh:
if self._max_per_action != 0 and inserted_this_ts >= self._max_per_action:
break
congestedRoute = True
numTries = 0
while (congestedRoute and numTries < 100): #try to distribute load
#(orig_taz, dest_taz) = self._od_matrix.select_od_taz()
orig_edg = random.choice(self._road_net._edges)
dest_edg = random.choice(self._road_net._edges)
theRoute = search.dijkstra(
self._road_net,
orig_edg,
dest_edg, None, True
)
#tries again if dest is not reachable from orig
if theRoute is None:
continue
edges = [edge.getID().encode('utf-8') for edge in theRoute]
congestedRoute = False
for e in edges:
if traci.edge.getLastStepOccupancy(e) > 0.7:
congestedRoute = True
break #the inner loop
numTries += 1
veh_id = self._aux_prefix + str(self._insertions)
traci.route.add(veh_id, edges)
traci.vehicle.add(veh_id, veh_id,
traci.vehicle.DEPART_NOW, 0, 13)
self._insertions += 1
inserted_this_ts += 1
num_veh += 1
def replanRoutes(self):
'''
Checks the replan conditions and recalculate drivers' routes, if needed
'''
#print 'Replan procedure...'
for d in self.drivers.getDriverList():
# does not try replanning if it is disabled, if drv is not on trip,
# if driver just exited a link or if current edge is internal
if not d.canReplan() or not d.onTrip() or d.changedLink(
) or not d._isValidEdge(d.currentEdge()):
continue
'''
### new replan ###
if d.currentEdge() != d.getDestination() and d._isValidEdge(d.currentEdge()):
#current_route = traci.vehicle.getRoute(d.id)
route_until_dest = d.remainingTripLinks()#d.getRoute()[d.getRoute().index(d.currentEdge()):]
new_route = dijkstra(
self.roadNetwork,
self.roadNetwork.getEdge(d.currentEdge()),
self.roadNetwork.getEdge(d.getDestination()),
lambda edge: d.known_travel_time(edge.getID()) # self.knownTT[d.getId()][edge.getID()]
)
new_route_edg_ids = [e.getID() for e in new_route]
#print '%s (%d, %d)' % (d.getId(), self.routeCost(d, route_until_dest), self.routeCost(d, new_route_edg_ids))
if self.routeCost(d, route_until_dest) > d.acceptableDelay() * self.routeCost(d, new_route_edg_ids):
#print '%s replanned to %s ' % (d.getId(), new_route_edg_ids)
d.setRoute(d.traversedTripLinks() + [e.encode('utf-8') for e in new_route_edg_ids])
d.incReplanNumber()
#print 'Vehicle %s has new route. Old cost: %s. New cost %s.' % (d.id, self.route_cost(route_until_dest), self.route_cost(new_route_edg_ids))
'''
### old_replan ###
#td = d.currentTravelTime()
remaining = d.remainingTripLinks()
# if td + sum([d.known_travel_time(j) for j in remaining]) >\
actual_remaining_cost = sum(
[d.known_travel_time(j) for j in remaining])
estimt_remaining_cost = sum([d.estimatedTTs[j] for j in remaining])
if int(actual_remaining_cost) > int(
d.acceptableDelay() * estimt_remaining_cost):
#print d.getId(), 'will replan'
route = dijkstra(
self.roadNetwork,
self.roadNetwork.getEdge(d.currentEdge()),
self.roadNetwork.getEdge(d.getDestination()),
lambda edge: d.known_travel_time(edge.getID(
)) #need to make sure lambda is OK
)
#need to add the vehicles before, right?
edges = [edge.getID().encode('utf-8') for edge in route]
d.updateETT(edges, self.knownTT[d.getId()])
if (d.getRoute() != d.traversedTripLinks() + edges):
if sum([d.known_travel_time(l)
for l in d.getRoute()]) < sum([
d.known_travel_time(l)
for l in d.traversedTripLinks() + edges
]):
print 'WARNING:', d.getId(
), ' is about to change to a more expensive route!'
print 'oldroute:', d.getRoute(), '- cost:', [
d.known_travel_time(l) for l in d.getRoute()
]
print 'newroute:', d.traversedTripLinks(
) + edges, ' - cost:', [
d.known_travel_time(l)
for l in d.traversedTripLinks() + edges
]
print 'current:', d.currentEdge(
), ' - remaining:', remaining, ' - newpart:', [
l.getID() for l in route
]
print 'rmncost > delay * ett: %s > %s * %s -- %s --- %d > %d' %\
(int(sum([d.known_travel_time(j) for j in remaining])),
d.acceptableDelay(), int(sum([d.estimatedTTs[j] for j in remaining])),
int(actual_remaining_cost) > int(d.acceptableDelay() * estimt_remaining_cost),
int(actual_remaining_cost), int(d.acceptableDelay() * estimt_remaining_cost) )
# print d.getId() + ' has a new route!'
# print d.getRoute(), d.traversedTripLinks() + edges
# print [self.knownTT[d.getId()][l] for l in d.getRoute()], [self.knownTT[d.getId()][l] for l in d.traversedTripLinks() + edges]
# print sum([self.knownTT[d.getId()][l] for l in d.getRoute()]), sum([self.knownTT[d.getId()][l] for l in d.traversedTripLinks() + edges])
d.setRoute(d.traversedTripLinks() + edges)
d.incReplanNumber()
) #+1 prevents div / 0
#print needToEmitt
if needToEmitt > len(net._edges):
needToEmitt = len(net._edges)
emitted = 0
while emitted < needToEmitt:
congestedRoute = True
while (congestedRoute): #try to distribute load
orig = random.choice(net._edges)
dest = random.choice(net._edges)
#print '%.2f\t%.2f' % (origOcc, destOcc)
theRoute = dijkstra(net, orig, dest, None, True)
edges = [edge.getID().encode('utf-8') for edge in theRoute]
congestedRoute = False
for e in edges:
if traci.edge.getLastStepOccupancy(e) > 0.7:
congestedRoute = True
break #the inner loop
currTime = traci.simulation.getCurrentTime() / 1000
#print currTime, i
id = str(i) + '-' + str(emitted)
traci.route.add(id, edges)
traci.vehicle.add(id, id, currTime, 5.10,
edgecolor='k',
facecolor='g')) # start
ax.add_patch(
Rectangle((goal[1] - 0.5, goal[0] - 0.5),
1,
1,
edgecolor='k',
facecolor='r')) # goal
# Graph settings
plt.title(title)
plt.axis('scaled')
plt.gca().invert_yaxis()
if __name__ == "__main__":
# Load the map
grid, start, goal = load_map('map.csv')
# Search
bfs_path, bfs_steps = bfs(grid, start, goal)
dfs_path, dfs_steps = dfs(grid, start, goal)
dij_path, dij_steps = dijkstra(grid, start, goal)
aster_path, aster_steps = astar(grid, start, goal)
# Show result
draw_path(grid, bfs_path, 'BFS')
draw_path(grid, dfs_path, 'DFS')
draw_path(grid, dij_path, 'Dijkstra')
draw_path(grid, aster_path, 'A*')
plt.show()
print(cost, path)
cost, path = bidirectional_mm_search(G, s, g, heuristic=None)
print(cost, path)
if __name__ == '__main__':
nodes = list(G.nodes())
ids = np.random.randint(0, len(nodes))
idg = np.random.randint(0, len(nodes))
while ids == idg:
idg = np.random.randint(0, len(nodes))
s = nodes[ids]
g = nodes[idg]
print(s, g)
cost, path, visited = dijkstra(G, s, g)
print("visited = {}".format(len(visited)))
print(cost, path)
print()
# dijkstra cost only
t_start = time.time()
cost, path, _ = dijkstra_cost(G, s, g)
t_uni = time.time() - t_start
print(cost, t_uni)
# bidirectional
t_start = time.time()
cost, path, _ = bidirectional_dijkstra_cost(G, s, g)
t_bi = time.time() - t_start
print(cost, t_bi)