s452422/Inteligentna_smieciarka
5
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

movement_planning #2

Merged
s473585 merged 13 commits from movement_planning into master 2023-05-24 14:28:17 +02:00
Conversation 0 Commits 13 Files Changed 13 +86 -42
11 changed files with 86 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit ba2ed3705d - Show all commits

BIN
__pycache__/astar.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
__pycache__/bfs.cpython-310.pyc
View File

Binary file not shown.

BIN
__pycache__/heuristicfn.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
__pycache__/state.cpython-310.pyc
View File

Binary file not shown.

BIN
__pycache__/succ.cpython-310.pyc
View File

Binary file not shown.

61
astar.py Normal file
View 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
View File

@ -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
View 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
View File

@ -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
View File

@ -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
View File

@ -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