bfs works, bfs_main.py

bfs_main.py

@@ -0,0 +1,56 @@
+import sys
+import time
+import pygame
+import regal
+import paczka
+from wozek import Wozek
+from wyszukiwanie import Stan, SearchSpace, Search
+
+pygame.init()
+screen = pygame.display.set_mode((980, 980))
+miejsce = pygame.image.load('images/miejsce_paczek.png')
+
+pygame.display.set_caption("Inteligentny wozek")
+icon = pygame.image.load('images/icon.png')
+pygame.display.set_icon(icon)
+
+def main():
+
+    wozek = Wozek(0,0)
+
+    grid_points = SearchSpace()
+
+    goal_state = Stan(5, 2, 0)
+
+    path = Search(grid_points).wyszukiwanie_bfs(wozek.state, goal_state)
+
+    if path:
+        for p in path:
+            wozek._move(p)
+            screen.fill((51,51,51))  # removes object trail
+            screen.blit(miejsce, (430, 400))
+            regal.Regal(1, 1, 2, 2)
+            regal.Regal(2, 1, 2, 3)
+            regal.Regal(3, 1, 3, 2)
+            regal.Regal(4, 1, 3, 3)
+
+            regal.Regal(5, 1, 8, 2)
+            regal.Regal(6, 1, 8, 3)
+            regal.Regal(7, 1, 9, 2)
+            regal.Regal(8, 1, 9, 3)
+
+            regal.Regal(9, 1, 2, 8)
+            regal.Regal(10, 1, 2, 9)
+            regal.Regal(11, 1, 3, 8)
+            regal.Regal(12, 1, 3, 9)
+
+            regal.Regal(13, 1, 8, 8)
+            regal.Regal(14, 1, 8, 9)
+            regal.Regal(15, 1, 9, 8)
+            regal.Regal(16, 1, 9, 9)
+            wozek.draw_bfs()
+            pygame.display.flip()
+            time.sleep(1) 
+
+if __name__ == "__main__":
+    main()

main.py

@@ -1,10 +1,8 @@
 import sys
 import pygame
 import regal
-import paczka
+from paczka import Paczka
 from wozek import Wozek
-from wyszukiwanie import wyszukiwanie_bfs, Stan, SearchSpace
-
 
 pygame.init()
 screen = pygame.display.set_mode((980, 980))
@@ -14,9 +12,14 @@ pygame.display.set_caption("Inteligentny wozek")
 icon = pygame.image.load('images/icon.png')
 pygame.display.set_icon(icon)
 
+def draw(self):
+    screen.blit(self.image, (self.x, self.y))
+
+
 
 def main():
     wozek = Wozek()
+
     while True:
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
@@ -24,64 +27,69 @@ def main():
             if event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE:
                 sys.exit(0)
 
-        grid = SearchSpace()
-        # create start and goal states
-        start_state = Stan(x=0, y=0, kierunek=0)  # for example
-        goal_state = Stan(x=0, y=1, kierunek=0)  # for example
+            if event.type == pygame.KEYDOWN:
+                if event.key == pygame.K_DOWN:
+                    wozek.y_change = 1
+                if event.key == pygame.K_UP:
+                    wozek.y_change = -1
+                if event.key == pygame.K_RIGHT:
+                    wozek.x_change = 1
+                if event.key == pygame.K_LEFT:
+                    wozek.x_change = -1
 
-        # perform BFS search
-        path = wyszukiwanie_bfs(start_state, goal_state, grid)
-
-        # print the path, if found
-        if path:
-            for p in path:
-                print(p.x, p.y)
-            print("Path found:", path)
-        else:
-            print("No path found.")
-
-        for state in path:
-            pygame.time.delay(200)
-            wozek.x = state.x
-            wozek.y = state.y
-            for event in pygame.event.get():
-                if event.type == pygame.QUIT:
-                    sys.exit(0)
-                if event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE:
-                    sys.exit(0)
+            if event.type == pygame.KEYUP:
+                if event.key == pygame.K_DOWN or event.key == pygame.K_UP:
+                    wozek.y_change = 0
+                if event.key == pygame.K_RIGHT or event.key == pygame.K_LEFT:
+                    wozek.x_change = 0
             
 
-                # Drawing
-            screen.fill((51,51,51))  # removes object trail
-            screen.blit(miejsce, (430, 400))
+        wozek.x += wozek.x_change
+        wozek.y += wozek.y_change
 
