implemented a* instead of bfs and heuristic function
This commit is contained in:
parent
a9e8d1b83d
commit
ba2ed3705d
BIN
__pycache__/astar.cpython-310.pyc
Normal file
BIN
__pycache__/astar.cpython-310.pyc
Normal file
Binary file not shown.
Binary file not shown.
BIN
__pycache__/heuristicfn.cpython-310.pyc
Normal file
BIN
__pycache__/heuristicfn.cpython-310.pyc
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
61
astar.py
Normal file
61
astar.py
Normal file
@ -0,0 +1,61 @@
|
||||
from succ import succ as successors
|
||||
from queue import PriorityQueue
|
||||
from state import State
|
||||
|
||||
def astar(istate, goalx, goaly, passedFields):
|
||||
fringe = PriorityQueue()
|
||||
fringe.put(istate)
|
||||
explored = set()
|
||||
steps = []
|
||||
while not fringe.empty():
|
||||
state = fringe.get()
|
||||
if state.xpos == goalx and state.ypos == goaly:
|
||||
steps.insert(0, state)
|
||||
while (state.parent != None):
|
||||
state = state.parent
|
||||
steps.insert(0, state)
|
||||
return steps
|
||||
|
||||
element = successors(state, passedFields, goalx, goaly)
|
||||
explored.add((state.xpos, state.ypos, state.orientation, state.priority))
|
||||
for value in element:
|
||||
val = (value.xpos, value.ypos, value.orientation, value.priority)
|
||||
if val in explored:
|
||||
continue
|
||||
cost = state.priority + value.priority
|
||||
succesorState = State(state, value.action,
|
||||
value.xpos, value.ypos,
|
||||
value.orientation, cost,
|
||||
value.heuristic)
|
||||
if value not in fringe.queue:
|
||||
fringe.put(succesorState)
|
||||
else :
|
||||
for element in fringe.queue:
|
||||
if (element.xpos==value.xpos and element.ypos==value.ypos) and element > value:
|
||||
element.parent = state
|
||||
element.priority = value.priority
|
||||
return False
|
||||
|
||||
|
||||
# def bfs(istate, goalx, goaly, passedFields):
|
||||
# fringe = [istate]
|
||||
# explored = []
|
||||
# steps = []
|
||||
# while fringe:
|
||||
# state = fringe.pop(0)
|
||||
# if state.xpos == goalx and state.ypos == goaly:
|
||||
# steps.insert(0, state)
|
||||
# while (state.parent != None):
|
||||
# state = state.parent
|
||||
# steps.insert(0, state)
|
||||
# return steps
|
||||
|
||||
# element = successors(state, passedFields)
|
||||
# explored.append((state.xpos, state.ypos, state.orientation))
|
||||
# for value in element:
|
||||
# val = (value.xpos, value.ypos, value.orientation)
|
||||
# if val not in explored and value not in fringe:
|
||||
# fringe.append(value)
|
||||
# return False
|
||||
|
||||
|
24
bfs.py
24
bfs.py
@ -1,24 +0,0 @@
|
||||
from succ import succ as successors
|
||||
from queue import PriorityQueue
|
||||
|
||||
|
||||
def bfs(istate, goalx, goaly, passedFields):
|
||||
fringe = [istate]
|
||||
explored = []
|
||||
steps = []
|
||||
while fringe:
|
||||
state = fringe.pop(0)
|
||||
if state.xpos == goalx and state.ypos == goaly:
|
||||
steps.insert(0, state)
|
||||
while (state.parent != None):
|
||||
state = state.parent
|
||||
steps.insert(0, state)
|
||||
return steps
|
||||
|
||||
element = successors(state, passedFields)
|
||||
explored.append((state.xpos, state.ypos, state.orientation))
|
||||
for value in element:
|
||||
val = (value.xpos, value.ypos, value.orientation)
|
||||
if val not in explored and value not in fringe:
|
||||
fringe.append(value)
|
||||
return False
|
2
heuristicfn.py
Normal file
2
heuristicfn.py
Normal file
@ -0,0 +1,2 @@
|
||||
def heuristicfn(startx, starty, goalx, goaly):
|
||||
return pow(((startx//50)-(starty//50)),2) + pow(((goalx//50)-(goaly//50)),2)
|
7
main.py
7
main.py
@ -1,9 +1,10 @@
|
||||
import pygame
|
||||
import random
|
||||
from bfs import bfs
|
||||
from astar import astar
|
||||
from state import State
|
||||
import time
|
||||
from garbage_truck import GarbageTruck
|
||||
from heuristicfn import heuristicfn
|
||||
|
||||
pygame.init()
|
||||
WIDTH, HEIGHT = 800, 800
|
||||
@ -44,7 +45,7 @@ def randomize_map(): # tworzenie mapy z losowymi polami
|
||||
if 0 <= prob <= 10:
|
||||
field_array_2.append(COBBLE)
|
||||
temp_priority.append(3)
|
||||
if 10 < prob <= 20:
|
||||
elif 10 < prob <= 20:
|
||||
field_array_2.append(SAND)
|
||||
temp_priority.append(2)
|
||||
else:
|
||||
@ -78,7 +79,7 @@ def main():
|
||||
run = False
|
||||
# keys_pressed = pygame.key.get_pressed()
|
||||
draw_window(agent, fields)
|
||||
steps = bfs(State(None, None, 0, 0, 'E'), 100, 50, fields)
|
||||
steps = astar(State(None, None, 0, 0, 'E', priority_array[0][0], heuristicfn(0,0,300,100)), 300, 100, priority_array)
|
||||
for interm in steps:
|
||||
if interm.action == 'LEFT':
|
||||
agent.turn_left()
|
||||
|
7
state.py
7
state.py
@ -1,8 +1,11 @@
|
||||
class State:
|
||||
def __init__(self, parent, action, xpos, ypos, orientation):
|
||||
def __init__(self, parent, action, xpos, ypos, orientation, priority, heuristic):
|
||||
self.parent = parent
|
||||
self.xpos = xpos
|
||||
self.ypos = ypos
|
||||
self.orientation = orientation
|
||||
self.action = action
|
||||
# self.priority = priority
|
||||
self.priority = priority
|
||||
self.heuristic = heuristic
|
||||
def __gt__(self, other):
|
||||
return self.priority > other.priority
|
27
succ.py
27
succ.py
@ -1,30 +1,31 @@
|
||||
from state import State
|
||||
from heuristicfn import heuristicfn
|
||||
FIELDWIDTH, FIELDCOUNT = 50, 16
|
||||
|
||||
def succ(st: State, passedFields):
|
||||
def succ(st: State, passedPriorities, goalx, goaly):
|
||||
successors = []
|
||||
|
||||
if st.orientation == 'N':
|
||||
successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'W'))
|
||||
successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'E'))
|
||||
successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'W', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'E', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
if st.ypos > 0:
|
||||
successors.append(State(st, 'FORWARD', st.xpos, st.ypos - FIELDWIDTH , 'N'))
|
||||
successors.append(State(st, 'FORWARD', st.xpos, st.ypos - FIELDWIDTH , 'N', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
|
||||
if st.orientation == 'S':
|
||||
successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'E'))
|
||||
successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'W'))
|
||||
successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'E', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'W', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
if st.ypos < FIELDWIDTH * (FIELDCOUNT - 1):
|
||||
successors.append(State(st, 'FORWARD', st.xpos, st.ypos + FIELDWIDTH , 'S'))
|
||||
successors.append(State(st, 'FORWARD', st.xpos, st.ypos + FIELDWIDTH , 'S', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
|
||||
if st.orientation == 'W':
|
||||
successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'S'))
|
||||
successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'N'))
|
||||
successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'S', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
successors.append(State(st,'RIGHT', st.xpos, st.ypos, 'N', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
if st.xpos > 0:
|
||||
successors.append(State(st, 'FORWARD', st.xpos - FIELDWIDTH , st.ypos, 'W'))
|
||||
successors.append(State(st, 'FORWARD', st.xpos - FIELDWIDTH , st.ypos, 'W', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
|
||||
if st.orientation == 'E':
|
||||
successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'N'))
|
||||
successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'S'))
|
||||
successors.append(State(st, 'LEFT', st.xpos, st.ypos, 'N', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'S', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
if st.xpos < FIELDWIDTH * (FIELDCOUNT - 1):
|
||||
successors.append(State(st, 'FORWARD', st.xpos + FIELDWIDTH , st.ypos, 'E'))
|
||||
successors.append(State(st, 'FORWARD', st.xpos + FIELDWIDTH , st.ypos, 'E', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
|
||||
return successors
|
||||
|
Loading…
Reference in New Issue
Block a user