eval cbs-ta works

Signed-off-by: Christian Henkel <[email protected]>

planner/astar/astar_grid48con.py

@@ -1,5 +1,6 @@
 import numpy as np
 
+from definitions import OBSTACLE
 from planner.astar import base
 
 
@@ -42,19 +43,16 @@ def get_children(current, grid):
 
 
 def heuristic(a, b, grid: np.array = False):
-    if grid:  # costmap values
-        h = np.mean(grid[grid >= 0]) * distance(a, b)
-    else:
-        h = distance(a, b)
+    h = distance(a, b)
     assert h >= 0, "Negative Heuristic"
     return h
 
 
 def cost(a, b, grid=False):
-    if grid:  # costmap values
-        return grid[a] * distance_manhattan(a, b)
-    else:
-        return distance_manhattan(a, b)
+    if not (isinstance(grid, bool) and not grid):  # costmap values
+        if grid[a] == OBSTACLE or grid[b] == OBSTACLE:
+            return np.inf
+    return distance_manhattan(a, b)
 
 
 def distance(a: tuple, b: tuple) -> float:

planner/cbs-ta_vs_tcbs/evaluation.py

@@ -1,7 +1,16 @@
+from pprint import pprint
+from random import Random
+
+import matplotlib.pyplot as plt
 import numpy as np
+import tqdm
 
+from definitions import FREE, INVALID, OBSTACLE
+from planner.eval.display import plot_results
 from planner.mapf_implementations.plan_cbs_ta import plan as cbs_ta_plan
+from planner.tcbs.plan import generate_config
 from planner.tcbs.plan import plan as tcbs_plan
+from scenarios.generators import tracing_pathes_in_the_dark
 
 
 def tcbs_with_single_goals(gridmap, starts, goals, timeout):
@@ -17,16 +26,21 @@ def tcbs_with_single_goals(gridmap, starts, goals, timeout):
     # map must be time-expanded
     t_max = gridmap.shape[0] * gridmap.shape[1] * 2
     _map = np.repeat(gridmap[:, :, np.newaxis], t_max, axis=2)
+    config = generate_config()
+    config["time_extended"] = False
     return tcbs_plan(
         agent_pos=[tuple(s) for s in starts],
         jobs=jobs,
         alloc_jobs=[],
         idle_goals=[],
         grid=_map,
+        config=config,
     )
 
 
 def execute_both_planners(gridmap, starts, goals, timeout):
+    if isinstance(goals, np.ndarray):
+        goals = [list(g) for g in goals]
     cbs_ta_res = cbs_ta_plan(gridmap, starts, goals, timeout)
     tcbs_res = tcbs_with_single_goals(gridmap, starts, goals, timeout)
     return cbs_ta_res, tcbs_res
@@ -42,6 +56,10 @@ def get_cost_from_tcbs_res(tcbs_res):
     _, _, paths = tcbs_res
     cost = 0
     for path in paths:
+        assert len(path) == 2, (
+            "This must not have time-expanded assignments. "
+            "(i.e., only one task per agent)"
+        )
         to_start, _ = path
         cost += len(to_start) - 1
 
@@ -55,27 +73,124 @@ def get_cost_from_cbs_ta_res(cbs_ta_res):
     return cbs_ta_res["statistics"]["cost"]
 
 
-if __name__ == "__main__":
-    gridmap = np.array([[0] * 3] * 3)
+def get_paths_from_cbs_ta_res(cbs_ta_res):
+    """
+    Return the paths from CBS-TA result in the format of TCBS result.
+    """
+    paths = []
+    agents = list(cbs_ta_res["schedule"].keys())
+    agents.sort()
+    for agent in agents:
+        path = []
+        for step in cbs_ta_res["schedule"][agent]:
+            path.append((step["x"], step["y"], step["t"]))
+        paths.append((path,))
+    return paths
+
+
+def demo():
+    gridmap = np.array([[FREE] * 3] * 3)
+    gridmap[1, 1] = OBSTACLE
     starts = [[0, 0], [0, 1], [0, 2]]
