Repository: APLA-Toolbox/pymapf (language: python)

Issue:
CA*
Cooperative A* (A* with a reservation table)

States are defined by adding another dimension: time.


Discussion / hints:

Fix patch:
diff --git a/LICENSE b/LICENSE
index 073854c..9860a01 100644
--- a/LICENSE
+++ b/LICENSE
@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2021 Automated Planning Toolbox
+Copyright (c) 2021 Erwin Lejeune
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
diff --git a/main.py b/main.py
new file mode 100644
index 0000000..e7f847f
--- /dev/null
+++ b/main.py
@@ -0,0 +1,23 @@
+from src.cooperative_astar import CooperativeAStar
+from src.world import World
+
+
+if __name__ == "__main__":
+    # all coordinates are given in format [y,x]
+    # import doctest
+
+    # doctest.testmod()
+
+    w = World(15, 15, 0.1)
+
+    # Initialize
+    cas = CooperativeAStar(w)
+    start, goal = w.get_start_goal(0.6)
+    cas.register_agent("A", start, goal)
+    start, goal = w.get_start_goal(0.6)
+    cas.register_agent("B", start, goal)
+    start, goal = w.get_start_goal(0.3)
+    cas.register_agent("C", start, goal)
+
+    cas.run_simulation()
+    cas.visualize("test")
diff --git a/pymapf/__init__.py b/pymapf/__init__.py
index 8b13789..8786c48 100644
--- a/pymapf/__init__.py
+++ b/pymapf/__init__.py
@@ -1,1 +1,1 @@
-
+# pymapf root module
diff --git a/pymapf/centralized/__init__.py b/pymapf/centralized/__init__.py
new file mode 100644
index 0000000..8980d47
--- /dev/null
+++ b/pymapf/centralized/__init__.py
@@ -0,0 +1,1 @@
+# centralized mapf common module
diff --git a/pymapf/centralized/animator.py b/pymapf/centralized/animator.py
new file mode 100644
index 0000000..d1b7b50
--- /dev/null
+++ b/pymapf/centralized/animator.py
@@ -0,0 +1,154 @@
+#!/usr/bin/env python3
+import matplotlib
+from random import random
+import os
+
+if "DISPLAY" not in os.environ:
+    matplotlib.use("agg")
+else:
+    matplotlib.use("TkAgg")
+from matplotlib.patches import Circle, Rectangle, Arrow
+from matplotlib.collections import PatchCollection
+import matplotlib.pyplot as plt
+import numpy as np
+from matplotlib import animation
+import math
+import logging
+
+Colors = ["orange", "blue", "green"]
+
+
+class Animator:
+    def __init__(self, world, paths, agents, simulation_time):
+        self.world = world
+        self.paths = paths
+        self.coop_agents = agents
+
+        aspect = self.world.length / self.world.height
+        self.fig = plt.figure(frameon=False, figsize=(4 * aspect, 4))
+        self.ax = self.fig.add_subplot(111, aspect="equal")
+        self.fig.subplots_adjust(
+            left=0, right=1, bottom=0, top=1, wspace=None, hspace=None
+        )
+        self.patches = []
+        self.artists = []
+        self.agents = dict()
+        self.agents_labels = dict()
+
+        xmin = -0.5
+        ymin = -0.5
+        xmax = self.world.length - 0.5
+        ymax = self.world.height - 0.5
+        plt.xlim(xmin, xmax)
+        plt.ylim(ymin, ymax)
+
+        self.__initialize_obstacles(xmin, ymin, xmax, ymax)
+        self.__initialize_agents()
+        self.simulation_time = simulation_time
+        self.__animate()
+
+    def __initialize_obstacles(self, xmin, ymin, xmax, ymax):
+        self.patches.append(
+            Rectangle(
+                (xmin, ymin),
+                xmax - xmin,
+                ymax - ymin,
+                facecolor="none",
+                edgecolor="red",
+            )
+        )
+
+        try:
+            for pos in self.world.walls:
+                self.patches.append(
+                    Rectangle(
+                        (pos[1] - 0.5, pos[0] - 0.5),
+                        1,
+                        1,
+                        facecolor="red",
+                        edgecolor="red",
+                    )
+                )
+        except BaseException as e:
+            logging.debug(e)
+
+    def __initialize_agents(self):
+        for _, agent in self.coop_agents.items():
+            self.patches.append(
+                Rectangle(
+                    (agent.goal_pos[1] - 0.25, agent.goal_pos[0] - 0.25),
+                    0.5,
+                    0.5,
+                    facecolor=Colors[0],
+                    edgecolor="black",
+                    alpha=0.5,
+                )
+            )
+            self.agents[agent.ident] = Circle(
+                (agent.init_pos[0], agent.init_pos[1]),
+                0.3,
+                facecolor=Colors[0],
+                edgecolor="black",
+            )
+            self.agents[agent.ident].original_face_color = Colors[0]
+            self.patches.append(self.agents[agent.ident])
+            self.agents_labels[agent.ident] = self.ax.text(
+                agent.init_pos[0], agent.init_pos[1], agent.ident
+            )
+            self.agents_labels[agent.ident].set_horizontalalignment("center")
+            self.agents_labels[agent.ident].set_verticalalignment("center")
+            self.artists.append(self.agents_labels[agent.ident])
+
+    def __animate(self):
+        self.anim = animation.FuncAnimation(
+            self.fig,
+            self.__animations,
+            init_func=self.__initialize_animation,
+            frames=int(self.simulation_time + 1) * 10,
+            interval=300,
+            blit=True,
+        )
+
+    def save(self, file_name):
+        self.anim.save(file_name + ".gif", "ffmpeg", fps=30)
+        logging.debug("Saved file as %s" % file_name + ".gif")
+
+    def show(self):
+        plt.show()
+
+    def __initialize_animation(self):
+        for p in self.patches:
+            self.ax.add_patch(p)
+        for a in self.artists:
+            self.ax.add_artist(a)
+        return self.patches + self.artists
+
+    def __animations(self, i):
+        for agent_name, path in self.paths.items():
+            try:
+                pos = (path[i].x, path[i].y)
+            except BaseException as e:
+                logging.debug(e)
+                pos = (path[-1].x, path[-1].y)
+            p = (pos[0], pos[1])
+            self.agents[agent_name].center = p
+            self.agents_labels[agent_name].set_position(p)
+
+        # reset all colors
+        for _, agent in self.agents.items():
+            agent.set_facecolor(agent.original_face_color)
+
+        # check drive-drive collisions
+        agents_array = [agent for _, agent in self.agents.items()]
+        for i, _ in enumerate(agents_array):
+            for j in range(i + 1, len(agents_array)):
+                d1 = agents_array[i]
+                d2 = agents_array[j]
+                pos1 = np.array(d1.center)
+                pos2 = np.array(d2.center)
+                if np.linalg.norm(pos1 - pos2) < 0.7:
+                    d1.set_facecolor("red")
+                    d2.set_facecolor("red")
+                    print("COLLISION! (agent-agent) ({}, {})".format(i, j))
+
+        return self.patches + self.artists
diff --git a/pymapf/centralized/common.py b/pymapf/centralized/common.py
new file mode 100644
index 0000000..e8edaf5
--- /dev/null
+++ b/pymapf/centralized/common.py
@@ -0,0 +1,5 @@
+# 1 for manhattan, 0 for euclidean
+HEURISTIC = 0
+DIAGONALS = False
+TIME = True
+PLOT = True
diff --git a/pymapf/centralized/cooperative_astar/__init__.py b/pymapf/centralized/cooperative_astar/__init__.py
new file mode 100644
index 0000000..4d9c523
--- /dev/null
+++ b/pymapf/centralized/cooperative_astar/__init__.py
@@ -0,0 +1,1 @@
+# cooperative astar module
diff --git a/pymapf/centralized/cooperative_astar/agent.py b/pymapf/centralized/cooperative_astar/agent.py
new file mode 100644
index 0000000..7d09e2d
--- /dev/null
+++ b/pymapf/centralized/cooperative_astar/agent.py
@@ -0,0 +1,106 @@
+"""
+An agent is defined by an Id, a start position and a goal position. 
+To do: add radius
+"""
+
+
+class Agent:
+    def __init__(
+        self, ident, init_pos, goal_pos, allow_diagonals=False, nodes_dict=None
+    ):
+        self.ident = ident
+        self.init_pos = init_pos
+        self.goal_pos = goal_pos
+        self.nodes_dict = nodes_dict
+        self.plan = []
+        self.conflicts_found = 0
+        self.opened_nodes = 0
+        self.allow_diagonals = allow_diagonals
+
+    def in_conflict(self, 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
+
+                conflict_9 = state
+                conflict_9.x -= 1
+                conflict_9.y -= 1
+                conflict_9.t += 1
+                conflicts = [
+                    conflict_1,
+                    conflict_2,
+                    conflict_3,
+                    conflict_4,
+                    conflict_5,
+                    conflict_6,
+                    conflict_7,
+                    conflict_8,
+                    conflict_9,
+                ]
+            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]
+
+            for c in conflicts:
+                if c in val:
+                    self.conflicts_found += 1
+                    return True
+        return False
+
+    def __str__(self):
+        return "Id: %d | Init: [%d;%d] | Goal: [%d;%d]" % (
+            self.ident,
+            self.init_pos[0],
+            self.init_pos[1],
+            self.goal_pos[0],
+            self.goal_pos[1],
+        )
diff --git a/pymapf/centralized/cooperative_astar/astar.py b/pymapf/centralized/cooperative_astar/astar.py
new file mode 100644
index 0000000..98f4abc
--- /dev/null
+++ b/pymapf/centralized/cooperative_astar/astar.py
@@ -0,0 +1,148 @@
+from .node import Node
+from typing import Tuple, List
+from termcolor import colored
+from ..world import World
+from .state import State
+import logging
+from .agent import Agent
+
+
+class AStar:
+    """
+    >>> wd = World(10, 10, 0.2)
+    >>> astar = AStar((0, 0), (len(wd.grid) - 1, len(wd.grid[0]) - 1), wd)
+    >>> (astar.start.pos_y + wd.delta[3][0], astar.start.pos_x + wd.delta[3][1])
+    (0, 1)
+    >>> [x.pos for x in astar.get_successors(astar.start)]
+    [(1, 0), (0, 1)]
+    >>> (astar.start.pos_y + wd.delta[2][0], astar.start.pos_x + wd.delta[2][1])
+    (1, 0)
+    >>> astar.retrace_path(astar.start)
+    [(0, 0)]
+    >>> astar.search()  # doctest: +NORMALIZE_WHITESPACE
+    [(0, 0), (1, 0), (2, 0), (2, 1), (2, 2), (2, 3), (3, 3),
+     (4, 3), (4, 4), (5, 4), (5, 5), (6, 5), (6, 6)]
+    """
+
+    def __init__(self, agent: Agent, world: World, global_paths):
+        self.start = Node(
+            agent.init_pos[1],
+            agent.init_pos[0],
+            0,
+            agent.goal_pos[1],
+            agent.goal_pos[0],
+            0,
+            False,
+            True,
+            None,
+        )
+        self.target = Node(
+            agent.goal_pos[1],
+            agent.goal_pos[0],
+            -1,
+            agent.goal_pos[1],
+            agent.goal_pos[0],
+            float("inf"),
+            False,
+            False,
+            None,
+        )
+        self.world = world
+        self.agent = agent
+
+        self.nodes = dict()
+        self.nodes[self.start.state] = self.start
+
+        self.global_paths = global_paths
+        self.opened_nodes = 1
+
+        self.reached = False
+
+    def search(self) -> List[Tuple[int]]:
+        opened_nodes = 0
+        while self.opened_nodes > 0:
+            # Open Nodes are sorted using __lt__
+            current_key = min(
+                [n for n in self.nodes if self.nodes[n].is_open],
+                key=(lambda k: self.nodes[k].f_cost),
+            )
+            current_node = self.nodes[current_key]
+
+            if current_node.pos == self.target.pos:
+                logging.debug("Found path for agent [%s]" % str(self.agent.ident))
+                self.agent.opened_nodes = opened_nodes
+                self.agent.path = self.retrace_path(current_node)
+                return self.agent.path
+
+            if current_node.t > 2 * self.start.h_cost:
+                break
+
+            current_node.is_closed = True
+            current_node.is_open = False
+            self.opened_nodes -= 1
+
+            successors = self.get_successors(current_node)
+            opened_nodes += len(successors)
+
+            for child_node in successors:
+                if child_node.state in self.nodes:
+                    if self.nodes[child_node.state].is_closed:
+                        continue
+                    if not self.nodes[child_node.pos].is_open:
+                        self.nodes[child_node.state] = child_node
+                        self.opened_nodes += 1
+                    else:
+                        if child_node.g_cost < self.nodes[child_node.pos].g_cost:
+                            self.nodes[child_node.pos] = child_node
+                            self.opened_nodes += 1
+                else:
+                    self.nodes[child_node.pos] = child_node
+                    self.opened_nodes += 1
+        logging.warning("Path not found for agent [%s]" % str(self.agent.ident))
+        return []
+
+    def get_successors(self, parent: Node) -> List[Node]:
+        """
+        Returns a list of successors (both in the world and free spaces)
+        """
+        successors = []
+        for action in self.world.delta:
+            pos_x = parent.pos_x + action[0]
+            pos_y = parent.pos_y + action[1]
+            if not (0 <= pos_x < self.world.length and 0 <= pos_y < self.world.height):
+                continue
+
+            if self.world.grid[pos_y][pos_x] != 0:
+                continue
+
+            if self.agent.in_conflict(
+                State(pos_x, pos_y, parent.t + 1), self.global_paths
+            ):
+                continue
+
+            successors.append(
+                Node(
+                    pos_x,
+                    pos_y,
+                    parent.t + 1,
+                    self.target.pos_x,
+                    self.target.pos_y,
+                    parent.g_cost + action[2],
+                    False,
+                    True,
+                    parent,
+                )
+            )
+        return successors
+
+    def retrace_path(self, node: Node) -> List[State]:
+        """
+        Retrace the path from parents to parents until start node
+        """
+        current_node = node
+        path = []
+        while current_node is not None:
+            path.append(current_node.state)
+            current_node = current_node.parent
+        path.reverse()
+        return path
diff --git a/pymapf/centralized/cooperative_astar/cooperative_astar.py b/pymapf/centralized/cooperative_astar/cooperative_astar.py
new file mode 100644
index 0000000..3bc332e
--- /dev/null
+++ b/pymapf/centralized/cooperative_astar/cooperative_astar.py
@@ -0,0 +1,63 @@
+from .agent import Agent
+from .astar import AStar
+import time
+from ..common import TIME
+from ..world import World
+from ..animator import Animator
+import coloredlogs
+import logging
+
+
+class CooperativeAStar:
+    def __init__(self, world, heuristic=0, log_level="WARNING"):
+        self.world = world
+        self.allow_diagonals = world.allow_diagonals
+        self.heuristic = 0
+        self.agents = dict()
+        self.paths = dict()
+        self.searches_sim_times = []
+
+        # Flags
+        self.simulation_complete = False
+        self.__init_logger(log_level)
+        coloredlogs.install(level=log_level)
+
+    def register_agent(self, ident, start, goal):
+        if ident in self.agents:
+            logging.warning("Agent ID already registered, ignoring...")
+            return
+        self.agents[ident] = Agent(
+            ident, start, goal, allow_diagonals=self.allow_diagonals
+        )
+
+    def run_simulation(self):
+        for _, agent in self.agents.items():
+            astar = AStar(agent, self.world, self.paths)
+            self.paths[agent.ident] = astar.search()
+            try:
+                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):
+        if not self.simulation_complete:
+            logging.warning("Simulation isn't yet complete: can't visualize.")
+            return
+        anim = Animator(
+            self.world, self.paths, self.agents, max(self.searches_sim_times)
+        )
+        anim.save(save_file)
+        anim.show()
+
+    def __init_logger(self, log_level):
+        import os
+
+        if not os.path.exists("logs"):
+            os.makedirs("logs")
+        logging.basicConfig(
+            filename="logs/main.log",
+            format="%(levelname)s:%(message)s",
+            filemode="w",
+            level=log_level,
+        )
diff --git a/pymapf/centralized/cooperative_astar/node.py b/pymapf/centralized/cooperative_astar/node.py
new file mode 100644
index 0000000..09d7c8e
--- /dev/null
+++ b/pymapf/centralized/cooperative_astar/node.py
@@ -0,0 +1,65 @@
+from math import sqrt
+from ..common import HEURISTIC
+from .state import State
+
+
+class Node:
+    """
+    >>> k = Node(0, 0, 4, 3, 0, None)
+    >>> k.calculate_heuristic()
+    5.0
+    >>> n = Node(1, 4, 3, 4, 2, None)
+    >>> n.calculate_heuristic()
+    2.0
+    >>> l = [k, n]
+    >>> n == l[0]
+    False
+    >>> l.sort()
+    >>> n == l[0]
+    True
+    """
+
+    def __init__(
+        self,
+        pos_x: int,
+        pos_y: int,
+        t: int,
+        goal_x: int,
+        goal_y: int,
+        g_cost: float,
+        is_closed: bool,
+        is_open: bool,
+        parent,
+    ):
+        self.pos_x = pos_x
+        self.pos_y = pos_y
+        self.t = t
+        self.state = State(pos_x, pos_y, t)
+        self.pos = (pos_y, pos_x)
+        self.goal_x = goal_x
+        self.goal_y = goal_y
+        self.is_closed = is_closed
+        self.is_open = is_open
+        self.g_cost = g_cost
+        self.parent = parent
+        self.h_cost = self.calculate_heuristic()
+        self.f_cost = self.g_cost + self.h_cost
+
+    def calculate_heuristic(self) -> float:
+        """
+        Heuristic for the A*
+        """
+        dy = self.pos_x - self.goal_x
+        dx = self.pos_y - self.goal_y
+        if HEURISTIC == 1:
+            return abs(dx) + abs(dy)
+        return sqrt(dy ** 2 + dx ** 2)
+
+    def print(self):
+        print("Node : <State = [x=%d; y=%d; t=%d]>" % (self.pos_x, self.pos_y, self.t))
+
+    def __str__(self):
+        return "Node : <State = [x=%d; y=%d; t=%d]>" % (self.pos_x, self.pos_y, self.t)
+
+    def __lt__(self, other) -> bool:
+        return self.f_cost <= other.f_cost
diff --git a/pymapf/centralized/cooperative_astar/state.py b/pymapf/centralized/cooperative_astar/state.py
new file mode 100644
index 0000000..a1554f1
--- /dev/null
+++ b/pymapf/centralized/cooperative_astar/state.py
@@ -0,0 +1,14 @@
+class State:
+    def __init__(self, x, y, t):
+        self.x = x
+        self.y = y
+        self.t = t
+
+    def __eq__(self, other):
+        return self.x == other.x and self.y == other.y and self.t == other.t
+
+    def __hash__(self):
+        return hash((self.x, self.y, self.t))
+
+    def __str__(self):
+        return "<Position: [%d; %d], Time: %d" % (self.x, self.y, self.t)
diff --git a/pymapf/centralized/world.py b/pymapf/centralized/world.py
new file mode 100644
index 0000000..df91ed5
--- /dev/null
+++ b/pymapf/centralized/world.py
@@ -0,0 +1,135 @@
+import numpy as np
+from math import sqrt
+from typing import List, Tuple
+from sys import stdout
+import random
+from termcolor import colored
+from .cooperative_astar.node import Node
+
+
+class World:
+    def __init__(self, length: int, height: int, p_walls: float, allow_diagonals=False):
+        self.length = length
+        self.height = height
+        self.p_walls = p_walls
+        self.path_added = False
+        self.walls = []
+        self.allow_diagonals = allow_diagonals
+        self.start_positions = []
+        self.goal_positions = []
+        self.grid = np.zeros((height, length), int)
+        self.__generate_walls()
+
+        if not allow_diagonals:
+            # up, left, down, right
+            self.delta = [[-1, 0, 1], [0, -1, 1], [1, 0, 1], [0, 1, 1]]
+        else:
+            # up, left, down, right
+            # upleft, upright, downleft, downright
+            self.delta = [
+                [-1, 0, 1],
+                [0, -1, 1],
+                [1, 0, 1],
+                [0, 1, 1],
+                [-1, -1, sqrt(2)],
+                [-1, 1, sqrt(2)],
+                [1, -1, sqrt(2)],
+                [1, 1, sqrt(2)],
+            ]
+
+    def add_path(self, path: List[Tuple[int]]):
+        i = 0
+        for elem in path:
+            if i == 0 or i == len(path) - 1:
+                self.grid[elem[0]][elem[1]] = 3
+            else:
+                self.grid[elem[0]][elem[1]] = 2
+            i += 1
+        self.path_added = True
+
+    def plot_grid(self):
+        for rows in self.grid:
+            for elem in rows:
+                if elem == 1:
+                    stdout.write(colored("█", "red"))
+                elif elem == 0:
+                    stdout.write(" ")
+                elif elem == 2:
+                    stdout.write(colored("¤", "blue"))
+                elif elem == 3:
+                    stdout.write(colored("x", "green"))
+                elif elem == 4:
+                    stdout.write(colored("*", "blue"))
+                elif elem == 5:
+                    stdout.write(colored("~", "magenta"))
+                elif elem == 6:
+                    stdout.write(colored("+", "cyan"))
+                else:
+                    stdout.write("o", "yellow")
+            print()
+
+    def get_random_available_position(self) -> Tuple[int]:
+        i = 0
+        while 1:
+            i += 1
+            random_row = random.randint(0, self.height - 1)
+            random_col = random.randint(0, self.length - 1)
+
+            if self.grid[random_row][random_col] != 1:
+                break
+            if i > self.length * self.height * 100:
+                print(colored("Couldn't find random available position.", "red"))
+                return None
+
+        return (random_row, random_col)
+
+    def get_start_goal(self, pHeuristic) -> Tuple[Tuple[int]]:
+        i = 0
+
+        # Initialization
+        start = self.get_random_available_position()
+        goal = self.get_random_available_position()
+        n = Node(start[1], start[0], 0, goal[1], goal[0], 0, False, False, None)
+        h = n.calculate_heuristic()
+
+        # Set goal value
+        nTarget = Node(
+            0, 0, -1, self.length - 1, self.height - 1, 0, False, False, None
+        )
+        hTarget = nTarget.calculate_heuristic()
+
+        while h < hTarget * pHeuristic or (
+            start in self.start_positions and goal in self.goal_positions
+        ):
+            i += 1
+            start = self.get_random_available_position()
+            goal = self.get_random_available_position()
+            n = Node(start[1], start[0], 0, goal[1], goal[0], 0, False, False, None)
+            h = n.calculate_heuristic()
+            if i > 100 * self.length * self.height:
+                print(
+                    colored(
+                        "Couldn't find start and goal respecting the constraints.",
+                        "red",
+                    )
+                )
+                return None
+        self.goal_positions.append(goal)
+        self.start_positions.append(start)
+        return start, goal
+
+    def change_grid(self, position: Tuple[int], value: int):
+        self.grid[position[1]][position[0]] = value
+
+    def __generate_walls(self):
+        for x in range(self.height):
+            for y in range(self.length):
+                if random.random() < self.p_walls:
+                    self.grid[x][y] = 1
+                    self.walls.append((x, y))
+                if x == 0 or x == self.height - 1:
+                    self.grid[x][y] = 1
+                    self.walls.append((x, y))
+                if y == 0 or y == self.length - 1:
+                    self.grid[x][y] = 1
+                    self.walls.append((x, y))
diff --git a/pymapf/decentralized/__init__.py b/pymapf/decentralized/__init__.py
index 883413c..2c5b0b1 100644
--- a/pymapf/decentralized/__init__.py
+++ b/pymapf/decentralized/__init__.py
@@ -1,2 +1,1 @@
-from .nmpc import MultiAgentNMPC
-from .velocity_obstacle import MultiAgentVelocityObstacle
+# pymapf decentralized submodule
diff --git a/pymapf/decentralized/nmpc/__init__.py b/pymapf/decentralized/nmpc/__init__.py
new file mode 100644
index 0000000..81381bf
--- /dev/null
+++ b/pymapf/decentralized/nmpc/__init__.py
@@ -0,0 +1,1 @@
+# pymapf nonlinear model predictive control subsubmodule
diff --git a/pymapf/decentralized/nmpc.py b/pymapf/decentralized/nmpc/nmpc.py
similarity index 94%
rename from pymapf/decentralized/nmpc.py
rename to pymapf/decentralized/nmpc/nmpc.py
index 4009d80..b3ff299 100644
--- a/pymapf/decentralized/nmpc.py
+++ b/pymapf/decentralized/nmpc/nmpc.py
@@ -3,7 +3,7 @@
 author: Erwin Lejeune (erwin.lejeune15@gmail.com)
 """
 
-from .obstacle import Obstacle
+from ..obstacle import Obstacle
 from .nmpc_agent import NMPCAgent
 import numpy as np
 import matplotlib as mpl
@@ -46,7 +46,7 @@ def __init__(
 
     def register_agent(
         self,
-        id,
+        ident,
         start,
         goal,
         qc=5.0,
@@ -56,11 +56,11 @@ def register_agent(
         vmin=0.2,
         horizon_length=4,
     ):
-        if id in self.agents:
+        if ident in self.agents:
             logging.warning("Failed to register agent: ID has already been registered.")
             return
         agent = NMPCAgent(
-            id,
+            ident,
             start,
             goal,
             self.number_of_timesteps,
@@ -73,7 +73,7 @@ def register_agent(
             vmin,
             horizon_length,
         )
-        self.agents[id] = agent
+        self.agents[ident] = agent
 
     def register_obstacle(self, velocity, theta, initial_position):
         obstacle = Obstacle(
@@ -93,7 +93,8 @@ def register_obstacle(self, velocity, theta, initial_position):
     def run_simulation(self):
         try:
             obstacles = self.obstacles
-        except:
+        except BaseException as e:
+            logging.debug(e)
             obstacles = []
         for i in range(self.number_of_timesteps):
             # self.other_agents = dict()
@@ -123,7 +124,8 @@ def __agent_step(self, key, agent, i, obstacles, other_agents_lst):
         # other_agents = self.other_agents.copy()
         # try:
         #     del other_agents[key]
-        # except:
+        # except BaseException as e:
+        #     logging.debug(e)
         #     pass
         # other_agents_lst = list(other_agents.values())
         state_history, vel, state = agent.simulate_step(i, obstacles, other_agents_lst)
@@ -137,8 +139,6 @@ def __agent_to_obstacle(self, velocity, pos):
         return np.concatenate((pos, velocity))
 
     def __init_logger(self, log_level):
-        import os
-
         if not os.path.exists("logs"):
             os.makedirs("logs")
         logging.basicConfig(
@@ -184,8 +184,8 @@ def __plot(self, saved_file, map_length, map_height):
                         edgecolor="black",
                     )
                 )
-        except:
-            pass
+        except BaseException as e:
+            logging.debug(e)
 
         def init():
             for agent in agents_list:
@@ -209,7 +209,7 @@ def animate(i):
                     self.global_state_history[key][1, :i],
                 )
                 k += 1
-            for j in range(len(obstacle_list)):
+            for j, _ in enumerate(obstacle_list):
                 obstacle_list[j].center = (
                     self.obstacles[0, i, j],
                     self.obstacles[1, i, j],
diff --git a/pymapf/decentralized/nmpc_agent.py b/pymapf/decentralized/nmpc/nmpc_agent.py
similarity index 86%
rename from pymapf/decentralized/nmpc_agent.py
rename to pymapf/decentralized/nmpc/nmpc_agent.py
index 001113a..f1e9130 100644
--- a/pymapf/decentralized/nmpc_agent.py
+++ b/pymapf/decentralized/nmpc/nmpc_agent.py
@@ -1,12 +1,13 @@
 import numpy as np
 from scipy.optimize import minimize, Bounds
 import time
+import logging
 
 
 class NMPCAgent:
     def __init__(
         self,
-        id,
+        ident,
         start,
         goal,
         number_of_timesteps,
@@ -20,7 +21,7 @@ def __init__(
         horizon_length=4,
     ):
         # Initializations
-        self.id = id
+        self.ident = ident
         self.start = np.array([start.x, start.y])
         self.goal = np.array([goal.x, goal.y])
 
@@ -97,7 +98,7 @@ def __tracking_cost(self, x, xref):
     def __total_collision_cost(self, robot, obstacles):
         total_cost = 0
         for i in range(self.horizon_length):
-            for j in range(len(obstacles)):
+            for j, _ in enumerate(obstacles):
                 obstacle = obstacles[j]
                 rob = robot[2 * i : 2 * i + 2]
                 obs = obstacle[2 * i : 2 * i + 2]
@@ -123,8 +124,8 @@ def __predict_obstacle_positions(self, obstacles, step, other_agents):
                 u = np.vstack([np.eye(2)] * self.horizon_length) @ obstacle_vel
                 obstacle_prediction = self.__update_state(obstacle_position, u)
                 obstacle_predictions.append(obstacle_prediction)
-        except:
-            pass
+        except BaseException as e:
+            logging.debug(e)
 
         for agent in other_agents:
             agent_position = agent[:2]
@@ -149,21 +150,24 @@ def __update_state(self, x0, u):
         return new_state
 
     def __hash__(self):
-        h = str(self.id) + str(self.start) + str(self.goal) + str(self.radius)
+        h = str(self.ident) + str(self.start) + str(self.goal) + str(self.radius)
         return h
 
     def __eq__(self, other):
-        return self.id == other.id
+        return self.ident == other.ident
 
     def __str__(self):
-        return "=======AGENT=======\nID = %s\nSTART_POSITION = %s\nGOAL_POSITION = %s\nRADIUS = %s\nVMIN = %s\nVMAX = %s\nHORIZON_LENGTH = %s\nQC = %s\nKappa = %s\n===================" % (
-            str(self.id),
-            str(self.start),
-            str(self.goal),
-            str(self.radius),
-            str(self.vmin),
-            str(self.vmax),
-            str(self.horizon_length),
-            str(self.qc),
-            str(self.kappa),
+        return (
+            "=======AGENT=======\nID = %s\nSTART_POSITION = %s\nGOAL_POSITION = %s\nRADIUS = %s\nVMIN = %s\nVMAX = %s\nHORIZON_LENGTH = %s\nQC = %s\nKappa = %s\n==================="
+            % (
+                str(self.ident),
+                str(self.start),
+                str(self.goal),
+                str(self.radius),
+                str(self.vmin),
+                str(self.vmax),
+                str(self.horizon_length),
+                str(self.qc),
+                str(self.kappa),
+            )
         )
diff --git a/pymapf/decentralized/velocity_obstacle/__init__.py b/pymapf/decentralized/velocity_obstacle/__init__.py
new file mode 100644
index 0000000..b793421
--- /dev/null
+++ b/pymapf/decentralized/velocity_obstacle/__init__.py
@@ -0,0 +1,1 @@
+# pymapf velocity_obstacle subsubmodule
diff --git a/pymapf/decentralized/velocity_agent.py b/pymapf/decentralized/velocity_obstacle/velocity_agent.py
similarity index 97%
rename from pymapf/decentralized/velocity_agent.py
rename to pymapf/decentralized/velocity_obstacle/velocity_agent.py
index 67d14b8..1e4cb61 100644
--- a/pymapf/decentralized/velocity_agent.py
+++ b/pymapf/decentralized/velocity_obstacle/velocity_agent.py
@@ -1,11 +1,12 @@
 import numpy as np
 import time
+import logging
 
 
 class VelocityAgent:
     def __init__(
         self,
-        id,
+        ident,
         start,
         goal,
         number_of_timesteps,
@@ -15,7 +16,7 @@ def __init__(
         vmin=0.2,
     ):
         # Agent Initialization
-        self.id = id
+        self.ident = ident
         self.start = np.array([start.x, start.y])
         self.goal = np.array([goal.x, goal.y])
 
@@ -61,7 +62,8 @@ def __compute_velocity(self, state, obstacles, step, v_desired, other_agents):
             pos_agent = state[:2]
             number_of_obstacles = np.shape(obstacles)[1]
 
-        except:
+        except BaseException as e:
+            logging.debug(e)
             pos_agent = state[:2]
             number_of_obstacles = 0
 
@@ -138,7 +140,8 @@ def __compute_velocity(self, state, obstacles, step, v_desired, other_agents):
             norm = np.linalg.norm(diffs, axis=0)
             min_index = np.where(norm == np.amin(norm))[0][0]
             cmd_vel = v_satisfying_constraints[:, min_index]
-        except:
+        except BaseException as e:
+            logging.debug(e)
             cmd_vel = np.array([0, 0])
         return cmd_vel
 
diff --git a/pymapf/decentralized/velocity_obstacle.py b/pymapf/decentralized/velocity_obstacle/velocity_obstacle.py
similarity index 91%
rename from pymapf/decentralized/velocity_obstacle.py
rename to pymapf/decentralized/velocity_obstacle/velocity_obstacle.py
index 6f250e0..88a1366 100644
--- a/pymapf/decentralized/velocity_obstacle.py
+++ b/pymapf/decentralized/velocity_obstacle/velocity_obstacle.py
@@ -3,7 +3,7 @@
 author: Erwin Lejeune (erwin.lejeune15@gmail.com)
 """
 
