Merge branch 'master' of https://git.wmi.amu.edu.pl/s452625/projektAI

src/main.py

@@ -1,5 +1,6 @@
 import pygame
 import random
+import time
 
 pygame.init()
 
@@ -8,11 +9,33 @@ WIDTH = 675
 HEIGHT = 675
 size = 75
 QTY_OF_PACKAGES = 8
+RECT_SIZE = 9
 
 IMAGE = pygame.image.load('img/wozek2.png')
 BACKGROUND = pygame.transform.scale(pygame.image.load('img/background.png'), (WIDTH, HEIGHT))
 DOCK = pygame.transform.scale(pygame.image.load('img/dock_left.png'), (75, 75))
 
+get_pix_from_position = {}
+
+
+def get_position_from_pix(pix_pos):
+    for key, value in get_pix_from_position.items():
+        if pix_pos == value:
+            return key
+    print("ERROR: THERE IS NO POSITION LIKE THIS (get_position_from_pix)")
+
+
+def create_postiions():
+    size_agent = 40
+    x_position_pix = size_agent
+    y_position_pix = size_agent
+    for position_x in range(RECT_SIZE):
+        for position_y in range(RECT_SIZE):
+            get_pix_from_position[(position_x, position_y)] = (x_position_pix, y_position_pix)
+            y_position_pix += size
+        y_position_pix = size_agent
+        x_position_pix += size
+
 
 def generate_package(a, b):
     rand_y = random.randint(1, 4)
@@ -22,6 +45,119 @@ def generate_package(a, b):
     p1 = Package((a, b), rand_x, name_list[rand_y-1], image_list[rand_y-1])
     return p1
 
+# FRINGE - struktura danych przechowująca wierzchołki do odwiedzenia
+# EXPLORED - lista odwiedzonych stanów
+# istate - stan początkowy
+# Succ - funkcja następnika
+# Goaltest -test spełnienia celu
+
+
+def breadth_first_search(istate, agent_direction):  # COORDINATES OF A START PLACE
+    fringe = []
+    explored = []
+    start = Node(istate, agent_direction)
+    fringe.append(start)
+    path = []
+
+    while True:
+        if not fringe:
+            return False
+
+        elem = fringe.pop(0)
+
+        if agent.goal_test(elem.state):
+            # return ciag akcji zbudowany z wykorzystaniem pol parent i action
+            while elem.parent is not None:
+                if type(elem.action) == list:
+                    elem.action.reverse()
+                    for each_action in elem.action:
+                        path.append(each_action)
+                else:
+                    path.append(elem.action)
+                elem = elem.parent
+
+            path.reverse()
+            return path
+
+        explored.append(elem)
+
+        for action, state, direction in elem.successor():
+            fringe_states = []
+            explored_states = []
+            for node in fringe:
+                fringe_states.append(node.state)
+            for node2 in explored:
+                explored_states.append(node2.state)
+
+            if state not in fringe_states and state not in explored_states:
+                x = Node(state, direction)
+                x.parent = elem
+                x.action = action
+                fringe.append(x)
+
+
+class Node:
+    def __init__(self, state, agent_direction, action=None, parent=None):
+        self.state = state
+        self.action = action
+        self.parent = parent
+        self.agent_direction = agent_direction
+
+    def successor(self):
+        neighbours = []
+        x = self.state[0]
+        y = self.state[1]
+
+        if x < 8:  # RIGHT NEIGHBOUR
+            if self.agent_direction == "right":
+                actions = "move"
+            elif self.agent_direction == "left":
+                actions = ["rotate_right", "rotate_right", "move"]
+            elif self.agent_direction == "up":
+                actions = ["rotate_right", "move"]
+            elif self.agent_direction == "down":
+                actions = ["rotate_left", "move"]
+
+            neighbours.append((actions, (x + 1, y), "right"))
+
+        if x > 0:  # LEFT NEIGHBOUR
+            if self.agent_direction == "right":
+                actions = ["rotate_left", "rotate_left", "move"]
+            elif self.agent_direction == "left":
+                actions = "move"
+            elif self.agent_direction == "up":
+                actions = ["rotate_left", "move"]
+            elif self.agent_direction == "down":
+                actions = ["rotate_right", "move"]
+
+            neighbours.append((actions, (x - 1, y), "left"))
+
+        if y > 0:  # UP NEIGHBOUR
+            if self.agent_direction == "right":
+                actions = ["rotate_left", "move"]
+            elif self.agent_direction == "left":
+                actions = ["rotate_right", "move"]
+            elif self.agent_direction == "up":
+                actions = "move"
+            elif self.agent_direction == "down":
+                actions = ["rotate_left", "rotate_left", "move"]
+
+            neighbours.append((actions, (x, y - 1), "up"))
+
+        if y < 8:  # DOWN NEIGHBOUR
+            if self.agent_direction == "right":
+                actions = ["rotate_right", "move"]
+            elif self.agent_direction == "left":
+                actions = ["rotate_left", "move"]
+            elif self.agent_direction == "up":
+                actions = ["rotate_left", "rotate_left", "move"]
+            elif self.agent_direction == "down":
+                actions = "move"
+
+            neighbours.append((actions, (x, y + 1), "down"))
+
+        return neighbours
+
 
 class Package:
     def __init__(self, pos, content, content_size, pack_image):