-    goals = [[2, 0], [2, 1], [1, 2]]
+    goals = [[2, 0], [2, 1], [2, 2]]
     res = execute_both_planners(gridmap, starts, goals, 10)
     res_cbs_ta, res_tcbs = res
 
     print("=" * 10)
 
     print("CBS-TA")
-    print(res_cbs_ta)
+    pprint(res_cbs_ta)
     print("-" * 10)
 
     print("Cost from CBS-TA")
     print(get_cost_from_cbs_ta_res(res_cbs_ta))
     print("-" * 10)
 
     print("TCBS")
-    print(res_tcbs)
+    pprint(res_tcbs)
     print("-" * 10)
 
     print("Cost from TCBS")
     print(get_cost_from_tcbs_res(res_tcbs))
     print("=" * 10)
+
+    fig = plt.figure()
+    ax_tcbs = fig.add_subplot(121, projection="3d")
+
+    plot_results(
+        ax_tcbs,
+        [],
+        res_tcbs[2],
+        [],
+        [],
+        gridmap,
+        [],
+        [],
+        "TCBS",
+    )
+
+    ax_cbs_ta = fig.add_subplot(122, projection="3d")
+    path_cbs_ta = get_paths_from_cbs_ta_res(res_cbs_ta)
+    plot_results(
+        ax_cbs_ta,
+        [],
+        path_cbs_ta,
+        [],
+        [],
+        gridmap,
+        [],
+        [],
+        "CBS-TA",
+    )
+
+    plt.show()
+
+
+def eval_same_cost_for_random_scenarios():
+    """
+    Compare the cost from CBS-TA and TCBS for random scenarios.
+    """
+    rng = Random(42)
+    n_runs = 10
+    n_agents = 3
+    size = 5
+
+    res_same_cost = 0
+    res_success_cbs_ta = 0
+    res_success_tcbs = 0
+
+    for _ in tqdm.tqdm(range(n_runs)):
+        gridmap, starts, goals = tracing_pathes_in_the_dark(
+            size=size,
+            fill=0.2,
+            n_agents=n_agents,
+            rng=rng,
+        )
+        res = execute_both_planners(gridmap, starts, goals, 10)
+        res_cbs_ta, res_tcbs = res
+
+        if res_cbs_ta != INVALID:
+            cost_cbs_ta = get_cost_from_cbs_ta_res(res_cbs_ta)
+            res_success_cbs_ta += 1
+        else:
+            cost_cbs_ta = INVALID
+
+        if res_tcbs != INVALID:
+            cost_tcbs = get_cost_from_tcbs_res(res_tcbs)
+            res_success_tcbs += 1
+        else:
+            cost_tcbs = INVALID
+
+        if cost_cbs_ta == cost_tcbs:
+            res_same_cost += 1
+
+    print(
+        f"Same cost: {res_same_cost}/{n_runs} = " + f"{res_same_cost/n_runs*100:.2f}%"
+    )
+    print(f"Success CBS-TA: {res_success_cbs_ta/n_runs*100:.2f}%")
+    print(f"Success TCBS: {res_success_tcbs/n_runs*100:.2f}%")
+
+
+if __name__ == "__main__":
+    # demo()
+    eval_same_cost_for_random_scenarios()

planner/eval/display.py

@@ -78,8 +78,13 @@ def plot_results(
 ):
     from mpl_toolkits.mplot3d import Axes3D
 
+    max_t = 0
+    for ps in _paths:
+        for p in ps:
+            max_t = max(max_t, p[-1][2])
+
     _ = Axes3D
-    ax.axis([-1, len(grid[:, 0]), -1, len(grid[:, 0])])
+    ax.axis([-1, grid.shape[0], -1, grid.shape[0], -1, max_t + 1])
     ax.set_facecolor("white")
 
     # Paths

planner/mapf_implementations/plan_cbs_ta.py

@@ -7,6 +7,7 @@
 import time
 import uuid
 from itertools import product
+from pprint import pprint
 from typing import Any, List
 
 import numpy as np
