def min_directional_distance(edge_id, adj_list):
"""Find the shortest directional distance from the specified edge to all the other edges in the graph.
:param edge_id: the ID of the source edge
:param adj_list: the adjacency list, e.g., G = [{1: 0, 26: 1, 50: 1}, {16: 1, 2: 0, 27: 1}, ...] indicates that
the edge 0 is incident with the edges 1, 26, and 50, and distances from those edges are
G[0][1] = 0, G[0][26] = 1, and G[0][50] = 1.
:return: a dictionary of shortest directional distances from the specified edge
"""
num_lines = len(
adj_list
) // 2 # the total number of line segments (i.e., undirected edges)
source_id_1 = edge_id
source_id_2 = edge_id + num_lines
# Run dijkstra twice, respectively based on the two directed source edges
D1, P1 = Dijkstra(adj_list, source_id_1)
D2, P2 = Dijkstra(adj_list, source_id_2)
# Compare D1 and D2 to get the shortest distance for each directed edge
dict_min_dist = {}
for k in D1:
if k in D2:
dict_min_dist[k] = min(D1[k], D2[k])
else:
dict_min_dist[k] = D1[k]
for k in D2:
if k in D1:
continue
else:
dict_min_dist[k] = D2[k]
# Compare the pairs of flipped edges to find the real shortest distance for the line segment
dict_real_min_dist = {}
for k, v in dict_min_dist.items():
# find the ID of the opposite edge, assuming k is no smaller than the total number of line segments
i = k - num_lines
# find the ID of the opposite edge, assuming k is smaller than the total number of line segments
j = k + num_lines
if i >= 0:
if i in dict_min_dist:
dict_real_min_dist[i] = min(dict_min_dist[i], v)
else:
dict_real_min_dist[i] = v
else:
if j in dict_min_dist:
dict_real_min_dist[k] = min(v, dict_min_dist[j])
else:
dict_real_min_dist[k] = v
return dict_real_min_dist
def store_nodeLandmarks(self):
dijk_obj = Dijkstra(_get_dbl_level_dict(self.G, self.noNodes),
self.noNodes)
for landmark in self.nodeLandmarks:
if landmark in self.sp:
continue
self.sp[landmark] = dijk_obj.shortest_path(self.vertices, landmark)
def __init__(self, mat, node, draw, random):
if not random:
if mat not in range(len(MATRICES)):
raise CustomError(
f"Matrice inexistante ! (matrice disponibles: {list(range(len(MATRICES)))})"
)
M = MATRICES[mat]
if node not in range(len(M)):
raise CustomError(
f"Sommet {node} inexistant ! (noeuds disponibles: {list(range(len(M)))} pour la matrice {mat})"
)
else:
M = self.generate_random()
print(
f"Matrice {mat if not random else 'aléatoire'} (nan = pas de chemin): \n",
M, "\n")
obj = Dijkstra(M, node)
obj.calc()
print(obj) # afficher le résulat dans la console
if draw:
obj.draw(
) # afficher le graphe et le résulat sur une interface graphique
def test_tushar_roy(self):
''' example from Tushar Roy's video https://www.youtube.com/watch?v=lAXZGERcDf4 '''
start_vertex = 'a'
verticies = ['a', 'b', 'c', 'd', 'e', 'f']
# edge_weights = {('a','b') : 2, ('a','f') : 4, ('a','d'):5, ('b','c') : 4, ('f','c') : 3, ('f','d') : 2, ('d','e') : 1}
edge_weights = {
('a', 'b'): 2,
('a', 'f'): 4,
('a', 'd'): 5,
('b', 'c'): 4,
('c', 'f'): 3,
('d', 'f'): 2,
('d', 'e'): 1
}
djk = Dijkstra(start_vertex, verticies, edge_weights)
# pdb.set_trace()
djk.iterate()
# self.assertRaises(ValueError, djk.iterate)
# print("parents=", djk.parents)
self.assertEqual(6, len(djk.parents)) #parents is empty
# print("parents=", djk.parents)
self.assertEqual([], djk.parents['a'])
self.assertEqual(['a'], djk.parents['b'])
self.assertEqual(['a'], djk.parents['d'])
self.assertEqual(['a'], djk.parents['f'])
self.assertEqual(['b'], djk.parents['c'])
def find_paths(self):
self.d = Dijkstra(_get_dbl_level_dict(self.G), len(self.nodes))
self.dijkPaths = {}
for each in self.sampled:
for e in each:
if self.dijkPaths.get(e, -1) == -1:
self.dijkPaths[e] = self.d.shortest_path(self.nodes, e)
def main():
i = No("I")
a = No("A")
b = No("B")
c = No("C")
d = No("D")
e = No("E")
f = No("F")
t = No("T")
i.Conectar(a, 6)
i.Conectar(b, 2)
a.Conectar(c, 4)
a.Conectar(e, 2)
b.Conectar(a, 4)
b.Conectar(c, 3)
b.Conectar(d, 7)
c.Conectar(e, 2)
c.Conectar(d, 3)
d.Conectar(t, 2)
e.Conectar(t, 4)
e.Conectar(f, 7)
f.Conectar(t, 3)
algoritmo = Dijkstra()
caminho_mais_curto = algoritmo.EncontrarCaminhoMaisCurto(i, t)
print(*InverterCaminho(caminho_mais_curto).items(), sep="\n")
def JohnsonAlgorithm(graph: WeightedDirectedGraph, verbose=True):
graph_edges_with_extra_node = graph.edges + [
WeightedEdge('new_node', 0, node) for node in graph.nodes
]
graph_with_extra_node = WeightedDirectedGraph(
graph_edges_with_extra_node.copy())
try:
bf_shortest_distances, predecessors = BellmanFord(
graph_with_extra_node, 'new_node', verbose=False)
except Exception as e:
return str(e)
for edge in graph.edges:
edge.weight = edge.weight + bf_shortest_distances[
edge.head] - bf_shortest_distances[edge.tail]
all_pairs_shortest_distance = {}
for source_node in graph.nodes:
if verbose:
print('\nShortest Distance with vertex ' + str(source_node) +
' as the source:\n')
dijkstra_shortest_distances, predecessors = Dijkstra(graph,
source_node,
verbose=verbose)
dijkstra_shortest_distances = {
k:
(v + bf_shortest_distances[k] - bf_shortest_distances[source_node])
for k, v in dijkstra_shortest_distances.items()
}
all_pairs_shortest_distance[source_node] = dijkstra_shortest_distances
return all_pairs_shortest_distance
def store(self, distance_hash, n):
self.order = n
self._dbl_lvl_dict = _get_dbl_level_dict(distance_hash)
sp_dijkstra = Dijkstra(self._dbl_lvl_dict, self.order)
for i in range(self.order):
self.ub_matrix.append([1] * self.order)
for i in range(self.order):
nodes = list(range(self.order))
sp = sp_dijkstra.shortest_path(nodes, i)
for index in range(self.order):
# distance, node, parent
self.ub_matrix[i][index] = sp[index][0]
self.ub_matrix[index][i] = sp[index][0]
self.search_started = [False] * n
for i in range(n):
self.lb_matrix.append([0] * n)
if n - i - 1 != 0:
self.uncalculated[i] = set(range(i + 1, n))
for k in distance_hash.keys():
x, y = k
self.lb_matrix[x][y] = distance_hash[k]
self.lb_matrix[y][x] = distance_hash[k]
self.uncalculated[min(x, y)].remove(max(x, y))
if len(self.uncalculated[min(x, y)]) == 0:
del self.uncalculated[min(x, y)]
self.sparse_matrix = SparseMatrix(distance_hash, n)
if self.max_path_length == 2:
for i in range(n):
for j in range(i + 1, n):
self._update(i, j)
else:
for i in range(n):
self._dfs(i)
def test_dijkstra():
graph = Graph()
nodes = []
while True:
str = input()
if str == "-1":
break
first, second, weight = str.split(' ')
if first not in nodes:
nodes.append(first)
if second not in nodes:
nodes.append(second)
graph.add_edge(first, second, int(weight))
dijkstra = Dijkstra(graph, nodes[0])
dijkstra.do_dijkstra()
print("weights : ")
for v in nodes:
print("%s : %s" % (v, dijkstra.get_distance(v)))
mst = []
for n in nodes:
path = dijkstra.get_path(n)
if len(path) == 1:
continue
for i in range(len(path) - 1):
edge = path[i] + " ---- " + path[i + 1]
if edge not in mst:
mst.append(edge)
print("MST : ")
for edge in mst:
print(edge)
def create(self, method_name): if method_name == "dijkstra": return Dijkstra() elif method_name == "astar": return AStar() else: return AStar()
def __init__(self, graph, vertices_count):
self.graph = graph
extra_vertex = {}
for tail in graph.keys():
for head in graph[tail]:
extra_vertex[tail] = 0
extra_vertex[head] = 0
self.graph[0] = extra_vertex
B = Bellman_ford(self.graph, vertices_count + 1, source_vertex=0)
self.p_values = B.distances
self.graph.pop(0)
self.graph_double_prime = defaultdict(dict)
for tail in graph.keys():
for head in graph[tail]:
self.graph_double_prime[tail][head] = graph[tail][head] + self.p_values[tail] - self.p_values[head]
self.shortest_paths_dist_prime = {}
for i in range(1, vertices_count +1 ):
print(i)
D = Dijkstra(self.graph_double_prime, vertices_count, source_vertex=i)
for j in D.shortest_path_dist.keys():
self.shortest_paths_dist_prime[(i, j)] = D.shortest_path_dist[j]
self.shortest_path_dist = {}
for tail, head in self.shortest_paths_dist_prime.keys():
self.shortest_path_dist[(tail, head)] = self.shortest_paths_dist_prime[(tail, head)] - self.p_values[tail] + self.p_values[head]
def multi_query(self, n, verbose=False):
if verbose:
print('Start multi_query')
t0 = time.time()
# Find nearest node to start/goal
start_prm = self.find_nearest_node(self.V, self.world.start)
goal_prm = self.find_nearest_node(self.V, self.world.goal)
# Initialize Dijkstra module
djk1 = Dijkstra(self.V, self.E)
djk2 = Dijkstra(self.V, self.E)
# Build a distance map
djk1.build(start_prm)
djk2.build(goal_prm)
if verbose:
t1 = time.time()
print('Build a distance map: {}'.format(t1 - t0))
# Generate multiple paths
self.path_list = []
while len(self.path_list) < n:
mid_point = np.random.randint(len(self.V))
djk1.query(mid_point)
djk2.query(mid_point)
if djk1.path is not None and djk2.path is not None:
djk1.path = np.vstack((self.world.start, djk1.path))
djk2.path = np.vstack((djk2.path[-2::-1], self.world.goal))
'''
smoothed_path1 = self.smoothing(djk1.path)
smoothed_path2 = self.smoothing(djk2.path)
self.smoothed_path = np.vstack((
smoothed_path1,
smoothed_path2))
'''
self.path = np.vstack((djk1.path, djk2.path))
self.smoothed_path = self.smoothing(self.path)
# '''
self.path_list.append(self.smoothed_path)
if verbose:
t2 = time.time()
print('Generate multiple paths: {}\n'.format(t2 - t1))
def generate_shortest_path_test(edges):
print("\n### Generate shortest path test")
try:
d = Dijkstra(make_dict_graph(edges), "O", "T")
distance_total, path = d.generate_shortest_path()
print(f" {path} \n")
except:
print("Error")
def store_edgeLandmarks(self):
dijk_obj = Dijkstra(_get_dbl_level_dict(self.G, self.noNodes),
self.noNodes)
for (x, y) in self.edgeLandmarks:
if x not in self.sp:
self.sp[x] = dijk_obj.shortest_path(self.vertices, x)
if y not in self.sp:
self.sp[y] = dijk_obj.shortest_path(self.vertices, y)
def __init__(self, edge_list, order_val, k):
self.edge_list = edge_list
self.k = k
self.order_val = order_val
self.dbl_dict = _get_dbl_level_dict(self.edge_list)
self.d = Dijkstra(self.dbl_dict, self.order_val)
self.landmarks = None
self.dijk_dict = {}
def test_one_point(self):
''' test simple 1 point '''
start_vertex = 0
verticies = [0]
edge_weights = {}
djk = Dijkstra(start_vertex, verticies, edge_weights)
djk.iterate()
# self.assertRaises(ValueError, djk.iterate)
self.assertEqual(1, len(djk.parents)) #parents is empty
self.assertEqual([], djk.parents[0])
self.assertEqual({0: 0}, djk.distances)
def setup_algos(root):
controller = root.winfo_children()[0]
cell_grid = root.winfo_children()[1].winfo_children()[0]
a_star = Astar(cell_grid)
Button(controller, text="A*", width=10,
command=a_star.trace).grid(row=0, column=8)
dijkstra = Dijkstra(cell_grid)
Button(controller, text="Dijkstra", width=10,
command=dijkstra.trace).grid(row=1, column=8)
def dijkstra_algorithm_test(edges):
print("\n### Dijkstra alogrith test")
try:
d = Dijkstra(make_dict_graph(edges), "O", "T")
distance_total, prev = d.dijkstra_algorithm()
print(f" Total distance: {distance_total}")
for p in prev:
if prev[p] != None:
print(f" {prev[p]} ")
except:
print("Error...")
def testDijkstra():
filename = "testG1.txt"
sparG = SparseGraph(5, True)
ReadGraph(sparG, filename)
#sparG.show()
dij = Dijkstra(sparG, 0)
for i in range(sparG.V()):
if (dij.hasPathTo(i)):
dij.showPath(i)
else:
print "No Path To %d " % i
def test_two_point(self):
''' test simple 2 point '''
start_vertex = 0
verticies = [0, 1]
edge_weights = {(0, 1): 1}
djk = Dijkstra(start_vertex, verticies, edge_weights)
# pdb.set_trace()
djk.iterate()
# self.assertRaises(ValueError, djk.iterate)
# print("parents=", djk.parents)
self.assertEqual(2, len(djk.parents)) #parents is empty
self.assertEqual([], djk.parents[0])
self.assertEqual([0], djk.parents[1])
self.assertEqual({0: 0, 1: 1}, djk.distances)
def test_dijkstra(self):
graph = [[(2, 1), (5, 2)], [(2, 0), (4, 2), (6, 3), (10, 4)],
[(5, 0), (4, 1), (2, 3)], [(2, 2), (6, 1), (1, 5)],
[(10, 1), (3, 5), (5, 6)], [(1, 3), (3, 4), (9, 6)],
[(5, 4), (9, 5)]]
djk = Dijkstra()
actual = djk.dijkstra(graph, 0)
expected = [0, 2, 5, 7, 11, 8, 16]
assert actual == expected
actual = djk.dijkstra(graph, 0, 6)
expected = [0, 2, 3, 5, 4, 6]
assert actual == expected
def __init__(self, locations, latitude_min, latitude_max,
longitude_min, longitude_max):
center_latitude = latitude_min + (latitude_max - latitude_min) // 2
center_longitude = longitude_min + (longitude_max - longitude_min) // 2
self.top_left_location = Location("top_left", latitude_min, longitude_min)
self.bottom_right_location = Location("bottom_right", latitude_max, longitude_max)
self.center_location = Location("center", center_latitude, center_longitude)
self.width = calc_distance(latitude_min, center_longitude, latitude_max, center_longitude)
self.height = calc_distance(center_latitude, longitude_min, center_latitude, longitude_max)
self.locations = locations
self.dijkstra = Dijkstra(self.locations)
self.selected_citys = []
self.road_paths = []
def test_four_point_a(self):
''' test simple 4 point '''
start_vertex = 0
verticies = [0, 1, 2, 3]
edge_weights = {(0, 1): 1, (0, 2): 1, (1, 3): 1, (2, 3): 2}
djk = Dijkstra(start_vertex, verticies, edge_weights)
# pdb.set_trace()
djk.iterate()
# self.assertRaises(ValueError, djk.iterate)
# print("parents=", djk.parents)
self.assertEqual(4, len(djk.parents)) #parents is empty
self.assertEqual([], djk.parents[0])
self.assertEqual([0], djk.parents[1])
self.assertEqual([0], djk.parents[2])
self.assertEqual([1], djk.parents[3])
self.assertEqual({0: 0, 1: 1, 2: 1, 3: 2}, djk.distances)
def path_callback(self, map):
self.map = map
if self.firstCall == False:
rospy.loginfo("+++++++++++++++++initX: " + str(self.initx) +
" initY: " + str(self.inity) + " goalX: " +
str(self.goalx) + " goalY: " + str(self.goaly))
self.firstCall = True
if self.world != 0:
dijkstra = Dijkstra(self.map)
self.path = dijkstra.planning(self.initx, self.inity,
self.goalx, self.goaly)
self.save_as_yaml(self.path)
x, y = self.path
x = x[::-1]
y = y[::-1]
my_path = Path()
my_path.header.frame_id = "map"
my_path.header.stamp = rospy.get_rostime()
size = range(len(self.path[0]))
poses = []
for i in size:
p_stamp = PoseStamped()
p_stamp.pose.position.x = x[i]
p_stamp.pose.position.y = y[i]
p_stamp.pose.position.z = 0
p_stamp.header.frame_id = "map"
p_stamp.header.stamp = rospy.get_rostime()
poses.append(p_stamp)
my_path.poses = poses
self.pathPublisher.publish(my_path)
else:
poses = []
self.path = [(self.goalx, self.goaly)]
my_path = Path()
my_path.header.frame_id = "map"
my_path.header.stamp = rospy.get_rostime()
for i in range(2):
p_stamp = PoseStamped()
p_stamp.pose.position.x = self.goalx
p_stamp.pose.position.y = self.goaly
p_stamp.pose.position.z = 0
p_stamp.header.frame_id = "map"
p_stamp.header.stamp = rospy.get_rostime()
poses.append(p_stamp)
my_path.poses = poses
self.pathPublisher.publish(my_path)
def test2():
inf = np.inf
scan=[inf, inf, inf, inf, inf, inf, 2.122950792312622, 1.8218177556991577, 1.59413743019104, 1.423231601715088, 1.323502540588379, 1.3081393241882324, 1.3145588636398315, 1.3490028381347656, 1.335472822189331, 1.319705605506897, 1.3400593996047974, 1.3491653203964233, 1.3466880321502686, 1.3696467876434326, 1.3745601177215576, 1.3833107948303223, 1.3923722505569458, 1.4062138795852661, 1.3897963762283325, 1.4243078231811523, 1.4298373460769653, 1.4416676759719849, 1.450016736984253, 1.471937656402588, 1.5024218559265137, 1.4850928783416748, 1.5270593166351318, 1.5272008180618286, 1.5350757837295532, 1.5613079071044922, 1.58877432346344, 1.6090070009231567, 1.6587506532669067, 1.6668319702148438, 1.6823045015335083, 1.711564540863037, 1.7509064674377441, 1.7584381103515625, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 2.650038242340088, 2.6445717811584473, 2.631784677505493, 2.6164636611938477, 2.6125850677490234, 2.5924696922302246, 2.5889229774475098, 2.5727877616882324, 2.5825552940368652, 2.5913491249084473, 2.5679054260253906, 2.575549840927124, 2.5961365699768066, 2.578033447265625, 2.563906669616699, 2.576000690460205, 2.586289644241333, 2.5772287845611572, 2.5811655521392822, 2.5825459957122803, 2.588222026824951, 2.5943994522094727, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 2.9890737533569336, 2.9071550369262695, 2.8188626766204834, 2.7494678497314453, 2.67634916305542, 2.6266491413116455, 2.5653467178344727, 2.5333642959594727, 2.4661786556243896, 2.4012131690979004, 2.3687829971313477, 2.323002338409424, 2.2887566089630127, 2.2677664756774902, 2.296781539916992, 2.3500185012817383, 2.360821008682251, 2.453289031982422, 2.4872214794158936, 2.548398017883301, 2.6022679805755615, 2.6568949222564697, 2.7322375774383545, 2.7981326580047607, 2.8849222660064697, 2.9328646659851074, 3.0372416973114014, inf, inf, inf, inf, inf, 3.4685451984405518, 3.4392693042755127, 3.423564910888672, 3.389524221420288, 3.3799402713775635, 3.355445146560669, 3.335869073867798, 3.295499086380005, 3.285273551940918, 3.2675883769989014, 3.2415120601654053, 3.2439682483673096, 3.21799898147583, 3.207578659057617, 3.2076492309570312, 3.3214356899261475, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 1.7193019390106201, 1.679825782775879, 1.650679349899292, 1.6179615259170532, 1.5983997583389282, 1.5532128810882568, 1.5364797115325928, 1.5386948585510254, 1.5013322830200195, 1.4845850467681885, 1.4534012079238892, 1.444892168045044, 1.4262295961380005, 1.4062203168869019, 1.4057549238204956, 1.3824982643127441, 1.367220163345337, 1.3536320924758911, 1.3183528184890747, 1.3393183946609497, 1.312820553779602, 1.3034974336624146, 1.2918084859848022, 1.2986373901367188, 1.2754086256027222, 1.271276831626892, 1.2715413570404053, 1.2496140003204346, 1.220840334892273, 1.259627103805542, 1.2267930507659912, 1.2130810022354126, 1.2276153564453125, 1.2212096452713013, 1.281704306602478, 1.3779208660125732, 1.5410783290863037, 1.7324031591415405, 1.9674873352050781, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 2.966913938522339, 2.879478931427002, 2.812376022338867, 2.7323076725006104, 2.667478322982788, 2.613341808319092, 2.5557971000671387, 2.4918932914733887, 2.439162492752075, 2.3888156414031982, 2.3447256088256836, 2.304054021835327, 2.2600510120391846, 2.2221076488494873, 2.216076374053955, 2.2552261352539062, 2.31876802444458, 2.357189178466797, 2.396010398864746, 2.4466495513916016, 2.4985275268554688, 2.564828395843506, 2.634127140045166, 2.671659231185913, 2.7563955783843994, 2.829598903656006, 2.9014203548431396, 2.9869801998138428, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]
o = Mapper()
prob_map = Mapper.main(o,scan)
d =Dijkstra()
d(prob_map , [36,36], [50,30])
print('\n ' + '-'*30 + "\n> Starting operation ...\n " + '-'*30 + '\n')
start_time = time.time()
final_path = d.find_path()
print('\n ' + '-'*30 + "\n> Time taken: {:.4} seconds.\n ".format(time.time() - start_time) + '-'*30 + '\n')
d.draw_final_graph(final_path)
def query(self, start, goal):
# Find nearest node to start/goal
start_prm = self.find_nearest_node(self.V, start)
goal_prm = self.find_nearest_node(self.V, goal)
# If nearest node cannot be found, self.path = None
if start_prm is None or goal_prm is None:
self.path = None
# else, find shortest path (Dijkstra's algorithm)
else:
djk = Dijkstra(self.V, self.E)
djk.build(start_prm)
djk.query(goal_prm)
if djk.path is not None:
self.path = np.vstack(
(self.world.start, djk.path, self.world.goal))
else:
self.path = None
def runRerouting(obstacle_list, start_grid, end_grid, is_save=False):
# start and goal position
sx = start_grid[0] # [m]
sy = start_grid[1] # [m]
gx = end_grid[0] # [m]
gy = end_grid[1] # [m]
grid_size = 0.4 # [m]
robot_radius = 0.3 # [m]
# set obstacle positions
ox, oy = [], []
path = []
for i in range(len(obstacle_list)):
ox.append(obstacle_list[i][0])
oy.append(obstacle_list[i][1])
if show_animation: # pragma: no cover
plt.title("Rerouting")
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "og")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
rx, ry = dijkstra.planning(sx, sy, gx, gy)
for i in range(len(rx)):
path.append([i, 1])
path.append([rx[i], ry[i]])
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
if is_save:
fig = plt.gcf()
fig.savefig(save_path + 'task3_rerouted.png')
plt.pause(0.01)
plt.show()
plt.close('all')
return path
def __init__(self):
self.fr = FlowRetrieval()
self.fs = FlowStatistics()
self.fm = FlowManagement()
self.dj = Dijkstra()
self.g = Graph()
#self.fm.deleteAllFlow()
try:
self.hostlist = self.fs.list_of_hosts()
self.switchlist = self.fs.list_of_switches()
except:
traceback.print_exc()
self.hostdict = {}
for i in range(1, len(self.hostlist) + 1):
self.hostdict[i] = self.hostlist[i - 1]
self.switchdict = {}
for i in range(1, len(self.switchlist) + 1):
self.switchdict[i] = self.switchlist[i - 1]
def x_reach(edge_id, max_cross, adj_list, na_seg_len):
"""Conduct intersection reach analysis.
:param edge_id: the ID of the source edge
:param max_cross: the maximum number of intersections allowed to cross
:param adj_list: an adjacency list, e.g., G = [{12: 0, 1: 1, 24: 1}, {0: 1, 24:1, 2: 1, 16:1}, ...]
indicates that the edge 0 is adjacent with edges 12, 1, and 24, and the cross distances from
those edges are G[0][12] = 0, G[0][1] = 1, and G[0][24] = 1.
:param na_seg_len: a list listing the length of each line segment
:return: the total street length accessible within max_cross intersections and
the list of lines that can be reached
"""
D, P = Dijkstra(adj_list, edge_id)
# find all the edges that are no more than max_cross intersections away from the source edge
list_reached_edges = [k for k, v in D.items() if v <= max_cross]
# compute the total length of the reached edges
total_len = 0
for i in list_reached_edges:
total_len += na_seg_len[i]
return total_len, list_reached_edges
def lookup(self, x, y):
u, v = min(x, y), max(x, y)
if self.G.get((u, v), -1) > -1:
return [self.G[(u, v)], self.G[(u, v)]]
ub = 1
lb = 0
dijk_obj = Dijkstra(self._dbl_lvl_dict, self.noNodes)
x_sp = dijk_obj.shortest_path(self.vertices, x)
y_sp = dijk_obj.shortest_path(self.vertices, y)
if y not in x_sp:
return [lb, ub]
if x_sp[y][0] < ub:
ub = x_sp[y][0]
for (a, b) in self.G.items():
s, t = a
if s in x_sp and t in x_sp:
lb = max(lb, b - x_sp[s][0] - y_sp[t][0],
b - x_sp[t][0] - y_sp[s][0])
return [lb, ub]
