def PATH_VISION(AUTO, BLACK, INITIAL, FINAL, RADIUS):
GRAPH = GRAPH_VISION(AUTO, BLACK, INITIAL, RADIUS)
P_calculus = dj.find_path(GRAPH, INITIAL, FINAL)
P_nodes = P_calculus.nodes
TRANS = path_trans(AUTO, P_nodes)
#print(P_nodes)
return (TRANS, P_nodes)
def dijkstra_planning(self):
graph = Graph()
temp_nodes = self.nodes[:]
temp_nodes.pop(0)
k = 0
for i in range(len(self.nodes)):
k = k + 1
for j in range(len(temp_nodes)):
node_a = self.nodes[i]
node_b = temp_nodes[j]
if np.linalg.norm(node_a - node_b) != 0:
print("add edge:", i, j + k)
graph.add_edge(i, j + k, np.linalg.norm(node_a - node_b))
if temp_nodes:
temp_nodes.pop(0)
print("done")
path = find_path(graph, 1, 3)
print("path:", path)
# if __name__ == "__main__":
# test_nodes = [np.array([21, 34]),
# np.array([45, 28]),
# np.array([76, 14]),
# np.array([12, 56]),
# np.array([48, 32])]
#
# motion_planner = Planner(test_nodes)
# motion_planner.dynamic_programming()
def findBestPath(self, src, dst):
graph = Graph()
for node1 in costBetweenNodes.keys():
for node2 in costBetweenNodes[node1].keys():
graph.add_edge(node1, node2, {'cost': costBetweenNodes[node1][node2]})
cost_func = lambda u, v, e, prev_e: e['cost']
return find_path(graph, src, dst, cost_func=cost_func).nodes
def find_conversions(source, target, versions_graph):
"""
This finds the minimum number of version upgrades required to reach the target version.
:param source: source version
:param target: target version
:param versions_graph: graph to fetch update path from
:return: list of conversions in order of execution
"""
graph = Graph()
for source_version, target_version in versions_graph.query(
"""SELECT ?source_version ?target_version{
?source_version version:convertsTo ?target_version .
}"""):
graph.add_edge(str(source_version), str(target_version),
{"conversions": 1})
# Find the shortest path
res = find_path(
graph,
str(source),
str(target),
cost_func=lambda u, v, e, prev_e: e["conversions"],
)
# Create and return the conversions
conversions = []
print(" -> ".join(res.nodes))
current = source
for node in res.nodes:
conversions.append((current, node))
current = node
return conversions[1:]
def execButton(graph, cost_func, inicio, fim, textRes, cities):
n_inicio = get_correct(inicio.get(), cities)
n_fim = get_correct(fim.get(), cities)
if (inicio.get() == "" or fim.get() == ""):
textRes.config(text="preencha os dois campos de cidade inicio e fim")
else:
try:
pesos = get_weights()
cost_func = lambda u, v, e, prev_e: e[
'distancia'
] # + e['situacaoPista'] + e['precoPedagios']+ e['perigo'] + e['tempoDeViagem']
print(n_inicio)
path = find_path(graph, n_inicio, n_fim, cost_func=cost_func)
#print("veja: ",path)
caminho = path.nodes
texto = "de '{}' ate '{}' o menor caminho eh {} com custo de {}km".format(
caminho[0], caminho[-1], caminho,
int(path.total_cost * 100) / 100)
print(texto)
textRes.config(text=texto)
pintarMapa(cidades, rodovias, master, w, path.nodes)
except:
textRes.config(
text="nao foi possivel achar caminho de '{}' ate '{}'".format(
n_inicio, n_fim))
def solve(data):
grid = data
base = grid
for j in range(1, 5):
appendix = base + j
appendix[appendix > 9] -= 9
grid = np.concatenate((grid, appendix), axis = 0)
base = grid
for i in range(1, 5):
appendix = base + i
appendix[appendix > 9] -= 9
grid = np.concatenate((grid, appendix), axis = 1)
h, w = np.shape(grid)
graph = Graph()
for j in range(0, h):
for a, b in pairwise(range(0, w)):
graph.add_edge(j * h + a, j * h + b, grid[j][b])
graph.add_edge(j * h + b, j * h + a, grid[j][a])
for i in range(0, w):
for a, b in pairwise(range(0, h)):
graph.add_edge(a * h + i, b * h + i, grid[b][i])
graph.add_edge(b * h + i, a * h + i, grid[a][i])
return find_path(graph, 0, h * w - 1).total_cost
def get_sorted_map_path_data(
map_data: Dict[str, Dict[str, int]]
) -> Dict[str, Dict[str, game_type.TargetPath]]:
"""
获取地图下各节点到目标节点的最短路径数据
Keyword arguments:
map_data -- 地图节点数据 当前节点:可通行节点:所需时间
Return arguments:
Dict[int,Dict[int,game_type.TargetPath]] -- 最短路径数据 当前节点:目标节点:路径对象
"""
graph = Graph()
sorted_path_data = {}
for node in map_data.keys():
for target in map_data[node]:
graph.add_edge(node, target, {"cost": map_data[node][target]})
cost_func = lambda u, v, e, prev_e: e["cost"]
for node in map_data.keys():
new_data = {node: {}}
for target in map_data.keys():
if target != node:
find_path_data = find_path(graph,
node,
target,
cost_func=cost_func)
target_path = game_type.TargetPath()
target_path.path = find_path_data.nodes[1:]
target_path.time = find_path_data.costs
new_data[node][target] = target_path
sorted_path_data.update(new_data)
return sorted_path_data
def shortestPathAStar(start_ind, goal_ind):
global R
# R[0] is the node for the start configuration
# R[1] is the node for the goal configuration
# each node has edges in the form: self.edges = [] List of tuples (node_id, dist)
# loop through our graph and convert it to a nice format for dijkstar
graph = Graph()
for node in R:
for edge in node.edges:
graph.add_edge(node.id, edge[0], edge[1])
cfg_array = []
# catch path not found exception
try:
pathinfo = find_path(graph, start_ind, goal_ind)
except:
print('Could NOT find a path from start to goal')
return cfg_array
# get the configurations from each node in this found path
for node_id in pathinfo.nodes:
cfg_array.append(R[node_id].cfg)
return cfg_array
def crea_matrice_distanze(dist):
initial_matr = np.array(dist)
print initial_matr
matr = initial_matr.copy()
# dict_matr = {(partenza, arrivi): lunghezza for partenza, arrivi in enumerate(dist) for arrivo, lunghezza in
# enumerate(arrivi)}
graph = Graph()
for partenza, arrivi in enumerate(dist):
for arrivo, lunghezza in enumerate(arrivi):
if lunghezza >= 0:
graph.add_edge(partenza, arrivo, {'cost': lunghezza})
cost_func = lambda u, v, e, prev_e: e['cost']
for partenza, arrivi in enumerate(dist):
for arrivo in range(len(arrivi)):
if partenza == arrivo:
matr[partenza, arrivo] = -1
else:
try:
min_dist = find_path(graph,
partenza,
arrivo,
cost_func=cost_func)[3]
matr[partenza, arrivo] = min_dist
except Exception as e:
matr[partenza, arrivo] = -1
print "matr ditanze\n", matr
return matr
def get_path_to_unexplored(self, unex):
paths = []
for candidate in self.get_neighbors(unex).intersection(self.explored):
paths.append(find_path(self.graph, self.position, candidate))
node_seq = min(paths, key=lambda x: x.total_cost).nodes
node_seq.append(unex)
return node_seq[1:]
def test_start_and_destination_same(self):
result = find_path(self.graph1, 1, 1)
nodes, edges, costs, total_cost = result
self.assertEqual(nodes, [1])
self.assertEqual(edges, [])
self.assertEqual(costs, [])
self.assertEqual(total_cost, 0)
def test_path_with_cost_func(self):
graph = {
'a': {
'b': (1, 10, 'A'),
'c': (1.5, 2, 'C')
},
'b': {
'c': (1, 2, 'B'),
'd': (1, 10, 'A')
},
'c': {
'b': (1, 3, 'B'),
'd': (1.5, 2, 'D')
},
}
def cost_func(u, v, e, prev_e):
cost = e[0]
cost *= e[1]
if prev_e is not None and e[2] != prev_e[2]:
cost *= 1.25
return cost
result = find_path(graph, 'a', 'd', cost_func=cost_func)
nodes, edges, costs, total_cost = result
self.assertEqual(nodes, ['a', 'c', 'd'])
def getSortedMapPathData(mapData: dict) -> dict:
'''
获取地图下各节点到目标节点的最短路径数据
Keyword arguments:
mapData -- 地图节点数据
'''
graph = Graph()
sortedPathData = {}
for node in mapData.keys():
for target in mapData[node]:
graph.add_edge(node, target, {'cost': mapData[node][target]})
cost_func = lambda u, v, e, prev_e: e['cost']
for node in mapData.keys():
newData = {node: {}}
for target in mapData.keys():
if target != node:
findPathData = find_path(graph,
node,
target,
cost_func=cost_func)
newData[node].update({
target: {
"Path": findPathData.nodes[1:],
"Time": findPathData.costs
}
})
sortedPathData.update(newData)
return sortedPathData
def get_best_path_configs(graph, start_node: int, stop_node: int) -> List[int]:
"""
Get the shortest path and calculate the nodes from it. Also print out some helpful information.
:param graph:
:param start_node: Node index of the start node
:param stop_node: Node index of the final node
:return:
:raises: NoValidPathFound if no path with finite cost is found
"""
# Find the initially shortest path to be checked for collisions.
try:
print('\nSearching minimum cost path...')
path = find_path(graph, start_node, stop_node)
except NoPathError as e:
raise NoValidPathFound from e
print(
f'=>Total cost for the current minimum cost path: {path.total_cost :.4f}'
)
if path.total_cost == float('inf'):
raise NoValidPathFound('Path cost is infinite (invalid transitions).')
pt_configurations = [
calc_conf_from_node(node_idx, pt_idx)
for pt_idx, node_idx in enumerate(path.nodes[1:-1])
]
print(
f'=>Configurations in current shortest path: {set(pt_configurations)}')
return pt_configurations
def find_path(self, graph, s, e, cost_func=None, heuristic_func=None):
start = s.closest_object
end = e.closest_object
annex = None
split_ways = {}
if isinstance(start, Street) or isinstance(end, Street):
annex = dijkstar.Graph()
if isinstance(start, Street):
start, *start_ways = self.split_way(start, s.geom, -1, -1, -2, graph, annex)
split_ways.update({w.id: w for w in start_ways})
if isinstance(end, Street):
end, *end_ways = self.split_way(end, e.geom, -2, -3, -4, graph, annex)
split_ways.update({w.id: w for w in end_ways})
try:
nodes, edge_attrs, costs, total_weight = dijkstar.find_path(
graph, start.id, end.id,
annex=annex, cost_func=cost_func, heuristic_func=heuristic_func)
except dijkstar.NoPathError:
raise NoRouteError(s, e)
assert nodes[0] == start.id
assert nodes[-1] == end.id
return nodes, edge_attrs, split_ways
def get_sorted_map_path_data(map_data: dict) -> dict:
"""
获取地图下各节点到目标节点的最短路径数据
Keyword arguments:
map_data -- 地图节点数据
"""
graph = Graph()
sorted_path_data = {}
for node in map_data.keys():
for target in map_data[node]:
graph.add_edge(node, target, {"cost": map_data[node][target]})
cost_func = lambda u, v, e, prev_e: e["cost"]
for node in map_data.keys():
new_data = {node: {}}
for target in map_data.keys():
if target != node:
find_path_data = find_path(graph,
node,
target,
cost_func=cost_func)
new_data[node].update({
target: {
"Path": find_path_data.nodes[1:],
"Time": find_path_data.costs,
}
})
sorted_path_data.update(new_data)
return sorted_path_data
def test_find_path_1(self):
result = find_path(self.graph1, 1, 4)
nodes, edges, costs, total_cost = result
self.assertEqual(nodes, [1, 2, 4])
self.assertEqual(edges, [1, 2])
self.assertEqual(costs, [1, 2])
self.assertEqual(total_cost, 3)
def solve_graph():
global maze, maze_cost, pacmanpos, world, goal_state
graph = Graph()
for j in range(0, height):
for i in range(0, width):
if maze[j][i] != 1:
#arriba
if (maze[j-1][i] != 1) and (j-1 > 0):
xi = str((j,i))
xj = str((j-1,i))
c = maze_cost[j-1][i]
graph.add_edge(xi,xj,{'cost':c})
#print(xi+' - '+xj)
#abajo
if (maze[j+1][i] != 1) and (j+1 < height):
xi = str((j,i))
xj = str((j+1,i))
c = maze_cost[j+1][i]
graph.add_edge(xi,xj,{'cost':c})
#print(xi+' - '+xj)
#derecha
if (maze[j][i+1] != 1) and (i+1 < width):
xi = str((j,i))
xj = str((j,i+1))
c = maze_cost[j][i+1]
graph.add_edge(xi,xj,{'cost':c})
#print(xi+' - '+xj)
#izquierda
if (maze[j][i-1] != 1) and (i-1 > 0):
xi = str((j,i))
xj = str((j,i-1))
c = maze_cost[j][i-1]
graph.add_edge(xi,xj,{'cost':c})
#print(xi+' - '+xj)
cost_func = lambda u, v, e, prev_e: e['cost']
pacman = worldToGrid(pacmanpos.pacmanPos.x, pacmanpos.pacmanPos.y)
start_state = (pacman['y'], pacman['x'])
#start_state = (25,14)
# goal_state = closest_bonus()
# print(goal_state)
#goal_state = (25,26)
path = find_path(graph, str(start_state), str(goal_state), cost_func=cost_func)
# print(path.nodes)
if len(path.nodes)>1:
next_state = eval(path.nodes[1])
aux = (start_state[0]-next_state[0], start_state[1]-next_state[1])
else:
print('yuca')
return 4
#print(str(start_state)+' '+str(next_state))
if aux == (1,0):
return 0
if aux == (-1,0):
return 1
if aux == (0,-1):
return 2
if aux == (0,1):
return 3
def __close_cycle(node, roundtrip):
graph = Graph(undirected=True)
__to_dijkstra(node, graph)
first_node = roundtrip[0].id
last_node = roundtrip[-1].id
path_info = find_path(graph, last_node, first_node)
return roundtrip + __to_nodes(node, path_info)[1:-2]
def get_path(graph, src, dest, image):
source_node = image[src[0]][src[1]][1]
dest_node = image[dest[0]][dest[1]][1]
path = find_path(graph, source_node, dest_node)
return path
def sol_6_b(data_str):
orbit_tree = read_orbits(data_str)
g = Graph()
for planet in orbit_tree:
g.add_edge(orbit_tree[planet], planet, 1)
g.add_edge(planet, orbit_tree[planet], 1)
path = find_path(g, "YOU", orbit_tree["SAN"])
return path.total_cost - 1
def test_find_path_with_annex(self):
annex = Graph({1: {2: 1, 3: 0.5}})
result = find_path(self.graph1, 1, 4, annex=annex)
nodes, edges, costs, total_cost = result
self.assertEqual(nodes, [1, 3, 4])
self.assertEqual(edges, [0.5, 2])
self.assertEqual(costs, [0.5, 2])
self.assertEqual(total_cost, 2.5)
def part_two():
with open("./input.txt") as input:
for line in input.readlines():
objs = line.strip().split(")")
galaxy.add_edge(objs[0], objs[1], 1)
path_info = find_path(galaxy, 'YOU', 'SAN')
print(path_info.total_cost - 2)
def dijkstra(d):
g = Graph()
for i in range(len(d)):
for j in range(len(d[i])):
dirs = ((i - 1, j), (i + 1, j), (i, j - 1), (i, j + 1))
for x, y in dirs:
if 0 <= x < len(d) and 0 <= y < len(d[i]):
g.add_edge((i, j), (x, y), d[x][y])
return find_path(g, (0, 0), (len(d) - 1, len(d[0]) - 1)).total_cost
def get_routes(name_num, point1, point2):
global_graph = Graph()
loc_name = get_loc_name(name_num)
global_graph = global_graph.load('./djicstra_graph/{}'.format(loc_name))
graph_route = find_path(global_graph, point1, point2).nodes
final_list = list()
for elem in graph_route:
final_list.append((get_position_name(name_num, elem)))
return (final_list)
def path_find(currentUserId, matchWith):
try:
path = find_path(graph,
currentUserId,
matchWith[0][0],
cost_func=cost_func)
return jsonify(path)
except:
traceback.print_exc()
def graph_search(self):
# set up the graph
graph = Graph()
self.nodes = 0
self.total_cost = 0
# add edges
for i in range(len(self.TE)):
# pdb.set_trace()
edge = self.TE[i]
dist = math.sqrt((edge[0][0] - edge[1][0])**2 +
(edge[0][1] - edge[1][1])**2)
# pdb.set_trace()
# find vertex from self.V that matches the edges
vertex1 = self.TV.index(edge[0])
vertex2 = self.TV.index(edge[1])
# pdb.set_trace()
graph.add_edge(vertex1, vertex2, dist)
# pdb.set_trace()
temp = find_path(graph, 0, len(self.TV) - 1)
self.nodes = temp[0]
self.total_cost = temp[3]
# now that we have the final path of the robot, we need to extract the ideal robot positions
# interpolate all of the vertices to make a series of points that the robot can follow
self.robot_positions = []
# iterate over each node in the final path and connect between them
for i in range(len(self.nodes) - 1):
# take the first and second nodes
from_node = self.TV[self.nodes[i]]
to_node = self.TV[self.nodes[i + 1]]
# take each edge and interpolate a distance equivalent 1/10th of the robot speed per second
diff_x = to_node[0] - from_node[0]
diff_y = to_node[1] - from_node[1]
dist_speed = self.speed * 1 / 10 # m/s * s = m
# now find number of points from distance between points and speed
dist_covered = math.sqrt(diff_x**2 + diff_y**2)
num_points = round(dist_covered / dist_speed)
# interpolate using these points
x_int = from_node[0]
y_int = from_node[1]
interpolateds = [x_int, y_int]
for i in range(num_points):
# append the robot positions
self.robot_positions.append(interpolateds)
# add the points in by moving the direction amount calculated in x and y
x_int = interpolateds[0] + diff_x / num_points
y_int = interpolateds[1] + diff_y / num_points
interpolateds = [x_int, y_int]
# append goal
if self.solution_found == 1:
self.robot_positions.append(self.qgoal)
# self.robot_positions.append(np.linspace(edge[0],edge[1], num = self.speed/50,endpoint=True,retstep=True))
return self.nodes, self.total_cost
def _findpath(self, source, destination):
path_list = find_path(self.graph,
source,
destination,
cost_func=self.cost_function).nodes
cost = find_path(self.graph,
source,
destination,
cost_func=self.cost_function).costs
position_path_list = []
cost = list(map(int, cost))
cost = sum(cost)
for i in range(len(path_list)):
node_coordinates = self.map.get_node_by_id(
path_list[i]).coordinates
position_path_list.append(
[node_coordinates[1], node_coordinates[0]])
# print(position_path_list)
return position_path_list, cost
def get_disktra(self, dict_probabilities, logaritmico):
# Return a Graph to apply djistra algorithm
grafo = Graph()
dict_of_graphs = dict()
for clave_transicion in dict_probabilities.keys():
origen_destino = clave_transicion.split(";")
origen = origen_destino[0]
destino = origen_destino[1]
valor_transicion = str(dict_probabilities.get(clave_transicion))
grafo.add_edge(origen, destino, int(float(valor_transicion) * 100))
for final_node in self.lista_nodos_finales:
if logaritmico:
dict_of_graphs[final_node] = find_path(grafo, "",
final_node + '_l')
else:
dict_of_graphs[final_node] = find_path(grafo, "", final_node)
return dict_of_graphs
def updateForwardingTable(self):
self.forwarding_table.clear()
for port_no, link in self.link_state.items():
self.forwarding_table[link[1]] = (link[1], port_no)
for link_state in self.link_state_local.values():
for link in link_state.values():
try:
path = find_path(self.network_graph, self.addr, link[1])
port = self.forwarding_table[path.nodes[1]][1]
self.forwarding_table[link[1]] = (path.nodes[1], port)
except:
pass
def ensure_traversable(self):
graph = self.make_graph()
blue = None
red = None
for x, y in product(range(self.width), range(self.height)):
cell = self.grid[x][y]
if isinstance(cell, BlueFlag):
blue = (x, y)
elif isinstance(cell, RedFlag):
red = (x, y)
if blue is not None and red is not None:
break
path = find_path(graph, blue, red)
return path
def calculate_path(self):
graph = Graph()
for i in range(len(self.links)):
graph.add_edge(self.links[i][0], self.links[i][1], {'cost': self.links[i][2]})
cost_func = lambda u, v, e, prev_e: e['cost']
#print self.links
result = find_path(graph, meta_data.source_id, meta_data.destination_id, cost_func=cost_func)
route = result[0]
# clear link list
del self.crn_manager.role.links[:]
# check and reply
if meta_data.INF in result[2]:
route = []
self.routing_request_log.append([self.crn_manager.get_virtual_time(), 0])
else:
self.routing_request_log.append([self.crn_manager.get_virtual_time(), 1])
#self.crn_manager.route = route
return route
def compute_shortest_path(self, source, target, graph=None):
from dijkstar import find_path
if graph == None:
if self.graph == None:
if os.path.exists(
os.path.join(self.graph_dir,
self.env.house.house['id'] + '.pkl')):
self.load_graph(
os.path.join(self.graph_dir,
self.env.house.house['id'] + '.pkl'))
else:
self.build_graph(
save_path=os.path.join(
graph_dir, self.env.house.house['id'] + '.pkl'))
graph = self.graph
cost_func = lambda u, v, e, prev_e: e['cost']
shortest_path = find_path(graph, source, target, cost_func=cost_func)
return shortest_path
def find_path(self, pt1, pt2):
def manhattan(u, v, edge, prev_edge):
return abs(u[0] - v[0]) + abs(u[1] - v[1])
try:
nodes, edges, costs, final_cost = find_path(
self.graph, pt1, pt2, heuristic_func=manhattan
)
result = []
f = self.frame
for i, (x, y) in enumerate(nodes):
ptype = ptypes[self.level[y][x].type_id]
if ptype.layer in ('d', 'e'):
return result
if f > self.frame and self.check_conflict(f, x, y):
if self.check_conflict((f + 1) % 1000, x, y):
return result
else:
result.append(result[-1])
f += 1
result.append((x, y))
f += 1
return result
except NoPathError:
return []
def plot_path(*args, **kwargs):
return find_path(*args, **kwargs)
def camino_mas_cercano(self, desde, hasta):
graph = self.setUpGraph()
cost_func = lambda u, v, e, prev_e: e['cost']
path = find_path(graph, desde, hasta, cost_func=cost_func)
return path