def print_dijkstra(G: Graph, s):
old_representation = G.representation_type
graph.change_graph_representation_to(
GraphRepresentationType.ADJACENCY_MATRIX)
p_s, d_s = dijkstra(G, s)
w = G.graph_weights
G.print_graph_representation()
G.print_weights()
print("\nSTART: s =", s)
for v in range(len(p_s)):
path = []
print('d({}) = {:>4} ===> '.format(v, d_s[v]), end='')
currNode = v
path.append(currNode)
while p_s[currNode] != None:
currNode = p_s[currNode]
path.append(currNode)
path = path[::-1] # reversing
print(path)
plot_graph(graph)
G.change_graph_representation_to(old_representation)
def johnson(g_rep, g_tup, weights, n):
g_rep_prim, g_tup_prim, weights_prim = add_node_to_graph(
g_rep, g_tup, n, weights)
d, p, bool = bellman_ford(g_rep_prim, weights_prim, n)
print(weights_prim)
if not bool:
print("G zawiera cykl o ujemnej wadze")
exit(-1)
h = d
w = {(u, v): weights_prim[(u, v)] + h[u] - h[v]
for (u, v) in weights_prim.keys()}
D = [[inf for j in range(n)] for i in range(n)]
adjList = {i: [] for i in range(n)}
for i, j in weights.keys():
adjList[i].append(j)
G = AdjacencyList({'list': adjList, 'position': {}, 'colors': {}})
for u in range(n):
d_prim, p_prim = dijkstra(G, w, u)
# print(f'd_prim {u}:', d_prim)
for v in range(n):
D[u][v] = d_prim[v] - h[u] + h[v]
return D
def lab3_task3(ret):
numberOfNodes = len(ret[0].nodeList)
distanceMatrix = [[] for _ in range(numberOfNodes)]
for i in range(numberOfNodes):
distanceMatrix[i].extend(*[dijkstra(ret[0], ret[1], i)[0].values()])
return distanceMatrix
def lab3_task2(numberOfNodes, numberOfEdges, startNode=1):
ret = rwg(numberOfNodes, numberOfEdges)[1]
dictOfEdges = {(i, j): z['weight']
for i, j, z in ret
} # kluczem jest krotka z krawedzia, wartoscia jest waga
adjList = {i: [] for i in range(numberOfNodes)}
for i, j in dictOfEdges.keys():
adjList[i].append(j)
adjList[j].append(i)
Graph = AdjacencyList({'list': adjList, 'position': {}, 'colors': {}})
return Graph, dictOfEdges, dijkstra(Graph, dictOfEdges, startNode)
def __init__(self):
rospy.init_node('eureka', anonymous=True)
### Subscriber
rospy.Subscriber('/robotis/sensor/camera/image_raw', Image, self.Imgcallback)
### Pub
self.planPub = rospy.Publisher('/thormang3/eureka_controller', eurekaCmd, queue_size=10)
self.w = 416 ### yolo input image w
self.h = 416 ### yolo input image h
self.arucoSize = 0.07
self.workSpaceW = 0.553#m
self.workSpaceH = 0.277 - self.arucoSize #m
self.ARw = int(self.workSpaceW * 2000) # for Perspective image
self.ARh = int(self.workSpaceH * 2000) # for Perspective image
self.isDetect4Marker = False
self.corners = None
self.ids = None
self.arr = np.zeros([480,640,3],dtype=np.uint8)
### getPerspectiveTransform dst array
self.Pdst = np.array([ [0,0],
[self.w,0],
[self.w,self.h],
[0,self.h]],np.float32)
### image from camera
self.img = np.zeros((480,640,3), np.uint8)
### Perspective image
self.warped = np.zeros((416,416,3), np.uint8)
### AR Perspective image
self.ARwarped = np.zeros((self.ARw,self.ARh,3), np.uint8)
###
self.robotStart = False
name2num = {
'init' : 44,
}
self.engine = engine.Engine(name2num)
###
self.turb = turb.Turb()
self.dijkstra = dijkstra.dijkstra()
orig = [['red','red'],\
['green','green'],\
['purple','purple']]
### yolo anchors
self.ANCHORS = [0.57273, 0.677385, 1.87446, 2.06253, 3.33843, 5.47434, 7.88282, 3.52778, 9.77052, 9.16828]
### build yolo model
self.mdir = os.path.dirname(os.path.abspath(__file__))
model_path = self.mdir + "/model_data/DrEurela.h5"
self.model = load_model(model_path)
self.model.summary()
for j in new_G[i]:
new_G[i][j] = 1. / new_G[i][j]
print len(new_G)
# Now new_G is weighted graph. The smallest weight means the minimum cost of path
search_list = [
'SPIDER-MAN/PETER PAR', 'GREEN GOBLIN/NORMAN ', 'WOLVERINE/LOGAN ',
'PROFESSOR X/CHARLES ', 'CAPTAIN AMERICA'
]
G = simple_G
total_result = 0
for i in search_list:
dist = dijkstra(new_G, search_list[0])
paths = shortest_paths(G, i)
costed_paths = {}
for j in paths:
cost = 0
for index in range(len(paths[j]) - 2):
node1 = paths[j][index]
node2 = paths[j][index + 1]
cost += new_G[node1][node2]
costed_paths[j] = cost
total_list = []
for i in costed_paths.keys():
if dist[i] != costed_paths[i]:
total_list.append(i)