From 2fe5d96595d0b77064008f3b311cb6d415d0b305 Mon Sep 17 00:00:00 2001 From: Wit Uta Date: Mon, 15 May 2023 13:12:27 +0200 Subject: [PATCH] Naprawa ruchu i A* --- res/map.txt | 10 ++-- src/agent.py | 126 ++++++++++++++++++++++++++++++---------------- src/simulation.py | 8 +-- 3 files changed, 93 insertions(+), 51 deletions(-) diff --git a/res/map.txt b/res/map.txt index 70e9291..34cd844 100644 --- a/res/map.txt +++ b/res/map.txt @@ -1,12 +1,12 @@ MP--H--------------------- RRRRRRRRRRRRRRRRRRR---G--- -SZ--R------G------R---GGG- -----R------G------R---G--- --RRRR------GGGGGGGRRRRR--- +SZ--G------G------R---GGG- +----G------G------R---G--- +-RRRG------GGGGGGGRRRRR--- -R--G-------------R------- --R--GGGGGGGGGGGGGGRH------ +-R--GGGGGGGGGRRRRRRH------ -R--G---G---GH----R------- --R--G---G---GH----RRRRRRR- +-R--G---G---G-----RRRRRRR- -R--G-------H-----R------- -R--G-----G----G--R------- -R--G-----G----G--R------- diff --git a/src/agent.py b/src/agent.py index 2b6391f..f4cf648 100644 --- a/src/agent.py +++ b/src/agent.py @@ -43,7 +43,7 @@ class Agent: if another_pos not in self.graph.keys(): self.graph[another_pos] = set() if another_pos in self.simulation.state.road_pos_r: - weight = 2 + weight = 1 elif another_pos in self.simulation.state.road_pos_g: weight = 3 else: @@ -79,7 +79,7 @@ class Agent: self.houses[vector_to_tuple(entity.position)] = HousePOI() self.path = self.A_star() - + pass def update(self): @@ -92,58 +92,60 @@ class Agent: def decide_move(self): - - - - move = self.path.pop - + 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 move[0] != 0: - if move[0] == 1: - return 90 - else: - return 270 - elif move[1] != 0: - if move[1] == 1: + if move[0] == 0: + if move[1] == -1: + self.path.pop(0) return pg.Vector2(move) else: return 180 + elif move[1] == 0: + if move[0] == -1: + return 270 + else: + return 90 elif self.orientation == 90: - if move[0] != 0: - if move[0] == 1: - return pg.Vector2(move) - else: - return 270 - elif move[1] != 0: - if move[1] == 1: + if move[0] == 0: + if move[1] == -1: return 0 else: 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: - if move[0] != 0: - if move[0] == 1: - return 90 - else: - return 270 - elif move[1] != 0: - if move[1] == 1: + if move[0] == 0: + if move[1] == -1: return 0 else: + self.path.pop(0) return pg.Vector2(move) - else: - if move[0] != 0: - if move[0] == 1: - return 90 + elif move[1] == 0: + if move[0] == -1: + return 270 else: - return pg.Vector2(move) - elif move[1] != 0: - if move[1] == 0: + 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 - def heuristic(self, start_pos, end_pos): - return abs((end_pos[0] - start_pos[0])) + abs((end_pos[1] - start_pos[1])) + 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): 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) def get_start_state(self): - entities = self.simulation.state.entities + # entities = self.simulation.state.entities # orientation - self.orientation position = self.current_pos house_list = tuple(self.houses) @@ -186,12 +188,13 @@ class Agent: return successors - def A_star(self): + '''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[0], goaltest[0]), node)) @@ -201,7 +204,6 @@ class Agent: parent = el[1] if elem == goaltest: - actions = [] while parent is not None: actions.append(elem[0]) elem = parent @@ -222,8 +224,48 @@ class Agent: i = next(i for node in enumerate(fringe) if node[1][0] == succ) if fringe[i][0] > p: 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): diff --git a/src/simulation.py b/src/simulation.py index 0d9effe..e29db27 100644 --- a/src/simulation.py +++ b/src/simulation.py @@ -276,9 +276,9 @@ class Interface: def update(self): self.state.update(self.move_truck) self.agent.update() - if isinstance(self.move_truck, int): + '''if isinstance(self.move_truck, int): self.state.update(self.move_truck) - self.agent.update() + self.agent.update()''' def render(self): if not self.debug_mode: @@ -298,5 +298,5 @@ class Interface: self.processAgentInput() self.update() self.render() - self.clock.tick(24) - pg.quit() \ No newline at end of file + self.clock.tick(12) + pg.quit()