updated bfs - works

main.py

@@ -1,9 +1,11 @@
 import sys
+import time
 import pygame
 import regal
 import paczka
 from wozek import Wozek
-from wyszukiwanie import wyszukiwanie_bfs, Stan, SearchSpace
+from wyszukiwanie import Stan, SearchSpace, Search, GridCellType
+from typing import Tuple, Dict
 
 
 pygame.init()
@@ -14,54 +16,35 @@ 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.state.x, self.y))
 
 def main():
-    wozek = Wozek()
-    while True:
-        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)
-    
-        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
 
-        # perform BFS search
-        path = wyszukiwanie_bfs(start_state, goal_state, grid)
+    wozek = Wozek(0,0)
 
-        # 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 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)
 
-        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)
+    grid_points = SearchSpace()
 
+    goal_state = Stan(5, 2, 0) # for example
+            # perform BFS search
+    path = Search(grid_points).wyszukiwanie_bfs(wozek.state, goal_state)
 
-                # Drawing
+    if path:
+        for p in path:
+            wozek._move(p)
             screen.fill((51,51,51))  # removes object trail
             screen.blit(miejsce, (430, 400))
-
-                # 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(5, 1, 8, 2)
             regal.Regal(6, 1, 8, 3)
             regal.Regal(7, 1, 9, 2)
@@ -76,10 +59,111 @@ def main():
             regal.Regal(14, 1, 8, 9)
             regal.Regal(15, 1, 9, 8)
             regal.Regal(16, 1, 9, 9)
-
             wozek.draw()
+            pygame.display.flip()
+            time.sleep(1) 
 
-            pygame.display.flip()  # updating frames
+            # wozek._drive(path)
+            # print(path)
+
+            # for action in path:
+            #     x = wozek.state.x
+            #     y = wozek.state.y
+            #     direction = wozek.state.kierunek
+
+            #     print(x)
+
+            #     if action == "Left":
+            #         wozek.state.kierunek = 3 if direction == 0 else direction - 1
+
+            #     # always legal
+            #     elif action == "Right":
+
+            #         # self._truck_direction(direction, action)
+            #         wozek.state.kierunek = (direction + 1) % 4
+
+            #     # check if its legal
+            #     elif action == "Forward":
+            #         t_x = x + 1 if direction == 1 else (x - 1 if direction == 3 else x)
+            #         t_y = y - 1 if direction == 0 else (y + 1 if direction == 2 else y)
+
+            #         # if self.points_grid[t_y][t_x].is_available:
+            #         wozek.state.x = t_x
+            #         wozek.state.y = t_y
+
+                    
+            #         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()
+
+            #         pygame.display.flip() 
+
+        #     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)
+
+
+        #         # Drawing
+        #     screen.fill((51,51,51))  # removes object trail
+        #     screen.blit(miejsce, (430, 400))
+
+        #         # 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(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()
+
+        #     pygame.display.flip()  # updating frames
 
 
 if __name__ == "__main__":

wozek.py

@@ -1,24 +1,34 @@
+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
-        self.x_change = 0
-        self.y_change = 0
+    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*98, self.state.y*98))
 
     # storage = ["none"] * 10
     storage = []
@@ -47,10 +57,51 @@ 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
+        direction = self.state.kierunek
+
+        # always legal
+        if action == "Left":
+
+            # self._truck_direction(direction, action)
+            self.state.kierunek = 3 if direction == 0 else direction - 1
+
+        # always legal
+        elif action == "Right":
+            # self._truck_direction(direction, action)
+            self.state.kierunek = (direction + 1) % 4
+
+        # check if its legal
+        elif action == "Forward":
+            t_x = x + 1 if direction == 1 else (x - 1 if direction == 3 else x)
+            t_y = y - 1 if direction == 0 else (y + 1 if direction == 2 else y)
+
+            # if self.points_grid[t_y][t_x].is_available:
+            self.state.x = t_x
+            self.state.y = t_y
+            # else:
+            #     print("[ MOVE LOG ] - You can't move in that direction!")
+
+
 
     # def ustaw_wozek_w_kierunku(self, kierunek):
     # TODO

wyszukiwanie.py

@@ -1,67 +1,282 @@
+from enum import Enum
+
+from typing import Tuple, Dict
+
+class GridCellType(Enum):
+    NOTHING = 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.NOTHING
         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
+            self.grid[(r,c)] = GridCellType.REGAL
 
