things are going well

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

planner/cbs-ta_vs_tcbs/evaluation.py

@@ -1,3 +1,4 @@
+import os
 from pprint import pprint
 from random import Random
 
@@ -26,8 +27,15 @@ def tcbs_with_single_goals(grid, starts, goals):
     # map must be time-expanded
     t_max = grid.shape[0] * grid.shape[1] * 2
     _map = np.repeat(grid[:, :, np.newaxis], t_max, axis=2)
+    # making sure we start with fresh path_save
+    fname = "path_save_eval.pkl"
+    if os.path.exists(fname):
+        os.remove(fname)
+    # configuration for TCBS
     config = generate_config()
+    config["filename_pathsave"] = fname
     config["time_extended"] = False
+    config["save_stats"] = True
     return tcbs_plan(
         agent_pos=[tuple(s) for s in starts],
         jobs=jobs,
@@ -54,7 +62,7 @@ def get_cost_from_tcbs_res(tcbs_res):
     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
+    _, _, paths, _ = tcbs_res
     cost = 0
     for path in paths:
         assert len(path) == 2, (
@@ -67,13 +75,61 @@ def get_cost_from_tcbs_res(tcbs_res):
     return cost
 
 
+def get_high_level_expanded_from_tcbs_res(tcbs_res):
+    """
+    Return the high-level expanded nodes from TCBS result.
+    """
+    _, _, _, stats = tcbs_res
+    return stats["n_high_level_expanded"]
+
+
+def get_low_level_expanded_from_tcbs_res(tcbs_res):
+    """
+    Return the low-level expanded nodes from TCBS result.
+    """
+    _, _, _, stats = tcbs_res
+    return stats["n_low_level_expanded"]
+
+
+def get_paths_from_tcbs_res(tcbs_res):
+    """
+    Return the paths from TCBS result in the format of TCBS result.
+    """
+    paths = tcbs_res[2]
+    paths_out = []
+    for ps in paths:
+        assert len(ps) == 2, (
+            "This must not have time-expanded assignments. "
+            "(i.e., only one task per agent)"
+        )
+        assert len(ps[1]) == 1, "Second element must be a single step"
+        assert ps[0][-1][0] == ps[1][0][0], "Last step must be the same (x)"
+        assert ps[0][-1][1] == ps[1][0][1], "Last step must be the same (y)"
+        paths_out.append((ps[0],))
+    return paths_out
+
+
 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"]
 
 
+def get_high_level_expanded_from_cbs_ta_res(cbs_ta_res):
+    """
+    Return the high-level expanded nodes from CBS-TA result.
+    """
+    return cbs_ta_res["statistics"]["highLevelExpanded"]
+
+
+def get_low_level_expanded_from_cbs_ta_res(cbs_ta_res):
+    """
+    Return the low-level expanded nodes from CBS-TA result.
+    """
+    return cbs_ta_res["statistics"]["lowLevelExpanded"]
+
+
 def get_paths_from_cbs_ta_res(cbs_ta_res):
     """
     Return the paths from CBS-TA result in the format of TCBS result.
@@ -102,27 +158,33 @@ def demo():
 
     print("CBS-TA")
     pprint(res_cbs_ta)
-    print("-" * 10)
 
-    print("Cost from CBS-TA")
-    print(get_cost_from_cbs_ta_res(res_cbs_ta))
+    print("Cost", get_cost_from_cbs_ta_res(res_cbs_ta))
+    print("HL expanded", get_high_level_expanded_from_cbs_ta_res(res_cbs_ta))
+    print("LL expanded", get_low_level_expanded_from_cbs_ta_res(res_cbs_ta))
+
     print("-" * 10)
 
     print("TCBS")
     pprint(res_tcbs)
-    print("-" * 10)
 
-    print("Cost from TCBS")
-    print(get_cost_from_tcbs_res(res_tcbs))
+    print("Cost", get_cost_from_tcbs_res(res_tcbs))
+    print("HL expanded", get_high_level_expanded_from_tcbs_res(res_tcbs))
+    print("LL expanded", get_low_level_expanded_from_tcbs_res(res_tcbs))
+
     print("=" * 10)
 
+    plot_comparison_results(grid, res_cbs_ta, res_tcbs)
+
+
+def plot_comparison_results(grid, res_cbs_ta, res_tcbs):
     fig = plt.figure()
     ax_tcbs = fig.add_subplot(121, projection="3d")
-
+    paths_tcbs = get_paths_from_tcbs_res(res_tcbs)
     plot_results(
         ax_tcbs,
         [],
-        res_tcbs[2],
+        paths_tcbs,
         [],
         [],
         grid,
@@ -154,17 +216,24 @@ def eval_same_cost_for_random_scenarios():
     """
     rng = Random(42)
     n_runs = 10
-    n_agents = 3
-    size = 5
+    n_agents = 4
+    size = 8
 
     res_same_cost = 0
     res_success_cbs_ta = 0
     res_success_tcbs = 0
 
+    res_tcbs_cost = []
+    res_tcbs_lle = []
+    res_tcbs_hle = []
+    res_cbs_ta_cost = []
+    res_cbs_ta_lle = []
+    res_cbs_ta_hle = []
+
     for _ in tqdm.tqdm(range(n_runs)):
         grid, starts, goals = tracing_paths_in_the_dark(
             size=size,
-            fill=0.2,
+            fill=0.5,
             n_agents=n_agents,
             rng=rng,
         )
@@ -173,25 +242,41 @@ def eval_same_cost_for_random_scenarios():
 
         if res_cbs_ta != INVALID:
             cost_cbs_ta = get_cost_from_cbs_ta_res(res_cbs_ta)
+            res_cbs_ta_cost.append(cost_cbs_ta)
+            res_cbs_ta_lle.append(get_low_level_expanded_from_cbs_ta_res(res_cbs_ta))
+            res_cbs_ta_hle.append(get_high_level_expanded_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_tcbs_cost.append(cost_tcbs)
+            res_tcbs_lle.append(get_low_level_expanded_from_tcbs_res(res_tcbs))
+            res_tcbs_hle.append(get_high_level_expanded_from_tcbs_res(res_tcbs))
             res_success_tcbs += 1
         else:
             cost_tcbs = INVALID
 
         if cost_cbs_ta == cost_tcbs:
             res_same_cost += 1
+        else:
+            plot_comparison_results(grid, res_cbs_ta, res_tcbs)
 
     percentage_same_cost = res_same_cost / n_runs * 100
     print(f"Same cost: {res_same_cost}/{n_runs} = {percentage_same_cost:.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}%")
+    print("CBS-TA")
+    print(f"  Success rate: {res_success_cbs_ta/n_runs*100:.2f}%")
+    print(f"  Average cost: {np.mean(res_cbs_ta_cost)}")
+    print(f"  Average LL expanded: {np.mean(res_cbs_ta_lle)}")
+    print(f"  Average HL expanded: {np.mean(res_cbs_ta_hle)}")
+    print("TCBS")
+    print(f"  Success rate: {res_success_tcbs/n_runs*100:.2f}%")
+    print(f"  Average cost: {np.mean(res_tcbs_cost)}")
+    print(f"  Average LL expanded: {np.mean(res_tcbs_lle)}")
+    print(f"  Average HL expanded: {np.mean(res_tcbs_hle)}")
 
 
 if __name__ == "__main__":
-    # demo()
+    demo()
     eval_same_cost_for_random_scenarios()

planner/common.py

@@ -34,17 +34,18 @@ def path(
       or False if path shouldn't have been calculated but was not saved either
     """
     _map = _map.copy()
+    n_expanded = 0
     for b in blocked:
         if b[0] == VERTEX:
             v = b[1]
             _map[(v[1], v[0], v[2])] = -1
             if v[:2] == start or v[:2] == goal:
-                return False, {}
+                return False, {}, 0
         elif b[0] == EDGE:
             for v in b[1][0:2]:
                 _map[(v[1], v[0], b[1][2])] = -1
                 if v[:2] == start or v[:2] == goal:
-                    return False, {}
+                    return False, {}, 0
                 # TODO: actually block edge!
 
     blocked.sort()
@@ -58,27 +59,28 @@ def path(
                 _path = astar_grid4con(
                     start + (0,), goal + (_map.shape[2] - 1,), _map.swapaxes(0, 1)
                 )
+                n_expanded += len(_path) - 1
             except NoPathException:
                 _path = []
 
             path_save_process[index] = _path
         else:
-            return False, {}
+            return False, {}, 0
     else:  # exists in the path_save
         _path = path_save[index]
 
     for b in blocked:
         if b[0] == VERTEX and b[1] in _path:
-            return False, {}
+            return False, {}, 0
         # if b[0] == EDGE and (
         #         (b[1][0] + (b[1][2],) in _path) or
         #         (b[1][1] + (b[1][2],) in _path)
         # ):
-        #     return False, {}
+        #     return False, {}, 0
 
     if not _path:
-        return False, {}
+        return False, {}, 0
 
     assert start == _path[0][0:2], "Planed path starts not from start"
     assert goal == _path[-1][0:2], "Planed path ends not in goal"
-    return _path, path_save_process
+    return _path, path_save_process, n_expanded

planner/tcbs/plan.py

@@ -14,6 +14,7 @@
 from planner.common import *
 from planner.eval.display import plot_inputs, plot_results
 from planner.tcbs.base import MAX_COST, astar_base
+from planner.tcbs.stats import StatsContainer
 from tools import ColoredLogger
 
 logging.setLoggerClass(ColoredLogger)
@@ -22,6 +23,7 @@
 
 _config = {}
 _distances = None
+_stats = StatsContainer()
 
 EXPECTED_MAX_N_BLOCKS = 1000
 
@@ -68,6 +70,11 @@ def plan(
     global _config
     _config = config
 
+    # stats
+    if _config["save_stats"]:
+        global _stats
+        _stats = StatsContainer()
+
     # load path_save
     if filename:  # TODO: check if file was created on same map
         load_paths(filename)
@@ -146,7 +153,10 @@ def plan(
     pool.terminate()
     pool.join()
 
-    return agent_job, _agent_idle, _paths
+    if _config["save_stats"]:
+        return agent_job, _agent_idle, _paths, _stats.as_dict()
+    else:
+        return agent_job, _agent_idle, _paths
 
 
 def alloc_threads():
@@ -326,6 +336,11 @@ def cost(_condition: dict, _state: tuple):
     (agent_job, agent_idle, block_state) = state2comp(_state)
     _cost = 0.0
 
+    global _config
+    global _stats
+    if _config["save_stats"]:
+        _stats.inc_high_level_expanded()
+
     _paths = get_paths(_condition, _state)
     if _paths == False:  # one path was not viable
         return MAX_COST, _state
@@ -625,47 +640,52 @@ def get_paths_for_agent(vals):
     assigned_jobs = agent_job[i_a]
     pose = agent_pos[i_a][0:2]
     t_shift = 0
+    n_expanded = 0
     for ij in assigned_jobs:
         if (i_a, ij) in alloc_jobs:  # can be first only; need to go to goal only
-            p, path_save_process = path(
+            p, path_save_process, exp = path(
                 pose, jobs[ij][1], _map, block, path_save_process, calc=True
             )
+            n_expanded += exp
             if not p:
                 return False
         else:
             # trip to start
             if len(paths_for_agent) > 0:  # no route yet -> keep pose
                 pose, t_shift = get_last_pose_and_t(paths_for_agent)
             block1 = time_shift_blocks(block, t_shift)
-            p1, path_save_process = path(
+            p1, path_save_process, exp = path(
                 pose, jobs[ij][0], _map, block1, path_save_process, calc=True
             )
+            n_expanded += exp
             if not p1:
                 return False
             paths_for_agent += (time_shift_path(p1, t_shift),)
             # start to goal
             pose, t_shift = get_last_pose_and_t(paths_for_agent)
             assert pose == jobs[ij][0], "Last pose should be the start"
             block2 = time_shift_blocks(block, t_shift)
-            p, path_save_process = path(
+            p, path_save_process, exp = path(
                 jobs[ij][0], jobs[ij][1], _map, block2, path_save_process, calc=True
             )
+            n_expanded += exp
             if not p:
                 return False
         paths_for_agent += (time_shift_path(p, t_shift),)
     if len(_agent_idle[i_a]):
-        p, path_save_process = path(
+        p, path_save_process, exp = path(
             agent_pos[i_a],
             idle_goals[_agent_idle[i_a][0]][0],
             _map,
             block,
             path_save_process,
             calc=True,
         )
+        n_expanded += exp
         if not p:
             return False
         paths_for_agent += (p,)
-    return paths_for_agent, path_save_process
+    return paths_for_agent, path_save_process, n_expanded
 
 
 def get_paths(_condition: dict, _state: tuple):
@@ -713,6 +733,7 @@ def get_paths(_condition: dict, _state: tuple):
 
         _paths.append(r[0])
         path_save.update(r[1])
+        _stats.add_low_level_expanded(r[2])
     longest = max(map(lambda p: len(reduce(lambda a, b: a + b, p, [])), _paths))
     if _config["finished_agents_block"]:
         (left_agent_pos, left_idle_goals, left_jobs) = clear_set(
@@ -1000,6 +1021,8 @@ def generate_config():
         "heuristic_colission": False,
         # whether to use time-extended assignments (agents can have multiple jobs)
         "time_extended": True,
+        # whether to save stats
+        "save_stats": False,
     }
 
 

planner/tcbs/plan_main.py

@@ -23,6 +23,6 @@
         "jobs": [((0, 0), (9, 9), 0.00072), ((0, 0), (9, 9), 0.000309)],
     }
 
-    res = get_paths_for_agent(vals)
+    res, n_expanded = get_paths_for_agent(vals)
 
     print(res)