-from .obstacle import Obstacle
+from ..obstacle import Obstacle
 from .velocity_agent import VelocityAgent
 import numpy as np
 import matplotlib as mpl
@@ -41,14 +41,21 @@ def __init__(self, simulation_time=8.0, timestep=0.1, log_level="WARNING"):
         self.__init_logger(log_level)
         coloredlogs.install(level=log_level)
 
-    def register_agent(self, id, start, goal, radius=0.5, vmax=2, vmin=0.2):
-        if id in self.agents:
+    def register_agent(self, ident, start, goal, radius=0.5, vmax=2, vmin=0.2):
+        if ident in self.agents:
             logging.warning("Failed to register agent: ID has already been registered.")
             return
         agent = VelocityAgent(
-            id, start, goal, self.number_of_timesteps, self.timestep, radius, vmax, vmin
+            ident,
+            start,
+            goal,
+            self.number_of_timesteps,
+            self.timestep,
+            radius,
+            vmax,
+            vmin,
         )
-        self.agents[id] = agent
+        self.agents[ident] = agent
 
     def register_obstacle(self, velocity, theta, initial_position):
         obstacle = Obstacle(
@@ -68,7 +75,8 @@ def register_obstacle(self, velocity, theta, initial_position):
     def run_simulation(self):
         try:
             obstacles = self.obstacles
-        except:
+        except BaseException as e:
+            logging.debug(e)
             obstacles = []
         for i in range(self.number_of_timesteps):
             # self.other_agents = dict()
@@ -98,7 +106,8 @@ def __agent_step(self, key, agent, i, obstacles, other_agents_lst):
         # other_agents = self.other_agents.copy()
         # try:
         #     del other_agents[key]
-        # except:
+        # except BaseException as e:
+        #     logging.debug(e)
         #     pass
         # other_agents_lst = list(other_agents.values())
         state_history, vel, state = agent.simulate_step(i, obstacles, other_agents_lst)
@@ -112,8 +121,6 @@ def __agent_to_obstacle(self, velocity, pos):
         return np.concatenate((pos, velocity))
 
     def __init_logger(self, log_level):
-        import os
-
         if not os.path.exists("logs"):
             os.makedirs("logs")
         logging.basicConfig(
@@ -159,8 +166,8 @@ def __plot(self, saved_file, map_length, map_height):
                         edgecolor="black",
                     )
                 )
