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