cleaning the code
This commit is contained in:
parent
3bb8657b17
commit
79463c2044
131
src/a_star.py
Normal file
131
src/a_star.py
Normal file
@ -0,0 +1,131 @@
|
||||
|
||||
WAREHOUSE_MAP = [
|
||||
[0, 0, 0, 10, 10, 0, 0, 0, 0],
|
||||
[0, 10, 0, 10, 0, 10, 0, 0, 0],
|
||||
[0, 10, 0, 0, 0, 200, 200, 200, 200],
|
||||
[0, 0, 10, 0, 0, 0, 0, 0, 0],
|
||||
[0, 0, 10, 0, 0, 200, 200, 200, 200],
|
||||
[0, 0, 10, 10, 0, 0, 0, 0, 0],
|
||||
[10, 0, 0, 0, 10, 200, 200, 200, 200],
|
||||
[0, 0, 10, 10, 10, 0, 0, 0, 0],
|
||||
[0, 10, 0, 0, 0, 200, 200, 200, 200],
|
||||
]
|
||||
|
||||
|
||||
def a_star_search(start, end, agent_direction):
|
||||
|
||||
open_nodes = []
|
||||
closed_nodes = []
|
||||
|
||||
start_node = NodeAStar(agent_direction, None, start)
|
||||
start_node.g = start_node.h = start_node.f = 0
|
||||
end_node = NodeAStar(None, None, end)
|
||||
end_node.g = end_node.h = end_node.f = 0
|
||||
|
||||
open_nodes.append(start_node)
|
||||
|
||||
while len(open_nodes) != 0:
|
||||
current_node = open_nodes[0]
|
||||
current_node_index = 0
|
||||
for index, node in enumerate(open_nodes):
|
||||
if node.f < current_node.f:
|
||||
current_node = node
|
||||
current_node_index = index
|
||||
|
||||
open_nodes.pop(current_node_index)
|
||||
closed_nodes.append(current_node)
|
||||
|
||||
# Sprawdzam czy jesteśmy u celu jeżeli tak zwracamy ścieżkę
|
||||
neighbour_nodes = []
|
||||
if current_node == end_node:
|
||||
path = []
|
||||
current = current_node
|
||||
while current is not None:
|
||||
#path.append(current.position)
|
||||
if current.action is not None:
|
||||
current.action.reverse()
|
||||
for each_action in current.action:
|
||||
path.append(each_action)
|
||||
|
||||
current = current.parent
|
||||
#path = path.pop()
|
||||
return path[::-1]
|
||||
#
|
||||
|
||||
x = current_node.position[0]
|
||||
y = current_node.position[1]
|
||||
if x < 8: # DOWN NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["rotate_right", "rotate_right", "move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["move"]
|
||||
neighbour_nodes.append(
|
||||
NodeAStar("down", current_node, (x + 1, y), actions))
|
||||
|
||||
if x > 0: # UP NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["rotate_right", "rotate_right", "move"]
|
||||
neighbour_nodes.append(NodeAStar("up", current_node, (x - 1, y), actions))
|
||||
|
||||
if y > 0: # LEFT NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["rotate_right", "move"]
|
||||
neighbour_nodes.append(NodeAStar("left", current_node, (x, y - 1), actions))
|
||||
|
||||
if y < 8: # RIGHT NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["rotate_left", "move"]
|
||||
neighbour_nodes.append(NodeAStar("right", current_node, (x, y + 1), actions))
|
||||
for neighbour in neighbour_nodes:
|
||||
if len([closed_neighbour for closed_neighbour in closed_nodes if closed_neighbour == neighbour]) > 0:
|
||||
continue
|
||||
|
||||
action_len = 0
|
||||
if current_node.action is not None:
|
||||
action_len = len(current_node.action)
|
||||
neighbour.g = current_node.g + WAREHOUSE_MAP[neighbour.position[0]][neighbour.position[1]] + action_len
|
||||
neighbour.h = abs(neighbour.position[0] - end_node.position[0]) + abs(
|
||||
neighbour.position[1] - end_node.position[1])
|
||||
neighbour.f = neighbour.g + neighbour.h
|
||||
|
||||
if len([open_node for open_node in open_nodes if
|
||||
neighbour.position == open_node.position and neighbour.g > open_node.g]) > 0:
|
||||
continue
|
||||
|
||||
open_nodes.append(neighbour)
|
||||
|
||||
|
||||
class NodeAStar:
|
||||
def __init__(self, agent_direction, parent=None, position=None, action=None):
|
||||
self.agent_direction = agent_direction
|
||||
self.parent = parent
|
||||
self.position = position
|
||||
self.action = action
|
||||
self.g = 0
|
||||
self.h = 0
|
||||
self.f = 0
|
||||
|
||||
def __eq__(self, other):
|
||||
return self.position == other.position
|
||||
113
src/bfs.py
Normal file
113
src/bfs.py
Normal file
@ -0,0 +1,113 @@
|
||||
|
||||
# FRINGE - struktura danych przechowująca wierzchołki do odwiedzenia
|
||||
# EXPLORED - lista odwiedzonych stanów
|
||||
# istate - stan początkowy
|
||||
# Succ - funkcja następnika
|
||||
# Goaltest -test spełnienia celu
|
||||
|
||||
|
||||
def breadth_first_search(istate, goal, agent_direction): # COORDINATES OF A START PLACE
|
||||
fringe = []
|
||||
explored = []
|
||||
start = Node(istate, agent_direction)
|
||||
fringe.append(start)
|
||||
path = []
|
||||
|
||||
while True:
|
||||
if not fringe:
|
||||
return False
|
||||
|
||||
elem = fringe.pop(0)
|
||||
|
||||
if elem.state == goal:
|
||||
# return ciag akcji zbudowany z wykorzystaniem pol parent i action
|
||||
while elem.parent is not None:
|
||||
if type(elem.action) == list:
|
||||
elem.action.reverse()
|
||||
for each_action in elem.action:
|
||||
path.append(each_action)
|
||||
else:
|
||||
path.append(elem.action)
|
||||
elem = elem.parent
|
||||
|
||||
path.reverse()
|
||||
return path
|
||||
|
||||
explored.append(elem)
|
||||
|
||||
for action, state, direction in elem.successor():
|
||||
fringe_states = []
|
||||
explored_states = []
|
||||
for node in fringe:
|
||||
fringe_states.append(node.state)
|
||||
for node2 in explored:
|
||||
explored_states.append(node2.state)
|
||||
|
||||
if state not in fringe_states and state not in explored_states:
|
||||
x = Node(state, direction)
|
||||
x.parent = elem
|
||||
x.action = action
|
||||
fringe.append(x)
|
||||
|
||||
|
||||
class Node:
|
||||
def __init__(self, state, agent_direction, action=None, parent=None):
|
||||
self.state = state
|
||||
self.action = action
|
||||
self.parent = parent
|
||||
self.agent_direction = agent_direction
|
||||
|
||||
def successor(self):
|
||||
neighbours = []
|
||||
x = self.state[0]
|
||||
y = self.state[1]
|
||||
|
||||
if x < 8: # RIGHT NEIGHBOUR
|
||||
if self.agent_direction == "right":
|
||||
actions = "move"
|
||||
elif self.agent_direction == "left":
|
||||
actions = ["rotate_right", "rotate_right", "move"]
|
||||
elif self.agent_direction == "up":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif self.agent_direction == "down":
|
||||
actions = ["rotate_left", "move"]
|
||||
|
||||
neighbours.append((actions, (x + 1, y), "right"))
|
||||
|
||||
if x > 0: # LEFT NEIGHBOUR
|
||||
if self.agent_direction == "right":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif self.agent_direction == "left":
|
||||
actions = "move"
|
||||
elif self.agent_direction == "up":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif self.agent_direction == "down":
|
||||
actions = ["rotate_right", "move"]
|
||||
|
||||
neighbours.append((actions, (x - 1, y), "left"))
|
||||
|
||||
if y > 0: # UP NEIGHBOUR
|
||||
if self.agent_direction == "right":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif self.agent_direction == "left":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif self.agent_direction == "up":
|
||||
actions = "move"
|
||||
elif self.agent_direction == "down":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
|
||||
neighbours.append((actions, (x, y - 1), "up"))
|
||||
|
||||
if y < 8: # DOWN NEIGHBOUR
|
||||
if self.agent_direction == "right":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif self.agent_direction == "left":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif self.agent_direction == "up":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif self.agent_direction == "down":
|
||||
actions = "move"
|
||||
|
||||
neighbours.append((actions, (x, y + 1), "down"))
|
||||
|
||||
return neighbours
|
||||
261
src/main.py
261
src/main.py
@ -2,6 +2,9 @@ import pygame
|
||||
import random
|
||||
import time
|
||||
|
||||
from src.a_star import a_star_search, WAREHOUSE_MAP
|
||||
from src.bfs import breadth_first_search
|
||||
|
||||
pygame.init()
|
||||
|
||||
WIDTH = 675
|
||||
@ -23,19 +26,6 @@ def get_position_from_pix(pix_pos):
|
||||
print("ERROR: THERE IS NO POSITION LIKE THIS (get_position_from_pix)")
|
||||
|
||||
|
||||
WAREHOUSE_MAP = [
|
||||
[0, 0, 0, 10, 10, 0, 0, 0, 0],
|
||||
[0, 10, 0, 10, 0, 10, 0, 0, 0],
|
||||
[0, 10, 0, 0, 0, 200, 200, 200, 200],
|
||||
[0, 0, 10, 0, 0, 0, 0, 0, 0],
|
||||
[0, 0, 10, 0, 0, 200, 200, 200, 200],
|
||||
[0, 0, 10, 10, 0, 0, 0, 0, 0],
|
||||
[10, 0, 0, 0, 10, 200, 200, 200, 200],
|
||||
[0, 0, 10, 10, 10, 0, 0, 0, 0],
|
||||
[0, 10, 0, 0, 0, 200, 200, 200, 200],
|
||||
]
|
||||
|
||||
|
||||
def create_positions():
|
||||
size_agent = 40
|
||||
x_position_pix = size_agent
|
||||
@ -57,239 +47,6 @@ def generate_package(a, b):
|
||||
return p1
|
||||
|
||||
|
||||
# FRINGE - struktura danych przechowująca wierzchołki do odwiedzenia
|
||||
# EXPLORED - lista odwiedzonych stanów
|
||||
# istate - stan początkowy
|
||||
# Succ - funkcja następnika
|
||||
# Goaltest -test spełnienia celu
|
||||
|
||||
|
||||
def breadth_first_search(istate, agent_direction): # COORDINATES OF A START PLACE
|
||||
fringe = []
|
||||
explored = []
|
||||
start = Node(istate, agent_direction)
|
||||
fringe.append(start)
|
||||
path = []
|
||||
|
||||
while True:
|
||||
if not fringe:
|
||||
return False
|
||||
|
||||
elem = fringe.pop(0)
|
||||
|
||||
if agent.goal_test(elem.state):
|
||||
# return ciag akcji zbudowany z wykorzystaniem pol parent i action
|
||||
while elem.parent is not None:
|
||||
if type(elem.action) == list:
|
||||
elem.action.reverse()
|
||||
for each_action in elem.action:
|
||||
path.append(each_action)
|
||||
else:
|
||||
path.append(elem.action)
|
||||
elem = elem.parent
|
||||
|
||||
path.reverse()
|
||||
return path
|
||||
|
||||
explored.append(elem)
|
||||
|
||||
for action, state, direction in elem.successor():
|
||||
fringe_states = []
|
||||
explored_states = []
|
||||
for node in fringe:
|
||||
fringe_states.append(node.state)
|
||||
for node2 in explored:
|
||||
explored_states.append(node2.state)
|
||||
|
||||
if state not in fringe_states and state not in explored_states:
|
||||
x = Node(state, direction)
|
||||
x.parent = elem
|
||||
x.action = action
|
||||
fringe.append(x)
|
||||
|
||||
|
||||
def a_star_search(start, end, agent_direction):
|
||||
open_nodes = []
|
||||
closed_nodes = []
|
||||
|
||||
start_node = NodeAStar(agent_direction, None, start)
|
||||
start_node.g = start_node.h = start_node.f = 0
|
||||
end_node = NodeAStar(None, None, end)
|
||||
end_node.g = end_node.h = end_node.f = 0
|
||||
|
||||
open_nodes.append(start_node)
|
||||
|
||||
while len(open_nodes) != 0:
|
||||
current_node = open_nodes[0]
|
||||
current_node_index = 0
|
||||
for index, node in enumerate(open_nodes):
|
||||
if node.f < current_node.f:
|
||||
current_node = node
|
||||
current_node_index = index
|
||||
|
||||
open_nodes.pop(current_node_index)
|
||||
closed_nodes.append(current_node)
|
||||
|
||||
# Sprawdzam czy jesteśmy u celu jeżeli tak zwracamy ścieżkę
|
||||
neighbour_nodes = []
|
||||
if current_node == end_node:
|
||||
path = []
|
||||
current = current_node
|
||||
while current is not None:
|
||||
#path.append(current.position)
|
||||
if current.action is not None:
|
||||
current.action.reverse()
|
||||
for each_action in current.action:
|
||||
path.append(each_action)
|
||||
|
||||
current = current.parent
|
||||
#path = path.pop()
|
||||
return path[::-1]
|
||||
#
|
||||
|
||||
x = current_node.position[0]
|
||||
y = current_node.position[1]
|
||||
if x < 8: # DOWN NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["rotate_right", "rotate_right", "move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["move"]
|
||||
neighbour_nodes.append(
|
||||
NodeAStar("down", current_node, (x + 1, y), actions))
|
||||
|
||||
if x > 0: # UP NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["rotate_right", "rotate_right", "move"]
|
||||
neighbour_nodes.append(NodeAStar("up", current_node, (x - 1, y), actions))
|
||||
|
||||
if y > 0: # LEFT NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["rotate_right", "move"]
|
||||
neighbour_nodes.append(NodeAStar("left", current_node, (x, y - 1), actions))
|
||||
|
||||
if y < 8: # RIGHT NEIGHBOUR
|
||||
if current_node.agent_direction == "right":
|
||||
actions = ["move"]
|
||||
elif current_node.agent_direction == "left":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif current_node.agent_direction == "up":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif current_node.agent_direction == "down":
|
||||
actions = ["rotate_left", "move"]
|
||||
neighbour_nodes.append(NodeAStar("right", current_node, (x, y + 1), actions))
|
||||
for neighbour in neighbour_nodes:
|
||||
if len([closed_neighbour for closed_neighbour in closed_nodes if closed_neighbour == neighbour]) > 0:
|
||||
continue
|
||||
|
||||
action_len = 0
|
||||
if current_node.action is not None:
|
||||
action_len = len(current_node.action)
|
||||
neighbour.g = current_node.g + WAREHOUSE_MAP[neighbour.position[0]][neighbour.position[1]] + action_len
|
||||
neighbour.h = abs(neighbour.position[0] - end_node.position[0]) + abs(
|
||||
neighbour.position[1] - end_node.position[1])
|
||||
neighbour.f = neighbour.g + neighbour.h
|
||||
|
||||
if len([open_node for open_node in open_nodes if
|
||||
neighbour.position == open_node.position and neighbour.g > open_node.g]) > 0:
|
||||
continue
|
||||
|
||||
open_nodes.append(neighbour)
|
||||
|
||||
|
||||
class NodeAStar:
|
||||
def __init__(self, agent_direction, parent=None, position=None, action=None):
|
||||
self.agent_direction = agent_direction
|
||||
self.parent = parent
|
||||
self.position = position
|
||||
self.action = action
|
||||
self.g = 0
|
||||
self.h = 0
|
||||
self.f = 0
|
||||
|
||||
def __eq__(self, other):
|
||||
return self.position == other.position
|
||||
|
||||
|
||||
|
||||
class Node:
|
||||
def __init__(self, state, agent_direction, action=None, parent=None):
|
||||
self.state = state
|
||||
self.action = action
|
||||
self.parent = parent
|
||||
self.agent_direction = agent_direction
|
||||
|
||||
def successor(self):
|
||||
neighbours = []
|
||||
x = self.state[0]
|
||||
y = self.state[1]
|
||||
|
||||
if x < 8: # RIGHT NEIGHBOUR
|
||||
if self.agent_direction == "right":
|
||||
actions = "move"
|
||||
elif self.agent_direction == "left":
|
||||
actions = ["rotate_right", "rotate_right", "move"]
|
||||
elif self.agent_direction == "up":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif self.agent_direction == "down":
|
||||
actions = ["rotate_left", "move"]
|
||||
|
||||
neighbours.append((actions, (x + 1, y), "right"))
|
||||
|
||||
if x > 0: # LEFT NEIGHBOUR
|
||||
if self.agent_direction == "right":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif self.agent_direction == "left":
|
||||
actions = "move"
|
||||
elif self.agent_direction == "up":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif self.agent_direction == "down":
|
||||
actions = ["rotate_right", "move"]
|
||||
|
||||
neighbours.append((actions, (x - 1, y), "left"))
|
||||
|
||||
if y > 0: # UP NEIGHBOUR
|
||||
if self.agent_direction == "right":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif self.agent_direction == "left":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif self.agent_direction == "up":
|
||||
actions = "move"
|
||||
elif self.agent_direction == "down":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
|
||||
neighbours.append((actions, (x, y - 1), "up"))
|
||||
|
||||
if y < 8: # DOWN NEIGHBOUR
|
||||
if self.agent_direction == "right":
|
||||
actions = ["rotate_right", "move"]
|
||||
elif self.agent_direction == "left":
|
||||
actions = ["rotate_left", "move"]
|
||||
elif self.agent_direction == "up":
|
||||
actions = ["rotate_left", "rotate_left", "move"]
|
||||
elif self.agent_direction == "down":
|
||||
actions = "move"
|
||||
|
||||
neighbours.append((actions, (x, y + 1), "down"))
|
||||
|
||||
return neighbours
|
||||
|
||||
|
||||
class Package:
|
||||
def __init__(self, pos, content, content_size, pack_image):
|
||||
self.pos = pos
|
||||
@ -414,7 +171,7 @@ board = pygame.Surface((WIDTH, HEIGHT), pygame.SRCALPHA) # transparently surfac
|
||||
create_positions()
|
||||
|
||||
|
||||
#Rysowanie lini
|
||||
# Rysowanie lini
|
||||
for x in range(9):
|
||||
for y in range(9):
|
||||
pygame.draw.rect(board, (0, 0, 0), (x * size, y * size, size, size), 3)
|
||||
@ -442,15 +199,15 @@ Package_list = [
|
||||
|
||||
print(a_star_search((6, 8), (0, 0), "down"))
|
||||
|
||||
#agent.path = breadth_first_search(agent.pos_coord, agent.agent_direction)
|
||||
#agent.path = breadth_first_search(agent.pos_coord, agent.goal, agent.agent_direction)
|
||||
agent.path = a_star_search((agent.pos_coord[1], agent.pos_coord[0]), (0, 0), agent.agent_direction)
|
||||
|
||||
|
||||
print(agent.path)
|
||||
|
||||
|
||||
Stain_list = []
|
||||
# Pętla służąca do tworzenia plam oleju na podstawie mapy magazynu
|
||||
Stain_list = []
|
||||
|
||||
for index_x in range(9):
|
||||
for index_y in range(9):
|
||||
if WAREHOUSE_MAP[index_x][index_y] == 10:
|
||||
@ -466,8 +223,6 @@ while running:
|
||||
if event.type == pygame.KEYDOWN:
|
||||
pass
|
||||
|
||||
|
||||
|
||||
if len(Package_list) < QTY_OF_PACKAGES:
|
||||
|
||||
is_dock_empty = True
|
||||
@ -491,7 +246,7 @@ while running:
|
||||
agent.goal = coord_goals.pop(0)
|
||||
agent.goal_achieved = False
|
||||
|
||||
# agent.path = breadth_first_search(agent.pos_coord, agent.agent_direction)
|
||||
#agent.path = breadth_first_search(agent.pos_coord, agent.goal, agent.agent_direction)
|
||||
agent.path = a_star_search((agent.pos_coord[1], agent.pos_coord[0]), (agent.goal[1], agent.goal[0]), agent.agent_direction)
|
||||
|
||||
for package in Package_list:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user