@@ -35,46 +171,112 @@ class Package:
 
 class Shelf:
     def __init__(self, pos, content, content_size):
+        self.pos_pix = get_pix_from_position[pos]
         self.content = content
         self.content_size = content_size
         self.image = pygame.transform.scale(pygame.image.load('img/shelf.png'), (60, 60))
-        self.rect = self.image.get_rect(center=pos)
+        self.rect = self.image.get_rect(center=self.pos_pix)
 
 
 class Agent:
-    def __init__(self, pos):
+    def __init__(self, pos, agent_direction="right"):
         self.pos = pos
+        self.pos_coord = get_position_from_pix(pos)
         self.image = IMAGE
         self.rect = self.image.get_rect(center=pos)
+        self.goal = (0, 0)
+        self.path = None
+        self.goal_achieved = False
+        self.agent_direction = agent_direction
 
-    def move(self, key):
+        self.image_left = pygame.transform.flip(IMAGE, True, False)
-        if key == pygame.K_UP and self.rect.y - size > 0:
+        self.image_right = IMAGE
-            self.rect.move_ip(0, -size)
+        self.image_down = pygame.transform.rotate(self.image_right, -90)
-            self.pos = (self.pos[0], self.pos[1] - size)
+        self.image_up = pygame.transform.rotate(self.image_left, -90)
 
-        if key == pygame.K_DOWN and self.rect.y + size < HEIGHT:
+    def move_bfs(self):
-            self.rect.move_ip(0, size)
+        if self.path is None or not self.path:
-            self.pos = (self.pos[0], self.pos[1] + size)
+            self.goal_achieved = True
 
-        if key == pygame.K_RIGHT and self.rect.x + size < WIDTH:
+        else:
-            self.rect.move_ip(size, 0)
+            move = self.path.pop(0)
-            self.pos = (self.pos[0] + size, self.pos[1])
 
-        if key == pygame.K_LEFT and self.rect.x - size > 0:
+            if move == "rotate_left":
-            self.rect.move_ip(-size, 0)
+                if self.agent_direction == "right":
-            self.pos = (self.pos[0] - size, self.pos[1])
+                    self.agent_direction = "up"
+                    self.image = self.image_up
 
-    def lift_package(self, key):
+                elif self.agent_direction == "left":
-        if key == pygame.K_e:
+                    self.agent_direction = "down"
-            for package in Package_list:
+                    self.image = self.image_down
-                if package.is_package_up:
+
-                    package.is_package_up = False
+                elif self.agent_direction == "up":
-                elif package.pos == agent.pos:
+                    self.agent_direction = "left"
-                    package.is_package_up = True
+                    self.image = self.image_left
+
+                elif self.agent_direction == "down":
+                    self.agent_direction = "right"
+                    self.image = self.image_right
+
+            elif move == "rotate_right":
+                if self.agent_direction == "right":
+                    self.agent_direction = "down"
+                    self.image = self.image_down
+
+                elif self.agent_direction == "left":
+                    self.agent_direction = "up"
+                    self.image = self.image_up
+
+                elif self.agent_direction == "up":
+                    self.agent_direction = "right"
+                    self.image = self.image_right
+
+                elif self.agent_direction == "down":
+                    self.agent_direction = "left"
+                    self.image = self.image_left
+
+            elif move == "move":
+                if self.agent_direction == "right":
+                    self.rect.move_ip(size, 0)
+                    self.pos = (self.pos[0] + size, self.pos[1])
+                    self.pos_coord = get_position_from_pix(self.pos)
+
+                elif self.agent_direction == "left":
+                    self.rect.move_ip(-size, 0)
+                    self.pos = (self.pos[0] - size, self.pos[1])
+                    self.pos_coord = get_position_from_pix(self.pos)
+
+                elif self.agent_direction == "up":
+                    self.rect.move_ip(0, -size)
+                    self.pos = (self.pos[0], self.pos[1] - size)
+                    self.pos_coord = get_position_from_pix(self.pos)
+
+                elif self.agent_direction == "down":
+                    self.rect.move_ip(0, size)
+                    self.pos = (self.pos[0], self.pos[1] + size)
+                    self.pos_coord = get_position_from_pix(self.pos)
+
+    def lift_package_bfs(self):
+        for package in Package_list:
+            if package.is_package_up:
+                 package.is_package_up = False
+            elif package.pos == agent.pos:
+                package.is_package_up = True
+
+    def goal_test(self, state):
+        if state == self.goal:
+            return True
+        else:
+            return False
 
 
 board = pygame.Surface((WIDTH, HEIGHT), pygame.SRCALPHA)  # transparently surface
 
