feat: astar

Environment.py

@@ -1,6 +1,8 @@
 import pygame
+
 from Shelf import Shelf
 from Grid import Grid
+from num_map import num_matrix
 
 HORIZONTAL = 1250
 VERTICAL = 750
@@ -15,8 +17,18 @@ class Environment:
         self.window = window
         self.grid = Grid(self.window)
         self.Shelf = Shelf(self.window)
+        self.image = pygame.image.load('resources/texture5.PNG').convert_alpha()
+        self.rock = pygame.image.load('resources/texture4.PNG').convert_alpha()
+        self.dirt = pygame.image.load('resources/texture3.PNG').convert_alpha()
 
     def drawEnviroment(self):
-        self.window.fill(background)
+        for j in range(0, int(VERTICAL / TILE_SIZE)):
+            for i in range(0, int(HORIZONTAL / TILE_SIZE)):
+                if num_matrix[j][i] == 0:
+                    self.window.blit(self.image, (i * TILE_SIZE, j * TILE_SIZE))
+                elif str(num_matrix[j][i]) in 'r':
+                    self.window.blit(self.rock, (i * TILE_SIZE, j * TILE_SIZE))
+                elif str(num_matrix[j][i]) in 'd':
+                    self.window.blit(self.dirt, (i * TILE_SIZE, j * TILE_SIZE))
         self.Shelf.drawShelves()
-        self.grid.drawGrid()
+        self.grid.drawGrid()

Grid.py

@@ -1,12 +1,11 @@
 import pygame
-import numpy as np
 
 HORIZONTAL = 1250
 VERTICAL = 750
 
 TILE_SIZE = 50
 
-line_color = (85, 85, 85)
+line_color = (0, 0, 0)
 
 
 class Grid:

Package.py

@@ -1,4 +1,3 @@
-import numpy as np
 import pygame
 import glob2
 import random

Program.py

@@ -1,8 +1,7 @@
 import pygame
-import bfs
 import random
+import a_star
 
-from num_map import num_matrix
 from pygame.locals import *
 from Forklift import Forklift
 
@@ -11,14 +10,14 @@ 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))
+        commands = a_star.a_star(state=initial_state, goal=(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))
+        commands = a_star.a_star(state=initial_state, goal=(row, col))
         print('I need to go to row: ' + str(row) + ' column: ' + str(col))
     return commands
 
@@ -32,69 +31,28 @@ class Program:
         self.agent.drawForklift()
 
         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)
+        initial_state = a_star.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:
-                        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:
-                        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:
-                        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:
-                        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)
+
+            initial_state.row = int(self.agent.y / 50)
+            initial_state.column = int(self.agent.x / 50)
+            initial_state.direction = self.agent.direction

Shelf.py

@@ -1,5 +1,6 @@
 import pygame
-import numpy as np
+
+from num_map import num_matrix
 
 HORIZONTAL = 1250
 VERTICAL = 750
@@ -14,12 +15,10 @@ class Shelf:
         self.color = shelf
         self.width = 5 * TILE_SIZE
         self.height = TILE_SIZE
+        self.image = pygame.image.load('resources/shelf.PNG').convert_alpha()
 
     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))
-
-        for k in range(VERTICAL - 5 * TILE_SIZE, VERTICAL, 2 * TILE_SIZE):
-            for l in range(TILE_SIZE + 1, HORIZONTAL - TILE_SIZE, 6 * TILE_SIZE):
-                pygame.draw.rect(self.window, shelf, pygame.Rect(l, k, self.width, self.height))
+        for j in range(0, int(VERTICAL / TILE_SIZE)):
+            for i in range(0, int(HORIZONTAL / TILE_SIZE)):
+                if str(num_matrix[j][i]) in 's':
+                    self.window.blit(self.image, (i * TILE_SIZE, j * TILE_SIZE))

a_star.py

