void ECBSSearch::printPaths(const MapLoader& ml) {
for (size_t i = 0; i < paths.size(); i++) {
cout << "AGENT " << i << " Path: ";
for (auto& entry : *paths[i]) {
std::cout << entry.location << " " << entry.action << ", ";
}
cout << endl;
}
{
using namespace pt;
ptree pt;
ptree agents;
for (size_t ag = 0; ag < paths.size(); ag++) {
ptree agent;
std::stringstream sstr;
sstr << "create" << ag + 1;
agent.put("name", sstr.str());
ptree path;
size_t t = 0;
for (auto& entry : *paths[ag]) {
ptree pathentry;
pathentry.put("x", ml.col_coordinate(entry.location));
pathentry.put("y", ml.row_coordinate(entry.location));
pathentry.put("theta", theta(entry.orientation));
pathentry.put("arrival", t);
path.push_back(std::make_pair("", pathentry));
++t;
}
agent.add_child("path", path);
agents.push_back(std::make_pair("", agent));
}
pt.add_child("agents", agents);
write_json("schedule_discrete.json", pt);
}
}
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;
}