+#
+create_postiions()
+
+#
+
 for x in range(9):
     for y in range(9):
         pygame.draw.rect(board, (0, 0, 0), (x * size, y * size, size, size), 3)
@@ -83,10 +285,13 @@ agent = Agent((40, 640))
 
 
 Shelf_list = [
-    Shelf((640, 340), 'Explosive', 5), Shelf((565, 340), 'Explosive', 5), Shelf((490, 340), 'Explosive', 5), Shelf((415, 340), 'Explosive', 5),
+    Shelf((5, 2), 'Explosive', 5), Shelf((6, 2), 'Explosive', 5), Shelf((7, 2), 'Explosive', 5), Shelf((8, 2), 'Explosive', 5),
-    Shelf((640, 190), 'Grocery', 5), Shelf((565, 190), 'Grocery', 5), Shelf((490, 190), 'Grocery', 5), Shelf((415, 190), 'Grocery', 5),
+    Shelf((5, 4), 'Grocery', 5), Shelf((6, 4), 'Grocery', 5), Shelf((7, 4), 'Grocery', 5), Shelf((8, 4), 'Grocery', 5),
-    Shelf((640, 640), 'Builders', 5), Shelf((565, 640), 'Builders', 5), Shelf((490, 640), 'Builders', 5), Shelf((415, 640), 'Builders', 5),
+    Shelf((5, 6), 'Builders', 5), Shelf((6, 6), 'Builders', 5), Shelf((7, 6), 'Builders', 5), Shelf((8, 6), 'Builders', 5),
-    Shelf((640, 490), 'Electronic', 5), Shelf((565, 490), 'Electronic', 5), Shelf((490, 490), 'Electronic', 5), Shelf((415, 490), 'Electronic', 5)]
+    Shelf((5, 8), 'Electronic', 5), Shelf((6, 8), 'Electronic', 5), Shelf((7, 8), 'Electronic', 5), Shelf((8, 8), 'Electronic', 5)]
+
+
+coord_goals = [(6, 8), (0, 0), (5,8), (0, 0), (7,8), (0, 0), (8,8), (0, 0), (5,6), (0, 0), (6,6), (0, 0), (7,6), (0, 0), (8,6), (0, 8)]
 
 screen = pygame.display.set_mode([WIDTH, HEIGHT])
 
@@ -94,6 +299,8 @@ Package_list = [
     generate_package(40, 40)
 ]
 
+agent.path = breadth_first_search(agent.pos_coord, agent.agent_direction)
+
 running = True
 while running:
 
@@ -102,12 +309,15 @@ while running:
             running = False
 
         if event.type == pygame.KEYDOWN:
-            agent.move(event.key)
+            pass
-            agent.lift_package(event.key)
+            # agent.move(event.key)
+            # agent.lift_package(event.key)
+            # agent.rotate(event.key)
 
     for package in Package_list:
         if package.is_package_up:
             package.rect.move_ip(agent.pos[0] - package.pos[0], agent.pos[1] - package.pos[1])
+
             package.pos = agent.pos
 
     if len(Package_list) < QTY_OF_PACKAGES:
@@ -121,8 +331,23 @@ while running:
         if is_dock_empty:
             Package_list.append(generate_package(40, 40))
 
-    # screen.fill((0, 0, 0))
+    # PATHING
+    if not agent.goal_achieved:
+        print(agent.path)
 
+        agent.move_bfs()
+
+    else:
+        time.sleep(.25)
+
+        agent.lift_package_bfs()
+        if coord_goals:
+            agent.goal = coord_goals.pop(0)
+            agent.goal_achieved = False
+
+        agent.path = breadth_first_search(agent.pos_coord, agent.agent_direction)
+
+    # DRAWING
     screen.blit(BACKGROUND, [0, 0])
     screen.blit(DOCK, [0, 0])
     screen.blit(board, board.get_rect())