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 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 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 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