@@ -0,0 +1,156 @@
+import heapq
+
+from num_map import num_matrix
+
+MAX_ROWS = 15
+MAX_COLS = 25
+
+mapping = {
+    'd': 10,
+    's': 20,
+    'r': 999
+}
+
+
+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
+
+    def __eq__(self, state: "State"):
+        return (state.row, state.column, state.direction) == (self.row, self.column, self.direction)
+
+    def __lt__(self, state: "State"):
+        return (self.row, self.column) < (state.row, state.column)
+
+    def __hash__(self):
+        return hash((self.row, self.column))
+
+
+class Node:
+    def __init__(self, state: "State", parent=None, action=None, cost=1):
+        self.state = state
+        self.parent = parent
+        self.action = action
+        self.cost = cost
+
+    def __lt__(self, node):
+        return self.state < node.state
+
+
+def h(state: State, goal: tuple[int, int]):
+    """
+    Heuristics calculating Manhattan distance
+    """
+    x1, y1 = state.row, state.column
+    x2, y2 = goal
+    return abs(x1 - x2) + abs(y1 + y2)
+
+
+def f(curr_node: Node, goal: tuple[int, int]):
+    """
+    f(n) = g(n) + h(n)
+    """
+    return curr_node.cost + h(state=curr_node.state, goal=goal)
+
+
+def goal_test(goal_list, state: State):
+    if (state.row, state.column) == goal_list:
+        return True
+    return False
+
+
+def is_valid_move(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"):
+    successors = list()
+
+    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 state.direction == 0:
+        if is_valid_move(target_row=state.row-1, target_column=state.column):
+            forward = State(row=state.row-1, column=state.column, direction=state.direction)
+            successors.append(('go', forward))
+    elif state.direction == 1:
+        if is_valid_move(target_row=state.row, target_column=state.column+1):
+            forward = State(row=state.row, column=state.column+1, direction=state.direction)
+            successors.append(('go', forward))
+    elif state.direction == 2:
+        if is_valid_move(target_row=state.row+1, target_column=state.column):
+            forward = State(row=state.row+1, column=state.column, direction=state.direction)
+            successors.append(('go', forward))
+    elif state.direction == 3:
+        if is_valid_move(target_row=state.row, target_column=state.column-1):
+            forward = State(row=state.row, column=state.column-1, direction=state.direction)
+            successors.append(('go', forward))
+
+    return successors
+
+
+def get_path_from_start(node: Node):
+    path = [node.action]
+
+    while node.parent is not None:
+        node = node.parent
+        if node.action:
+            path.append(node.action)
+
+    path.reverse()
+    return path
+
+
+def check_cost(row: int, col: int, action):
+    if action in ('rotate_left', 'rotate_right'):
+        return 1
+    else:
+        return mapping.get(num_matrix[row][col], 1)
+
+
+def a_star(state: State, goal: tuple[int, int]):
+    node = Node(state=state, parent=None, action=None)
+
+    fringe = list()
+    heapq.heappush(fringe, (f(node, goal), node))
+    explored_states = set()
+
+    while fringe:
+        r, node = heapq.heappop(fringe)
+
+        if goal_test(goal, node.state):
+            return get_path_from_start(node)
+
+        explored_states.add(node.state)
+
+        for successor in get_successor(node.state):
+            action, next_state = successor
+            movement_cost = check_cost(row=next_state.row, col=next_state.column, action=action)+node.cost
+            child = Node(state=next_state, parent=node, action=action, cost=movement_cost)
+
+            p = f(child, goal=goal)
+
+            if child.state not in explored_states and (p, child) not in fringe:
+                heapq.heappush(fringe, (p, child))
+            elif (r, child) in fringe and r > p:
+                heapq.heappush(fringe, (p, child))

bfs.py

@@ -32,8 +32,6 @@ class Node:
         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

main.py

@@ -1,5 +1,3 @@
-import pygame
-from pygame.locals import *
 from Program import Program
 
 if __name__ == "__main__":

num_map.py

@@ -1,15 +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]]
+num_matrix = [[0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 'd', 0, 0, 0, 'd', 0, 0, 0, 0, 0, 0],
+              [0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
+              ['d', 0, 0, 0, 0, 0, 0, 'd', 0, 'd', 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 'd', 'd', 'd', 'd'],
+              [0, 's', 's', 's', 's', 's', 'd', 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
+              [0, 0, 'd', 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 0, 'd', 0, 'd', 0, 0, 0, 0, 0, 0, 0],
+              [0, 'd', 0, 0, 'd', 0, 0, 'd', 0, 0, 0, 'd', 0, 0, 0, 0, 'd', 0, 'd', 'd', 'd', 0, 'r', 'r', 0],
+              ['d', 0, 'd', 0, 0, 0, 0, 0, 'd', 0, 'd', 0, 0, 'd', 0, 'd', 'd', 'd', 'd', 0, 0, 0, 'r', 0, 0],
+              [0, 0, 0, 0, 0, 'd', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 0, 'r', 'r', 0],
+              ['d', 0, 0, 0, 0, 0, 0, 'd', 'd', 0, 0, 'd', 0, 0, 0, 'd', 0, 0, 0, 'd', 0, 0, 0, 0, 0],
+              [0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 'd', 'd', 'd', 0, 0, 'd', 0, 'd', 0, 0],
+              ['d', 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
+              [0, 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 0, 'd', 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 0],
+              [0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0]]