from asyncio.windows_events import NULL from collections import deque from lib2to3.pgen2.token import CIRCUMFLEX from queue import Queue import sys from unicodedata import name import pygame import random import time screen = [] objectArray = [] pathToPrint = [] class Object: def __init__(self, name, xPos, yPos): self.name = name self.xPos = xPos self.yPos = yPos class Field: def __init__(self, xPos, yPos, isObject): self.xPos = xPos self.yPos = yPos self.visited = False self.parent = None number = random.randint(0, 9) if number > 5 and not isObject: self.isBlock = True else: self.isBlock = False def printXandY(self): print("X: " + str(self.xPos) + ". Y: " + str(self.yPos)) def draw(square_num, objectArr, pathToTarget, num): grid_color = (0, 0, 0) grid_size = 500 square = grid_size/square_num a = 50 b = 10 for i in range(square_num): pygame.draw.line(screen, grid_color, (a + i*square, b), (a + i*square, b + grid_size), 2) pygame.draw.line(screen, grid_color, (a, b + i*square), (a + grid_size, b + i*square), 2) pygame.draw.line(screen, grid_color, (a, b + grid_size), (a + grid_size, b + grid_size), 2) pygame.draw.line(screen, grid_color, (a + grid_size, b), (a + grid_size, b + grid_size), 2) circleX = objectArr[0].xPos * square + square + square / 2 circleY = objectArr[0].yPos * square + square / 2 pygame.draw.circle(screen, (0, 0, 255), (circleX + 15, circleY + 15), 8) circleX = objectArray[1].xPos * square + square * 2 circleY = objectArr[1].yPos * square + square radius = 8 grid_color = (255, 0, 0) pygame.draw.circle(screen, grid_color, (circleX, circleY), radius) def kb_listen(objectArray, gridLength): for event in pygame.event.get(): if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT and objectArray[0].xPos > 0: objectArray[0].xPos = objectArray[0].xPos - 1 print(objectArray[0].xPos) if event.key == pygame.K_RIGHT and objectArray[0].xPos < gridLength - 1: objectArray[0].xPos = objectArray[0].xPos + 1 print(objectArray[0].xPos) if event.key == pygame.K_UP and objectArray[0].yPos > 0: objectArray[0].yPos = objectArray[0].yPos - 1 print(objectArray[0].yPos) if event.key == pygame.K_DOWN and objectArray[0].yPos < gridLength - 1: objectArray[0].yPos = objectArray[0].yPos + 1 print(objectArray[0].yPos) if event.type == pygame.QUIT: sys.exit() fieldQueue = [] isGoalAchieved = False def startQueue(agentX, agentY, fieldList, gridNum): currentX = agentX currentY = agentY currentIndex = currentY * gridNum + currentX fieldQueue.append(fieldList[currentIndex]) checkFlag = False while not checkFlag: result = checkGoal(fieldList, gridNum) if result: checkFlag = True return result def checkGoal(fieldList, gridNum): currentField = fieldQueue.pop(0) currentY = currentField.yPos currentX = currentField.xPos currentIndex = currentY * gridNum + currentX # SPRAWDZAMY CZY DANE POLE JEST POLEM W KTORYM ZNAJDUJE SIE NASZ CEL if currentField.xPos == objectArray[1].xPos and currentField.yPos == objectArray[1].yPos: return currentField # SPRAWDZAMY CZY DANE POLE ZOSTALO JUZ WCZESNIEJ ODWIEDZONE - JEZELI NIE TO USTAWIAMY STATUS if not currentField.visited: currentField.visited = True # JEZELI DANE POLE NIE ZOSTALO ODWIEDZONE, TO DODAJEMY DO KOLEJKI WSZYSTKIE POLA OBOK I LECIMY DALEJ if currentIndex + 15 <= 224 and fieldList[currentIndex + 15].visited == False and fieldList[currentIndex + 15].isBlock == False: fieldQueue.append(fieldList[currentIndex + 15]) fieldList[currentIndex + 15].parent = fieldList[currentIndex] if currentIndex - 15 > -1 and fieldList[currentIndex - 15].visited == False and fieldList[currentIndex - 15].isBlock == False: fieldQueue.append(fieldList[currentIndex - 15]) fieldList[currentIndex - 15].parent = fieldList[currentIndex] if (currentIndex + 1) % 15 != 0 and fieldList[currentIndex + 1].visited == False and fieldList[currentIndex + 1].isBlock == False: fieldQueue.append(fieldList[currentIndex + 1]) fieldList[currentIndex + 1].parent = fieldList[currentIndex] if (currentIndex - 1) % 15 != 14 and not currentIndex - 1 < 0 and fieldList[currentIndex - 1].visited == False and fieldList[currentIndex - 1].isBlock == False: fieldQueue.append(fieldList[currentIndex - 1]) fieldList[currentIndex - 1].parent = fieldList[currentIndex] def generateStartEndPos(): startX = random.randint(0, 14) startY = random.randint(0, 14) targetX = None targetY = None while targetX == None and targetY == None or targetX == startX and targetY == startY: targetX = random.randint(0, 14) targetY = random.randint(0, 14) return [startX, startY, targetX, targetY] def drawFull(index): c = (255, 255, 255) screen.fill(c) for node in pathToTarget: if node == pathToTarget[0] or node == pathToTarget[-1]: continue if pathToTarget.index(node) <= index: continue green = (0, 255, 0) pygame.draw.circle(screen, green, ((node.xPos + 1) * 500 / 15 + 30, (node.yPos + 1) * 500 / 15), 5) draw(15, objectArray, pathToTarget, 1) for node in fields: if node.isBlock == True: black = (0, 0, 0) pygame.draw.rect(screen, black, pygame.Rect((node.xPos + 1) * 500 / 15 + 30, (node.yPos + 1) * 500 / 15 - 10, 10, 10)) ##kb_listen(objectArray, 15) pygame.display.update() if __name__ == '__main__': pygame.init() width = 600 height = 530 screen = pygame.display.set_mode( (width, height)) [sX, sY, eX, eY] = generateStartEndPos() fields = [] for y in range(15): for x in range(15): if x == sX and y == sY or x == eX and y == eY: newField = Field(x, y, True) else: newField = Field(x, y, False) fields.append(newField) agent = Object("AGENT", sX, sY) target = Object("TARGET", eX, eY) objectArray.append(agent) objectArray.append(target) result = startQueue(objectArray[0].xPos, objectArray[0].yPos, fields, 15) print("RED DOT X AND Y POSITION ============================") print("X: " + str(objectArray[1].xPos) + " Y: " + str(objectArray[1].yPos)) print("TRUCK X AND Y POSITION ============================") print("X: " + str(objectArray[0].xPos) + " Y: " + str(objectArray[0].yPos)) result_parent = result.parent currentIndex = (objectArray[0].yPos) * 15 + objectArray[0].xPos pathToTarget = deque() pathToTarget.appendleft(result) while result_parent.parent != None: pathToTarget.appendleft(result_parent) result_parent = result_parent.parent pathToTarget.appendleft(fields[currentIndex]) print("PRINTING PATH TO TARGET ============================") for node in pathToTarget: node.printXandY() print("============================ SEARCH PROBLEM DIAGRAM ============================") for node in pathToTarget: currentNode = node parentNode = node.parent direction = None index = pathToTarget.index(node) if parentNode == None: continue if currentNode.xPos != parentNode.xPos: if currentNode.xPos > parentNode.xPos: direction = "EAST" objectArray[0].xPos = objectArray[0].xPos + 1 else: direction = "WEST" objectArray[0].xPos = objectArray[0].xPos - 1 else: if currentNode.yPos > parentNode.yPos: direction = "SOUTH" objectArray[0].yPos = objectArray[0].yPos + 1 else: direction = "NORTH" objectArray[0].yPos = objectArray[0].yPos - 1 time.sleep(1) print("---- NEXT STEP ---- ") print("AGENT TURNING TO " + direction) print("AGENT MOVING FORWARD") print("ACTUAL AGENT STATE: (X -> " + str(objectArray[0].xPos) + ", Y -> " + str(objectArray[0].yPos) + ", " + direction + ")") draw(15, objectArray, pathToTarget, 1) pygame.display.update() drawFull(index) time.sleep(5) quit()