Merge pull request #28 from APLA-Toolbox/improve-castar-reliability

Improve cooperative astar reliability

pymapf/centralized/animator.py

@@ -14,8 +14,9 @@
 from matplotlib import animation
 import math
 import logging
+logging.getLogger('matplotlib').setLevel(logging.WARNING)
 
-Colors = ["orange", "blue", "green"]
+Colors = ["skyblue", "blue", "orange"]
 
 
 class Animator:
@@ -54,7 +55,7 @@ def __initialize_obstacles(self, xmin, ymin, xmax, ymax):
                 xmax - xmin,
                 ymax - ymin,
                 facecolor="none",
-                edgecolor="red",
+                edgecolor="black",
             )
         )
 
@@ -65,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:
@@ -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):
@@ -146,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))

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

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
 

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):

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,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, 1],
                 [-1, -1, sqrt(2)],
                 [-1, 1, sqrt(2)],
                 [1, -1, sqrt(2)],

pymapf/decentralized/nmpc/nmpc.py

@@ -21,6 +21,7 @@
 
 # import threading
 import logging
+logging.getLogger('matplotlib').setLevel(logging.WARNING)
 
 
 class MultiAgentNMPC:

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:

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: