s473575/inteligentna_smieciarka
1
1
Fork 1
Code Issues Pull Requests Projects Releases Wiki Activity

Final A*

Browse Source
This commit is contained in:
s473575 2023-05-31 21:03:00 +02:00
parent 019c38c344
commit 88eff2c4b7
3 changed files with 125 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
src/__pycache__/agent.cpython-311.pyc Normal file
View File

Binary file not shown.

BIN
src/__pycache__/simulation.cpython-311.pyc Normal file
View File

Binary file not shown.

291
src/agent.py
View File

@ -3,6 +3,20 @@ import pygame as pg
import heapq import heapq
class Node:
def __init__(self, state, parent, cost):
self.state = state
self.parent = parent
self.cost = cost
def __lt__(self, other):
return self.cost < other.cost
class State:
def __init__(self, position, house_list):
self.position = position
self.house_list = house_list
def vector_to_tuple(vector): def vector_to_tuple(vector):
tup = (int(vector.x), int(vector.y)) tup = (int(vector.x), int(vector.y))
return tup return tup
@ -27,43 +41,42 @@ class Agent:
self.weights = {} self.weights = {}
self.orientation = 90 self.orientation = 90
# utworzenie grafu dróg # utworzenie grafu dróg
roads_pos = [vector_to_tuple(pos) for pos in self.simulation.state.road_pos_g + self.simulation.state.road_pos_r] roads_pos = [vector_to_tuple(pos) for pos in
self.simulation.state.road_pos_g + self.simulation.state.road_pos_r]
roads_pos.sort() roads_pos.sort()
for index, pos in enumerate(roads_pos): for index, pos in enumerate(roads_pos):
if pos[0] < 0 or pos[0] >= 27 or pos[1] < 0 or pos[1] >= 14: if pos[0] < 0 or pos[0] >= 27 or pos[1] < 0 or pos[1] >= 14:
continue continue
for another_pos in roads_pos[index:]: for another_pos in roads_pos[index:]:
if pos == another_pos or another_pos[0] < 0 or another_pos[0] >= 27 or another_pos[1] < 0 or another_pos[1] >= 14: if pos == another_pos or another_pos[0] < 0 or another_pos[0] >= 27 or another_pos[1] < 0 or \
another_pos[1] >= 14:
continue continue
if ((abs(pos[0] - another_pos[0]) == 1 and abs(pos[1] - another_pos[1]) == 0) or (abs(pos[0] - another_pos[0]) == 0 and abs(pos[1] - another_pos[1]) == 1)): if ((abs(pos[0] - another_pos[0]) == 1 and abs(pos[1] - another_pos[1]) == 0) or (
abs(pos[0] - another_pos[0]) == 0 and abs(pos[1] - another_pos[1]) == 1)):
if pos not in self.graph.keys(): if pos not in self.graph.keys():
self.graph[pos] = set() self.graph[pos] = set()
if another_pos not in self.graph.keys(): if another_pos not in self.graph.keys():
self.graph[another_pos] = set() self.graph[another_pos] = set()
if another_pos in self.simulation.state.road_pos_r: if another_pos in self.simulation.state.road_pos_r:
weight = 1 weight = 1
elif another_pos in self.simulation.state.road_pos_g: elif another_pos in self.simulation.state.road_pos_g:
weight = 3 weight = 100
else: else:
weight = 1 weight = 1
self.graph[pos].add(another_pos) self.graph[pos].add(another_pos)
self.graph[another_pos].add(pos) self.graph[another_pos].add(pos)
self.weights[(pos,another_pos)] = weight self.weights[(pos, another_pos)] = weight
self.weights[(another_pos, pos)] = weight self.weights[(another_pos, pos)] = weight
# dołączenie domów i składowisk do grafu dróg # dołączenie domów i składowisk do grafu dróg
entities = self.simulation.state.entities entities = self.simulation.state.entities
for entity in entities: for entity in entities:
entity_pos = vector_to_tuple(entity.position) entity_pos = vector_to_tuple(entity.position)
for neighbour_pos in [(entity_pos[0]-1, entity_pos[1]), for neighbour_pos in [(entity_pos[0] - 1, entity_pos[1]),
(entity_pos[0]+1, entity_pos[1]), (entity_pos[0] + 1, entity_pos[1]),
(entity_pos[0], entity_pos[1]-1), (entity_pos[0], entity_pos[1] - 1),
(entity_pos[0], entity_pos[1]+1)]: (entity_pos[0], entity_pos[1] + 1)]:
if neighbour_pos[0] < 0 or neighbour_pos[0] >= 27 or neighbour_pos[1] < 0 or neighbour_pos[1] >= 14: if neighbour_pos[0] < 0 or neighbour_pos[0] >= 27 or neighbour_pos[1] < 0 or neighbour_pos[1] >= 14:
continue continue
if neighbour_pos in roads_pos: if neighbour_pos in roads_pos:
@ -77,10 +90,9 @@ class Agent:
self.dumps[entity.trash_type] = vector_to_tuple(entity.position) self.dumps[entity.trash_type] = vector_to_tuple(entity.position)
if entity.entity_type == 'house': if entity.entity_type == 'house':
self.houses[vector_to_tuple(entity.position)] = HousePOI() self.houses[vector_to_tuple(entity.position)] = HousePOI()
self.path = self.A_star() self.path = self.a_star()
pass pass
def update(self): def update(self):
entities = self.simulation.state.entities entities = self.simulation.state.entities
@ -90,183 +102,130 @@ class Agent:
self.fullness = entity.fullness self.fullness = entity.fullness
self.orientation = entity.orientation self.orientation = entity.orientation
def decide_move(self): def decide_move(self):
move_a = self.path[0] if self.path:
move = (move_a[0] - self.current_pos[0], move_a[1] - self.current_pos[1]) move_a = self.path[0]
move = (move_a[0] - self.current_pos[0], move_a[1] - self.current_pos[1])
if self.orientation == 0: if self.orientation == 0:
if move[0] == 0: if move[0] == 0:
if move[1] == -1: if move[1] == -1:
self.path.pop(0) self.path.pop(0)
return pg.Vector2(move) return pg.Vector2(move)
else: else:
return 180 return 180
elif move[1] == 0: elif move[1] == 0:
if move[0] == -1: if move[0] == -1:
return 270 return 270
else: else:
return 90 return 90
elif self.orientation == 90: elif self.orientation == 90:
if move[0] == 0: if move[0] == 0:
if move[1] == -1: if move[1] == -1:
return 0 return 0
else: else:
return 180 return 180
elif move[1] == 0: elif move[1] == 0:
if move[0] == -1: if move[0] == -1:
return 270 return 270
else: else:
self.path.pop(0) self.path.pop(0)
return pg.Vector2(move) return pg.Vector2(move)
elif self.orientation == 180: elif self.orientation == 180:
if move[0] == 0: if move[0] == 0:
if move[1] == -1: if move[1] == -1:
return 0 return 0
else: else:
self.path.pop(0) self.path.pop(0)
return pg.Vector2(move) return pg.Vector2(move)
elif move[1] == 0: elif move[1] == 0:
if move[0] == -1: if move[0] == -1:
return 270 return 270
else: else:
return 90 return 90
else:
if move[0] == 0:
if move[1] == -1:
return 0
else:
return 180
elif move[1] == 0:
if move[0] == -1:
self.path.pop(0)
return pg.Vector2(move)
else:
return 90
else: else:
if move[0] == 0: return pg.Vector2(0, 0)
if move[1] == -1:
return 0
else:
return 180
elif move[1] == 0:
if move[0] == -1:
self.path.pop(0)
return pg.Vector2(move)
else:
return 90
def heuristic(self, start, end):
return abs((end[0][0] - start[0][0])) + abs((end[0][1] - start[0][1])) // (abs(len(start[1]) - len(end[1])) + 1)
def weight_cost(self, start_pos, end_pos): def successors(self, state):
return self.weights[(start_pos, end_pos)]
def get_move_cost(self, start_pos, end_pos):
return self.heuristic(start_pos, end_pos) + self.weight_cost(start_pos, end_pos)
def get_start_state(self):
# entities = self.simulation.state.entities
# orientation - self.orientation
position = self.current_pos
house_list = tuple(self.houses)
start_state = (position, house_list)
return start_state
def get_end_state(self):
position = (0,1)
house_list = ()
end_state = (position, house_list)
return end_state
def succesor(self, state):
successors_pos = self.graph[state[0]] successors_pos = self.graph[state[0]]
house_list = state[1] house_list = state[1]
successors = () successors = []
for pos in successors_pos: for pos in successors_pos:
if pos in house_list: if pos in house_list:
house_list = list(house_list) house_list = list(house_list)
house_list.remove(pos) house_list.remove(pos)
house_list = tuple(house_list) house_list = tuple(house_list)
successors = list(successors) successors.append((pos, tuple(house_list)))
successors.append((pos, tuple(house_list)))
successors = tuple(successors)
else:
successors = list(successors)
successors.append((pos, tuple(house_list)))
successors = tuple(successors)
return successors return successors
def cost(self, current_node, succ_state):
if succ_state[0] not in current_node.state[1]:
cost = current_node.cost + self.weights[current_node.state[0], succ_state[0]]
else:
cost = float(1)
return cost
'''def A_star(self): def heuristic(self, succ_state, goal_state):
position = succ_state[0]
if len(succ_state[1]) != 0:
estimated_cost = 0
for house in succ_state[1]:
estimated_cost += abs(position[0] - house[0]) + abs(position[1] - house[1])
else:
goal_position = goal_state[0]
estimated_cost = abs(position[0] - goal_position[0]) + abs(position[1] - goal_position[1])
return estimated_cost
def a_star(self):
fringe = [] fringe = []
explored = set() explored = set()
istate = self.get_start_state() initial_state = (self.current_pos, tuple(self.houses))
goaltest = self.get_end_state() goal_state = (self.current_pos, ())
node = (istate, None) start_node = Node(initial_state, None, 0)
actions = []
heapq.heappush(fringe, (self.heuristic(istate[0], goaltest[0]), node)) heapq.heappush(fringe, start_node)
while fringe: while fringe:
_, el = heapq.heappop(fringe) current_node = heapq.heappop(fringe)
elem = el[0]
parent = el[1]
if elem == goaltest: if current_node.state == goal_state:
while parent is not None: actions = []
actions.append(elem[0]) while current_node:
elem = parent actions.append(current_node.state[0])
actions.reverse() current_node = current_node.parent
return actions c = (4, 0) in actions
return list(reversed(actions))
explored.add(elem) explored.add(current_node.state)
succs = self.succesor(elem) succ_states = self.successors(current_node.state)
for succ in succs:
node = (succ, elem) for succ_state in succ_states:
p = self.heuristic(succ[0], goaltest[0]) + self.weight_cost(elem[0], succ[0]) if succ_state in explored:
continue
if succ not in explored and not any(tup[1][0] == succ for tup in fringe): g = self.cost(current_node, succ_state)
heapq.heappush(fringe, (p,node)) h = self.heuristic(succ_state, goal_state)
elif any(tup[1] == succ for tup in fringe): succ_cost = g + h
i = next(i for node in enumerate(fringe) if node[1][0] == succ) succ_node = Node(succ_state, current_node, succ_cost)
if fringe[i][0] > p:
fringe[i] = (p,node)
return actions'''
def A_star(self): heapq.heappush(fringe, succ_node)
fringe = []
explored = set()
istate = self.get_start_state()
goaltest = self.get_end_state()
node = (istate, None)
actions = []
heapq.heappush(fringe, (self.heuristic(istate, goaltest), node))
while fringe:
_, el = heapq.heappop(fringe)
elem = el[0]
parent = el[1]
if elem == goaltest:
while parent is not None:
actions.append(elem[0])
elem = parent[0]
parent = parent[1]
actions.reverse()
return actions
explored.add(elem)
succs = self.succesor(elem)
for succ in succs:
node = (succ, el)
heuristic = self.heuristic(succ, goaltest)
cost = 7*self.weight_cost(elem[0], succ[0])
p = heuristic + cost
if succ not in explored and not any(tup[1][0] == succ for tup in fringe):
heapq.heappush(fringe, (p, node))
elif any(tup[1][0] == succ for tup in fringe):
i = next(i for i, node in enumerate(fringe) if node[1][0] == succ)
if fringe[i][0] > p:
fringe[i] = (p, node)
return actions
return None
def discover(self): def discover(self):
if self.current_pos in self.houses.keys(): if self.current_pos in self.houses.keys():