C++ PriorityQueue::get方法代码示例

void a_star_search (Graph graph, typename Graph::Location start, typename Graph::Location goal,
					unordered_map<typename Graph::Location, typename Graph::Location>& came_from,
					unordered_map<typename Graph::Location, int>& cost_so_far)
{
	typedef typename Graph::Location Location;
	PriorityQueue<Location> frontier;
	frontier.put(start, 0);
	came_from[start] = start;
	cost_so_far[start] = 0;
	while (!frontier.empty()) {
		auto current = frontier.get();
		
		if (current == goal) {
			break;
		}

		for (auto next : graph.neighbors(current)) {
			int new_cost = cost_so_far[current] + graph.cost(current, next);
			if (!cost_so_far.count(next) || new_cost < cost_so_far[next]) {
				cost_so_far[next] = new_cost;
				int priority = new_cost + heuristic(next, goal);
				frontier.put(next, priority);
				came_from[next] = current;
			}
		}
	}
}

void a_star_search
  (
	void (*get_map_neighbors)( Pair<int> ), // pointer to static member that gives us neighbors
   Pair<int> start,
   Pair<int> goal,
   unordered_map< Pair<int>, Pair<int> >& came_from,
   unordered_map< Pair<int>, int >& cost_so_far)
{
  PriorityQueue< Pair<int> > frontier;
  frontier.put(start, 0);

  came_from[start] = start;
  cost_so_far[start] = 0;

  while (!frontier.empty()) {
    Pair<int> current = frontier.get();

    if (current == goal) {
      break;
    }

    for (Pair<int> next : (*get_map_neighbors)(current)) {
    	/* this number 1 is the cost to move - no weighting scheme is being used right now
    	 */
      int new_cost = cost_so_far[current] + 1;

      if (!cost_so_far.count(next) || new_cost < cost_so_far[next]) {
        cost_so_far[next] = new_cost;
        int priority = new_cost + heuristic(next, goal);
        frontier.put(next, priority);
        came_from[next] = current;
      }
    }
  }
}

ActionPlan Planner::Plan2(){
  ActionPlan result;
  auto curr = preConds;
  auto goal = postConds;

  ActionPtr nullAction = Action::ActionFactory("NULL",{},{},0,0);
  ActionCondition start = {curr, nullAction};

  PriorityQueue<ActionNodePtr> acs;
  auto curr_ac  = std::make_shared<ActionNode>();
  curr_ac->action = nullAction;
  curr_ac->conds = curr;
  curr_ac->cost_so_far = 0;
  curr_ac->parent = nullptr;
  
  acs.put(curr_ac,0);
  bool success = false;
  while(!acs.empty()){
    curr_ac  = acs.get();

    if(!satisfies(curr_ac->conds, goal)){
      for(auto& ac : findActions(actions, curr_ac->conds)){
	bool skip = false;
	auto temp_ac = curr_ac;
	while(temp_ac->action->getName() != nullAction->getName()){
	  temp_ac = temp_ac->parent;
	  if(ac.first == temp_ac->conds){
	    skip=true;
	    break;
	  }
	}
	if(skip)
	  continue;
	auto node = std::make_shared<ActionNode>();
	node->action = ac.second;
	node->conds = ac.first;
	node->cost_so_far = ac.second->getCost()+curr_ac->cost_so_far;
	node->parent = curr_ac;
	acs.put(node,node->cost_so_far);
      }
    }
    else{
      success=true;
      break;
    }
  }
  std::cout <<"PLANNED " << std::boolalpha << success << "\n";
  if(!success)
    return result;
  while(curr_ac != nullptr){
    result.push_back(curr_ac->action);
    //std::cout << curr_ac->action << " ";
    curr_ac = curr_ac->parent;
  }
     
  std::reverse(result.begin(), result.end());
  return result;
}

/* Get shortest path as array of nodes in the graph by using the A Star algorithm */
vector<std::shared_ptr<Node>> Graph::aStar(std::shared_ptr<Node> start, std::shared_ptr<Node> goal)
{
	PriorityQueue<std::shared_ptr<Node>> frontier; // List of open, not yet evaluated nodes
	frontier.put(start, 0);			// Place starting node in that list

	map<std::shared_ptr<Node>, std::shared_ptr<Node>> cameFrom; // Mapping of visited nodes and from where we reached them
	map<std::shared_ptr<Node>, int> costSoFar; // Mapping of costs to get to a specific node

	cameFrom[start] = start;
	costSoFar[start] = 0;

	/* Find goal node by repeatedly searching the frontier and expanding it */
	while (!frontier.empty())
	{
		auto current = frontier.get(); // Get top priority node and pop from queue

									   // If current node is goal then stop for it has been reached
		if (current == goal) { break; }

		// For all edges coming from current node
		for (std::shared_ptr<Edge> myEdge : edgeListVector[current->getIndex()])
		{
			// Calculate cost to get from current node to next
			int newCost = costSoFar[current] + myEdge->getCost();

			// If either we haven't visited connected node yet or if cost is less
			if (!costSoFar.count(myEdge->getTo()) || newCost < costSoFar[myEdge->getTo()])
			{
				costSoFar[myEdge->getTo()] = newCost;	// Set cost to get to the node
				int priority = newCost + heuristic(myEdge->getTo(), goal);
				frontier.put(myEdge->getTo(), priority);		// Push node onto frontier
				cameFrom[myEdge->getTo()] = current;	// Set from where we reached the node
			}
		}
	}
	std::shared_ptr<Node> current = goal;

	/* Goal is found. Now we need to establish a path towards it */
	vector<std::shared_ptr<Node>> path;
	path.push_back(current);

	// Walk back along trail and push nodes until start is reached
	while (current != start)
	{
		current = cameFrom[current];
		path.push_back(current);
	}

	reverse(path.begin(), path.end()); // Reverse path to get correct order


	return path;
}

void AStarSearcher::aStarSearch(MapHolderAStar mapGraph, Location start, Location target,
								LocationMatrix& parentsMap, IntMatrix& costMap)
{
	// Define the sizes of the matrices
	parentsMap.DefineSize(mapGraph._mapObj._map._height, mapGraph._mapObj._map._width);
	costMap.DefineSize(mapGraph._mapObj._map._height, mapGraph._mapObj._map._width);
	costMap._defaultValue = 0;

	PriorityQueue frontier;

	// initialize the start node
	frontier.put(start, 0);
	parentsMap._matrix[start.getY()][start.getX()] = start;
	costMap._matrix[start.getY()][start.getX()] = 0;

	// Perform the search
	while (!frontier.empty())
	{
		Location current = frontier.get();
		vector<Location> neighbors = mapGraph.getNeighborsOfLocation(current);

		// Run over all the neighbors of the current location
		for (int i = 0; i < neighbors.size(); i++)
		{
			Location next = neighbors[i];

			// Calculating the new cost of the path to the current neighbor
			int newCost = costMap._matrix[current.getY()][current.getX()] + mapGraph.getCostOfPath(current,next);

			// Checking if we didn't visit yet in the next node or the new cost is lower than the existing
			// cost of the next node
			if ((costMap.isPositionDefault(next)) || (newCost < costMap._matrix[next.getY()][next.getX()]))
			{
				costMap._matrix[next.getY()][next.getX()] = newCost;

				// Get the new priority of the node
				int priority = newCost + AStarSearcher::heuristic(next, target);

				frontier.put(next, priority);
				parentsMap._matrix[next.getY()][next.getX()] = current;
			}
		}
	}

	// Checking if we didn't reach the target
	if (parentsMap.isPositionDefault(target))
	{
		cout << "Target wasn't reached :(" << endl;
	}
}