def pathfind_to(self, target_location):
print nx.astar_path(self.map, self.location, target_location)[1:]
try:
return nx.astar_path(self.map, self.location, target_location)[1:]
except nx.exception.NetworkXNoPath:
return None
def heuristic(isDropNode, package, state):
driver = state.getVehicleList()
#a backwards array of the capacity to award points
points = [i for i in range(0, driver.getCapacity())]
points.reverse()
#drop off
if isDropNode:
star = len(nx.astar_path(Problem.graph, driver.getCurrLocation(), package.getNodeEndLocation()))
# don't penalize for dropping off
totalVal = star
#pick up
else:
star = len(nx.astar_path(Problem.graph, driver.getCurrLocation(), package.getNodeStartLocation()))
# the lower the difference between capacity and num packages you have, the greater the penalty
totalVal = star + points[(driver.getCapacity() - len(driver.getPackageList()))]
return totalVal
# def heuristic1(isDropNode, package, state):
# driver = state.getVehicleList()
# #drop off
# if isDropNode:
# star = len(nx.astar_path(Problem.graph, driver.getCurrLocation(), package.getNodeEndLocation()))
#
# #pick up
# else:
# star = len(nx.astar_path(Problem.graph, driver.getCurrLocation(), package.getNodeStartLocation()))
#
# return totalVal
def LabelFeature(self, graph):
# for each graph
# pick a random source and a random target
# run each of the networkx src tgt shortest path algorithms one by one
# time how long they each take
# repeat for N different srcs/tgts
# find the average time for each algorithm
# make the label for that graph the one with the shortest time
# feature key: 0 = dijkstra, 1 = bidijkstra 2 = astar
numIters = 10
n = networkx.number_of_nodes(graph)
dijkstraTimes = np.zeros(numIters)
biDijkstraTimes = np.zeros(numIters)
aStarTimes = np.zeros(numIters)
for i in xrange(numIters):
# pick a random source and target
src = np.random.randint(0, n) + 1
tgt = np.random.randint(0, n) + 1
while tgt == src:
tgt = np.random.randint(0, n) + 1
dijkstraTime = time.time()
try:
networkx.dijkstra_path(graph, src, tgt)
except:
# no path found
i -= 1
continue
dijkstraTime = time.time() - dijkstraTime
dijkstraTimes[i] = dijkstraTime
biDijkstraTime = time.time()
networkx.bidirectional_dijkstra(graph, src, tgt)
biDijkstraTime = time.time() - biDijkstraTime
biDijkstraTimes[i] = biDijkstraTime
aStarTime = time.time()
networkx.astar_path(graph, src, tgt)
aStarTime = time.time() - aStarTime
aStarTimes[i] = aStarTime
meanDijkstra = np.mean(dijkstraTimes)
meanBiDijkstra = np.mean(biDijkstraTimes)
meanAStar = np.mean(aStarTimes)
minTime = min(meanDijkstra, meanBiDijkstra, meanAStar)
if meanDijkstra == minTime:
label = 0
elif meanBiDijkstra == minTime:
label = 1
else:
label = 2
return label
def pickFarthestPackageAwayPlusDistanceToGarage(self, state, farthestReachablePackage):
driverHomeLocation = state.getVehicleList().getHomeLocation()
driverCurrLocation = state.getVehicleList().getCurrLocation()
#print("Driver's Curr location: {0}" .format(driverCurrLocation))
packageLocation = farthestReachablePackage.getNodeStartLocation()
#print("Package's location: {0}" .format(packageLocation))
driverToPackageDistance = len(nx.astar_path(Problem2.graph, driverCurrLocation, packageLocation))
packageToHomeDistance = len(nx.astar_path(Problem2.graph, packageLocation, driverHomeLocation))
# over lapping value so minus 1
projectedDistace = driverToPackageDistance + packageToHomeDistance - 1
return projectedDistace
def findPathScattered(self,source,destination): path = nx.astar_path(self.G,source,destination,self.heuristics1) if len(path)< MAX_NODE_LOOKUP: #print "recompute the destination as one of the random 4 corners " destination = self.corners[random.randrange(0,4)] #print "destination", destination path = nx.astar_path(self.G,source,destination,self.heuristics1) if(len(path)>1): return path[1] else: return path[0]
def find_bridges_between_VLs(img,
points,
centerOfVLs,
result=[],
numOfBridges=0):
src, des = points
graph = sknw.build_sknw(img)
forkPoints = findingNodes(img)
try:
path = nx.astar_path(graph, src, des)[1:-1]
coordinates = np.array([
coor for p in path for coor in get_node_coordinate(graph, p)
]).tolist()
forkPoints = np.array(findingNodes(segment_skeleton)).tolist()
forkPoints = [p[::-1] for p in forkPoints]
voronoi_points = [
coors for coors in coordinates if coors in forkPoints
]
result.append(voronoi_points)
circle_image(img, coordinates, "bridge", numOfBridges)
# print(voronoi_points)
for row, col in coordinates:
img[row][col] = 0
debug = convert_binary_to_normal_im(img)
cv2.imwrite(cwd + "/debug/debug.png", debug)
except:
return result
numOfBridges += 1
def path_find(graph, start, end):
path = []
try:
path.extend(nx.astar_path(graph, start, end, heuristic=dist))
except:
print("shit")
return path
def find_path(self, x1, y1, x2, y2):
g = self.map_to_graph()
path = nx.astar_path(g,
source=World.c_tostring(x1, y1),
target=World.c_tostring(x2, y2))
path_np = [np.fromstring(p, dtype=int) for p in path]
return [(p[0], p[1]) for p in path_np]
def test_astar_undirected2(self):
XG3 = nx.Graph()
edges = [(0, 1, 2), (1, 2, 12), (2, 3, 1), (3, 4, 5), (4, 5, 1),
(5, 0, 10)]
XG3.add_weighted_edges_from(edges)
assert nx.astar_path(XG3, 0, 3) == [0, 1, 2, 3]
assert nx.astar_path_length(XG3, 0, 3) == 15
def _get_connected_road_geometry(
self, matched_sequence: List[Candidate]
) -> Tuple[List[LineString], Union[defaultdict, OrderedDict]]:
# TODO IMPROVE FInd CONNECTED GEOMETRY ALGORITHM
connected_shape = list()
connected_info = OrderedDict()
visited = list()
# TODO ALTERNATIVES TO FOR LOOP
for previous, current in zip(matched_sequence, matched_sequence[1:]):
road_ids = self._road_ids_along_shortest_path(
nx.astar_path(
self.road_network.graph,
previous.road.property.u,
current.road.property.v,
)
)
for road in road_ids:
if int(road) not in visited:
connected_shape.append(shape(self.road_network.geometry(int(road))))
connected_info[int(road)] = self.road_network.entry(int(road))
visited.append(int(road))
return connected_shape, connected_info
def mysubgraph(_urlShpFile):
roadGraphd = nx.read_shp(_urlShpFile)
roadGraph = roadGraphd.to_undirected()
nodeList = roadGraph.nodes(data=True)
_nodeList = roadGraph.nodes(data=False)
nNode = len(nodeList)
pos = []
for i in xrange(nNode):
pos.append(nodeList[i][0])
pass
shpLayout = dict(zip(roadGraph, pos))
_node1 = random.choice(_nodeList)
_node2 = random.choice(_nodeList)
_path = nx.astar_path(roadGraph, _node1, _node2, None)
subG = nx.subgraph(roadGraph, _path)
plt.figure(1, figsize=(12, 12))
nx.draw_networkx(subG,
pos=shpLayout,
edgelist=None,
node_size=40,
node_color='r',
node_shape='d',
edgewidth=10,
edge_color='r',
with_labels=False)
plt.show()
def get_shortest_path(g, event, query_metadata, reaching_points):
"""
Finding shortest path
@args:
g: graph,
event: Ai2THOR's event metadata (returned on controller's step/reset),
query_metadata: Metadata of the query object,
reaching_points: the points that can be traversed by the robot in current env
@returns:
shortest path, agent_pos_node, query_pos_node
"""
assert (len(g.nodes) != 0)
agent_metadata = event.metadata['agent']
agent_pos_node = (agent_metadata['position']['x'],
agent_metadata['position']['z'])
query_pos = {
'x': float(query_metadata['position']['x']),
'z': float(query_metadata['position']['z'])
}
query_pos_node = get_closest_reachable_point(query_pos, reaching_points)
shortest_path = nx.astar_path(g, agent_pos_node, query_pos_node)
shortest_path_length = len(shortest_path)
return shortest_path_length, agent_pos_node, query_pos_node
def astar_path(node_pair):
source, target = node_pair
return nx.astar_path(source=source,
target=target,
G=graph,
heuristic=heuristic,
weight=weight)
def process_trips(G, trips, heuristic):
"""
Processes trips and plots them on the graph.
Parameters: (G, trips, heuristic)
G - networkx.graph()
trips - [trips]
heuristic - Callable
trip - (node, node)
node - (lat, lon)
"""
for trip in trips:
n1 = trip.start
n2 = trip.stop
print(f"\nGoing from {n1} to {n2}")
print("Calculating traffic...")
try:
path = nx.astar_path(G, n1, n2, heuristic)
print(
f"Cost of trip: {nx.astar_path_length(G, n1, n2, heuristic)}")
print(f"Nodes in trip: {len(path)}")
print_trip_info(n1, n2, path, G)
draw_path(path)
except:
print("Couldn't find a path")
def plan(self, state=None, alternative=False):
if self.graph.size() == 0:
LogicPlanner.logger.warn('LGP graph is not built yet! Plan nothing.')
return [], []
if state is None:
state = self.current_state
if not self.graph.has_node(state):
# check if current state could connected to feasibility graph
for act in self.ground_actions:
if LogicPlanner.applicable(state, act.positive_preconditions, act.negative_preconditions):
new_state = LogicPlanner.apply(state, act.add_effects, act.del_effects)
if new_state == state: # ignore same state transition
continue
self.graph.add_edge(state, new_state, action=act)
if not self.graph.has_node(state):
LogicPlanner.logger.warn('State: %s \n is not recognized in LGP graph. Could not find feasible path from this state to goal!.' % str(state))
return [], []
paths = []
act_seqs = []
for goal in self.goal_states:
try:
if alternative:
all_paths = [path for path in nx.all_shortest_paths(self.graph, source=state, target=goal)]
paths.extend(all_paths)
for path in all_paths:
act_seq = [self.graph[path[i]][path[i + 1]]['action'] for i in range(len(path) - 1)]
act_seqs.append(act_seq)
else:
path = nx.astar_path(self.graph, source=state, target=goal, heuristic=self.heuristic)
paths.append(path)
act_seq = [self.graph[path[i]][path[i + 1]]['action'] for i in range(len(path) - 1)]
act_seqs.append(act_seq)
except:
LogicPlanner.logger.warn(f'{goal} is not reachable from {state}.')
return paths, act_seqs
def get_random_route(self, t):
while True:
start, end = random.choice(list(self.snet.nodes)), random.choice(
list(self.snet.nodes))
paths = []
gen = nx.all_simple_paths(self.snet, source=start, target=end)
def dist(a, b):
(x1, y1) = a
(x2, y2) = b
# Pythagoras
return ((x1 - x2)**2 + (y1 - y2)**2)**0.5
paths.append(
nx.astar_path(self.snet,
source=start,
target=end,
heuristic=dist))
path = random.choice(paths)
base_speed = random.randint(70, 150) # kmph
transport = Merklin(
path=path,
net=self.snet,
start_time=t,
pause_p=self.pause_p,
net_node_delay_params=self.net_node_delay_params,
base_speed=base_speed,
pause_minmax=(0, 45),
net_mapping=self.snet_mapping,
transport_id=self.last_transport_id)
self.last_transport_id += 1
return transport
def gen_tab(self):
"""
Generate array of [string, fret]
"""
self.graph = self._gen_graph()
# run the A* algorithm
self.path = nx.astar_path(self.graph, 1, self.graph.number_of_nodes())
# remove start and end nodes
del self.path[0], self.path[-1]
strums = []
for n in self.path:
n = self.graph.node[n]
guitar_event = n['guitar_event']
score_event = n['score_event']
plucks = []
if isinstance(guitar_event, Pluck):
plucks.append((score_event.pname, score_event.oct, guitar_event))
else:
for pluck, note in zip(guitar_event.plucks, score_event.notes):
plucks.append((note.id, pluck))
strums.append(plucks)
fingering = np.empty([0,2], dtype=int)
for s in strums:
for ss in s:
fingering = np.append(fingering,[[ss[2].string+1, ss[2].fret+1]], axis=0)
return fingering
def do_something(p, m, g):
# print(p.__dict__)
# print_map(m)
# print(g.nodes())
goal_tile = None
if p.CarriedRessources < p.CarryingCapacity:
goal_tile = TileContent.Resource
else:
goal_tile = TileContent.House
player_pos = (p.Position.to_tuple()[0] - m[0][0].x,
p.Position.to_tuple()[1] - m[0][0].y)
dest_pos = bfs(m, g, player_pos, goal_tile)
if dest_pos is None:
goal_tile = TileContent.Wall
g = create_graph(m, True)
dest_pos = bfs(m, g, player_pos, goal_tile)
next_pos = nx.astar_path(g, player_pos, dest_pos)[1]
# print(player_pos)
# print(dest_pos)
# print(next_pos)
if next_pos == dest_pos:
if (goal_tile == TileContent.Resource):
return p.collect(
Point(next_pos[0] + m[0][0].x, next_pos[1] + m[0][0].y))
elif (goal_tile == TileContent.Wall):
return p.attack(
Point(next_pos[0] + m[0][0].x, next_pos[1] + m[0][0].y))
return p.move(Point(next_pos[0] + m[0][0].x, next_pos[1] + m[0][0].y))
def test_weights_planning():
plot_map()
start_pos = [ 2650, 2650 ]
L, c = grid_graph(start_pos, dim=10, width=1000)
filename = os.path.join(root, 'flash', 'fft2', 'processed', 'map.png')
img_data = imread(filename)
custom_labels = add_weights(L, img_data)
astar_path = nx.astar_path(L, (5, 5), (0, 4))
H = L.subgraph(astar_path)
h_pos = nx.get_node_attributes(H, 'pos')
pos = nx.get_node_attributes(L,'pos')
nx.draw(L, pos, node_size=5)
edge_weight=dict([((u,v,),int(d['weight'])) for u,v,d in L.edges(data=True)])
nx.draw_networkx_edge_labels(L,pos,edge_labels=edge_weight)
nx.draw_networkx_nodes(L,pos, node_size=0)
nx.draw_networkx_edges(L,pos)
nx.draw_networkx_labels(L,pos, labels=custom_labels)
nx.draw(H,h_pos, node_size=5, edge_color='r')
plt.show()
def find_bridges_between_VLs(img,
points,
centerOfVLs,
result=[],
numOfBridges=0):
src, des = points
graph = sknw.build_sknw(img)
# forkPoints = find_nodes(img)
# show_img(img)
try:
# print("Finding the path...")
path = nx.astar_path(graph, src, des)[1:-1]
coordinates = np.array([
coor for p in path for coor in get_node_coordinate(graph, p)
]).tolist()
forkPoints = np.array(find_nodes(img)).tolist()
forkPoints = [p[::-1] for p in forkPoints]
voronoi_points = [
coors for coors in coordinates if coors in forkPoints
]
# print(voronoi_points)
result.append(voronoi_points)
for row, col in coordinates:
img[row][col] = 0
debug = convert_binary_to_normal_im(img)
# cv2.imwrite("removed_nodes_{}.png".format(numOfBridges), debug)
except:
# print("Result: ", result)
return result
numOfBridges += 1
return find_bridges_between_VLs(img, [src, des], centerOfVLs, result,
numOfBridges)
def getAnticipatoryStigmergyRoute(self, veh_id):
route = traci.vehicle.getRoute(veh_id)
reroute = []
if traci.vehicle.getRoadID(veh_id) != "":
current_road_id = traci.vehicle.getRoadID(veh_id)
origin_node_id = self.sumo_net.getEdge(current_road_id).getToNode().getID().encode('utf-8')
else:
current_road_id = self.sumo_net.getEdge(route[0]).getID().encode('utf-8')
origin_node_id = self.sumo_net.getEdge(current_road_id).getFromNode().getID().encode('utf-8')
dest_node_id = self.sumo_net.getEdge(route[-1]).getToNode().getID().encode('utf-8')
if self.conf.dce == True:
if self.dce_stigmergy[origin_node_id]['anticipatory'] is None:
network = self.calcDCE(origin_node_id, self.anticipatory_stigmergy_network)
self.dce_stigmergy[origin_node_id]['anticipatory'] = network
else:
network = self.dce_stigmergy[origin_node_id]['anticipatory']
else:
network = self.anticipatory_stigmergy_network
if self.conf.real_net == True:
new_route = netutil.searchRouteFromNode(self.sumo_net, network, origin_node_id, dest_node_id)
else:
new_nodes = nx.astar_path(network, origin_node_id, dest_node_id, self.distFromNode)
new_route = netutil.nodes2Route(self.original_network, new_nodes)
reroute = route[:route.index(current_road_id)] + new_route
return route, reroute
def closestPackageWithHypotenuse(car, packages, city):
bestPackage = packages[1]
closestPackageDistance = math.inf
i = 0
while i < len(packages):
carPath = math.hypot(car.location[0] - packages[i].source[0],
car.location[1] - packages[i].source[1])
#carPath = (car.location[0] - packages[i].source[0])^2 + (car.location[0] - packages[i].source[0])^2
#carPath = math.sqrt(carPath)
#print(carPath)
if carPath < closestPackageDistance:
bestPackage = packages[i]
closestPackageDistance = carPath
i += 1
#print(bestPackage.source)
bestPath = nx.astar_path(city, car.location, bestPackage.source)
g2 = nx.Graph()
g2.add_nodes_from(city)
i = 1
while i < len(bestPath):
g2.add_edge(bestPath[i - 1], bestPath[i])
i += 1
draw(g2)
return bestPackage
def test_unorderable_nodes(self):
"""Tests that A* accomodates nodes that are not orderable.
For more information, see issue #554.
"""
# TODO In Python 3, instances of the `object` class are
# unorderable by default, so we wouldn't need to define our own
# class here, we could just instantiate an instance of the
# `object` class. However, we still support Python 2; when
# support for Python 2 is dropped, this test can be simplified
# by replacing `Unorderable()` by `object()`.
class Unorderable(object):
def __le__(self):
raise NotImplemented
def __ge__(self):
raise NotImplemented
# Create the cycle graph on four nodes, with nodes represented
# as (unorderable) Python objects.
nodes = [Unorderable() for n in range(4)]
G = nx.Graph()
G.add_edges_from(pairwise(nodes, cyclic=True))
path = nx.astar_path(G, nodes[0], nodes[2])
assert_equal(len(path), 3)
def find_shortest_path(graph):
"""
Take networkx graph and calculate the shortest path.
args:
graph - a networkx graph
returns:
List, that contains shortest path
"""
G1 = graph
G = create_graph2(G1.graph['labyrinth1'])
list_of_nodes = nx.nodes(G)
if len(list_of_nodes) == 1:
return list_of_nodes
for i in list_of_nodes:
if i.count("A0") == 1:
start_node = i
if G.node[i]['lastnode'] == 1:
end_node = i
length = nx.astar_path(G, start_node, end_node)
return length
def plot_paths(graph, start_pts_list, exit_pt, ows, settings):
"""
Provides an option to do pathfinding and plot results. Primarily useful for debugging,
"""
# This should by definition be impossible, since the floor @ exit exists
assert exit_pt in graph, "Specified exit point not in walkable path"
plt.imshow(ows, cmap="gray_r")
ax = plt.gca()
ax.autoscale(False)
for p_start in start_pts_list:
msg = "Specified starting point is not walkable; ignore soldier @ {} "
assert p_start in graph, msg.format(p_start)
path_to_exit = nx.astar_path(graph,
source=p_start,
target=exit_pt,
weight='weight',
heuristic=a_star_heuristic)
# Don't plot the final exit point; that's for internal bookkeeping. We care more about
# what edge the soldier stepped on right before exiting.
coord_lists = zip(*path_to_exit)
ax.plot(coord_lists[1], coord_lists[0], linewidth=3, alpha=0.3)
ax.plot(exit_pt[1], exit_pt[0], marker="^", color='r')
save_folder = os.path.join("results", settings["run_id"],
str(settings["voxel_size"]))
plt.savefig(os.path.join(save_folder, "troop_exit_paths.png"))
def test_astar_undirected3(self):
XG4 = nx.Graph()
edges = [(0, 1, 2), (1, 2, 2), (2, 3, 1), (3, 4, 1), (4, 5, 1),
(5, 6, 1), (6, 7, 1), (7, 0, 1)]
XG4.add_weighted_edges_from(edges)
assert nx.astar_path(XG4, 0, 2) == [0, 1, 2]
assert nx.astar_path_length(XG4, 0, 2) == 4
def make_corridors(self, level: 'Level') -> SquareStore:
def heuristic(pos1: tuple, pos2: tuple) -> Union[float, int]:
"""A* heuristic for corridor creation (Manhattan distance)."""
if any(level.locate(p) for p in (pos1, pos2)):
return inf # Avoid squares that contain something other than corridors
return abs(pos1[0] - pos2[0]) + abs(pos1[1] - pos2[1])
result = SquareStore()
graph = nx.grid_2d_graph(Position.SCREEN_W, Position.SCREEN_H)
# Sorts the rooms and then swaps them, in order to get a not-so-random dungeon
rooms = sorted(list(level.rooms), key= lambda r: r.top_left)
swaps = 0
while d(1, 4) > 1 and swaps < len(level.rooms):
a, b = sample(range(len(level.rooms)), 2)
rooms[a], rooms[b] = rooms[b], rooms[a]
swaps += 1
graph.remove_nodes_from(pos for r in rooms for pos in r)#.squares)
for r1, r2 in zip(rooms, rooms[1:]):
start, end = CorridorFactory._rnd_doorway(r1), CorridorFactory._rnd_doorway(r2)
del r1[start]
del r2[end]
for p in start, end:
graph.add_node(p)
graph.add_edges_from((p, n) for n in p.neighbors(False) if level.locate(n) is None)
result[position(*p)] = Square(SquareType.DOORWAY)
path = nx.astar_path(graph, start, end, heuristic)
result.update({position(*pos): Square(SquareType.CORRIDOR) for pos in path[1:-1]})
graph.remove_nodes_from((start, end)) # Avoid using doorways in next computations
return result
def _add_ground_edges(self):
nodes = sorted(list(node for node in self.G.nodes(data=True)
if node[0] not in ["Source", "Sink"]),
key=lambda x: x[1]['pos'][0])
for n in nodes:
# first element in tuple has string 'Airport_time'
# Second element in tuple has node data
n_name, n_data = n[0], n[1]
n_time = float(n_name.split('_')[1])
ground_nodes_n = sorted(
list(k for k in nodes if float(k[0].split('_')[1]) > n_time
and n_data['airport'] == k[1]['airport']),
key=lambda x: x[1]['pos'][0])
# ground_nodes_n.sort()
if ground_nodes_n:
m = ground_nodes_n[0]
# for m in ground_nodes_n:
path = None
m_name, m_data = m[0], m[1]
m_time = float(m_name.split('_')[1])
if not self.G.has_edge(*(n_name, m_name)):
try:
path = astar_path(self.G, n_name, m_name)
except NetworkXNoPath:
self._add_edge(n_name, n_data, n_time, m_name, m_data,
m_time)
if path and not all(
p.split('_')[0] == n_data['airport']
for p in path):
# if edge doesn't exist, add it
self._add_edge(n_name, n_data, n_time, m_name, m_data,
m_time)
class TestAStar:
def setUp(self):
self.XG=nx.DiGraph()
self.XG.add_edges_from([('s','u',{'weight':10}),
('s','x',{'weight':5}),
('u','v',{'weight':1}),
('u','x',{'weight':2}),
('v','y',{'weight':1}),
('x','u',{'weight':3}),
('x','v',{'weight':5}),
('x','y',{'weight':2}),
('y','s',{'weight':7}),
('y','v',{'weight':6})])
def test_random_graph(self):
def dist((x1, y1), (x2, y2)):
return ((x1 - x2) ** 2 + (y1 - y2) ** 2) ** 0.5
G = nx.Graph()
points = [(random(), random()) for _ in xrange(100)]
# Build a path from points[0] to points[-1] to be sure it exists
for p1, p2 in zip(points[:-1], points[1:]):
G.add_edge(p1, p2, weight=dist(p1, p2))
# Add other random edges
for _ in xrange(100):
p1, p2 = choice(points), choice(points)
G.add_edge(p1, p2, weight=dist(p1, p2))
path = nx.astar_path(G, points[0], points[-1], dist)
assert path == nx.dijkstra_path(G, points[0], points[-1])
def plotPath(self, startLocation, endLocation):
# TODO
# Make this more efficient
startX = startLocation.getX()
startY = startLocation.getY()
endX = endLocation.getX()
endY = endLocation.getY()
nodes = self.graph.nodes()
closestStartNode = nodes[0]
closestStartX = closestStartNode[0]
closestStartY = closestStartNode[1]
closestEndNode = nodes[0]
closestEndX = closestEndNode[0]
closestEndY = closestEndNode[1]
closestStartDist = self.getDistance(startX, startY, closestStartX, closestStartY)
closestEndDist = self.getDistance(endX, endY, closestEndX, closestEndY)
for node in nodes:
startDist = self.getDistance(node[0], node[1], closestStartX, closestStartY)
if startDist < closestStartDist:
closestStartNode = node
closestStartDist = startDist
endDist = self.getDistance(node[0], node[1], closestEndX, closestEndY)
if endDist < closestEndDist:
closestEndNode = node
closestEndDist = endDist
path = networkx.astar_path(self.graph, closestStartNode, closestEndNode, self.nodeDistance)
# TODO: need to smooth out path
# return a list of locations
return [location.Location(x,y) for (x,y) in path]
def shortest_path(source, next_sample, G):
"""
Computes shortest safe path from a source to the next state-action pair
the agent needs to sample
Parameters
----------
source: int
Staring node for the path
next_sample: (int, int)
Next state-action pair the agent needs to sample. First entry is the
number that indicates the state. Second entry indicates the action
G: networkx DiGraph
Graph that indicates the dynamics. It is linked to S matrix
Returns
-------
path: list
shortest safe path
"""
# Extract safe graph
safe_edges = [edge for edge in G.edges_iter(data=True) if edge[2]['safe']]
graph_safe = nx.DiGraph(safe_edges)
# Compute shortest path
target = next_sample[0]
action = next_sample[1]
path = nx.astar_path(graph_safe, source, target)
for _, next_node, data in graph_safe.out_edges(nbunch=target, data=True):
if data["action"] == action:
path = path + [next_node]
return path
def test_astar_w1(self):
G = nx.DiGraph()
G.add_edges_from([('s', 'u'), ('s', 'x'), ('u', 'v'), ('u', 'x'),
('v', 'y'), ('x', 'u'), ('x', 'w'), ('w', 'v'),
('x', 'y'), ('y', 's'), ('y', 'v')])
assert_equal(nx.astar_path(G, 's', 'v'), ['s', 'u', 'v'])
assert_equal(nx.astar_path_length(G, 's', 'v'), 2)
def test_unorderable_nodes(self):
"""Tests that A* accomodates nodes that are not orderable.
For more information, see issue #554.
"""
# TODO In Python 3, instances of the `object` class are
# unorderable by default, so we wouldn't need to define our own
# class here, we could just instantiate an instance of the
# `object` class. However, we still support Python 2; when
# support for Python 2 is dropped, this test can be simplified
# by replacing `Unorderable()` by `object()`.
class Unorderable(object):
def __le__(self):
raise NotImplemented
def __ge__(self):
raise NotImplemented
# Create the cycle graph on four nodes, with nodes represented
# as (unorderable) Python objects.
nodes = [Unorderable() for n in range(4)]
G = nx.Graph()
G.add_edges_from(pairwise(nodes, cyclic=True))
path = nx.astar_path(G, nodes[0], nodes[2])
assert_equal(len(path), 3)
def routepath(G,lat1,lon1,lat2,lon2,tolerance=0.02,factor=1000):
#print "G",G
startnode = getnode(G,lat1,lon1)[0]
# print "startnode:",startnode
endnode = getnode(G,lat2,lon2)[0]
# print "endnode:",endnode
###
# for n in G:
# print n
# print "path nodes:"
start_time=time.time()
pathnodes =networkx.astar_path(G, startnode,endnode)
duration=time.time()-start_time
print "pathing time:",duration
#print "pathnodes",pathnodes
path = zip([G.node[n]['data'].lat for n in pathnodes], [G.node[n]['data'].lon for n in pathnodes])
# print path
# fig, ax = plt.subplots()
# ax.plot([G.node[n]['data'].lat for n in pathnodes], [G.node[n]['data'].lon for n in pathnodes], '-')
# ax.scatter([G.node[n]['data'].lat for n in G], [G.node[n]['data'].lon for n in G], s=50)
# ax.set_aspect('equal')
# plt.show()
#print "path",path
polyline=PolylineCodec().encode(path)
# print polyline
return polyline#ordered list of nodes to be visited in order, forming a path
def test_astar_undirected3(self):
XG4 = nx.Graph()
edges = [(0, 1, 2), (1, 2, 2), (2, 3, 1), (3, 4, 1), (4, 5, 1),
(5, 6, 1), (6, 7, 1), (7, 0, 1)]
XG4.add_weighted_edges_from(edges)
assert_equal(nx.astar_path(XG4, 0, 2), [0, 1, 2])
assert_equal(nx.astar_path_length(XG4, 0, 2), 4)
def artist_path2(start_aid, end_aid):
start = time.time()
aids = nx.astar_path(G, start_aid, end_aid)
end = time.time()
path = get_path(aids)
print len(path['links']), 'path, calculated in', end - start, 'secs'
return path
def searchRouteFromNode(sumo_net, nx_net, origin_node_id, dest_node_id):
def distFromEdge(edge_A_id, edge_B_id):
s = sumo_net.getEdge(edge_A_id).getToNode().getCoord()
t = sumo_net.getEdge(edge_B_id).getToNode().getCoord()
return ((s[0] - t[0]) ** 2 + (s[1] - t[1]) ** 2) ** 0.5
new_route = []
min_travel_time = float('inf')
origin_node = sumo_net.getNode(origin_node_id)
dest_node = sumo_net.getNode(dest_node_id)
for origin_edge in origin_node.getOutgoing():
for dest_edge in dest_node.getIncoming():
origin_edge_id = origin_edge.getID().encode('utf-8')
dest_edge_id = dest_edge.getID().encode('utf-8')
if origin_edge_id == "-" + dest_edge_id or "-" + origin_edge_id == dest_edge_id:
continue
if not origin_edge_id in nx_net.nodes() or not dest_edge_id in nx_net.nodes():
continue
try:
candidate = nx.astar_path(nx_net, origin_edge_id, dest_edge_id, distFromEdge)
sum_travel_time = sum([freeFlowTravelTime(sumo_net, edge_id) for edge_id in candidate])
if min_travel_time > sum_travel_time:
min_travel_time = sum_travel_time
new_route = candidate
except nx.NetworkXNoPath:
continue
return new_route
def test_astar_w1(self):
G = nx.DiGraph()
G.add_edges_from([('s', 'u'), ('s', 'x'), ('u', 'v'), ('u', 'x'),
('v', 'y'), ('x', 'u'), ('x', 'w'), ('w', 'v'),
('x', 'y'), ('y', 's'), ('y', 'v')])
assert_equal(nx.astar_path(G, 's', 'v'), ['s', 'u', 'v'])
assert_equal(nx.astar_path_length(G, 's', 'v'), 2)
def search_match(graph, seekingList, offeringList, seeking_product, offering_product):
seekingIdxOccurrence = find_index_matches_in_seeking(seeking_product, seekingList)
if len(seekingIdxOccurrence) == 0:
return "noSeek"
offeringIdxOccurrence = find_index_matches_in_offering(offering_product, offeringList)
if len(offeringIdxOccurrence) == 0:
return "noHave"
paths = []
for idx_offer in offeringIdxOccurrence:
for idx_seek in seekingIdxOccurrence:
try:
paths.append(nx.astar_path(graph, offeringIdxOccurrence[0],
seekingIdxOccurrence[0]))
except nx.NetworkXNoPath:
pass
items_list = []
url_list = []
if len(paths) != 0:
min(paths)
for node in min(paths):
items_list.append(graph.node[node]["Offering"])
url_list.append(graph.node[node]["URL"])
return {"items": items_list, "link": url_list}
else:
return "noPath"
def test_astar_undirected2(self):
XG3 = nx.Graph()
edges = [(0, 1, 2), (1, 2, 12), (2, 3, 1), (3, 4, 5), (4, 5, 1),
(5, 0, 10)]
XG3.add_weighted_edges_from(edges)
assert_equal(nx.astar_path(XG3, 0, 3), [0, 1, 2, 3])
assert_equal(nx.astar_path_length(XG3, 0, 3), 15)
def plot_paths(graph, start_pts_list, exit_pt, ows, settings):
"""
Provides an option to do pathfinding and plot results. Primarily useful for debugging,
"""
# This should by definition be impossible, since the floor @ exit exists
assert exit_pt in graph, "Specified exit point not in walkable path"
plt.imshow(ows, cmap="gray_r")
ax = plt.gca()
ax.autoscale(False)
for p_start in start_pts_list:
msg = "Specified starting point is not walkable; ignore soldier @ {} "
assert p_start in graph, msg.format(p_start)
path_to_exit = nx.astar_path(graph,
source=p_start,
target=exit_pt,
weight='weight',
heuristic=a_star_heuristic)
# Don't plot the final exit point; that's for internal bookkeeping. We care more about
# what edge the soldier stepped on right before exiting.
coord_lists = zip(*path_to_exit)
ax.plot(coord_lists[1], coord_lists[0], linewidth=3, alpha=0.3)
ax.plot(exit_pt[1], exit_pt[0], marker="^", color='r')
save_folder = os.path.join("results", settings["run_id"], str(settings["voxel_size"]))
plt.savefig(os.path.join(save_folder, "troop_exit_paths.png"))
def plan_path(start_pos, goal_pos):
""" Actual path planneer that integrates local/global graphs and finds path
"""
#for now, just hard code this
filename = os.path.join(root, 'flash', 'fft2', 'processed', 'map.png')
img_data = imread(filename)
#make local unconstrained motion graph
#create unconstrained local graph at the start
start_local_graph, start_center = grid_graph(start_pos,
dim=LOCAL_GRAPH_DIM,
width=LOCAL_GRAPH_WIDTH)
add_weights(start_local_graph, img_data)
#create unconstrained local graph at the goal
goal_local_graph, goal_center = grid_graph(goal_pos,
dim=LOCAL_GRAPH_DIM,
width=LOCAL_GRAPH_WIDTH)
add_weights(goal_local_graph, img_data)
#make global graph based on waypoints
filename = os.path.join(root, 'flash', 'fft2',
'export', 'binaryData', '910.bin')
global_graph = graph_from_waypoints(filename)
#make kd-tree from the global graph
pos = nx.get_node_attributes(global_graph, 'pos')
#sorted by keys
d_x = OrderedDict(sorted(pos.items(), key=lambda t: t[0])).values()
c_x = numpy.array(d_x)
global_tree = scipy.spatial.cKDTree(c_x)
#stitch together unconstrained local with global
u_graph = stitch(start_local_graph, global_graph, global_tree, 100, start_center, 'S-')
u_graph = stitch(goal_local_graph, u_graph, global_tree, 100, goal_center, 'G-')
astar_path = nx.astar_path(u_graph, 'S-' + str(start_center), 'G-' + str(goal_center))
#rename node labels from '0' to final node, i.e. '35'
count = 0
mapping = {}
for node in astar_path:
mapping[node] = count
count += 1
planned_path = u_graph.subgraph(astar_path)
planned_path = nx.relabel_nodes(planned_path, mapping)
return planned_path
def test_astar_undirected(self):
GG = self.XG.to_undirected()
# make sure we get lower weight
# to_undirected might choose either edge with weight 2 or weight 3
GG['u']['x']['weight'] = 2
GG['y']['v']['weight'] = 2
assert nx.astar_path(GG, 's', 'v') == ['s', 'x', 'u', 'v']
assert nx.astar_path_length(GG, 's', 'v') == 8
def test_astar_undirected(self):
GG = self.XG.to_undirected()
# make sure we get lower weight
# to_undirected might choose either edge with weight 2 or weight 3
GG["u"]["x"]["weight"] = 2
GG["y"]["v"]["weight"] = 2
assert nx.astar_path(GG, "s", "v") == ["s", "x", "u", "v"]
assert nx.astar_path_length(GG, "s", "v") == 8
def astar(start,terminal):
cost = 0
path = nx.astar_path(G, start, terminal)
cost = nx.astar_path_length(G, start, terminal)
print("A Star Shortest Path: ")
for i in range(len(path)-1):
print(path[i],"->",path[i+1])
print("Astar Shortest Path Cost: ",cost)
def test_astar_undirected(self):
GG = self.XG.to_undirected()
# make sure we get lower weight
# to_undirected might choose either edge with weight 2 or weight 3
GG['u']['x']['weight'] = 2
GG['y']['v']['weight'] = 2
assert_equal(nx.astar_path(GG, 's', 'v'), ['s', 'x', 'u', 'v'])
assert_equal(nx.astar_path_length(GG, 's', 'v'), 8)
def main():
mx = 800
my = 400
gx = 10
gy = 5
G = nx.grid_2d_graph(gx, gy)
def init_data(g, n):
g.node[n] = make_node(8)
call_nodes(G, init_data)
random_blocks(G, bias=3, weight=random.randint(300, 500))
random_blocks(G, bias=9, weight=300)
start = (0, 0)
end = (gx-1, gy-1)
dwg = draw_graph(G, mx, my, gx, gy, 'graph')
dwg.save()
cols = []
for c in xrange(gx-1):
cols.append([(c, r) for r in xrange(gy-1)])
make_bottle_necks(G, cols)
main_path = list(iterate_pairs(nx.astar_path(G, start, end, dist_h)))
avoid_G = G.copy()
set_nodes_weight(avoid_G, main_path, 9999999)
_sp = Scaler(mx, my, gx, gy)
a = random_point_path(gx, gy, main_path, False)
b = random_point_path(gx, gy, main_path, True)
try:
path1 = iterate_pairs(nx.astar_path(avoid_G, a, b, dist_h))
_draw_edges(dwg, path1, _sp, style=PATH_STYLE)
path2 = iterate_pairs(nx.astar_path(avoid_G, b, end, dist_h))
PATH_STYLE.update([('stroke', 'blue')])
_draw_edges(dwg, path2, _sp, style=PATH_STYLE)
except nx.exception.NetworkXNoPath:
logging.error('unable to find path {}'.format([start, a, end]))
PATH_STYLE.update([('stroke', 'red')])
_draw_edges(dwg, main_path, _sp, style=PATH_STYLE)
dwg.save()
def compute_path(self, target_location):
try:
return networkx.astar_path(self.npc.realm.path_graph, self.npc.location,
target_location, manhattan)
except KeyError:
raise IncompletePathGraphError
except networkx.NetworkXNoPath:
raise PassabilityError
def deliverAllPackagesAllCars(carList, packageList, city, garage):
allPackageCount = len(packageList)
currentCar = None
currentPackage = None
carPackageCount = 0
while (allPackageCount > 0):
carPackageCount = 0
allPackageCount = 0
for car in carList:
allPackageCount += len(car.packageList)
carPackageCount += len(car.packageList)
allPackageCount += len(packageList)
if (allPackageCount <= 0):
for car in carList:
toGarage(car, garage, city)
return
if (len(packageList) > 0):
temp = shortestCarToPackagePath(carList, packageList, city)
currentCar = temp[0]
currentPackage = temp[1]
else:
temp3 = shortestCarToDestinationPath(carList, city)
currentCar = temp3[0]
currentPackage = temp3[1]
if (carPackageCount > 0):
temp2 = shortestCarToDestinationPath(carList, city)
lengthToBestDestination = len(
nx.astar_path(city, temp2[0].location, temp2[1].destination))
lengthToBestPackage = len(
nx.astar_path(city, currentCar.location,
currentPackage.source))
if (lengthToBestDestination <= lengthToBestPackage
or currentPackage == None):
currentCar = temp2[0]
currentPackage = temp2[1]
if (currentPackage.pickedUp):
deliverPackage(currentCar, currentPackage, city)
else:
packageList.remove(currentPackage)
getPackage(currentCar, currentPackage, city)
def test_astar_undirected(self):
GG = self.XG.to_undirected()
# make sure we get lower weight
# to_undirected might choose either edge with weight 2 or weight 3
GG["u"]["x"]["weight"] = 2
GG["y"]["v"]["weight"] = 2
assert_equal(nx.astar_path(GG, "s", "v"), ["s", "x", "u", "v"])
assert_equal(nx.astar_path_length(GG, "s", "v"), 8)
def search_path(self, source, target):
""" Search for a path from ``source`` to ``target`` using A*"""
def metric(a, b):
if self.edge_exists(source, target):
return -1*self.gea(source, target, 'reward')
return 1000
path = nx.astar_path(self.G, source, target, heuristic=metric)
return path
def performAstar(self, idFrom, idTo):
print('performing astar')
try:
score = nx.astar_path(self.graph, idFrom, idTo, self.soup.LCA)
return score
except nx.NetworkXNoPath as np:
print("no results buddy")
return None
def a_star(self, source, target, distance='cosine'):
if distance == 'cosine':
heuristic = self.heuristicFunctionCosine
elif distance == 'kl':
heuristic = self.heuristicFunctionKl
path = nx.astar_path(self.nxG, source, target, heuristic)
length = sum(self.nxG[u][v].get('weight', 1) for u, v in zip(path[:-1], path[1:]))
return path, length
def successors(self, state):
driver = state.getVehicleList()
packageList = state.getPackageList()
updatedStateList = []
if(packageList and driver.getCapacity() > len(driver.getPackageList())):
for packageIndex in range(0, len(packageList)):
copyState = copy.deepcopy(state)
updatedDriver = copyState.getVehicleList()
packagePickedUp = copyState.getPackageList().pop(packageIndex)
#print("Going to package {0}" .format(packagePickedUp.getNodeStartLocation()))
updatedDriver.getPackageList().append(packagePickedUp)
copyState.setProjectedCost(self.heuristicFunction(self, copyState, packagePickedUp))
#print("Projected Cost: {0}" .format(copyState.getProjectedCost()))
copyState.setActualCost(OptimizeDistanceHeuristic.returnActualCostPickUp(self, copyState, packagePickedUp))
#print("Actual Cost: {0}" .format(copyState.getActualCost()))
copyState.setAStarPath(OptimizeDistanceHeuristic.returnAStarPathPickUp(self, copyState, packagePickedUp))
copyState.getVehicleList().setCurrLocation(packagePickedUp.getNodeStartLocation())
print("State Created")
self.counter += 1
print(self.counter)
updatedStateList.append(copyState)
# the driver has one or more packages on his drop off list
if(driver.getPackageList()):
for driverPackageIndex in range(0, len(driver.getPackageList())):
copyState = copy.deepcopy(state)
droppedPackage = copyState.getVehicleList().getPackageList().pop(driverPackageIndex)
print("Going to package destination")
copyState.setProjectedCost(self.heuristicFunction(self, copyState, droppedPackage))
copyState.setActualCost(OptimizeDistanceHeuristic.returnActualCostDropOff(self, copyState, droppedPackage))
copyState.setAStarPath(OptimizeDistanceHeuristic.returnAStarPathDropOff(self, copyState, droppedPackage))
copyState.getVehicleList().setCurrLocation(droppedPackage.getNodeEndLocation())
print("Driver State Created")
self.counter += 1
print(self.counter)
updatedStateList.append(copyState)
#nothing in either package list or drop off list
# go home
if(not driver.getPackageList() and not packageList and ((driver.getCurrLocation() == driver.getHomeLocation()) == False)):
print("Going Home")
star = nx.astar_path(OptimizeDistanceHeuristic.graph, driver.getCurrLocation(), driver.getHomeLocation())
copyState = copy.deepcopy(state)
copyState.getVehicleList().setCurrLocation(driver.getHomeLocation())
copyState.setProjectedCost(len(star))
#copyState.setActualCost(len(star))
copyState.setAStarPath(star)
copyState.setActualCost(len(star))
updatedStateList.append(copyState)
print("Final State Created")
self.counter += 1
print(self.counter)
return updatedStateList
def test_astar_undirected2(self):
XG3=nx.Graph()
XG3.add_edges_from([ [0,1,{'weight':2}],
[1,2,{'weight':12}],
[2,3,{'weight':1}],
[3,4,{'weight':5}],
[4,5,{'weight':1}],
[5,0,{'weight':10}] ])
assert nx.astar_path(XG3,0,3)==[0, 1, 2, 3]
assert nx.astar_path_length(XG3,0,3)==15
def test_astar_directed2(self):
XG2=nx.DiGraph()
XG2.add_edges_from([[1,4,{'weight':1}],
[4,5,{'weight':1}],
[5,6,{'weight':1}],
[6,3,{'weight':1}],
[1,3,{'weight':50}],
[1,2,{'weight':100}],
[2,3,{'weight':100}]])
assert nx.astar_path(XG2,1,3)==[1, 4, 5, 6, 3]
def findMaxScorePath(CandidateNodesList,SelectNodesGraph, i): CanPath = [] for j in CandidateNodesList[i[0]]: for k in CandidateNodesList[i[-1]]: CanPath.append((nx.astar_path(SelectNodesGraph,j,k),nx.astar_path_length(SelectNodesGraph,j,k))) MaxScore = min([j[1] for j in CanPath]) for j in CanPath: if MaxScore==j[1]: MaxScorePath = j[0] return MaxScorePath
