feat: bfs

.idea/inspectionProfiles/profiles_settings.xml

@@ -1,5 +1,6 @@
 <component name="InspectionProjectProfileManager">
   <settings>
+    <option name="PROJECT_PROFILE" value="Default" />
     <option name="USE_PROJECT_PROFILE" value="false" />
     <version value="1.0" />
   </settings>

.idea/misc.xml

@@ -1,4 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.9 (sztuczna)" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.10 (SI projekt)" project-jdk-type="Python SDK" />
+  <component name="PythonCompatibilityInspectionAdvertiser">
+    <option name="version" value="3" />
+  </component>
 </project>

.idea/sztuczna.iml

@@ -4,7 +4,7 @@
     <content url="file://$MODULE_DIR$">
       <excludeFolder url="file://$MODULE_DIR$/venv" />
     </content>
-    <orderEntry type="inheritedJdk" />
+    <orderEntry type="jdk" jdkName="Python 3.10 (SI projekt)" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
 </module>

Environment.py

@@ -9,6 +9,7 @@ TILE_SIZE = 50
 
 background = (49, 51, 53)
 
+
 class Environment:
     def __init__(self, window):
         self.window = window

Forklift.py

@@ -4,15 +4,17 @@ from Package import Package
 
 TILE_SIZE = 50
 
+
 class Forklift:
     def __init__(self, window):
         self.window = window
-        self.image = pygame.image.load('resources/images.png').convert_alpha()
+        self.image = pygame.image.load('resources/forklift1.png').convert_alpha()
         self.x = 50
         self.y = 350
         self.environment = Environment(self.window)
         self.hasPackage = False
         self.package = Package(self.window)
+        self.direction = 1
 
     def drawForklift(self):
         self.environment.drawEnviroment()
@@ -35,3 +37,24 @@ class Forklift:
     def moveForkliftDown(self):
         self.y += TILE_SIZE
         self.drawForklift()
+
+    def rotate_forklift_left(self):
+        self.direction = (self.direction - 1) % 4
+        self.image = pygame.image.load('resources/forklift' + str(self.direction) + '.png').convert_alpha()
+        self.drawForklift()
+
+    def rotate_forklift_right(self):
+        self.direction = (self.direction + 1) % 4
+        self.image = pygame.image.load('resources/forklift' + str(self.direction) + '.png').convert_alpha()
+        self.drawForklift()
+
+    def forklift_move(self):
+        match self.direction:
+            case 0:
+                self.moveForkliftUp()
+            case 1:
+                self.moveForkliftRight()
+            case 2:
+                self.moveForkliftDown()
+            case 3:
+                self.moveForkliftLeft()

Grid.py

@@ -1,4 +1,5 @@
 import pygame
+import numpy as np
 
 HORIZONTAL = 1250
 VERTICAL = 750
@@ -7,6 +8,7 @@ TILE_SIZE = 50
 
 line_color = (85, 85, 85)
 
+
 class Grid:
     def __init__(self, window):
         self.window = window
@@ -21,4 +23,4 @@ class Grid:
             pygame.draw.line(self.window, line_color, (x, 0), (x, VERTICAL))
         for i in range(num_of_rows):
             y += TILE_SIZE
-            pygame.draw.line(self.window, line_color, (0, y), (HORIZONTAL, y))
+            pygame.draw.line(self.window, line_color, (0, y), (HORIZONTAL, y))

Package.py

@@ -5,6 +5,7 @@ import random
 
 TILE_SIZE = 50
 
+
 class Package:
     def __init__(self, window):
         self.window = window

Program.py

@@ -1,4 +1,8 @@
 import pygame
+import bfs
+import random
+
+from num_map import num_matrix
 from pygame.locals import *
 from Forklift import Forklift
 
@@ -7,6 +11,18 @@ VERTICAL = 750
 
 TILE_SIZE = 50
 
+## 4x6x5
+def handle_turn(initial_state, turn_count):
+    if turn_count % 2 == 0:
+        commands = bfs.graphsearch(initial_state=initial_state, goal_list=(7, 23))
+    else:
+        row = random.randint(0, 1) * 10 + 2 * random.randint(0, 2)
+        col = random.randint(1, 5) + 6 * random.randint(0, 3)
+        commands = bfs.graphsearch(initial_state=initial_state, goal_list=(row, col))
+        print('I need to go to row: ' + str(row) + ' column: ' + str(col))
+    return commands
+
+
 class Program:
     def __init__(self):
         pygame.init()
