556 lines
17 KiB
Python
556 lines
17 KiB
Python
from os import path
|
|
import heapq
|
|
import copy
|
|
|
|
from settings import *
|
|
from sprites import Direction
|
|
|
|
|
|
class PlanRoute():
|
|
""" The problem of moving the Agent from one place to other """
|
|
|
|
def __init__(self, initial, goal, allowed, puddles=None, dimrow=None):
|
|
""" Define goal state and initialize a problem """
|
|
self.initial = initial
|
|
self.goal = goal
|
|
self.dimrow = dimrow
|
|
self.goal = goal
|
|
self.allowed = allowed
|
|
self.puddles = puddles
|
|
|
|
def actions(self, state):
|
|
possible_actions = ['Forward', 'Left', 'Right']
|
|
x, y = state.get_location()
|
|
orientation = state.get_orientation()
|
|
|
|
# Prevent Bumps
|
|
if y == 0 and orientation == 'LEFT':
|
|
if 'Forward' in possible_actions:
|
|
possible_actions.remove('Forward')
|
|
if x == 0 and orientation == 'DOWN':
|
|
if 'Forward' in possible_actions:
|
|
possible_actions.remove('Forward')
|
|
if y == self.dimrow and orientation == 'RIGHT':
|
|
if 'Forward' in possible_actions:
|
|
possible_actions.remove('Forward')
|
|
if x == self.dimrow and orientation == 'UP':
|
|
if 'Forward' in possible_actions:
|
|
possible_actions.remove('Forward')
|
|
|
|
return possible_actions
|
|
|
|
def result(self, state, action):
|
|
""" Given state and action, return a new state that is the result of the action.
|
|
Action is assumed to be a valid action in the state """
|
|
x, y = state.get_location()
|
|
proposed_loc = list()
|
|
#proposed_loc = []
|
|
|
|
# Move Forward
|
|
if action == 'Forward':
|
|
if state.get_orientation() == 'UP':
|
|
proposed_loc = [x + 1, y]
|
|
elif state.get_orientation() == 'DOWN':
|
|
proposed_loc = [x - 1, y]
|
|
elif state.get_orientation() == 'LEFT':
|
|
proposed_loc = [x, y - 1]
|
|
elif state.get_orientation() == 'RIGHT':
|
|
proposed_loc = [x, y + 1]
|
|
else:
|
|
raise Exception('InvalidOrientation')
|
|
|
|
# Rotate counter-clockwise
|
|
elif action == 'Right':
|
|
if state.get_orientation() == 'UP':
|
|
state.set_orientation('LEFT')
|
|
elif state.get_orientation() == 'DOWN':
|
|
state.set_orientation('RIGHT')
|
|
elif state.get_orientation() == 'LEFT':
|
|
state.set_orientation('DOWN')
|
|
elif state.get_orientation() == 'RIGHT':
|
|
state.set_orientation('UP')
|
|
else:
|
|
raise Exception('InvalidOrientation')
|
|
|
|
# Rotate clockwise
|
|
elif action == 'Left':
|
|
if state.get_orientation() == 'UP':
|
|
state.set_orientation('RIGHT')
|
|
elif state.get_orientation() == 'DOWN':
|
|
state.set_orientation('LEFT')
|
|
elif state.get_orientation() == 'LEFT':
|
|
state.set_orientation('UP')
|
|
elif state.get_orientation() == 'RIGHT':
|
|
state.set_orientation('DOWN')
|
|
else:
|
|
raise Exception('InvalidOrientation')
|
|
|
|
|
|
if(proposed_loc):
|
|
tupled_proposed_loc = tuple([proposed_loc[0], proposed_loc[1]])
|
|
|
|
if tupled_proposed_loc in self.allowed:
|
|
state.set_location(proposed_loc[0], proposed_loc[1])
|
|
|
|
return state
|
|
|
|
def goal_test(self, state):
|
|
""" Given a state, return True if state is a goal state or False, otherwise """
|
|
|
|
return state.get_location() == self.goal.get_location()
|
|
|
|
def path_cost(self, c, state1, action, state2):
|
|
if action == "Forward" or action == "Left" or action == "Right":
|
|
|
|
|
|
x1, y1 = state1.get_location()
|
|
location1 = tuple([x1, y1])
|
|
x2, y2 = state2.get_location()
|
|
location2 = tuple([x1, y1])
|
|
|
|
|
|
if location2 in self.puddles:
|
|
return c + 2
|
|
if location1 == location2 and state1 in self.puddles:
|
|
return c + 2
|
|
return c+1
|
|
|
|
def h(self, node):
|
|
""" Return the heuristic value for a given state."""
|
|
|
|
# Manhattan Heuristic Function
|
|
x1, y1 = node.state.get_location()
|
|
x2, y2 = self.goal.get_location()
|
|
|
|
return abs(x2 - x1) + abs(y2 - y1)
|
|
|
|
|
|
class Node:
|
|
def __init__(self, state, parent=None, action=None, path_cost=0):
|
|
"""Create a search tree Node, derived from a parent by an action."""
|
|
self.state = state #AgentPosition?
|
|
self.parent = parent
|
|
self.action = action
|
|
self.path_cost = path_cost
|
|
|
|
def __repr__(self):
|
|
return "<Node {}>".format(self.state)
|
|
|
|
def solution(self):
|
|
"""Return the sequence of actions to go from the root to this node."""
|
|
return [node.action for node in self.path()[1:]]
|
|
|
|
|
|
def expand(self, problem):
|
|
"""List the nodes reachable in one step from this node."""
|
|
test_node_list = [self.child_node(problem, action)
|
|
for action in problem.actions(self.state)]
|
|
|
|
return [self.child_node(problem, action)
|
|
for action in problem.actions(self.state)]
|
|
|
|
def child_node(self, problem, action):
|
|
|
|
|
|
next_state = problem.result(copy.deepcopy(self.state), action)
|
|
next_node = Node(next_state, self, action, problem.path_cost(
|
|
self.path_cost, self.state, action, next_state))
|
|
#print(problem.path_cost(
|
|
# self.path_cost, self.state, action, next_state))
|
|
return next_node
|
|
|
|
def __eq__(self, other):
|
|
return isinstance(other, Node) and self.state == other.state
|
|
|
|
def __lt__(self, other):
|
|
return isinstance(other, Node) and self.state == other.state
|
|
|
|
|
|
|
|
def __hash__(self):
|
|
# We use the hash value of the state
|
|
# stored in the node instead of the node
|
|
# object itself to quickly search a node
|
|
# with the same state in a Hash Table
|
|
return hash(self.state)
|
|
|
|
def path(self):
|
|
"""Return a list of nodes forming the path from the root to this node."""
|
|
node, path_back = self, []
|
|
while node:
|
|
path_back.append(node)
|
|
node = node.parent
|
|
return list(reversed(path_back))
|
|
|
|
|
|
class AgentPosition:
|
|
def __init__(self, x, y, orientation):
|
|
self.X = x
|
|
self.Y = y
|
|
self.orientation = orientation
|
|
|
|
def get_location(self):
|
|
return self.X, self.Y
|
|
|
|
def set_location(self, x, y):
|
|
self.X = x
|
|
self.Y = y
|
|
|
|
def get_orientation(self):
|
|
return self.orientation
|
|
|
|
def set_orientation(self, orientation):
|
|
self.orientation = orientation
|
|
|
|
def __eq__(self, other):
|
|
if (other.get_location() == self.get_location() and
|
|
other.get_orientation() == self.get_orientation()):
|
|
return True
|
|
else:
|
|
return False
|
|
|
|
|
|
def __hash__(self):
|
|
return hash((self.X, self.Y, self.orientation))
|
|
|
|
|
|
class SweeperAgent:
|
|
def __init__(self, dimensions=None):
|
|
self.dimrow = dimensions
|
|
self.current_position = None
|
|
self.orientation = ""
|
|
self.initial = set()
|
|
self.goal = set()
|
|
self.allowed_points = set()
|
|
self.puddle_points = set()
|
|
|
|
|
|
def where_am_i(self):
|
|
temp_map = [list(item) for item in SweeperAgent.loadMap("map.txt")]
|
|
|
|
for row in range(MAP_SIZE):
|
|
for column, pos in enumerate(temp_map[row]):
|
|
if pos == ">" or pos == "<" or pos == "^" or pos == "v":
|
|
self.row = row
|
|
self.column = column
|
|
|
|
return row, column
|
|
|
|
# add orientation
|
|
|
|
def where_to_go(self):
|
|
temp_map = [list(item) for item in SweeperAgent.loadMap("goal_map.txt")]
|
|
|
|
for row in range(MAP_SIZE):
|
|
for column, pos in enumerate(temp_map[row]):
|
|
if pos == ">" or pos == "<" or pos == "^" or pos == "v":
|
|
self.row = row
|
|
self.column = column
|
|
|
|
return row, column
|
|
|
|
@staticmethod
|
|
def set_allowed(allowed_points):
|
|
temp_map = [list(item) for item in SweeperAgent.loadMap('map.txt')]
|
|
|
|
a_row = 0
|
|
a_column = 0
|
|
|
|
for row in range(MAP_SIZE):
|
|
for column, pos in enumerate(temp_map[row]):
|
|
if pos == "." or pos == 'p' or pos == '>' or pos == '<' or pos == 'v' or pos == '^':
|
|
a_row = row
|
|
a_column = column
|
|
location = tuple([a_row, a_column])
|
|
allowed_points.add(location)
|
|
|
|
@staticmethod
|
|
def set_puddles(puddle_points):
|
|
temp_map = [list(item) for item in SweeperAgent.loadMap('map.txt')]
|
|
|
|
a_row = 0
|
|
a_column = 0
|
|
|
|
for row in range(MAP_SIZE):
|
|
for column, pos in enumerate(temp_map[row]):
|
|
if pos == "p" :
|
|
a_row = row
|
|
a_column = column
|
|
location = tuple([a_row, a_column])
|
|
puddle_points.add(location)
|
|
|
|
|
|
@staticmethod
|
|
def get_goal():
|
|
temp_map = [list(item) for item in SweeperAgent.loadMap('goal_map.txt')]
|
|
|
|
a_row = 0
|
|
a_column = 0
|
|
|
|
for row in range(MAP_SIZE):
|
|
for column, pos in enumerate(temp_map[row]):
|
|
if pos == '>' or pos == '<' or pos == 'v' or pos == '^':
|
|
a_row = row
|
|
a_column = column
|
|
return a_row, a_column
|
|
|
|
|
|
@staticmethod
|
|
def set_initial(initial):
|
|
temp_map = [list(item) for item in SweeperAgent.loadMap('map.txt')]
|
|
|
|
a_row = 0
|
|
a_column = 0
|
|
|
|
for row in range(MAP_SIZE):
|
|
for column, pos in enumerate(temp_map[row]):
|
|
if pos == '>' or pos == '<' or pos == 'v' or pos == '^':
|
|
a_row = row
|
|
a_column = column
|
|
location = tuple([a_row, a_column])
|
|
initial.add(location)
|
|
|
|
|
|
|
|
|
|
@staticmethod
|
|
def set_orientation():
|
|
temp_map = [list(item) for item in SweeperAgent.loadMap('map.txt')]
|
|
|
|
orientation = ""
|
|
|
|
for row in range(MAP_SIZE):
|
|
for column, pos in enumerate(temp_map[row]):
|
|
if pos == ">":
|
|
orientation = "RIGHT"
|
|
if pos == "<":
|
|
orientation = "LEFT"
|
|
if pos == "^":
|
|
orientation = "UP"
|
|
if pos == "v":
|
|
orientation = "DOWN"
|
|
|
|
return orientation
|
|
|
|
@staticmethod
|
|
def set_goal_orientation():
|
|
temp_map = [list(item) for item in SweeperAgent.loadMap('goal_map.txt')]
|
|
|
|
orientation = ""
|
|
|
|
for row in range(MAP_SIZE):
|
|
for column, pos in enumerate(temp_map[row]):
|
|
if pos == ">":
|
|
orientation = "RIGHT"
|
|
if pos == "<":
|
|
orientation = "LEFT"
|
|
if pos == "^":
|
|
orientation = "UP"
|
|
if pos == "v":
|
|
orientation = "DOWN"
|
|
|
|
return orientation
|
|
|
|
|
|
@staticmethod
|
|
def run(self):
|
|
self.orientation = SweeperAgent.set_orientation()
|
|
goal_orientation = SweeperAgent.set_goal_orientation()
|
|
#SweeperAgent.set_initial(self.initial)
|
|
#SweeperAgent.set_goal(self.goal)
|
|
SweeperAgent.set_allowed(self.allowed_points)
|
|
SweeperAgent.set_puddles(self.puddle_points)
|
|
|
|
x, y = self.where_am_i()
|
|
x1, y1 = SweeperAgent.get_goal()
|
|
|
|
agent_position = AgentPosition(x, y, self.orientation)
|
|
goal_position = AgentPosition(x1, y1, goal_orientation)
|
|
|
|
return self.plan_route(agent_position, goal_position, self.allowed_points, self.puddle_points)
|
|
|
|
|
|
|
|
"""print("allowed: ")
|
|
print("(row, column)")
|
|
print(sorted(self.allowed_points))
|
|
print("puddles:")
|
|
print(sorted(self.puddle_points))
|
|
print("initial:")
|
|
print(self.initial)
|
|
print("goal:")
|
|
print(self.goal)
|
|
print("orientation:")
|
|
print(self.orientation)"""
|
|
|
|
|
|
def plan_route(self, current, goals, allowed, puddles):
|
|
problem = PlanRoute(current, goals, allowed, puddles, MAP_SIZE-1)
|
|
return SweeperAgent.astar_search(problem, problem.h)
|
|
#return SweeperAgent.astar_search(problem, problem.h)
|
|
|
|
|
|
|
|
|
|
|
|
"""TODO"""
|
|
# liczenie kosztów
|
|
#
|
|
|
|
@staticmethod
|
|
def loadMap(map_name=''):
|
|
maze = []
|
|
map_folder = path.dirname(__file__)
|
|
with open(path.join(map_folder, map_name), 'rt') as f:
|
|
for line in f:
|
|
maze.append(line.rstrip('\n'))
|
|
|
|
# print(maze)
|
|
return maze
|
|
|
|
@staticmethod
|
|
def astar_search(problem, h=None):
|
|
"""A* search is best-first graph search with f(n) = g(n)+h(n).
|
|
You need to specify the h function when you call astar_search, or
|
|
else in your Problem subclass."""
|
|
# h = memoize(h or problem.h, 'h')
|
|
return SweeperAgent.best_first_graph_search(problem, lambda n: n.path_cost + h(n))
|
|
#return best_first_graph_search(problem)
|
|
|
|
@staticmethod
|
|
#def best_first_graph_search(problem, f, display=False):
|
|
def best_first_graph_search(problem, f, display=True):
|
|
|
|
|
|
"""TODO"""
|
|
# Zaimplementować klasę Node dla Astar
|
|
|
|
history = []
|
|
|
|
node = Node(problem.initial)
|
|
frontier = PriorityQueue('min', f)
|
|
frontier.append(node)
|
|
explored = set()
|
|
while frontier:
|
|
node = frontier.pop()
|
|
if problem.goal_test(node.state):
|
|
if display:
|
|
print(len(explored), "paths have been expanded and", len(frontier), "paths remain in the frontier")
|
|
while(node.parent != None):
|
|
history.append(node.action)
|
|
node = node.parent
|
|
#return child
|
|
history.reverse()
|
|
print(history)
|
|
return history
|
|
#return history
|
|
#break
|
|
#return node
|
|
#break
|
|
explored.add(copy.deepcopy(node.state))
|
|
test_child_chamber = node.expand(problem)
|
|
for child in node.expand(problem):
|
|
if child.state not in explored and child not in frontier:
|
|
frontier.append(copy.deepcopy(child))
|
|
elif child in frontier:
|
|
if f(child) < frontier[child]:
|
|
del frontier[child]
|
|
frontier.append(child)
|
|
|
|
return history
|
|
#return None
|
|
|
|
|
|
|
|
class PriorityQueue:
|
|
"""A Queue in which the minimum (or maximum) element (as determined by f and
|
|
order) is returned first.
|
|
If order is 'min', the item with minimum f(x) is
|
|
returned first; if order is 'max', then it is the item with maximum f(x).
|
|
Also supports dict-like lookup."""
|
|
|
|
def __init__(self, order='min', f=lambda x: x):
|
|
self.heap = []
|
|
if order == 'min':
|
|
self.f = f
|
|
elif order == 'max': # now item with max f(x)
|
|
self.f = lambda x: -f(x) # will be popped first
|
|
else:
|
|
raise ValueError("Order must be either 'min' or 'max'.")
|
|
|
|
def append(self, item):
|
|
"""Insert item at its correct position."""
|
|
heapq.heappush(self.heap, (self.f(item), item))
|
|
|
|
def extend(self, items):
|
|
"""Insert each item in items at its correct position."""
|
|
for item in items:
|
|
self.append(item)
|
|
|
|
def pop(self):
|
|
"""Pop and return the item (with min or max f(x) value)
|
|
depending on the order."""
|
|
if self.heap:
|
|
return heapq.heappop(self.heap)[1]
|
|
else:
|
|
raise Exception('Trying to pop from empty PriorityQueue.')
|
|
|
|
def __len__(self):
|
|
"""Return current capacity of PriorityQueue."""
|
|
return len(self.heap)
|
|
|
|
def __contains__(self, key):
|
|
"""Return True if the key is in PriorityQueue."""
|
|
return any([item == key for _, item in self.heap])
|
|
|
|
def __getitem__(self, key):
|
|
"""Returns the first value associated with key in PriorityQueue.
|
|
Raises KeyError if key is not present."""
|
|
for value, item in self.heap:
|
|
if item == key:
|
|
return value
|
|
raise KeyError(str(key) + " is not in the priority queue")
|
|
|
|
def __delitem__(self, key):
|
|
"""Delete the first occurrence of key."""
|
|
try:
|
|
del self.heap[[item == key for _, item in self.heap].index(True)]
|
|
except ValueError:
|
|
raise KeyError(str(key) + " is not in the priority queue")
|
|
heapq.heapify(self.heap)
|
|
|
|
|
|
class Test:
|
|
@staticmethod
|
|
def run():
|
|
|
|
allowed_points = set()
|
|
puddle_points = set()
|
|
|
|
initial = set()
|
|
goal = set()
|
|
|
|
orientation = SweeperAgent.set_orientation()
|
|
|
|
SweeperAgent.set_initial(initial)
|
|
SweeperAgent.set_goal(goal)
|
|
|
|
SweeperAgent.set_allowed(allowed_points)
|
|
SweeperAgent.set_puddles(puddle_points)
|
|
|
|
|
|
|
|
print("allowed: ")
|
|
print("(row, column)")
|
|
print(sorted(allowed_points))
|
|
print("puddles:")
|
|
print(sorted(puddle_points))
|
|
print("initial:")
|
|
print(initial)
|
|
print("goal:")
|
|
print(goal)
|
|
print("orientation:")
|
|
print(orientation)
|
|
|
|
|