예제 #1
0
CBSSearch::CBSSearch(MapLoader& ml, const AgentsLoader& al, const EgraphReader& egr, double e_w, bool tweak_g_val) {
	cols = ml.cols;
	num_expanded = 0;
	num_of_types = al.num_of_types;
	nums_of_agents = al.nums_of_agents;
	map_size = ml.rows*ml.cols;
	solution_found = false;
	solution_cost = -1;
	current_makespan = 0;
	max_makespan = 1.5 * (ml.rows + ml.cols);
	start_locations = vector<vector<int>>(num_of_types);
	goal_locations = vector<vector<int>>(num_of_types);
	search_engines = vector < SingleTypeSearch* >(num_of_types);

	for (int t = 0; t < num_of_types; t++) {
		vector<int> init_locs_list(nums_of_agents[t]);
		vector<int> goal_locs_list(nums_of_agents[t]);
		for (int i = 0; i < nums_of_agents[t]; i++) {
			int init_loc = ml.linearize_coordinate(al.initial_locations[t][i].first, al.initial_locations[t][i].second);
			int goal_loc = ml.linearize_coordinate(al.goal_locations[t][i].first, al.goal_locations[t][i].second);
			init_locs_list[i] = init_loc;
			goal_locs_list[i] = goal_loc;
		}
		start_locations[t] = init_locs_list;
		goal_locations[t] = goal_locs_list;
		
		//determine starting makespan
		for (int i = 0; i < nums_of_agents[t]; i++) {
			int min_distance_to_a_goal = map_size;
			//vector<int> distances = ml.getDistancesToAllLocations(init_locs_list[i]);
			//ComputeHeuristic ch(goal_locs_list[i], ml.get_map(), ml.rows*ml.cols, ml.actions_offset, e_w, &egr);
			for (int j = 0; j < nums_of_agents[t]; j++) {				
				//int manhattan_distance = distances[goal_locs_list[j]];
				int manhattan_distance = abs(ml.row_coordinate(init_locs_list[i]) - ml.row_coordinate(goal_locs_list[j])) + abs(ml.col_coordinate(init_locs_list[i]) - ml.col_coordinate(goal_locs_list[j]));
				min_distance_to_a_goal = (manhattan_distance < min_distance_to_a_goal) ? manhattan_distance : min_distance_to_a_goal;
			}
			current_makespan = (current_makespan < min_distance_to_a_goal) ? min_distance_to_a_goal : current_makespan;
		}
	}

	// initialize paths_found_initially (contain all individual optimal policies)
	paths_found_initially.resize(num_of_types);
	
	
	bool all_agents_found_path = true;

	std::clock_t start;
	start = std::clock();
	
	for (int t = 0; t < num_of_types; t++) {
		paths = paths_found_initially;

		search_engines[t] = new SingleTypeSearch(t, start_locations[t], goal_locations[t],
			NULL,
			ml.get_map(), ml.rows, ml.cols, ml.actions_offset,
			&egr,
			e_w,
			tweak_g_val);

		for (this->current_makespan; this->current_makespan <= max_makespan; this->current_makespan++) {
			if (search_engines[t]->findPath(paths, current_makespan) == true) {
				paths_found_initially[t] = new vector<vector<int> >(*search_engines[t]->getPath());
				break;
			}
		}
		if (current_makespan == max_makespan) {
			all_agents_found_path = false;
		}

	}

	pre_time = (std::clock() - start);

	if (all_agents_found_path == false) {
		cout << "NO SOLUTION EXISTS";
	}

	paths = paths_found_initially;


	dummy_start = new CBSNode();
	dummy_start->type_id = -1;
	dummy_start->g_val = current_makespan;//compute_g_val(num_of_types);
	dummy_start->open_handle = heap.push(dummy_start);
	current_makespan = dummy_start->g_val;
}
예제 #2
0
ECBSSearch::ECBSSearch(const MapLoader& ml, const AgentsLoader& al, const EgraphReader& egr, double e_w, double f_w, bool tweak_g_val) {
  focal_w = f_w;
  HL_num_expanded = 0;
  HL_num_generated = 0;
  LL_num_expanded = 0;
  LL_num_generated = 0;
  num_of_agents = al.num_of_agents;
  map_size = ml.rows*ml.cols;
  solution_found = false;
  solution_cost = -1;
  ll_min_f_vals = vector <double> (num_of_agents);
  paths_costs = vector <double> (num_of_agents);
  ll_min_f_vals_found_initially = vector <double> (num_of_agents);
  paths_costs_found_initially = vector <double> (num_of_agents);
  search_engines = vector < SingleAgentECBS* > (num_of_agents);
  for (int i = 0; i < num_of_agents; i++) {
    int init_loc = ml.linearize_coordinate((al.initial_locations[i]).first, (al.initial_locations[i]).second);
    int goal_loc = ml.linearize_coordinate((al.goal_locations[i]).first, (al.goal_locations[i]).second);
    ComputeHeuristic ch(goal_loc, ml.get_map(), ml.rows*ml.cols, ml.moves_offset, ml.actions_offset, e_w, &egr);
    search_engines[i] = new SingleAgentECBS(init_loc, goal_loc, MapLoader::orientation_t::FACE_EAST,
                                            ch.getHVals(),
                                            ml.get_map(), ml.rows*ml.cols,
                                            ml.moves_offset, ml.actions_offset,
                                            &egr,
                                            e_w,
                                            tweak_g_val);
  }

  // initialize allNodes_table (hash table)
  empty_node = new ECBSNode();
  empty_node->time_generated = -2; empty_node->agent_id = -2;
  deleted_node = new ECBSNode();
  deleted_node->time_generated = -3; deleted_node->agent_id = -3;
  allNodes_table.set_empty_key(empty_node);
  allNodes_table.set_deleted_key(deleted_node);

  // initialize all initial paths to NULL
  paths_found_initially.resize(num_of_agents);
  for (int ag=0; ag < num_of_agents; ag++)
    paths_found_initially[ag] = NULL;

  // initialize paths_found_initially
  for (int i = 0; i < num_of_agents; i++) {
    //    cout << "Computing initial path for agent " << i << endl; fflush(stdout);
    paths = paths_found_initially;
    size_t max_plan_len = getPathsMaxLength();
    bool* res_table = new bool[map_size * max_plan_len]();  // initialized to false
    updateReservationTable(res_table, max_plan_len, i);
    //    cout << "*** CALCULATING INIT PATH FOR AGENT " << i << ". Reservation Table[MAP_SIZE x MAX_PLAN_LEN]: " << endl;
    //    printResTable(res_table, max_plan_len);
    if ( search_engines[i]->findPath ( f_w, NULL, res_table, max_plan_len ) == false)
      cout << "NO SOLUTION EXISTS";
    paths_found_initially[i] = new vector<pathEntry> (*(search_engines[i]->getPath()));
    ll_min_f_vals_found_initially[i] = search_engines[i]->min_f_val;
    paths_costs_found_initially[i] = search_engines[i]->path_cost;
    LL_num_expanded += search_engines[i]->num_expanded;
    LL_num_generated += search_engines[i]->num_generated;
    delete[] res_table;
    //    cout << endl;
  }

  paths = paths_found_initially;
  ll_min_f_vals = ll_min_f_vals_found_initially;
  paths_costs = paths_costs_found_initially;

  // generate dummy start and update data structures
  dummy_start = new ECBSNode();
  dummy_start->agent_id = -1;
  dummy_start->g_val = 0;
  for (int i = 0; i < num_of_agents; i++)
    dummy_start->g_val += paths_costs[i];
  dummy_start->ll_min_f_val = 0;
  dummy_start->sum_min_f_vals = compute_hl_lower_bound();
  dummy_start->open_handle = open_list.push(dummy_start);
  dummy_start->focal_handle = focal_list.push(dummy_start);
  HL_num_generated++;
  dummy_start->time_generated = HL_num_generated;
  allNodes_table[dummy_start] = dummy_start;

  min_sum_f_vals = dummy_start->sum_min_f_vals;
  focal_list_threshold = focal_w * dummy_start->sum_min_f_vals;

  //  cout << "Paths in START (high-level) node:" << endl;
  //  printPaths();
  //  cout << "SUM-MIN-F-VALS: " << dummy_start->sum_min_f_vals << endl;
}