-    def is_passable(self, x, y):
-        return 0 <= x < 10 and 0 <= y < 10 and self.grid[y][x] != 'X'
-    
 class Stan:
     def __init__(self, x, y, kierunek):
         self.x = x
         self.y = y
         self.kierunek = kierunek
 
+
 class Wezel:
-    def __init__(self, stan):
+    def __init__(self, stan: Stan, action = None, parent = None):
         self.stan = stan
+        self.action = action
+        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, points_grid: SearchSpace):
+        self.points_grid = points_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)
+
+            for action, stan in self.nastepnik(wezel):
+                if wezel not in fringe and stan not in odwiedzone:
+                    x_node = Wezel(stan, action, wezel)
+                    fringe.append(x_node)
+        return False
     
-    odwiedzone = set()
+    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
+            
+    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
+        right_state = Stan( state.x,  state.y,(state.kierunek + 1) % 4)
 
-    while fringe:
-        wezel = fringe.pop(0)
+        right_node = ["Right", right_state]
 
-        if stan_docelowy.x == wezel.stan.x and stan_docelowy.y == wezel.stan.y:
-            return odwiedzone
+        # 90 degree left state - cost of turn is 1
+        left_state = Stan (state.x, state.y,3 if state.kierunek == 0 else state.kierunek - 1)
 
-        odwiedzone.add(wezel.stan)
+        # storing temporarily node as list
+        left_node = ["Left", left_state]
 
