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 import subprocess from os import environ environ['PYGAME_HIDE_SUPPORT_PROMPT'] = '1' 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 > 3 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): if len(fieldQueue) == 0: print("THE PROGRAM FAILED TO FIND THE PATH FOR A GIVEN WALL/AGENT/TARGET TEMPLATE...") print("========= PLEASE DO NOT TURN THE PROGRAM AGAIN ===========") print("========= THE PROGRAM WILL CREATE A NEW TEMPLATE AUTOMATICALLY ==========") subprocess.Popen(['python', 'board']) sys.exit() 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__': [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) pygame.init() width = 600 height = 530 screen = pygame.display.set_mode( (width, height)) 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()