-                # idRegału, Długość regału podana w kratkach, Współrzędne od których ma być tworzony regał (wiersz,kolumna) - poziomo
-                # Współrzędne od (1,1) do (10,10)
-            regal.Regal(1, 1, 2, 2)
-            regal.Regal(2, 1, 2, 3)
-            regal.Regal(3, 1, 3, 2)
-            regal.Regal(4, 1, 3, 3)
+        if wozek.x <= 0:
+            wozek.x = 0
+        elif wozek.x >= 916:
+            wozek.x = 916
+        if wozek.y <= 0:
+            wozek.y = 0
+        elif wozek.x >= 916:
+            wozek.x = 916
 
-            regal.Regal(5, 1, 8, 2)
-            regal.Regal(6, 1, 8, 3)
-            regal.Regal(7, 1, 9, 2)
-            regal.Regal(8, 1, 9, 3)
+        # Drawing
+        screen.fill((51,51,51))  # removes object trail
+        screen.blit(miejsce, (430, 400))
 
-            regal.Regal(9, 1, 2, 8)
-            regal.Regal(10, 1, 2, 9)
-            regal.Regal(11, 1, 3, 8)
-            regal.Regal(12, 1, 3, 9)
+        # idRegału, Długość regału podana w kratkach, Współrzędne od których ma być tworzony regał (wiersz,kolumna) - poziomo
+        # Współrzędne od (1,1) do (10,10)
+        regal.Regal(1, 1, 2, 2)
+        regal.Regal(2, 1, 2, 3)
+        regal.Regal(3, 1, 3, 2)
+        regal.Regal(4, 1, 3, 3)
 
-            regal.Regal(13, 1, 8, 8)
-            regal.Regal(14, 1, 8, 9)
-            regal.Regal(15, 1, 9, 8)
-            regal.Regal(16, 1, 9, 9)
+        regal.Regal(5, 1, 8, 2)
+        regal.Regal(6, 1, 8, 3)
+        regal.Regal(7, 1, 9, 2)
+        regal.Regal(8, 1, 9, 3)
 
-            wozek.draw()
+        regal.Regal(9, 1, 2, 8)
+        regal.Regal(10, 1, 2, 9)
+        regal.Regal(11, 1, 3, 8)
+        regal.Regal(12, 1, 3, 9)
 
-            pygame.display.flip()  # updating frames
+        regal.Regal(13, 1, 8, 8)
+        regal.Regal(14, 1, 8, 9)
+        regal.Regal(15, 1, 9, 8)
+        regal.Regal(16, 1, 9, 9)
+
+        draw(wozek)
+        if wozek.ln == 0:
+            for x in packageList.list:
+                draw(x)
+        #demo_paczka.narysuj(430,400,screen)
+
+        pygame.display.flip()  # updating frames
 
 
 if __name__ == "__main__":
     main()
-

wozek.py

@@ -1,24 +1,39 @@
+import time
+from typing import Tuple
 import pygame
 
+from wyszukiwanie import Stan, SearchSpace
+
+# from main import screen, miejsce
+
 # screen nie działa
 
 class Wozek():
-    def __init__(self):
-        self.x = 55
-        self.y = 55
+    def __init__(self, y, x):
+        # self.x = 55
+        # self.y = 55
         self.x_change = 0
         self.y_change = 0
         self.height = 64
         self.width = 64
         self.image = pygame.image.load("images/pusty_wozek.png")
+        # self.screen = pygame.display.set_mode((980, 980))
+        # self.miejsce = pygame.image.load('images/miejsce_paczek.png')
         # Credit: Forklift icons created by Smashicons - Flaticon
         # https://www.flaticon.com/free-icons/forklift
-        self.__zainicjuj_stan_poczatkowy()
+        #self.__zainicjuj_stan_poczatkowy()
+        self.state = Stan(y, x, 0)
 
     def draw(self):
         from main import screen
        # screen.blit(self.image, (self.x, self.y))
