CBS+disjoint_splitting.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import time\n",
    "import matplotlib.pyplot as plt\n",
    "import random\n",
    "from heapq import heappop, heappush\n",
    "\n",
    "from MAPF import Map\n",
    "from MAPF import read_map_from_movingai_file, read_tasks_from_movingai_file\n",
    "\n",
    "from CT import HighNode, LowNode\n",
    "\n",
    "from OpenClosed import OpenHigh, OpenLow, ClosedLow\n",
    "\n",
    "from Heuristics import ManhattanDistance\n",
    "\n",
    "from CT import MakePath\n",
    "from AstarTimeSteps import AstarTimesteps\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# CBS with disjoint splitting, ничего не поменялось, только там где добавляются препятствия.\n",
    "\n",
    "def CBSDS(gridMap, Starts, Goals, vertexCons, edgeCons): # vertex/edge- Cons - нужны только при вызове CBS в качестве нижнего уровня MACBS\n",
    "    root = HighNode()\n",
    "    OPEN = OpenHigh()\n",
    "    CLOSED = ClosedHigh()\n",
    "    agents = list(range(len(Starts)))\n",
    "    k = 0\n",
    "    \n",
    "    for a in agents:\n",
    "        planner = AstarTimesteps(gridMap, Starts[a][0], Starts[a][1], Goals[a][0], Goals[a][1], [], [])\n",
    "        result = planner.FindPath()\n",
    "        if result[0]:\n",
    "            path = MakePath(result[1])\n",
    "            root.sol[a], _ = path\n",
    "        else:\n",
    "            return (False, None, OPEN, CLOSED)\n",
    "    \n",
    "    root.g = sum([len(path) for path in root.sol.values()])\n",
    "    OPEN.AddNode(root)\n",
    "    k += 1\n",
    "    \n",
    "    while not OPEN.isEmpty():\n",
    "        s = OPEN.GetBestNode(CLOSED)\n",
    "        CLOSED.AddNode(s)  \n",
    "        newVertexCons = []\n",
    "        newEdgeCons = []\n",
    "        \n",
    "        for a in agents:\n",
    "            for b in agents[a + 1 :]:\n",
    "                for step in range(min(len(s.sol[a]), len(s.sol[b]))):\n",
    "                    if s.sol[a][step].i == s.sol[b][step].i and s.sol[a][step].j == s.sol[b][step].j:\n",
    "                        newVertexCons.append((a, b, (s.sol[a][step].i, s.sol[a][step].j), step))\n",
    "                    if step + 1 < min(len(s.sol[a]), len(s.sol[b])) and \\\n",
    "                       s.sol[a][step].i == s.sol[b][step + 1].i and s.sol[a][step].j == s.sol[b][step + 1].j and \\\n",
    "                       s.sol[a][step + 1].i == s.sol[b][step].i and s.sol[a][step + 1].i == s.sol[b][step].i:\n",
    "                        newEdgeCons.append((\n",
    "                            a,\n",
    "                            b,\n",
    "                            (s.sol[a][step].i, s.sol[a][step].j),\n",
    "                            (s.sol[a][step + 1].i, s.sol[a][step + 1].j),\n",
    "                            step,\n",
    "                        ))\n",
    "                        \n",
    "        if len(newVertexCons) == 0 and len(newEdgeCons) == 0:\n",
    "            return (True, s, OPEN, CLOSED)\n",
    "        \n",
    "        # Сейчас сначала разрешаются вершинные конфликты, потом реберные\n",
    "        if len(newVertexCons) > 0:\n",
    "            a, b, (i, j), t = newVertexCons[0]\n",
    "            \n",
    "            # Разбиваем CT на ноды A и B, разрешая вершинный конфликт\n",
    "            # первый нод останется таким же: запрещаем агенту а быть там где указано\n",
    "            \n",
    "            tmp = s.vertexCons.copy()\n",
    "            if a in tmp:\n",
    "                tmp[a].append(((i, j), t))   \n",
    "            else:\n",
    "                tmp[a] = [((i, j), t)]\n",
    "            \n",
    "            A = HighNode(\n",
    "                vertexCons=tmp,\n",
    "                edgeCons=s.edgeCons,\n",
    "                sol=s.sol,\n",
    "                parent=s,\n",
    "                k=k,\n",
    "            )\n",
    "            \n",
    "            ec = []\n",
    "            if a in A.edgeCons:\n",
    "                ec = A.edgeCons[a]\n",
    "            \n",
    "            planner = AstarTimesteps(gridMap, Starts[a][0], Starts[a][1], Goals[a][0], Goals[a][1], A.vertexCons[a], ec)\n",
    "            result = planner.FindPath()\n",
    "            if result[0]:\n",
    "                path = MakePath(result[1])\n",
    "                A.sol[a], _ = path\n",
    "                A.g = sum([len(path) for path in A.sol.values()]) # SIC, можно использовать другой cost; добавить подсчет h, F\n",
    "                if not CLOSED.WasExpanded(A):\n",
    "                    OPEN.AddNode(A)\n",
    "                k += 1\n",
    "                \n",
    "            # Cо вторым агентом поступаем не так: не добавляем для него бебру, а наоборот обязываем агента \n",
    "            # а быть там где надо. \n",
    "            tmp = s.vertexCons.copy()\n",
    "            if a in tmp:\n",
    "                #запретить все кроме (i,j) в момент времени t\n",
    "                for (x,y) in gridMap.cells():\n",
    "                    if (x,y) != (i,j):\n",
    "                        tmp[a].append(((x, y), t))   \n",
    "            else:\n",
    "                tmp[a] = []\n",
    "                for (x,y) in gridMap.cells():\n",
    "                    if (x,y) != (i,j):\n",
    "                        tmp[a].append(((x, y), t)) \n",
    "                \n",
    "            B = HighNode(\n",
    "                vertexCons=tmp,\n",
    "                edgeCons=s.edgeCons,\n",
    "                sol=s.sol,\n",
    "                parent=s,\n",
    "                k=k,\n",
    "            )\n",
    "            #??? вопрос --- что в таком случае добавлять в эти ec? Видимо все так же для а...\n",
    "            ec = []\n",
    "            if a in B.edgeCons:\n",
    "                ec = B.edgeCons[a]\n",
    "            \n",
    "            planner = AstarTimesteps(gridMap, Starts[a][0], Starts[a][1], Goals[a][0], Goals[a][1], B.vertexCons[a], ec)\n",
    "            result = planner.FindPath()\n",
    "            if result[0]:\n",
    "                path = MakePath(result[1])\n",
    "                B.sol[a], _ = path\n",
    "                B.g = sum([len(path) for path in B.sol.values()]) # SIC, можно использовать другой cost; добавить подсчет h, F\n",
    "                if not CLOSED.WasExpanded(B):\n",
    "                    OPEN.AddNode(B)\n",
    "                k += 1\n",
    "                \n",
    "        elif len(newEdgeCons) > 0:\n",
    "            a, b, (i1, j1), (i2, j2), t = newEdgeCons[0]\n",
    "            \n",
    "            # Разбиваем CT на ноды A и B, разрешая реберный конфликт \n",
    "            \n",
    "            tmp = s.edgeCons.copy()\n",
    "            if a in tmp:\n",
    "                tmp[a].append(((i1, j1), (i2, j2), t))   \n",
    "            else:\n",
    "                tmp[a] = [((i1, j1), (i2, j2), t)]\n",
    "            \n",
    "            A = HighNode(\n",
    "                vertexCons=s.vertexCons,\n",
    "                edgeCons=tmp,\n",
    "                sol=s.sol,\n",
    "                parent=s,\n",
    "                k=k,\n",
    "            )\n",
    "            \n",
    "            vc = []\n",
    "            if a in A.vertexCons:\n",
    "                vc = A.vertexCons[a]\n",
    "            \n",
    "            planner = AstarTimesteps(gridMap, Starts[a][0], Starts[a][1], Goals[a][0], Goals[a][1], vc, A.edgeCons[a])\n",
    "            result = planner.FindPath()\n",
    "            if result[0]:\n",
    "                path = MakePath(result[1])\n",
    "                A.sol[a], _ = path\n",
    "                A.g = sum([len(path) for path in A.sol.values()]) # SIC, можно использовать другой cost; добавить подсчет h, F\n",
    "                if not CLOSED.WasExpanded(A):\n",
    "                    OPEN.AddNode(A)\n",
    "                k += 1\n",
    "                \n",
    "            # обязываем а пройти по данному ребру в момент времени t. Это же так работает?(:    \n",
    "            tmp = s.edgeCons.copy()\n",
    "            if a in tmp:\n",
    "                for (x,y) in gridMap.cells():\n",
    "                    if (x,y) != (i1,j1) and (x+1,y) != (i2,j2):\n",
    "                        tmp[a].append(((x, y), (x+1, y), t))   \n",
    "                    elif (x,y) != (i1,j1) and (x-1,y) != (i2,j2):\n",
    "                        tmp[a].append(((x, y), (x-1, y), t))  \n",
    "                    elif (x,y) != (i1,j1) and (x,y-1) != (i2,j2):\n",
    "                        tmp[a].append(((x, y), (x, y-1), t))  \n",
    "                    elif (x,y) != (i1,j1) and (x,y+1) != (i2,j2):\n",
    "                        tmp[a].append(((x, y), (x, y+1), t)) \n",
    "            else:\n",
    "                tmp[a] = []\n",
    "                for (x,y) in gridMap.cells():\n",
    "                    if (x,y) != (i1,j1) and (x+1,y) != (i2,j2):\n",
    "                        tmp[a].append(((x, y), (x+1, y), t))   \n",
    "                    elif (x,y) != (i1,j1) and (x-1,y) != (i2,j2):\n",
    "                        tmp[a].append(((x, y), (x-1, y), t))  \n",
    "                    elif (x,y) != (i1,j1) and (x,y-1) != (i2,j2):\n",
    "                        tmp[a].append(((x, y), (x, y-1), t))  \n",
    "                    elif (x,y) != (i1,j1) and (x,y+1) != (i2,j2):\n",
    "                        tmp[a].append(((x, y), (x, y+1), t)) \n",
    "            \n",
    "            B = HighNode(\n",
    "                vertexCons=s.vertexCons,\n",
    "                edgeCons=tmp,\n",
    "                sol=s.sol,\n",
    "                parent=s,\n",
    "                k=k,\n",
    "            )\n",
    "            \n",
    "            vc = []\n",
    "            if a in B.vertexCons:\n",
    "                vc = B.vertexCons[a]\n",
    "            \n",
    "            planner = AstarTimesteps(gridMap, Starts[a][0], Starts[a][1], Goals[a][0], Goals[a][1], vc, B.edgeCons[a])\n",
    "            result = planner.FindPath()\n",
    "            if result[0]:\n",
    "                path = MakePath(result[1])\n",
    "                B.sol[a], _ = path\n",
    "                B.g = sum([len(path) for path in B.sol.values()]) # SIC, можно использовать другой cost; добавить подсчет h, F\n",
    "                if not CLOSED.WasExpanded(B):\n",
    "                    OPEN.AddNode(B)\n",
    "                k += 1\n",
    "                \n",
    "    return (False, None, OPEN, CLOSED)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "ename": "TypeError",
     "evalue": "CBSDS() missing 2 required positional arguments: 'vertexCons' and 'edgeCons'",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-3-f23b64cb106a>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m     14\u001b[0m \u001b[0mGoals\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;33m[\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mitem\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m2\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mitem\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m3\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m)\u001b[0m \u001b[1;32mfor\u001b[0m \u001b[0mitem\u001b[0m \u001b[1;32min\u001b[0m \u001b[0mtasks\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m2\u001b[0m\u001b[1;33m:\u001b[0m\u001b[0mnum\u001b[0m\u001b[1;33m+\u001b[0m\u001b[1;36m2\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     15\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 16\u001b[1;33m \u001b[0mresult\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mCBSDS\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mtask_map\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mStarts\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mGoals\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     17\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     18\u001b[0m \u001b[1;32mif\u001b[0m \u001b[0mresult\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m0\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mTypeError\u001b[0m: CBSDS() missing 2 required positional arguments: 'vertexCons' and 'edgeCons'"
     ]
    }
   ],
   "source": [
    "map_file = 'tests/room-32-32-4.map'\n",
    "scen_file = 'tests/scen-even/room-32-32-4-even-19.scen'\n",
    "\n",
    "\n",
    "w, h, cells = read_map_from_movingai_file(map_file)\n",
    "tasks = read_tasks_from_movingai_file(scen_file)\n",
    "\n",
    "task_map = Map()\n",
    "task_map.SetGridCells(w, h, cells)\n",
    "\n",
    "num = 4\n",
    "\n",
    "Starts = [(item[0], item[1]) for item in tasks[2:num+2]]\n",
    "Goals = [(item[2], item[3]) for item in tasks[2:num+2]]\n",
    "\n",
    "result = CBSDS(task_map, Starts, Goals)\n",
    "\n",
    "if result[0]:\n",
    "    print(f'Final cost {result[1].g},\\nEuclidean cost {sum([tasks[i][-1] for i in range(num)])}')\n",
    "    \n",
    "    obs_x = []\n",
    "    obs_y = []\n",
    "\n",
    "    for y, cell_y in enumerate(task_map.cells):\n",
    "        for x, cell_x in enumerate(cell_y):\n",
    "            if cell_x == 1:\n",
    "                obs_x.append(x)\n",
    "                obs_y.append(y)\n",
    "            \n",
    "    x = np.array(obs_x)\n",
    "    y = np.array(obs_y)\n",
    "\n",
    "    fig = plt.figure(figsize=(5, 5))\n",
    "    ax = fig.add_subplot(111)\n",
    "    ax.set_ylim(task_map.height, 0)\n",
    "    ax.xaxis.set_ticks_position('top')\n",
    "\n",
    "    ax.scatter(x, y, c='black', marker='s', s=50)\n",
    "    ax.set_facecolor('white')\n",
    "    ax.set_title(map_file)\n",
    "\n",
    "    color = [\"#\" + ''.join([random.choice('0123456789ABCDEF') for j in range(6)]) + ''.join(['A', '0'])\n",
    "             for i in range(num)]\n",
    "\n",
    "    for a in result[1].sol:\n",
    "        path = result[1].sol[a]\n",
    "        x = []\n",
    "        y = []\n",
    "        for node in path:\n",
    "            x.append(node.i)\n",
    "            y.append(node.j)\n",
    "        ax.scatter(x[1:-1], y[1:-1], c=color[a], marker='s', s=50)\n",
    "        ax.scatter(x[:1], y[:1], c='blue', marker='s', s=50)\n",
    "        ax.scatter(x[-1:], y[-1:], c='red', marker='s', s=50)\n",
    "\n",
    "    plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}