Compare commits

...

14 Commits

Author SHA1 Message Date
b8c51c0d70 Merge pull request 'movement_planning' (#2) from movement_planning into master
Reviewed-on: #2
2023-05-24 14:28:16 +02:00
fcc46fcaf5 Merge remote-tracking branch 'origin/movement_planning' into movement_planning 2023-05-05 12:21:35 +02:00
21bcecd101 Added visual rotation of agent, changed heuristic 2023-05-05 12:20:29 +02:00
Maksymilian Mikołajczak
9d9c1aaa4c optimalized astar 2023-05-05 12:13:47 +02:00
Maksymilian Mikołajczak
ba2ed3705d implemented a* instead of bfs and heuristic function 2023-05-04 20:44:35 +02:00
a9e8d1b83d Changed map generating 2023-05-04 18:21:09 +02:00
Maksymilian Mikołajczak
674393a4d9 added priority for blocks 2023-05-03 12:07:13 +02:00
Mateusz Szlachetka
15abf755b9 added constants, created agent methods 2023-04-21 14:31:17 +02:00
Mateusz Szlachetka
fc588a4ad5 fixed search 2023-04-20 10:01:48 +02:00
Mateusz
d503ee4f0c implemented path finding 2023-04-19 23:53:36 +02:00
Mateusz
9406038dcb implemented movement planning 2023-04-19 23:51:24 +02:00
Maksymilian Mikołajczak
bdb0ed2d3c basic bfs implementation 2023-04-19 22:12:02 +02:00
Mateusz
c91e016294 added state class 2023-04-19 21:40:40 +02:00
Mateusz
1e88149789 created successor function, adjustet truck class 2023-04-19 21:16:40 +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.houses = [] #lista domów do odwiedzenia
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