Naprawa ruchu i A*
This commit is contained in:
parent
c443087ab0
commit
2fe5d96595
10
res/map.txt
10
res/map.txt
@ -1,12 +1,12 @@
|
|||||||
MP--H---------------------
|
MP--H---------------------
|
||||||
RRRRRRRRRRRRRRRRRRR---G---
|
RRRRRRRRRRRRRRRRRRR---G---
|
||||||
SZ--R------G------R---GGG-
|
SZ--G------G------R---GGG-
|
||||||
----R------G------R---G---
|
----G------G------R---G---
|
||||||
-RRRR------GGGGGGGRRRRR---
|
-RRRG------GGGGGGGRRRRR---
|
||||||
-R--G-------------R-------
|
-R--G-------------R-------
|
||||||
-R--GGGGGGGGGGGGGGRH------
|
-R--GGGGGGGGGRRRRRRH------
|
||||||
-R--G---G---GH----R-------
|
-R--G---G---GH----R-------
|
||||||
-R--G---G---GH----RRRRRRR-
|
-R--G---G---G-----RRRRRRR-
|
||||||
-R--G-------H-----R-------
|
-R--G-------H-----R-------
|
||||||
-R--G-----G----G--R-------
|
-R--G-----G----G--R-------
|
||||||
-R--G-----G----G--R-------
|
-R--G-----G----G--R-------
|
||||||
|
|||||||
122
src/agent.py
122
src/agent.py
@ -43,7 +43,7 @@ class Agent:
|
|||||||
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 = 2
|
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 = 3
|
||||||
else:
|
else:
|
||||||
@ -79,7 +79,7 @@ class Agent:
|
|||||||
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
|
||||||
|
|
||||||
|
|
||||||
def update(self):
|
def update(self):
|
||||||
@ -92,58 +92,60 @@ class Agent:
|
|||||||
|
|
||||||
|
|
||||||
def decide_move(self):
|
def decide_move(self):
|
||||||
|
move_a = self.path[0]
|
||||||
|
move = (move_a[0] - self.current_pos[0], move_a[1] - self.current_pos[1])
|
||||||
|
|
||||||
move = self.path.pop
|
|
||||||
|
|
||||||
if self.orientation == 0:
|
if self.orientation == 0:
|
||||||
if move[0] != 0:
|
if move[0] == 0:
|
||||||
if move[0] == 1:
|
if move[1] == -1:
|
||||||
return 90
|
self.path.pop(0)
|
||||||
else:
|
|
||||||
return 270
|
|
||||||
elif move[1] != 0:
|
|
||||||
if move[1] == 1:
|
|
||||||
return pg.Vector2(move)
|
return pg.Vector2(move)
|
||||||
else:
|
else:
|
||||||
return 180
|
return 180
|
||||||
|
elif move[1] == 0:
|
||||||
|
if move[0] == -1:
|
||||||
|
return 270
|
||||||
|
else:
|
||||||
|
return 90
|
||||||
elif self.orientation == 90:
|
elif self.orientation == 90:
|
||||||
if move[0] != 0:
|
if move[0] == 0:
|
||||||
if move[0] == 1:
|
if move[1] == -1:
|
||||||
return pg.Vector2(move)
|
|
||||||
else:
|
|
||||||
return 270
|
|
||||||
elif move[1] != 0:
|
|
||||||
if move[1] == 1:
|
|
||||||
return 0
|
return 0
|
||||||
else:
|
else:
|
||||||
return 180
|
return 180
|
||||||
|
elif move[1] == 0:
|
||||||
|
if move[0] == -1:
|
||||||
|
return 270
|
||||||
|
else:
|
||||||
|
self.path.pop(0)
|
||||||
|
return pg.Vector2(move)
|
||||||
elif self.orientation == 180:
|
elif self.orientation == 180:
|
||||||
if move[0] != 0:
|
if move[0] == 0:
|
||||||
if move[0] == 1:
|
if move[1] == -1:
|
||||||
return 90
|
|
||||||
else:
|
|
||||||
return 270
|
|
||||||
elif move[1] != 0:
|
|
||||||
if move[1] == 1:
|
|
||||||
return 0
|
return 0
|
||||||
else:
|
else:
|
||||||
|
self.path.pop(0)
|
||||||
return pg.Vector2(move)
|
return pg.Vector2(move)
|
||||||
else:
|
elif move[1] == 0:
|
||||||
if move[0] != 0:
|
if move[0] == -1:
|
||||||
if move[0] == 1:
|
return 270
|
||||||
return 90
|
|
||||||
else:
|
else:
|
||||||
return pg.Vector2(move)
|
return 90
|
||||||
elif move[1] != 0:
|
else:
|
||||||
if move[1] == 0:
|
if move[0] == 0:
|
||||||
|
if move[1] == -1:
|
||||||
return 0
|
return 0
|
||||||
else:
|
else:
|
||||||
return 180
|
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_pos, end_pos):
|
def heuristic(self, start, end):
|
||||||
return abs((end_pos[0] - start_pos[0])) + abs((end_pos[1] - start_pos[1]))
|
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 weight_cost(self, start_pos, end_pos):
|
||||||
return self.weights[(start_pos, end_pos)]
|
return self.weights[(start_pos, end_pos)]
|
||||||
@ -152,7 +154,7 @@ class Agent:
|
|||||||
return self.heuristic(start_pos, end_pos) + self.weight_cost(start_pos, end_pos)
|
return self.heuristic(start_pos, end_pos) + self.weight_cost(start_pos, end_pos)
|
||||||
|
|
||||||
def get_start_state(self):
|
def get_start_state(self):
|
||||||
entities = self.simulation.state.entities
|
# entities = self.simulation.state.entities
|
||||||
# orientation - self.orientation
|
# orientation - self.orientation
|
||||||
position = self.current_pos
|
position = self.current_pos
|
||||||
house_list = tuple(self.houses)
|
house_list = tuple(self.houses)
|
||||||
@ -186,12 +188,13 @@ class Agent:
|
|||||||
return successors
|
return successors
|
||||||
|
|
||||||
|
|
||||||
def A_star(self):
|
'''def A_star(self):
|
||||||
fringe = []
|
fringe = []
|
||||||
explored = set()
|
explored = set()
|
||||||
istate = self.get_start_state()
|
istate = self.get_start_state()
|
||||||
goaltest = self.get_end_state()
|
goaltest = self.get_end_state()
|
||||||
node = (istate, None)
|
node = (istate, None)
|
||||||
|
actions = []
|
||||||
|
|
||||||
heapq.heappush(fringe, (self.heuristic(istate[0], goaltest[0]), node))
|
heapq.heappush(fringe, (self.heuristic(istate[0], goaltest[0]), node))
|
||||||
|
|
||||||
@ -201,7 +204,6 @@ class Agent:
|
|||||||
parent = el[1]
|
parent = el[1]
|
||||||
|
|
||||||
if elem == goaltest:
|
if elem == goaltest:
|
||||||
actions = []
|
|
||||||
while parent is not None:
|
while parent is not None:
|
||||||
actions.append(elem[0])
|
actions.append(elem[0])
|
||||||
elem = parent
|
elem = parent
|
||||||
@ -222,8 +224,48 @@ class Agent:
|
|||||||
i = next(i for node in enumerate(fringe) if node[1][0] == succ)
|
i = next(i for node in enumerate(fringe) if node[1][0] == succ)
|
||||||
if fringe[i][0] > p:
|
if fringe[i][0] > p:
|
||||||
fringe[i] = (p,node)
|
fringe[i] = (p,node)
|
||||||
return False
|
return actions'''
|
||||||
|
|
||||||
|
def A_star(self):
|
||||||
|
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
|
||||||
|
|
||||||
|
|
||||||
def discover(self):
|
def discover(self):
|
||||||
|
|||||||
@ -276,9 +276,9 @@ class Interface:
|
|||||||
def update(self):
|
def update(self):
|
||||||
self.state.update(self.move_truck)
|
self.state.update(self.move_truck)
|
||||||
self.agent.update()
|
self.agent.update()
|
||||||
if isinstance(self.move_truck, int):
|
'''if isinstance(self.move_truck, int):
|
||||||
self.state.update(self.move_truck)
|
self.state.update(self.move_truck)
|
||||||
self.agent.update()
|
self.agent.update()'''
|
||||||
|
|
||||||
def render(self):
|
def render(self):
|
||||||
if not self.debug_mode:
|
if not self.debug_mode:
|
||||||
@ -298,5 +298,5 @@ class Interface:
|
|||||||
self.processAgentInput()
|
self.processAgentInput()
|
||||||
self.update()
|
self.update()
|
||||||
self.render()
|
self.render()
|
||||||
self.clock.tick(24)
|
self.clock.tick(12)
|
||||||
pg.quit()
|
pg.quit()
|
||||||
Loading…
Reference in New Issue
Block a user