@@ -15,22 +31,70 @@ class Program:
         self.agent = Forklift(self.window)
         self.agent.drawForklift()
 
-    def run(self):
         running = True
+
+        currRow = 7
+        currCol = 1
+        currDirec = 1
+
+        turn_count = 0
+
+        initial_state = bfs.State(row=int(self.agent.y/50), column=int(self.agent.x/50), direction=self.agent.direction)
+
         while running:
+            initial_state.row = int(self.agent.y/50)
+            initial_state.column = int(self.agent.x/50)
+            initial_state.direction = self.agent.direction
+
+            commands = handle_turn(initial_state=initial_state, turn_count=turn_count)
+
             for event in pygame.event.get():
                 if event.type == KEYDOWN:
                     if event.key == K_RIGHT:
-                        self.agent.moveForkliftRight()
+                        if currCol + 1 < 25 and num_matrix[currRow][currCol + 1] == 0:
+                            num_matrix[currRow][currCol] = 0
+                            currCol = currCol + 1
+                            num_matrix[currRow][currCol] = -1
+                            self.agent.moveForkliftRight()
                     if event.key == K_LEFT:
-                        self.agent.moveForkliftLeft()
+                        if currCol - 1 > -1 and num_matrix[currRow][currCol - 1] == 0:
+                            num_matrix[currRow][currCol] = 0
+                            currCol = currCol - 1
+                            num_matrix[currRow][currCol] = -1
+                            self.agent.moveForkliftLeft()
                     if event.key == K_UP:
-                        self.agent.moveForkliftUp()
+                        if currRow - 1 > -1 and num_matrix[currRow - 1][currCol] == 0:
+                            num_matrix[currRow][currCol] = 0
+                            currRow -= 1
+                            num_matrix[currRow][currCol] = -1
+                            self.agent.moveForkliftUp()
                     if event.key == K_DOWN:
-                        self.agent.moveForkliftDown()
+                        if currRow + 1 < 15 and num_matrix[currRow + 1][currCol] == 0:
+                            num_matrix[currRow][currCol] = 0
+                            currRow += 1
+                            num_matrix[currRow][currCol] = -1
+                            self.agent.moveForkliftDown()
                     if event.key == K_ESCAPE:
                         running = False
                 elif event.type == pygame.QUIT:
                     running = False
 
-
+            while commands:
+                pygame.event.poll()
+                step = commands.pop(0)
+                if step == 'rotate_left':
+                    self.agent.rotate_forklift_left()
+                    currDirec = self.agent.direction
+                elif step == 'rotate_right':
+                    self.agent.rotate_forklift_right()
+                    currDirec = self.agent.direction
+                elif step == 'go':
+                    self.agent.forklift_move()
+                    currRow = int(self.agent.y/50)
+                    currCol = int(self.agent.x/50)
+                pygame.time.delay(250)
+                print('row: ' + str(currRow) + ' col: ' + str(currCol))
+                print(commands)
+            turn_count += 1
+            print(turn_count)
+            pygame.time.delay(2000)

Shelf.py

@@ -1,4 +1,5 @@
 import pygame
+import numpy as np
 
 HORIZONTAL = 1250
 VERTICAL = 750
@@ -6,6 +7,7 @@ VERTICAL = 750
 TILE_SIZE = 50
 shelf = (138, 126, 74)
 
+
 class Shelf:
     def __init__(self, window):
         self.window = window
@@ -14,7 +16,6 @@ class Shelf:
         self.height = TILE_SIZE
 
     def drawShelves(self):
-
         for j in range(0, 5 * TILE_SIZE, 2 * TILE_SIZE):
             for i in range(TILE_SIZE + 1, HORIZONTAL - TILE_SIZE, 6 * TILE_SIZE):
                 pygame.draw.rect(self.window, shelf, pygame.Rect(i, j, self.width, self.height))

bfs.py