-        screen.blit(self.image, (self.obecnyStan.x, self.obecnyStan.y))
+        #screen.blit(self.image, (self.obecnyStan.x, self.obecnyStan.y))
+        screen.blit(self.image, (self.state.x, self.state.y))
+    def draw_bfs(self):
+        from main import screen
+       # screen.blit(self.image, (self.x, self.y))
+        #screen.blit(self.image, (self.obecnyStan.x, self.obecnyStan.y))
+        screen.blit(self.image, (self.state.x*98, self.state.y*98))
 
     # storage = ["none"] * 10
     storage = []
@@ -47,10 +62,39 @@ class Wozek():
         elif (len(storage) == 10):
             wozek.image = pygame.image.load("images/pelny_wozek_full_3_crates.png")
 
-    def __zainicjuj_stan_poczatkowy(self):
-        from wyszukiwanie import Stan
-        # self.obecnyStan = Stan(55, 55, 3)
-        self.obecnyStan = Stan(0, 0, 3)
+    # def __zainicjuj_stan_poczatkowy(self):
+    #     from wyszukiwanie import Stan
+    #     # self.obecnyStan = Stan(55, 55, 3)
+    #     self.obecnyStan = Stan(1, 1, 3)
+
+    # def _drive(self, actions):
+    #     # for action in actions.reverse():
+    #     for action in actions:
+    #         self._move(action)
+    #         pygame.event.pump()
+    #         time.sleep(0.01)
+    #         # self.screen.fill((51,51,51))  # removes object trail
+    #         # self.screen.blit(self.miejsce, (430, 400))
+    #         self.draw()
+    #         pygame.display.flip() 
+            
+    def _move(self, action):
+        x = self.state.x
+        y = self.state.y
+        kierunek = self.state.kierunek
+
+        if action == "Lewo":
+            self.state.kierunek = 3 if kierunek == 0 else kierunek - 1
+
+        elif action == "Prawo":
+            self.state.kierunek = (kierunek + 1) % 4
+
+        elif action == "Do przodu":
+            new_x = x + 1 if kierunek == 1 else (x - 1 if kierunek == 3 else x)
+            new_y = y - 1 if kierunek == 0 else (y + 1 if kierunek == 2 else y)
+
+            self.state.x = new_x
+            self.state.y = new_y
 
     # def ustaw_wozek_w_kierunku(self, kierunek):
     # TODO

wyszukiwanie.py

@@ -1,12 +1,25 @@
+from enum import Enum
+
+from typing import Tuple, Dict
+
+class GridCellType(Enum):
+    FREE = 0
+    REGAL = 1
+    # dodać oznaczenie na miejsce dla paczek 
+
 class SearchSpace:
-    def __init__(self):
-        self.grid = [['1']*10 for _ in range(10)]  # create a 10x10 grid of cells
+    grid: Dict[Tuple[int, int], GridCellType] = {}
+
+    def __init__(self) -> None:
+        self._init_grid()
+
+    def _init_grid(self) -> None:
+        for i in range (0,10):
+            for j in range(0,10):
+                self.grid[(i, j)] = GridCellType.FREE
         for r, c in [(2,2), (2,3), (3,2), (3,3), (8,2), (8,3), (9,2), (9,3),
                      (2,8), (2,9), (3,8), (3,9), (8,8), (8,9), (9,8), (9,9)]:
-            self.grid[r][c] = 'X'  # set the cells with a shelf to not passable
-
-    def is_passable(self, x, y):
-        return 0 <= x < 10 and 0 <= y < 10 and self.grid[y][x] != 'X'
+            self.grid[(r,c)] = GridCellType.REGAL
 
 class Stan:
     def __init__(self, x, y, kierunek):
@@ -14,54 +27,99 @@ class Stan:
         self.y = y
         self.kierunek = kierunek
 
+
 class Wezel:
-    def __init__(self, stan):
+    def __init__(self, stan: Stan, akcja = None, parent = None):
         self.stan = stan
+        self.akcja = akcja
+        self.parent = parent
 
-def poprzednik(wezel, search_space):
- # gora -> prawo -> dol -> lewo          | obrot w prawo
-    # gora -> lewo -> dol -> prawo          | obrot w lewo
-    # 0 gora 1 prawo 2 dol 3 lewo
-    y = wezel.stan.x
-    x = wezel.stan.y
-    obrot_w_prawo = Stan(x, y, (wezel.stan.kierunek + 1) % 4)
-    obrot_w_lewo = Stan(x, y, (wezel.stan.kierunek - 1) % 4)
+class Search:
+    def __init__(self, search_grid: SearchSpace):
+        self.search_grid = search_grid
 
