movement_planning #2

Merged
s473585 merged 13 commits from movement_planning into master 2023-05-24 14:28:17 +02:00
13 changed files with 193 additions and 25 deletions

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

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))
for value in element:
val = (value.xpos, value.ypos, value.orientation)
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

View File

@ -1,3 +1,5 @@
FIELDWIDTH = 50
class GarbageTank:
def __init__(self, volume_capacity, mass_capacity):
self.vcapacity = volume_capacity #m^3
@ -8,10 +10,27 @@ class Engine:
self.power = power #HP
class GarbageTruck:
def __init__(self, dump_location, fuel_capacity, start_pos):
def __init__(self, dump_location, fuel_capacity, rect, orientation):
self.dump_location = dump_location
self.tank = GarbageTank(15, 18000)
self.engine = Engine(400)
self.fuel = fuel_capacity
self.pos = start_pos
self.rect = rect
self.orientation = orientation
self.houses = [] #lista domów do odwiedzenia
def turn_left(self):
self.orientation = (self.orientation - 1) % 4
def turn_right(self):
self.orientation = (self.orientation + 1) % 4
def forward(self):
if self.orientation == 0:
self.rect.x += FIELDWIDTH
elif self.orientation == 1:
self.rect.y += FIELDWIDTH
elif self.orientation == 2:
self.rect.x -= FIELDWIDTH
else:
self.rect.y -= FIELDWIDTH

3
heuristicfn.py Normal file
View File

@ -0,0 +1,3 @@
def heuristicfn(startx, starty, goalx, goaly):
return abs(startx - goalx) + abs(starty - goaly)
# return pow(((startx//50)-(starty//50)),2) + pow(((goalx//50)-(goaly//50)),2)

87
main.py
View File

@ -1,5 +1,10 @@
import pygame
import random
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
@ -16,52 +21,90 @@ SAND = pygame.transform.scale(SAND_IMG, (50, 50))
COBBLE_IMG = pygame.image.load("cobble.jpeg")
COBBLE = pygame.transform.scale(COBBLE_IMG, (50, 50))
FPS = 10
FIELDCOUNT = 16
FIELDWIDTH = 50
class Agent:
def __init__(self, rect, direction):
self.rect = rect
self.direction = direction
def randomize_map(): # tworzenie mapy z losowymi polami
fields_list = [DIRT, GRASS, SAND, COBBLE]
field_array_1 = []
field_array_2 = []
field_priority = []
for i in range(16):
temp_priority = []
for j in range(16):
field_array_2.append(random.choice(fields_list))
if i in (0, 1) and j in (0, 1):
field_array_2.append(GRASS)
temp_priority.append(1)
else:
prob = random.uniform(0, 100)
if 0 <= prob <= 12:
field_array_2.append(COBBLE)
temp_priority.append(3)
elif 12 < prob <= 24:
field_array_2.append(SAND)
temp_priority.append(2)
else:
field_array_2.append(GRASS)
temp_priority.append(1)
field_array_1.append(field_array_2)
field_array_2 = []
return field_array_1
field_priority.append(temp_priority)
return field_array_1, field_priority
def draw_window(agent, fields):
def draw_window(agent, fields, flip):
if flip:
direction = pygame.transform.flip(AGENT, True, False)
else:
direction = pygame.transform.flip(AGENT, False, False)
for i in range(16):
for j in range(16):
window.blit(fields[i][j], (i * 50, j * 50))
window.blit(AGENT, (agent.x, agent.y)) # wyswietlanie agenta
window.blit(direction, (agent.rect.x, agent.rect.y)) # wyswietlanie agenta
pygame.display.update()
def agent_movement(keys_pressed, agent): # sterowanie
if keys_pressed[pygame.K_LEFT] and agent.x > 0:
agent.x -= 50
if keys_pressed[pygame.K_RIGHT] and agent.x < 750:
agent.x += 50
if keys_pressed[pygame.K_UP] and agent.y > 0:
agent.y -= 50
if keys_pressed[pygame.K_DOWN] and agent.y < 750:
agent.y += 50
def main():
clock = pygame.time.Clock()
run = True
agent = pygame.Rect(0, 0, 50, 50) # tworzenie pola dla agenta
fields = randomize_map()
x, y = [0, 0]
agent = GarbageTruck(0, 0, pygame.Rect(x, y, 50, 50), 0) # tworzenie pola dla agenta
fields, priority_array = randomize_map()
print(priority_array)
final_x, final_y = [100, 300]
while run:
clock.tick(FPS)
for event in pygame.event.get(): # przechwycanie zamknięcia okna
for event in pygame.event.get():
if event.type == pygame.QUIT:
run = False
keys_pressed = pygame.key.get_pressed()
draw_window(agent, fields)
agent_movement(keys_pressed, agent)
# keys_pressed = pygame.key.get_pressed()
draw_window(agent, fields, False) # false = kierunek east (domyslny), true = west
steps = astar(State(None, None, x, y, 'E', priority_array[0][0], heuristicfn(x, y, final_x, final_y)), final_x, final_y, priority_array)
for interm in steps:
if interm.action == 'LEFT':
agent.turn_left()
draw_window(agent, fields, True)
elif interm.action == 'RIGHT':
agent.turn_right()
draw_window(agent, fields, False)
elif interm.action == 'FORWARD':
agent.forward()
if agent.orientation == 0:
draw_window(agent, fields, False)
elif agent.orientation == 2:
draw_window(agent, fields, True)
else:
draw_window(agent, fields, False)
time.sleep(0.5)
while True:
pass
pygame.quit()

11
state.py Normal file
View File

@ -0,0 +1,11 @@
class State:
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.heuristic = heuristic
def __gt__(self, other):
return self.priority > other.priority

31
succ.py Normal file
View File

@ -0,0 +1,31 @@
from state import State
from heuristicfn import heuristicfn
FIELDWIDTH, FIELDCOUNT = 50, 16
def succ(st: State, passedPriorities, goalx, goaly):
successors = []
if st.orientation == 'N':
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', 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', 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', 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', 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', 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', 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', passedPriorities[st.xpos//50][st.ypos//50], heuristicfn(st.xpos, st.ypos, goalx, goaly)))
return successors