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

Merge pull request 'movement_planning' (#2) from movement_planning into master 

Reviewed-on: #2
This commit is contained in:
Mateusz Szlachetka 2023-05-24 14:28:16 +02:00
parent bb6c1d0c2b fcc46fcaf5
commit b8c51c0d70
13 changed files with 193 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

Binary file not shown.

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

Binary file not shown.

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

Binary file not shown.

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

Binary file not shown.

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

Binary file not shown.

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

Binary file not shown.

BIN
__pycache__/succ.cpython-310.pyc Normal file
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))
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

25
garbage_truck.py
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