@@ -55,13 +56,14 @@ def to_inputfile(gridmap, starts, goals, fname):
                 lambda i_a: {
                     "name": f"agent{i_a}",
                     "start": [starts[i_a, 0].item(), starts[i_a, 1].item()],
-                    "potentialGoals": goals,
+                    "potentialGoals": [[g[0].item(), g[1].item()] for g in goals],
                 },
                 range(len(starts)),
             )
         ),
     }
-    print("data", data)
+    # print("data:")
+    # pprint(data)
     with open(fname, "w") as f:
         yaml.dump(data, f, default_flow_style=True)
 
@@ -135,7 +137,7 @@ def plan(gridmap: np.ndarray, starts, goals, timeout):
     fname_infile = "/tmp/" + uuid_str + "_in.yaml"
     fname_outfile = "/tmp/" + uuid_str + "_out.yaml"
     to_inputfile(gridmap, starts, goals, fname_infile)
-    print("fname_infile", fname_infile)
+    # print("fname_infile", fname_infile)
     out_data = call_subprocess(fname_infile, fname_outfile, timeout)
     return out_data
 

planner/tcbs/plan.py

@@ -72,7 +72,7 @@ def plan(
         load_paths(filename)
 
     pool = alloc_threads()
-    alloc_threads()
+    # alloc_threads()
     agent_job = []
     _agent_idle = []
 
@@ -161,7 +161,7 @@ def alloc_threads():
 
 def get_children(_condition: dict, _state: tuple) -> list:
     """
-    Get all following states
+    Get all following states.
 
     Args:
       _condition: The conditions of the problem
@@ -170,7 +170,6 @@ def get_children(_condition: dict, _state: tuple) -> list:
 
     Returns:
       : A list of children
-
     """
     (agent_pos, jobs, alloc_jobs, idle_goals, _) = condition2comp(_condition)
     (agent_job, agent_idle, blocked) = state2comp(_state)
@@ -256,6 +255,9 @@ def assign_jobs(agent_idle, agent_job, agent_pos, blocked, jobs, left_jobs):
     for left_job in left_jobs:  # this makes many children ...
         job_to_assign = jobs.index(left_job)
         for i_a in range(len(agent_pos)):
+            if not _config["time_extended"]:
+                if len(agent_job[i_a]) > 0:  # has assignment
+                    continue
             agent_job_new = agent_job.copy()
             agent_job_new[i_a] += (job_to_assign,)
             children.append(comp2state(tuple(agent_job_new), agent_idle, blocked))
@@ -364,8 +366,8 @@ def cost(_condition: dict, _state: tuple):
             return MAX_COST, _state
         seen.add(b)
 
-    _cost += block_state.__len__() / EXPECTED_MAX_N_BLOCKS
-    assert block_state.__len__() < EXPECTED_MAX_N_BLOCKS, "more blocks than we expected"
+    # _cost += block_state.__len__() / EXPECTED_MAX_N_BLOCKS
+    # assert block_state.__len__() < EXPECTED_MAX_N_BLOCKS, "more blocks than we expected"
 
     _state = comp2state(agent_job, agent_idle, block_state)
     return _cost, _state
@@ -398,7 +400,7 @@ def heuristic(_condition: dict, _state: tuple) -> float:
     assert len(paths) == len(agent_pos), "All agents should have paths"
     for i_agent in range(len(paths)):
         pathset = paths[i_agent]
-        if pathset:
+        if pathset and _config["time_extended"]:
             agentposes.append(pathset[-1][-1][0:2])  # last pose of agent
         else:
             agentposes.append(agent_pos[i_agent])  # no path yet
@@ -698,8 +700,8 @@ def get_paths(_condition: dict, _state: tuple):
             }
         )
     global pool
-    res_out = pool.map(get_paths_for_agent, valss)
-    # res_out = map(get_paths_for_agent, valss)
+    # res_out = pool.map(get_paths_for_agent, valss)
+    res_out = map(get_paths_for_agent, valss)
     res = list(res_out)
     for r in res:
         if not r:
@@ -995,6 +997,8 @@ def generate_config():
         "all_collisions": False,
         # whether to use heuristic collisions resolution (suboptimal)
         "heuristic_colission": False,
+        # whether to use time-extended assignments (agents can have multiple jobs)
+        "time_extended": True,
     }