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