def compute_paths(map,
starts,
goals,
heuristics,
agents_need_update,
child,
group_idx=None):
if isinstance(agents_need_update, int) or len(agents_need_update) == 1:
if isinstance(agents_need_update, list):
agent = agents_need_update[0]
path = a_star(map, starts[agent], goals[agent], heuristics[agent],
agent, flatten_constraints(child['constraints']))
if path is None:
return False
child['paths'][agent] = path
else:
cbs = CBSSolver(map, [starts[m] for m in agents_need_update],
[goals[m] for m in agents_need_update])
agents_mapping = {}
for agent in agents_need_update:
agents_mapping[agent] = len(agents_mapping)
meta_constraints = []
for constraint in child['constraints']:
if constraint['group'] == group_idx:
for agent in constraint['agent']:
meta_constraints.append(copy.copy(constraint))
# print('agents:', constraint['agent'])
# print('mapping:', agents_mapping)
meta_constraints[len(meta_constraints) -
1]['agent'] = agents_mapping[agent]
result = cbs.find_solution(meta_constraints=meta_constraints)
if isinstance(result, tuple):
path = result[0]
elif result is None:
return False
for (idx, m) in enumerate(agents_need_update):
child['paths'][m] = path[idx]
# print("path", path)
return True
for file in sorted(glob.glob(args.instance)):
print("***Import an instance***")
my_map, starts, goals = import_mapf_instance(file)
print_mapf_instance(my_map, starts, goals)
paths = None
expands_node = None
generate_node = None
if args.solver == "CBS":
print("***Run CBS***")
try:
with time_limit(300):
start_time = timer.time()
cbs = CBSSolver(my_map, starts, goals)
paths = cbs.find_solution(args.disjoint)
expands_node = cbs.num_of_expanded
generate_node = cbs.num_of_generated
time = timer.time() - start_time
except TimeoutException as e:
time = float("inf")
elif args.solver == "CBS_N":
print("**Run CBS With Normal A*")
try:
with time_limit(300):
start_time = timer.time()
cbs = CBSSolver_normal(my_map, starts, goals)
paths = cbs.find_solution(args.disjoint)
expands_node = cbs.num_of_expanded
generate_node = cbs.num_of_generated
.format(solver.__name__, '-',
str(e).split('|')[3],
str(e).split('|')[1],
str(e).split('|')[2], map_file,
scen_file, agents_limit,
conflict_bound, 'timeout'))
else:
result_file.write(
"{}, {}, {}, {}, {}, {}, {}, {}, {}, {}\n"
.format(solver.__name__, '-', '-', '-',
'-', map_file, scen_file,
agents_limit, conflict_bound,
'Fail:' + str(e)))
if args.solver == "CBS":
print("***Run CBS***")
cbs = CBSSolver(my_map, starts, goals, args.merge_thresh)
(paths, time, n_exp, n_gen) = cbs.find_solution()
elif args.solver == "MetaCBSwCBS":
print("***Run MetaCBS with CBS as low level solver***")
ma_cbs = MetaAgCBSWithCBS(my_map, starts, goals,
args.merge_thresh)
(paths, time, n_exp, n_gen) = ma_cbs.find_solution()
print(
"***Finished Run MetaCBS with CBS as low level solver***")
elif args.solver == "MetaCBSwMstar":
print("***Run MetaCBS with Mstar as low level solver***")
ma_mstar = MetaAgCBSWithMstar(my_map, starts, goals,
args.merge_thresh)
(paths, time, n_exp, n_gen) = ma_mstar.find_solution()
print(
"***Finished Run MetaCBS with Mstar as low level solver***"
with open("results.csv", "w", buffering=1) as result_file:
result_file.write("{},{},{},{},{},{}\n".format("Instance", "CPU time",
"Memory Usage",
"Sum of Costs",
"Expanded Nodes",
"Generated Nodes"))
for file in sorted(glob.glob(args.instance)):
print("***Import an instance***")
my_map, starts, goals = import_mapf_instance(file)
print_mapf_instance(my_map, starts, goals)
if args.solver == "CBS":
print("***Run CBS***")
cbs = CBSSolver(my_map, starts, goals, file)
paths = cbs.find_solution()
elif args.solver == "ICBS":
print("***Run Improved CBS***")
icbs = ICBSSolver(my_map, starts, goals, file)
paths = icbs.find_solution()
elif args.solver == "Conflict":
print("***Run Conflict Graph Improved CBS***")
icbs = ICBSSolver(my_map, starts, goals, file)
paths = icbs.find_solution(True)
else:
raise RuntimeError("Unknown solver!")
if not args.batch:
print("***Test paths on a simulation***")
animation = Animation(my_map, starts, goals, paths)
default=None,
help=
'The heuristic to use (only supports: CG), defaults to no heuristic')
args = parser.parse_args()
# result_file = open("results.csv", "w", buffering=1)
for file in sorted(glob.glob(args.instance)):
print("***Import an instance***")
my_map, starts, goals = import_mapf_instance(file)
print_mapf_instance(my_map, starts, goals)
if args.solver == "CBS":
print("***Run CBS***")
cbs = CBSSolver(my_map, starts, goals)
paths = cbs.find_solution(args.iterative, args.heuristic)
elif args.solver == "Independent":
print("***Run Independent***")
solver = IndependentSolver(my_map, starts, goals)
paths = solver.find_solution()
else:
raise RuntimeError("Unknown solver!")
# cost = get_sum_of_cost(paths)
# result_file.write("{},{}\n".format(file, cost))
if not args.batch:
print("***Test paths on a simulation***")
animation = Animation(my_map, starts, goals, paths)
# animation.save("output.mp4", 1.0)
def run(self, start_pts):
agent_paths = [] # N
agent_state_idx = [] # N - 1
n_agents = len(start_pts)
# find the initial goal
init_picked_flag = dict()
occupied_flag = dict()
for agent_start_loc in start_pts:
dists = [
manhattan_distance(agent_start_loc, state[0])
for state in self.state_dict
]
dists = np.asarray(dists)
idx = np.argsort(dists)
j = 0
picked_id = idx[j]
loc = self.state_dict[picked_id][0]
loc_key = '%d-%d' % (loc[0], loc[1])
while loc_key in init_picked_flag:
j += 1
picked_id = idx[j]
loc = self.state_dict[picked_id][0]
loc_key = '%d-%d' % (loc[0], loc[1])
if picked_id > idx.shape[0]:
raise Exception('Can not found any start states.')
init_picked_flag[loc_key] = True
path = [(agent_start_loc, self.state_dict[picked_id][1])]
path.append(self.state_dict[picked_id])
agent_paths.append(path)
agent_state_idx.append([picked_id])
occupied_flag[picked_id] = True
# continue search the remaining vertices
n_picked_states = len(occupied_flag)
def exists_in_path(paths, loc):
loc_key = '%d-%d' % (loc[0], loc[1])
for path in paths:
path_last_loc = path[-1][0]
path_loc_key = '%d-%d' % (path_last_loc[0], path_last_loc[1])
if loc_key == path_loc_key:
return True
return False
iter = 0
while n_picked_states <= len(self.state_dict):
agent_id = iter % n_agents
other_agent_ids = []
for i in range(n_agents):
if i != agent_id:
other_agent_ids.append(i)
# agent_path = agent_paths[agent_id]
cur_state_idx = agent_state_idx[agent_id][-1]
next_state_idx = cur_state_idx + 1
if next_state_idx >= len(self.state_dict):
next_state_idx = 0
next_loc = self.state_dict[next_state_idx][0]
try_times = 0
found_flag = True
while next_state_idx in occupied_flag:
next_state_idx += 1
if next_state_idx >= len(self.state_dict):
next_state_idx = 0
next_loc = self.state_dict[next_state_idx][0]
try_times += 1
if try_times >= len(self.state_dict):
found_flag = False
break
# check if the loc is exists
other_agent_paths = [agent_paths[i] for i in other_agent_ids]
while exists_in_path(other_agent_paths, next_loc):
next_state_idx += 1
if next_state_idx >= len(self.state_dict):
next_state_idx = 0
next_loc = self.state_dict[next_state_idx][0]
try_times += 1
if try_times >= len(self.best_state):
found_flag = False
break
if found_flag is False:
break
else:
occupied_flag[next_state_idx] = True
agent_state_idx[agent_id].append(next_state_idx)
agent_paths[agent_id].append(self.state_dict[next_state_idx])
n_picked_states += 1
iter += 1
if n_picked_states < len(self.state_dict):
raise Exception('Not all states are collected')
# Run CBS
max_path_length = 0
for path in agent_paths:
path_length = len(path)
if path_length > max_path_length:
max_path_length = path_length
output_agent_pathes = []
output_agent_idx = dict()
start_time = datetime.datetime.now()
for path_id in range(0, max_path_length - 1):
agend_ids = []
start_pos = []
start_dir = []
goal_pos = []
goal_dir = []
for agent_id in range(len(start_pts)):
agent_path = agent_paths[agent_id]
start = agent_path[path_id] if path_id < len(
agent_path) else None
end = agent_path[path_id +
1] if path_id + 1 < len(agent_path) else None
if start is not None and end is not None:
start_pos.append(start[0])
start_dir.append(start[1])
goal_pos.append(end[0])
goal_dir.append(end[1])
agend_ids.append(agent_id)
# run CBS
cbs = CBSSolver(self.my_map, start_pos, goal_pos)
solutions = cbs.find_solution(disjoint=True)
max_sol_length = 0
for solution in solutions:
if len(solution) > max_sol_length:
max_sol_length = len(solution)
current_last_item = []
for agent_id in range(len(start_pts)):
if agent_id not in output_agent_idx:
output_agent_idx[agent_id] = True
output_agent_pathes.append([])
if agent_id in agend_ids:
# locate the solution given agent id
sol_idx = 0
for s_idx in range(0, len(agend_ids)):
if agent_id == agend_ids[s_idx]:
sol_idx = s_idx
break
current_last_item.append(solutions[sol_idx][-1])
else:
current_last_item.append(output_agent_pathes[agent_id][-1])
for agent_id in range(len(start_pts)):
if agent_id in agend_ids:
# locate the solution given agent id
sol_idx = 0
for s_idx in range(0, len(agend_ids)):
if agent_id == agend_ids[s_idx]:
sol_idx = s_idx
break
sol = solutions[sol_idx]
s_dir = start_dir[sol_idx]
g_dir = goal_dir[sol_idx]
for sol_item in range(0, max_sol_length):
if sol_item < len(sol) - 1:
output_agent_pathes[agent_id].append(
(sol[sol_item], s_dir))
elif sol_item == len(sol) - 1:
output_agent_pathes[agent_id].append(
(sol[sol_item], g_dir))
else:
output_agent_pathes[agent_id].append(
(sol[-1], g_dir))
else:
for sol_item in range(0, max_sol_length):
last_item = output_agent_pathes[agent_id][-1]
output_agent_pathes[agent_id].append(last_item)
print("Time for CBS Solver is ", datetime.datetime.now() - start_time)
return output_agent_pathes