@@ -0,0 +1,128 @@
+from typing import List
+from num_map import num_matrix
+
+MAX_ROWS = 15
+MAX_COLS = 25
+
+
+class State:
+    """
+    Directions
+    UP: 0
+    RIGHT: 1
+    DOWN: 2
+    LEFT: 3
+    """
+    def __init__(self, row, column, direction):
+        self.direction = direction
+        self.row = row
+        self.column = column
+
+    def rotate_left(self):
+        return (self.direction - 1) % 4
+
+    def rotate_right(self):
+        return (self.direction + 1) % 4
+
+
+class Node:
+    def __init__(self, state: "State", parent=None, action=None):
+        self.state = state
+        self.parent = parent
+        self.action = action
+
+
+## Temporarily removed collisons to allow bfs to work without putting in any extra work
+## Should be: if 0 <= target_row < MAX_ROWS and 0 < target_column < MAX_COLS and num_matrix[target_row][target_column] not in [1]:
+def is_valid_move(num_map, target_row, target_column):
+    if 0 <= target_row < MAX_ROWS and 0 < target_column < MAX_COLS:
+        return True
+    return False
+
+
+def get_successor(state: "State", num_matrix):
+    successors = list()
+    direction = state.direction
+    row = state.row
+    col = state.column
+
+    rotate_left = State(row=state.row, column=state.column, direction=state.rotate_left())
+    rotate_right = State(row=state.row, column=state.column, direction=state.rotate_right())
+
+    successors.append(('rotate_left', rotate_left))
+    successors.append(('rotate_right', rotate_right))
+
+    if direction == 0:
+        if is_valid_move(num_map=num_matrix, target_row=row-1, target_column=col):
+            forward = State(row=row-1, column=col, direction=direction)
+            successors.append(('go', forward))
+    elif direction == 1:
+        if is_valid_move(num_map=num_matrix, target_row=row, target_column=col+1):
+            forward = State(row=row, column=col+1, direction=direction)
+            successors.append(('go', forward))
+    elif direction == 2:
+        if is_valid_move(num_map=num_matrix, target_row=row+1, target_column=col):
+            forward = State(row=row+1, column=col, direction=direction)
+            successors.append(('go', forward))
+    elif direction == 3:
+        if is_valid_move(num_map=num_matrix, target_row=row, target_column=col-1):
+            forward = State(row=row, column=col-1, direction=direction)
+            successors.append(('go', forward))
+
+    return successors
+
+
+def graphsearch(initial_state: State, mp=num_matrix, goal_list=(6, 1), fringe: List[Node] = None, explored: List[Node] = None):
+    if fringe is None:
+        fringe = list()
+    if explored is None:
+        explored = list()
+    explored_states = set()
+    fringe_states = set()
+
+    fringe.append(Node(initial_state))
+    fringe_states.add((initial_state.row, initial_state.column, initial_state.direction))
+
+    while True:
+        if not any(fringe):
+            return []
+
+        element = fringe.pop(0)
+        fringe_states.remove((element.state.row, element.state.column, element.state.direction))
+
+        if is_reached(goal_list, element.state):
+            actions_sequence = [element.action]
+            parent = element.parent
+
+            while parent is not None:
+                if parent.action is not None:
+                    actions_sequence.append(parent.action)
+                parent = parent.parent
+
+            actions_sequence.reverse()
+            return actions_sequence
+
+        explored.append(element)
+        explored_states.add((element.state.row, element.state.column, element.state.direction))
+
+        for successor in get_successor(element.state, num_matrix=mp):
+            successor_state = (successor[1].row, successor[1].column, successor[1].direction)
+
+            if successor_state not in fringe_states and successor_state not in explored_states:
+                new_node = Node(state=successor[1],
+                                parent=element,
+                                action=successor[0])
+                fringe.append(new_node)
+                fringe_states.add((new_node.state.row, new_node.state.column, new_node.state.direction))
+
+
+def is_reached(goal_list, state: State):
+    comp = (state.row, state.column)
+    if comp == goal_list:
+        return True
+    return False
+
+
+if __name__ == "__main__":
+    initial_state = State(row=7, column=1, direction=1)
+    print(graphsearch(initial_state=initial_state))

num_map.py

@@ -0,0 +1,15 @@
+num_matrix = [[0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+              [0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+              [0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+              [0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+              [0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+              [0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+              [0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0]]