-    if wezel.stan.kierunek == 0:
-        y -= 1
-    elif wezel.stan.kierunek == 1:
-        x += 1
-    elif wezel.stan.kierunek == 2:
-        y += 1
-    elif wezel.stan.kierunek == 3:
-        x -= 1
+    def wyszukiwanie_bfs(self, stan_poczatkowy: Stan, stan_docelowy: Stan):
+        pierwszy_wezel = Wezel(stan_poczatkowy)
+        fringe = [pierwszy_wezel]
 
-    wezly = [obrot_w_prawo, obrot_w_lewo]
-    ruch_w_przod = Stan(x, y, wezel.stan.kierunek)
+        odwiedzone = []
 
-    # sprawdzenie czy nie wyjdzie poza plansze
-    if 0 <= x <= 916 and 0 <= y <= 916:
-        wezly.append(ruch_w_przod)
+        while fringe:
+            wezel = fringe.pop(0)
 
-    return wezly
+            if self.goal_test(state = wezel.stan, goal_state = stan_docelowy):
+                return self.get_actions(wezel)
 
-def wyszukiwanie_bfs(stan_poczatkowy, stan_docelowy, search_space):
-    pierwszy_wezel = Wezel(stan_poczatkowy)
-    fringe = [pierwszy_wezel]
+            odwiedzone.append(wezel.stan)
 
-    odwiedzone = set()
+            for akcja, stan in self.nastepnik(wezel):
+                if wezel not in fringe and stan not in odwiedzone:
+                    x_node = Wezel(stan, akcja, wezel)
+                    fringe.append(x_node)
+        return False
     
-    while fringe:
-        wezel = fringe.pop(0)
+    def goal_test(self, state: Stan, goal_state: Stan):
+            if state.x == goal_state.x and state.y == goal_state.y:
+                return True
+            else:
+                return False
             
-        if stan_docelowy.x == wezel.stan.x and stan_docelowy.y == wezel.stan.y:
-            return odwiedzone
+    def nastepnik(self, wezel: Wezel):
+        # gora -> prawo -> dol -> lewo          | obrot w prawo
+        # gora -> lewo -> dol -> prawo          | obrot w lewo
+        # 0 gora 1 prawo 2 dol 3 lewo
+        state = wezel.stan
 
-        odwiedzone.add(wezel.stan)
+        stan_prawego = Stan(state.x, state.y,(state.kierunek + 1) % 4)
+        ruch_w_prawo = ["Prawo", stan_prawego]
 
-        for stan in poprzednik(wezel, search_space):
-            if stan not in odwiedzone:
-                nowy_wezel = Wezel(stan)
-                fringe.append(nowy_wezel)
-                odwiedzone.add(stan)
-    return odwiedzone
+        stan_lewego = Stan(state.x, state.y, 3 if state.kierunek == 0 else state.kierunek - 1)
+        ruch_w_lewo = ["Lewo", stan_lewego]
+
+        nodes = [ruch_w_prawo, ruch_w_lewo]
+
+        y = state.x
+        x = state.y
+
+        if wezel.stan.kierunek == 0:
+            y -= 1
+        elif wezel.stan.kierunek == 1:
+            x += 1
+        elif wezel.stan.kierunek == 2:
+            y += 1
+        elif wezel.stan.kierunek == 3:
+            x -= 1
+
+        place = None
+
+        if 0 <= y < 98 and 0 <= x < 98:
+            place = self.search_grid.grid[(x,y)]
+        
+        if place is not None and place is GridCellType.FREE:
+            stan_w_przod = Stan(y, x, wezel.stan.kierunek)
+            ruch_w_przod = ["Do przodu", stan_w_przod]
+            nodes.append(ruch_w_przod)
+        
+        return nodes
+
+    def get_actions(self, wezel: Wezel):
+        akcje = []
+        moves_forward = 0
+        turns = 0
+        parent = wezel
+        while True:
+            akcja = parent.akcja
+            parent = parent.parent
+
+            if akcja is None:
+                break
+
+            if akcja == "Forward":
+                moves_forward = moves_forward + 1
+            else:
+                turns = turns + 1
+
+            akcje.append(akcja)
+        akcje.reverse()
+
+        return akcje