visual_algos/BFS.py at main · JustinCosta10/visual_algos · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
import numpy as np

graph = {}
rows = 20
cols = 30

for coord_x in range(rows):
    for coord_y in range(cols):
        neighbors = []
        if coord_x + 1 < rows:
            neighbors.append((coord_x + 1, coord_y))
        if coord_x - 1 >= 0:
            neighbors.append((coord_x - 1, coord_y))
        if coord_y + 1 < cols:
            neighbors.append((coord_x, coord_y + 1))
        if coord_y - 1 >= 0:
            neighbors.append((coord_x, coord_y - 1))
        graph[(coord_x,coord_y)] = neighbors

collision_map = np.random.rand(rows, cols)<0.1

start = (np.random.randint(0,rows), np.random.randint(0,cols))

goal = (np.random.randint(0,rows),np.random.randint(0,cols))


class BFS:
    def __init__(self, collision_map, graph : dict):
        self.collision_map = collision_map
        self.graph = graph


    def getNeighbors(self, coord): #checks all possible neighbors and filters out nodes with a collision
        neighbors = self.graph.get(coord, [])
        safe_neighbors = []
        for neighbor in neighbors:
            if self.collision_map[neighbor[0]][neighbor[1]] == False:
                safe_neighbors.append(neighbor)
        return safe_neighbors

    def path_trace(self, prev_nodes : dict, node : tuple):
        p = node
        path = []

        while p != start:
            path.append(p)
            p = prev_nodes.get(p)
        path.append(start)
        path.reverse()

        return path

    def search(self, start, goal):
        queue = [start]
        prev_nodes = {}
        visited = set()
        while queue:
            current_node = queue.pop(0)

            if current_node == goal:
                print("Goal found!")
                print(current_node)
                path = self.path_trace(prev_nodes, current_node)
                print(path)

                return path

            current_neighbors = self.getNeighbors(current_node)
            visited.add(current_node)
            for neighbor in current_neighbors:
                if neighbor not in visited and neighbor not in queue:
                    prev_nodes[neighbor] = current_node
                    queue.append(neighbor)
        print("Goal not found.")

if __name__ == "__main__":
    #Generating maps, start and goal
    graph = {}
    rows = 20
    cols = 30

    for coord_x in range(rows):
        for coord_y in range(cols):
            neighbors = []
            if coord_x + 1 < rows:
                neighbors.append((coord_x + 1, coord_y))
            if coord_x - 1 >= 0:
                neighbors.append((coord_x - 1, coord_y))
            if coord_y + 1 < cols:
                neighbors.append((coord_x, coord_y + 1))
            if coord_y - 1 >= 0:
                neighbors.append((coord_x, coord_y - 1))
            graph[(coord_x,coord_y)] = neighbors

    collision_map = np.random.rand(rows, cols)<0.1

    start = (np.random.randint(0,rows), np.random.randint(0,cols))

    goal = (np.random.randint(0,rows),np.random.randint(0,cols))

    #Running BFS
    bfs = BFS(collision_map, graph)
    result = bfs.search(start, goal)