def __init__(self, my_map, starts, goals, merge_thresh): """my_map - list of lists specifying obstacle positions starts - [(x1, y1), (x2, y2), ...] list of start locations goals - [(x1, y1), (x2, y2), ...] list of goal locations """ self.my_map = my_map self.starts = starts self.goals = goals self.num_of_agents = len(goals) self.num_of_generated = 0 self.num_of_expanded = 0 self.CPU_time = 0 self.open_list = [] self.heuristics = [] for goal in self.goals: self.heuristics.append(compute_heuristics(my_map, goal)) # Merging bound parameter self.B = merge_thresh # Conflict count table self.conflict_matrix = [[0 for i in range(self.num_of_agents)] for j in range(self.num_of_agents)] # cache computed path {contraint: path} self.cache = {}
def __init__(self, my_map, starts, goals): """my_map - list of lists specifying obstacle positions starts - [(x1, y1), (x2, y2), ...] list of start locations goals - [(x1, y1), (x2, y2), ...] list of goal locations """ self.my_map = my_map self.starts = starts self.goals = goals self.num_of_agents = len(goals) self.CPU_time = 0 # compute heuristics for the low-level search self.heuristics = [] for goal in self.goals: self.heuristics.append(compute_heuristics(my_map, goal))
def find_solution(self, disjoint=True, heuristic=None): """ Finds paths for all agents from their start locations to their goal locations disjoint - use disjoint splitting or not """ self.start_time = timer.time() # Generate the root node # constraints - list of constraints # paths - list of paths, one for each agent # [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...] # collisions - list of collisions in paths root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []} for i in range(self.num_of_agents): # Find initial path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, root['constraints']) if path is None: raise BaseException('No solutions') root['paths'].append(path) root['cost'] = get_sum_of_cost(root['paths']) root['collisions'] = detect_collisions(root['paths']) root['h_val'] = 0 self.push_node(root) # Task 3.1: Testing #print(root['collisions']) # Task 3.2: Testing #for collision in root['collisions']: # print(standard_splitting(collision)) ############################## # Task 3.3: High-Level Search # Repeat the following as long as the open list is not empty: # 1. Get the next node from the open list (you can use self.pop_node() # 2. If this node has no collision, return solution # 3. Otherwise, choose the first collision and convert to a list of constraints (using your # standard_splitting function). Add a new child node to your open list for each constraint # Ensure to create a copy of any objects that your child nodes might inherit while len(self.open_list) > 0: p = self.pop_node() if len(p['collisions']) == 0: self.sum_of_costs = get_sum_of_cost(p['paths']) return p['paths'] collision = p['collisions'][0] constraints = standard_splitting( collision) #disjoint_splitting(collision) for constraint in constraints: q = { 'cost': 0, 'constraints': copy.deepcopy(p['constraints'] + [constraint]), 'paths': copy.deepcopy(p['paths']), 'collisions': [] } """ Task 4 if constraint['positive'] == True: if paths_violate_constraint(constraint, q['paths']): continue """ agent = constraint['agent'] path = a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, q['constraints']) if path != None: q['paths'][agent] = path q['collisions'] = detect_collisions(q['paths']) q['cost'] = get_sum_of_cost(q['paths']) if self.CT_heuristic == None: q['h_val'] = 0 else: MDD_list = [mdd.MDD(self.my_map, self.starts[i], self.goals[i],\ compute_heuristics(self.my_map, self.goals[i]), i, q['constraints'])\ for i in range(len(self.starts))] if self.CT_heuristic == "CG": graph_inst = mdd.graph_from_MDD(MDD_list) mvc_inst = mvc.BruteForceMVC(graph_inst) q['h_val'] = mvc_inst.getMVC() elif self.CT_heuristic == "DG": graph_inst = joint_mdd.graph_from_MDD(MDD_list) mvc_inst = mvc.BruteForceMVC(graph_inst) q['h_val'] = mvc_inst.getMVC() elif self.CT_heuristic == "WDG": graph_inst = joint_mdd.weighted_graph_from_MDD( MDD_list, self.my_map, self.starts, self.goals) ewmvc_inst = mvc.BruteForceEWMVC(graph_inst) q['h_val'] = ewmvc_inst.getEWMVC() self.push_node(q) if path is None: raise BaseException('No solutions') self.print_results(root) return root['paths']
return graph ### testing code if __name__ == '__main__': parser = argparse.ArgumentParser(description='Generate MDD for agents') parser.add_argument('--instance', type=str, default=None, help='The name of the instance file(s)') args = parser.parse_args() for file in sorted(glob.glob(args.instance)): print("***Import an instance***") my_map, starts, goals = run_experiments.import_mapf_instance(file) run_experiments.print_mapf_instance(my_map, starts, goals) MDD_list = [] for i in range(len(starts)): h_values = compute_heuristics(my_map, goals[i]) temp_MDD = MDD(my_map, starts[i], goals[i], h_values, i, []) print("\n=====\nagent {}:".format(i)) temp_MDD.print_all() #temp_MDD.print_levels() MDD_list.append(temp_MDD) conflict_graph = graph_from_MDD(MDD_list) print("conflict graph: {}".format(conflict_graph))