예제 #1
0
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);

    }


}
예제 #2
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;
}