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;
}
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;
}
