an example comparing them!!

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

.gitignore

@@ -59,3 +59,4 @@ roadmaps/benchmark_plots/benchmark_*.png
 
 # build folder for cvt
 roadmaps/cvt/build/*
+path_save.pkl

planner/cbs-ta_vs_tcbs/evaluation.py

@@ -0,0 +1,81 @@
+import numpy as np
+
+from planner.mapf_implementations.plan_cbs_ta import plan as cbs_ta_plan
+from planner.tcbs.plan import plan as tcbs_plan
+
+
+def tcbs_with_single_goals(gridmap, starts, goals, timeout):
+    """
+    Adapter to call TCBS with single goals.
+    (Normally, TCBS defines jobs as transport tasks,
+    i.e., with start and goal locations.)
+    """
+    # jobs must be transport tasks
+    jobs = []
+    for g in goals:
+        jobs.append((tuple(g), tuple(g), 0))  # start = goal = g, time = 0
+    # map must be time-expanded
+    t_max = gridmap.shape[0] * gridmap.shape[1] * 2
+    _map = np.repeat(gridmap[:, :, np.newaxis], t_max, axis=2)
+    return tcbs_plan(
+        agent_pos=[tuple(s) for s in starts],
+        jobs=jobs,
+        alloc_jobs=[],
+        idle_goals=[],
+        grid=_map,
+    )
+
+
+def execute_both_planners(gridmap, starts, goals, timeout):
+    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
+
+
+def get_cost_from_tcbs_res(tcbs_res):
+    """
+    Return the sum of costs from TCBS result.
+
+    This must be comparable to the cost from CBS-TA.
+    So we have to ignore the last step, where the agents stay at the goal.
+    """
+    _, _, paths = tcbs_res
+    cost = 0
+    for path in paths:
+        to_start, _ = path
+        cost += len(to_start) - 1
+
+    return cost
+
+
+def get_cost_from_cbs_ta_res(cbs_ta_res):
+    """
+    Return the sum of costs from CBS-TA result.
+    """
+    return cbs_ta_res["statistics"]["cost"]
+
+
+if __name__ == "__main__":
+    gridmap = np.array([[0] * 3] * 3)
+    starts = [[0, 0], [0, 1], [0, 2]]
+    goals = [[2, 0], [2, 1], [1, 2]]
+    res = execute_both_planners(gridmap, starts, goals, 10)
+    res_cbs_ta, res_tcbs = res
+
+    print("=" * 10)
+
+    print("CBS-TA")
+    print(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)
+    print("-" * 10)
+
+    print("Cost from TCBS")
+    print(get_cost_from_tcbs_res(res_tcbs))
+    print("=" * 10)

planner/mapf_implementations/plan_cbs_ta.py

@@ -128,7 +128,7 @@ def call_subprocess(fname_infile, fname_outfile, timeout):
     return out_data
 
 
-def plan_in_gridmap(gridmap: np.ndarray, starts, goals, timeout):
+def plan(gridmap: np.ndarray, starts, goals, timeout):
     # solving memoryview: underlying buffer is not C-contiguous
     gridmap = np.asarray(gridmap, order="C")
     uuid_str = str(uuid.uuid1())
@@ -144,5 +144,5 @@ def plan_in_gridmap(gridmap: np.ndarray, starts, goals, timeout):
     gridmap = np.array([[0, 0, 1, 1], [0, 0, 0, 1], [0, 0, 0, 0], [0, 0, 0, 1]])
     starts = [[1, 2], [3, 2], [2, 1]]
     goals = [[3, 0], [2, 2], [0, 0]]
-    res = plan_in_gridmap(gridmap, starts, goals, 10)
+    res = plan(gridmap, starts, goals, 10)
     print(res)

planner/mapf_implementations/plan_cbs_ta_test.py

@@ -2,15 +2,15 @@
 
 import numpy as np
 
-from planner.mapf_implementations.plan_cbs_ta import plan_in_gridmap
+from planner.mapf_implementations.plan_cbs_ta import plan
 
 
 class TestPlanCBS_TA(unittest.TestCase):
     def test_basic_example(self):
         gridmap = np.array([[0, 0, 1, 1], [0, 0, 0, 1], [0, 0, 0, 0], [0, 0, 0, 1]])
         starts = [[1, 2], [3, 2], [2, 1]]
         goals = [[3, 0], [2, 2], [0, 0]]
-        res = plan_in_gridmap(gridmap, starts, goals, 10)
+        res = plan(gridmap, starts, goals, 10)
 
         self.assertEqual(res["statistics"]["cost"], 6)
         self.assertEqual(res["statistics"]["makespan"], 3)

planner/tcbs/plan.py

@@ -151,6 +151,7 @@ def plan(
 def alloc_threads():
     global pool
     n = multiprocessing.cpu_count()
+    n = min(n, 8)
     pool = multiprocessing.Pool(processes=n)
     return pool
 
@@ -697,7 +698,9 @@ def get_paths(_condition: dict, _state: tuple):
             }
         )
     global pool
-    res = list(pool.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:
             return False