-        except:
-            pass
+        except BaseException as e:
+            logging.debug(e)
 
         def init():
             for agent in agents_list:
@@ -184,7 +191,7 @@ def animate(i):
                     self.global_state_history[key][1, :i],
                 )
                 k += 1
-            for j in range(len(obstacle_list)):
+            for j, _ in enumerate(obstacle_list):
                 obstacle_list[j].center = (
                     self.obstacles[0, i, j],
                     self.obstacles[1, i, j],
diff --git a/requirements.txt b/requirements.txt
index 39adefc..99cef65 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -2,3 +2,4 @@ numpy==1.19.5
 scipy==1.6.0
 coloredlogs==15.0
 matplotlib==3.3.3
+termcolor==1.1.0
diff --git a/scripts/benchmark_nmpc.py b/scripts/benchmark_nmpc.py
index 278a787..f8699b9 100644
--- a/scripts/benchmark_nmpc.py
+++ b/scripts/benchmark_nmpc.py
@@ -2,7 +2,7 @@
 import sys
 
 sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
-from pymapf.decentralized.nmpc import MultiAgentNMPC
+from pymapf.decentralized.nmpc.nmpc import MultiAgentNMPC
 from pymapf.decentralized.position import Position
 import numpy as np
 import time
diff --git a/scripts/benchmark_nmpc_agents_scale.py b/scripts/benchmark_nmpc_agents_scale.py
index 9f72733..daf168d 100644
--- a/scripts/benchmark_nmpc_agents_scale.py
+++ b/scripts/benchmark_nmpc_agents_scale.py
@@ -2,7 +2,7 @@
 import sys
 
 sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
-from pymapf.decentralized.nmpc import MultiAgentNMPC
+from pymapf.decentralized.nmpc.nmpc import MultiAgentNMPC
 from pymapf.decentralized.position import Position
 import numpy as np
 import time
diff --git a/scripts/benchmark_nmpc_space_scale.py b/scripts/benchmark_nmpc_space_scale.py
index 38f12b2..8253965 100644
--- a/scripts/benchmark_nmpc_space_scale.py
+++ b/scripts/benchmark_nmpc_space_scale.py
@@ -2,7 +2,7 @@
 import sys
 
 sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
-from pymapf.decentralized.nmpc import MultiAgentNMPC
+from pymapf.decentralized.nmpc.nmpc import MultiAgentNMPC
 from pymapf.decentralized.position import Position
 import numpy as np
 import time
diff --git a/scripts/cooperative_astar.py b/scripts/cooperative_astar.py
new file mode 100644
index 0000000..507d2b8
--- /dev/null
+++ b/scripts/cooperative_astar.py
@@ -0,0 +1,20 @@
+from os import path
+import sys
+
+sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
+from pymapf.centralized.cooperative_astar.cooperative_astar import CooperativeAStar
+from pymapf.centralized.world import World
+from pymapf.decentralized.position import Position
+import numpy as np
+import random
+import time
+
+w = World(12, 12, 0.1)
+campf = CooperativeAStar(w)
+agents_labels = ["A", "B", "C", "D", "E"]
+for label in agents_labels:
+    start, goal = w.get_start_goal(random.random() * 0.5)
+    campf.register_agent(label, start, goal)
+
+campf.run_simulation()
+campf.visualize("castar")
diff --git a/scripts/switch_positions_nmpc.py b/scripts/switch_positions_nmpc.py
index 6894bd3..e6e8038 100644
--- a/scripts/switch_positions_nmpc.py
+++ b/scripts/switch_positions_nmpc.py
@@ -2,7 +2,7 @@
 import sys
 
 sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
-from pymapf.decentralized.nmpc import MultiAgentNMPC
+from pymapf.decentralized.nmpc.nmpc import MultiAgentNMPC
 from pymapf.decentralized.position import Position
 import numpy as np
 
diff --git a/scripts/switch_positions_vel_obstacles.py b/scripts/switch_positions_vel_obstacles.py
index 436b5db..4d92baa 100644
--- a/scripts/switch_positions_vel_obstacles.py
+++ b/scripts/switch_positions_vel_obstacles.py
@@ -2,7 +2,9 @@
 import sys
 
 sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
-from pymapf.decentralized.velocity_obstacle import MultiAgentVelocityObstacle
+from pymapf.decentralized.velocity_obstacle.velocity_obstacle import (
+    MultiAgentVelocityObstacle,
+)
 from pymapf.decentralized.position import Position
 import numpy as np
 


Test patch:
diff --git a/.github/workflows/pip-tests.yml b/.github/workflows/pip-tests.yml
index 6cea7ae..2c89d8a 100644
--- a/.github/workflows/pip-tests.yml
+++ b/.github/workflows/pip-tests.yml
@@ -1,6 +1,6 @@
 name: pip-tests
 
-on: [push]
+on: [prerelease]
 
 jobs:
   test:
diff --git a/test.gif b/test.gif
new file mode 100644
index 0000000..caa9dfe
Binary files /dev/null and b/test.gif differ
diff --git a/tests/test_centralized_world.py b/tests/test_centralized_world.py
new file mode 100644
index 0000000..29c0b2e
--- /dev/null
+++ b/tests/test_centralized_world.py
@@ -0,0 +1,13 @@
+from os import path
+import sys
+
+sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
+from pymapf.centralized.world import World
+import numpy as np
+import random
+import time
+
+
+def test_world_plot():
+    w = World(10, 10, 0)
+    w.plot_grid()
diff --git a/tests/test_cooperative_astar_agent.py b/tests/test_cooperative_astar_agent.py
new file mode 100644
index 0000000..a16a747
--- /dev/null
+++ b/tests/test_cooperative_astar_agent.py
@@ -0,0 +1,15 @@
+from os import path
+import sys
+
+sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
+from pymapf.centralized.cooperative_astar.agent import Agent
+from pymapf.centralized.world import World
+from pymapf.decentralized.position import Position
+import numpy as np
+import random
+import time
+
+
+def test_agent():
+    a = Agent("h", (2, 2), (3, 3), allow_diagonals=True)
+    assert a.init_pos == (2, 2) and a.goal_pos == (3, 3)
diff --git a/tests/test_cooperative_astar_manager.py b/tests/test_cooperative_astar_manager.py
new file mode 100644
index 0000000..29855d7
--- /dev/null
+++ b/tests/test_cooperative_astar_manager.py
@@ -0,0 +1,45 @@
+from os import path
+import sys
+
+sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
+from pymapf.centralized.cooperative_astar.cooperative_astar import CooperativeAStar
+from pymapf.centralized.world import World
+from pymapf.decentralized.position import Position
+import numpy as np
+import random
+import time
+
+
+def test_cooperative_astar_sim():
+    w = World(12, 12, 0.1)
+    campf = CooperativeAStar(w)
+    agents_labels = ["A", "B", "C", "D", "E"]
+    for label in agents_labels:
+        start, goal = w.get_start_goal(random.random() * 0.5)
+        campf.register_agent(label, start, goal)
+
+    campf.run_simulation()
+
+
+def test_cooperative_astar_sim_diagonals():
+    w = World(12, 12, 0.1, allow_diagonals=True)
+    campf = CooperativeAStar(w)
+    agents_labels = ["A", "B", "C", "D", "E"]
+    for label in agents_labels:
+        start, goal = w.get_start_goal(random.random() * 0.5)
+        campf.register_agent(label, start, goal)
+
+    campf.run_simulation()
+    campf.visualize("test")
+
+
+def test_cooperative_astar_viz():
+    w = World(12, 12, 0.1)
+    campf = CooperativeAStar(w)
+    agents_labels = ["A", "B", "C", "D", "E"]
+    for label in agents_labels:
+        start, goal = w.get_start_goal(random.random() * 0.5)
+        campf.register_agent(label, start, goal)
+
+    campf.run_simulation()
+    campf.visualize("test")
diff --git a/tests/test_cooperative_astar_state.py b/tests/test_cooperative_astar_state.py
new file mode 100644
index 0000000..e69de29
diff --git a/tests/test_nmpc.py b/tests/test_nmpc.py
index a04aeb7..a20fbaf 100644
--- a/tests/test_nmpc.py
+++ b/tests/test_nmpc.py
@@ -5,7 +5,7 @@
 
 sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
 
-from pymapf.decentralized.nmpc import MultiAgentNMPC
+from pymapf.decentralized.nmpc.nmpc import MultiAgentNMPC
 from pymapf.decentralized.position import Position
 
 
diff --git a/tests/test_nmpc_agent.py b/tests/test_nmpc_agent.py
index 41e439e..8921e68 100644
--- a/tests/test_nmpc_agent.py
+++ b/tests/test_nmpc_agent.py
@@ -5,7 +5,7 @@
 
 sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
 
-from pymapf.decentralized.nmpc_agent import NMPCAgent
+from pymapf.decentralized.nmpc.nmpc_agent import NMPCAgent
 from pymapf.decentralized.position import Position
 import numpy as np
 
diff --git a/tests/test_velocity_agent.py b/tests/test_velocity_agent.py
index 87c5e9b..412f4a8 100644
--- a/tests/test_velocity_agent.py
+++ b/tests/test_velocity_agent.py
@@ -5,7 +5,7 @@
 
 sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
 
-from pymapf.decentralized.velocity_agent import VelocityAgent
+from pymapf.decentralized.velocity_obstacle.velocity_agent import VelocityAgent
 from pymapf.decentralized.position import Position
 import numpy as np
 
diff --git a/tests/test_velocity_obstacle.py b/tests/test_velocity_obstacle.py
index ed42e38..aede658 100644
--- a/tests/test_velocity_obstacle.py
+++ b/tests/test_velocity_obstacle.py
@@ -5,7 +5,9 @@
 
 sys.path.append(path.dirname(path.dirname(path.abspath(__file__))))
 
-from pymapf.decentralized.velocity_obstacle import MultiAgentVelocityObstacle
+from pymapf.decentralized.velocity_obstacle.velocity_obstacle import (
+    MultiAgentVelocityObstacle,
+)
 from pymapf.decentralized.position import Position