Added BFS algorithm to search path using 3 actions: turn left/right, go straight #19

Merged
s473628 merged 3 commits from rotation_actions into main 2023-05-04 21:32:51 +02:00
6 changed files with 144 additions and 25 deletions

3
.gitignore vendored
View File

@ -2,3 +2,6 @@
.DS_Store
/.vscode
__pycache__
#PyCharm
.idea/

View File

@ -14,7 +14,7 @@ class State:
return self.x == other.x and self.y == other.y
class StateGraphSearchBFS:
class GoAnyDirectionBFS:
def __init__(self, world: World, start_state: State, goal_state: State):
self.start_state = start_state
self.goal_state = goal_state

View File

@ -0,0 +1,83 @@
import queue
from domain.world import World
class State:
def __init__(self, x, y, direction=(1, 0)):
self.x = x
self.y = y
self.direction = direction
def __hash__(self):
return hash((self.x, self.y))
def __eq__(self, other):
return (self.x == other.x and self.y == other.y
and self.direction == other.direction)
class Node:
def __init__(self, state: State):
self.state = state
self.parent = None
self.action = None
def action_sequence(node: Node):
actions = []
while node.parent:
actions.append(node.action)
node = node.parent
actions.reverse()
return actions
class RotateAndGoBFS:
def __init__(self, world: World, start_state: State, goal_state: State):
self.world = world
self.start_state = start_state
self.goal_state = goal_state
self.fringe = queue.Queue()
self.enqueued_states = set()
self.explored = set()
self.actions = []
def search(self):
self.fringe.put(Node(self.start_state))
while self.fringe:
elem = self.fringe.get()
if self.is_goal(elem.state):
self.actions = action_sequence(elem)
return True
self.explored.add(elem.state)
for (action, state) in self.successors(elem.state):
if state in self.explored or state in self.enqueued_states:
continue
next_node = Node(state)
next_node.action = action
next_node.parent = elem
self.fringe.put(next_node)
self.enqueued_states.add(state)
return False
def successors(self, state: State):
new_successors = [
# rotate right
("RR", State(state.x, state.y, (-state.direction[1], state.direction[0]))),
# rotate left
("RL", State(state.x, state.y, (state.direction[1], -state.direction[0]))),
]
if self.world.accepted_move(state.x + state.direction[0], state.y + state.direction[1]):
new_successors.append(
("GO", State(state.x + state.direction[0], state.y + state.direction[1], state.direction)))
return new_successors
def is_goal(self, state: State) -> bool:
return (
state.x == self.goal_state.x
and state.y == self.goal_state.y
)

View File

@ -5,6 +5,7 @@ from domain.world import World
class Vacuum(Entity):
def __init__(self, x: int, y: int):
super().__init__(x, y, "VACUUM")
self.direction = (1, 0)
self.battery = 100
self.cleaning_detergent = 100
self.container_filling = 0

67
main.py
View File

@ -11,7 +11,8 @@ from domain.entities.vacuum import Vacuum
from domain.entities.docking_station import Doc_Station
from domain.world import World
from view.renderer import Renderer
from AI_brain.movement import StateGraphSearchBFS, State
# from AI_brain.movement import GoAnyDirectionBFS, State
from AI_brain.rotate_and_go_bfs import RotateAndGoBFS, State
config = configparser.ConfigParser()
@ -48,37 +49,57 @@ class Main:
start_state = State(self.world.vacuum.x, self.world.vacuum.y)
end_state = State(self.world.doc_station.x, self.world.doc_station.y)
SGS_BFS = StateGraphSearchBFS(self.world, start_state, end_state)
if not SGS_BFS.search():
# path_searcher = GoAnyDirectionBFS(self.world, start_state, end_state)
path_searcher = RotateAndGoBFS(self.world, start_state, end_state)
if not path_searcher.search():
print("No solution")
exit(0)
SGS_BFS.actions.reverse()
path_searcher.actions.reverse()
while self.running:
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.running = False
if len(path_searcher.actions) > 0:
action_direction = path_searcher.actions.pop()
# self.handle_action1(action_direction)
self.handle_action2(action_direction)
self.update()
self.renderer.render(self.world)
self.clock.tick(5)
if len(SGS_BFS.actions) > 0:
action_direction = SGS_BFS.actions.pop()
if action_direction == "UP":
self.commands.append(
VacuumMoveCommand(self.world, self.world.vacuum, (0, -1))
)
elif action_direction == "DOWN":
self.commands.append(
VacuumMoveCommand(self.world, self.world.vacuum, (0, 1))
)
elif action_direction == "LEFT":
self.commands.append(
VacuumMoveCommand(self.world, self.world.vacuum, (-1, 0))
)
elif action_direction == "RIGHT":
self.commands.append(
VacuumMoveCommand(self.world, self.world.vacuum, (1, 0))
)
self.update()
pygame.quit()
def handle_action1(self, action):
if action == "UP":
self.commands.append(
VacuumMoveCommand(self.world, self.world.vacuum, (0, -1))
)
elif action == "DOWN":
self.commands.append(
VacuumMoveCommand(self.world, self.world.vacuum, (0, 1))
)
elif action == "LEFT":
self.commands.append(
VacuumMoveCommand(self.world, self.world.vacuum, (-1, 0))
)
elif action == "RIGHT":
self.commands.append(
VacuumMoveCommand(self.world, self.world.vacuum, (1, 0))
)
def handle_action2(self, action):
if action == "GO":
self.commands.append(
VacuumMoveCommand(self.world, self.world.vacuum, self.world.vacuum.direction)
)
elif action == "RR":
self.world.vacuum.direction = (-self.world.vacuum.direction[1], self.world.vacuum.direction[0])
elif action == "RL":
self.world.vacuum.direction = (self.world.vacuum.direction[1], -self.world.vacuum.direction[0])
def process_input(self):
for event in pygame.event.get():
if event.type == pygame.QUIT:

View File

@ -159,6 +159,7 @@ class Renderer:
draw_pos[1] + self.tile_height,
)
self.screen.blit(text_surface, text_pos)
sprite = self.create_vacuum_sprite(entity)
if "DOC_STATION" in entity.type:
draw_pos = (
(entity.x - 0.1) * self.tile_width,
@ -166,6 +167,16 @@ class Renderer:
)
self.screen.blit(sprite, draw_pos)
def create_vacuum_sprite(self, vacuum):
angles = {
(1, 0): 0,
(-1, 0): 180,
(0, 1): 270,
(0, -1): 90,
}
init_sprite = self.sprites.get(vacuum.type, None)
return pygame.transform.rotate(init_sprite, angles[vacuum.direction])
def draw_sprite(self, x: int, y: int, sprite_name: str):
self.screen.blit(
self.sprites[sprite_name], (x * self.tile_width, y * self.tile_height)