From 1c7d9680dbbaf20782fb7b4d720cc8d82ff29235 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Fri, 29 Jan 2021 18:25:52 +0100 Subject: [PATCH 1/2] improve reliability castar --- pymapf/centralized/animator.py | 7 +- pymapf/centralized/cooperative_astar/agent.py | 80 ++++--------------- pymapf/centralized/cooperative_astar/astar.py | 6 +- .../cooperative_astar/cooperative_astar.py | 3 + pymapf/centralized/world.py | 1 + pymapf/decentralized/nmpc/nmpc.py | 1 + .../velocity_obstacle/velocity_obstacle.py | 1 + scripts/cooperative_astar.py | 2 +- 8 files changed, 31 insertions(+), 70 deletions(-) diff --git a/pymapf/centralized/animator.py b/pymapf/centralized/animator.py index d1b7b50..a1f89ba 100644 --- a/pymapf/centralized/animator.py +++ b/pymapf/centralized/animator.py @@ -14,6 +14,7 @@ from matplotlib import animation import math import logging +logging.getLogger('matplotlib').setLevel(logging.WARNING) Colors = ["orange", "blue", "green"] @@ -105,12 +106,12 @@ def __animate(self): self.__animations, init_func=self.__initialize_animation, frames=int(self.simulation_time + 1) * 10, - interval=300, - blit=True, + interval=250, + blit=False, ) def save(self, file_name): - self.anim.save(file_name + ".gif", "ffmpeg", fps=30) + self.anim.save(file_name + ".gif", "ffmpeg", fps=5) logging.debug("Saved file as %s" % file_name + ".gif") def show(self): diff --git a/pymapf/centralized/cooperative_astar/agent.py b/pymapf/centralized/cooperative_astar/agent.py index 7d09e2d..a4eb42a 100644 --- a/pymapf/centralized/cooperative_astar/agent.py +++ b/pymapf/centralized/cooperative_astar/agent.py @@ -3,6 +3,7 @@ To do: add radius """ +import logging class Agent: def __init__( @@ -17,81 +18,32 @@ def __init__( self.opened_nodes = 0 self.allow_diagonals = allow_diagonals - def in_conflict(self, state, other_agents_paths): + def in_conflict(self, current_state, future_state, other_agents_paths): for key, val in other_agents_paths.items(): if key == self.ident: continue - if self.allow_diagonals: - conflict_1 = state - conflict_2 = state - conflict_2.t += 1 - - conflict_3 = state - conflict_3.x += 1 - conflict_3.t += 1 - - conflict_4 = state - conflict_4.x -= 1 - conflict_4.t += 1 - - conflict_5 = state - conflict_5.y += 1 - conflict_5.t += 1 - - conflict_6 = state - conflict_6.y -= 1 - conflict_6.t += 1 - - conflict_7 = state - conflict_7.x += 1 - conflict_7.y += 1 - conflict_7.t += 1 - - conflict_8 = state - conflict_8.x -= 1 - conflict_8.y += 1 - conflict_8.t += 1 + try: + if future_state.x == val[-1].x and future_state.y == val[-1].y: + logging.debug("Found conflict between agent %s and agent %s" % (self.ident, key)) + return True + except BaseException as e: + logging.debug("Agent %s path is empty: %s" % (key, str(e))) - conflict_9 = state - conflict_9.x -= 1 - conflict_9.y -= 1 - conflict_9.t += 1 + if self.allow_diagonals: conflicts = [ - conflict_1, - conflict_2, - conflict_3, - conflict_4, - conflict_5, - conflict_6, - conflict_7, - conflict_8, - conflict_9, + future_state, + current_state, ] else: - conflict_1 = state - conflict_2 = state - conflict_2.t += 1 - - conflict_3 = state - conflict_3.x += 1 - conflict_3.t += 1 - - conflict_4 = state - conflict_4.x -= 1 - conflict_4.t += 1 - - conflict_5 = state - conflict_5.y += 1 - conflict_5.t += 1 - - conflict_6 = state - conflict_6.y -= 1 - conflict_6.t += 1 - conflicts = [conflict_1, conflict_2, conflict_3, conflict_4, conflict_5] + conflicts = [ + future_state, + current_state, + ] for c in conflicts: if c in val: + logging.warning("Found conflict between agent %s and agent %s" % (self.ident, key)) self.conflicts_found += 1 return True return False diff --git a/pymapf/centralized/cooperative_astar/astar.py b/pymapf/centralized/cooperative_astar/astar.py index 98f4abc..9a94010 100644 --- a/pymapf/centralized/cooperative_astar/astar.py +++ b/pymapf/centralized/cooperative_astar/astar.py @@ -99,7 +99,7 @@ def search(self) -> List[Tuple[int]]: self.nodes[child_node.pos] = child_node self.opened_nodes += 1 logging.warning("Path not found for agent [%s]" % str(self.agent.ident)) - return [] + return [self.start.state] def get_successors(self, parent: Node) -> List[Node]: """ @@ -116,7 +116,9 @@ def get_successors(self, parent: Node) -> List[Node]: continue if self.agent.in_conflict( - State(pos_x, pos_y, parent.t + 1), self.global_paths + State(parent.pos_x, parent.pos_y, parent.t), + State(pos_x, pos_y, parent.t+1), + self.global_paths ): continue diff --git a/pymapf/centralized/cooperative_astar/cooperative_astar.py b/pymapf/centralized/cooperative_astar/cooperative_astar.py index 3bc332e..82d401c 100644 --- a/pymapf/centralized/cooperative_astar/cooperative_astar.py +++ b/pymapf/centralized/cooperative_astar/cooperative_astar.py @@ -3,6 +3,7 @@ import time from ..common import TIME from ..world import World +from .state import State from ..animator import Animator import coloredlogs import logging @@ -29,6 +30,7 @@ def register_agent(self, ident, start, goal): self.agents[ident] = Agent( ident, start, goal, allow_diagonals=self.allow_diagonals ) + self.paths[ident] = [State(start[1], start[0], 0)] def run_simulation(self): for _, agent in self.agents.items(): @@ -38,6 +40,7 @@ def run_simulation(self): self.searches_sim_times.append(self.paths[agent.ident][-1].t) except BaseException as e: logging.debug(e) + self.simulation_complete = True def visualize(self, save_file): diff --git a/pymapf/centralized/world.py b/pymapf/centralized/world.py index df91ed5..632466c 100644 --- a/pymapf/centralized/world.py +++ b/pymapf/centralized/world.py @@ -31,6 +31,7 @@ def __init__(self, length: int, height: int, p_walls: float, allow_diagonals=Fal [0, -1, 1], [1, 0, 1], [0, 1, 1], + [0, 0, sqrt(3)], [-1, -1, sqrt(2)], [-1, 1, sqrt(2)], [1, -1, sqrt(2)], diff --git a/pymapf/decentralized/nmpc/nmpc.py b/pymapf/decentralized/nmpc/nmpc.py index b3ff299..7a8bad7 100644 --- a/pymapf/decentralized/nmpc/nmpc.py +++ b/pymapf/decentralized/nmpc/nmpc.py @@ -21,6 +21,7 @@ # import threading import logging +logging.getLogger('matplotlib').setLevel(logging.WARNING) class MultiAgentNMPC: diff --git a/pymapf/decentralized/velocity_obstacle/velocity_obstacle.py b/pymapf/decentralized/velocity_obstacle/velocity_obstacle.py index 88a1366..e4f4c93 100644 --- a/pymapf/decentralized/velocity_obstacle/velocity_obstacle.py +++ b/pymapf/decentralized/velocity_obstacle/velocity_obstacle.py @@ -21,6 +21,7 @@ from matplotlib.patches import Circle import coloredlogs import logging +logging.getLogger('matplotlib').setLevel(logging.WARNING) class MultiAgentVelocityObstacle: diff --git a/scripts/cooperative_astar.py b/scripts/cooperative_astar.py index 507d2b8..1970e01 100644 --- a/scripts/cooperative_astar.py +++ b/scripts/cooperative_astar.py @@ -9,7 +9,7 @@ import random import time -w = World(12, 12, 0.1) +w = World(12, 12, 0.2) campf = CooperativeAStar(w) agents_labels = ["A", "B", "C", "D", "E"] for label in agents_labels: From 60f15eded324f09144adf6c766ade50c3532974c Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Wed, 17 Feb 2021 18:41:29 +0100 Subject: [PATCH 2/2] improve animation --- pymapf/centralized/animator.py | 10 +++++----- pymapf/centralized/world.py | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/pymapf/centralized/animator.py b/pymapf/centralized/animator.py index a1f89ba..1552c1f 100644 --- a/pymapf/centralized/animator.py +++ b/pymapf/centralized/animator.py @@ -16,7 +16,7 @@ import logging logging.getLogger('matplotlib').setLevel(logging.WARNING) -Colors = ["orange", "blue", "green"] +Colors = ["skyblue", "blue", "orange"] class Animator: @@ -55,7 +55,7 @@ def __initialize_obstacles(self, xmin, ymin, xmax, ymax): xmax - xmin, ymax - ymin, facecolor="none", - edgecolor="red", + edgecolor="black", ) ) @@ -66,8 +66,8 @@ def __initialize_obstacles(self, xmin, ymin, xmax, ymax): (pos[1] - 0.5, pos[0] - 0.5), 1, 1, - facecolor="red", - edgecolor="red", + facecolor="black", + edgecolor="black", ) ) except BaseException as e: @@ -147,7 +147,7 @@ def __animations(self, i): d2 = agents_array[j] pos1 = np.array(d1.center) pos2 = np.array(d2.center) - if np.linalg.norm(pos1 - pos2) < 0.7: + if np.linalg.norm(pos1 - pos2) < 1: d1.set_facecolor("red") d2.set_facecolor("red") print("COLLISION! (agent-agent) ({}, {})".format(i, j)) diff --git a/pymapf/centralized/world.py b/pymapf/centralized/world.py index 632466c..e41f8a7 100644 --- a/pymapf/centralized/world.py +++ b/pymapf/centralized/world.py @@ -22,7 +22,7 @@ def __init__(self, length: int, height: int, p_walls: float, allow_diagonals=Fal if not allow_diagonals: # up, left, down, right - self.delta = [[-1, 0, 1], [0, -1, 1], [1, 0, 1], [0, 1, 1]] + self.delta = [[-1, 0, 1], [0, -1, 1], [1, 0, 1], [0, 1, 1], [0, 0, 1]] else: # up, left, down, right # upleft, upright, downleft, downright @@ -31,7 +31,7 @@ def __init__(self, length: int, height: int, p_walls: float, allow_diagonals=Fal [0, -1, 1], [1, 0, 1], [0, 1, 1], - [0, 0, sqrt(3)], + [0, 0, 1], [-1, -1, sqrt(2)], [-1, 1, sqrt(2)], [1, -1, sqrt(2)],