-        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
+        # always two nodes are possible because we can turn in both sides
+        nodes = [right_node, left_node]
+
+        y = state.x
+        x = state.y
+        # obrot_w_prawo = Stan(x, y, (wezel.stan.kierunek + 1) % 4)
+        # obrot_w_lewo = Stan(x, y, (wezel.stan.kierunek - 1) % 4)
+
+        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.points_grid.grid[(y,x)]
+        
+        if place is not None and place is GridCellType.NOTHING:
+            ruch_w_przod = Stan(y, x, wezel.stan.kierunek)
+            forward_node = ["Forward", ruch_w_przod]
+            nodes.append(forward_node)
+
+        # sprawdzenie czy nie wyjdzie poza plansze
+        # if 0 <= x <= 916 and 0 <= y <= 916:
+        #     wezly.append(ruch_w_przod)
+        
+        return nodes
+
+    def get_actions(self, wezel: Wezel):
+        actions = []
+        moves_forward = 0
+        turns = 0
+        parent = wezel
+        while True:
+            action = parent.action
+            parent = parent.parent
+            if action is None:
+                break
+
+            if action == "Forward":
+                moves_forward = moves_forward + 1
+            else:
+                turns = turns + 1
+
+            actions.append(action)
+
+        actions.reverse()
+
+        return actions
+    
+
+
+
+
+
+
+
+
+
+# class Kierunek(Enum):
+#     POLNOC = 0
+#     POLUDNIE = 1
+#     ZACHOD = 2
+#     WSCHOD = 3
+#     def kierunekNaLewo(self):
+#         if self == Kierunek.POLNOC:
+#             return Kierunek.ZACHOD
+#         elif self == Kierunek.POLUDNIE:
+#             return Kierunek.WSCHOD
+#         elif self == Kierunek.ZACHOD:
+#             return Kierunek.POLUDNIE
+#         elif self == Kierunek.WSCHOD:
+#             return Kierunek.POLNOC
+
+#     def kierunekNaPrawo(self):
+#         if self == Kierunek.POLNOC:
+#             return Kierunek.WSCHOD
+#         elif self == Kierunek.POLUDNIE:
+#             return Kierunek.ZACHOD
+#         elif self == Kierunek.ZACHOD:
+#             return Kierunek.POLNOC
+#         elif self == Kierunek.WSCHOD:
+#             return Kierunek.POLUDNIE
+
+# class Akcja(Enum):
+#     OBROT_W_LEWO = 0
+#     OBROT_W_PRAWO = 1
+#     KROK_W_PRZOD = 2
+
+# class Stan:
+#     def __init__(self, kierunek: Kierunek, x, y):
+#         self.x = x
+#         self.y = y
+#         self.kierunek = kierunek
+
+#     def skopiuj(self):
+#         return Stan(self.x, self.y, self.kierunek)
+    
+# class Nastepnik:
+#     def __init__(self, akcja: Akcja or None, stan: Stan, poprzednik):
+#         self.akcja = akcja
+#         self.stan = stan
+#         self._poprzednik = poprzednik
+
+#     def getPoprzednik(self):
+#         return self._poprzednik
+
+#     def setPoprzednik(self, x):
+#         raise Exception
+
+#     poprzednik = property(getPoprzednik, setPoprzednik)
+
+#     def skopiuj(self):
+#         return Nastepnik(self.akcja, self.stan.skopiuj(), self.poprzednik)
+
+# def goaltest(stan: Stan, cel: Stan):
+#     if stan.x != cel.x:
+#         return False
+#     elif stan.y != cel.y:
+#         return False
+#     else:
+#         return True
+
+# def stos_akcji(stan_koncowy: Nastepnik):
+#     stos = deque()
+#     while stan_koncowy.poprzednik is not None:
+#         stos.append(stan_koncowy.akcja)
+#         stan_koncowy = stan_koncowy.poprzednik
+#     return stos
+
+# def stan_w_liscie_nastepnikow(stan: Stan, lista_nastepnikow):
+#     for i in lista_nastepnikow:
+#         if i.stan.kierunek != stan.kierunek:
+#             continue
+#         elif i.stan.x != stan.x:
+#             continue
+#         elif i.stan.y != stan.y:
+#             continue
+#         else:
+#             return True
+#     return False
+
+# def nastepnik_kroku_w_przod(nastepnik: Nastepnik):
+#     akcja = Akcja.KROK_W_PRZOD
+#     stan = Stan(nastepnik.stan.kierunek, nastepnik.stan.x, nastepnik.stan.y)
+#     if stan.kierunek == Kierunek.POLNOC:
+#         stan.x -= 1
+#     elif stan.kierunek == Kierunek.POLUDNIE:
+#         stan.x += 1
+#     elif stan.kierunek == Kierunek.ZACHOD:
+#         stan.y -= 1
+#     elif stan.kierunek == Kierunek.WSCHOD:
+#         stan.y += 1
+#     return Nastepnik(akcja, stan, nastepnik)
+
+# def nastepnik_obrotu_w_lewo(nastepnik: Nastepnik):
+#     akcja = Akcja.OBROT_W_LEWO
+#     stan = Stan(nastepnik.stan.kierunek.kierunekNaLewo(), nastepnik.stan.x, nastepnik.stan.y)
+#     return Nastepnik(akcja, stan, nastepnik)
+# #Nastepnik(Akcja.OBROT_W_LEWO, Stan(nastepnik.stan.kierunek.kierunekNaLewo(), nastepnik.stan.poleStartoweGorne), nastepnik)
+# #
+
+# def nastepnik_obrotu_w_prawo(nastepnik: Nastepnik):
+#     akcja = Akcja.OBROT_W_PRAWO
+#     stan = Stan(nastepnik.stan.kierunek.kierunekNaPrawo(),nastepnik.stan.x, nastepnik.stan.y)
+#     return Nastepnik(akcja, stan, nastepnik)
+
+# def succ(nastepnik: Nastepnik):
+#     wynik = []
+#     pom = nastepnik.skopiuj()
+#     #wynik.append(nastepnik_obrotu_w_lewo(pom))
+#     wynik.append(nastepnik_obrotu_w_lewo(pom))
+#     pom = nastepnik.skopiuj()
+#     wynik.append(nastepnik_obrotu_w_prawo(pom))
+#     pom = nastepnik.skopiuj()
+
+#     wynik.append(nastepnik_kroku_w_przod(pom))
+#     return wynik
+
+# def graphsearch(istate: Stan, cel: Stan):
+#     fringe = deque()
+#     explored = []
+#     fringe.append(Nastepnik(None, istate, None))
+#     while fringe:
+#         # for i in fringe:
+#         #     print("F",i.stan.kierunek,i.stan.poleStartoweGorne.wiersz,i.stan.poleStartoweGorne.kolumna,end=" ")
+#         # print()
+#         element: Nastepnik = fringe.popleft()
+#         if goaltest(element.stan, cel):
+#             return stos_akcji(element)
+#         explored.append(element)
+#         for nastepnik in succ(element):
+#             if not stan_w_liscie_nastepnikow(nastepnik, fringe) and not stan_w_liscie_nastepnikow(nastepnik, explored):
+#                 fringe.append(nastepnik)
+#     return False