bool AStar::getShortestPath(Node _start, Node _end){
Node goal = _end;
Node start =_start;
came_from_map[start.id] = -1;
addNodeToSet(openSet,start);
while(!(openSet.size() == 0)){
#if DEBUG
printf("Size of open list is %d \n", openSet.size());
#endif
Node current = getMinimumNode(openSet);
#if DEBUG
printf("Expanding node:id= %lld\n", current.id);
#endif
if(current.data == goal.data){
cout<<"\nPath found.Reconstructing the path.\n";
reconstructPath(current.id,0);
return true;
}
removeMinimum(openSet,current);
addNodeToSet(closedSet,current);
#if DEBUG
printf ("Adding node= %lld to Closed multiset\n", current.id);
#endif
vector<Node> nodeNbrs = getNeighbours(current);
for(int i=0;i<nodeNbrs.size();i++){
//nbr is a neighbouring node
Node nbr = nodeNbrs[i];
if(findInSet(closedSet,nbr)){
//TODO: remove the node from the closed and shift in open
#if DEBUG
cout<<"Node found in closed multiset "<< nbr.id<<endl;
#endif
Node nodeInClosedList = getNodeFromSet(closedSet,nbr);
int temp_g_score = current.g_score + distance(current,nbr);
if(temp_g_score < nodeInClosedList.g_score){
printf(" Removing a Node from closed list id %lld in previous %lld \n", nodeInClosedList.id,came_from_map[nodeInClosedList.id]);
printf(" New gscore is %d , old gscore is %d new wannabe parent %lld\n",temp_g_score, nodeInClosedList.g_score,current.id );
printf(" hscore for %lld wannabe parent %d \n" ,current.id , H(current,goal));
//printf(" hscore for %lld previous parent \n" ,current.id , H(current,goal));
// reconstructPath(nodeInClosedList.id],0);
//exit(0);
removeNodeFromSet(closedSet,nodeInClosedList);
}
else{
continue;
}
}
int tentative_g_score = current.g_score + distance(current,nbr);
#if DEBUG
nbr.printData();
#endif
//TODO update this with the follwing logic.
// update if it is found in open multiset with higher g score
// or else it is not in open multiset.
pair<Node,bool> check = findInOpenSet(nbr);
if(check.second && tentative_g_score < check.first.g_score){
removeNodeFromSet(openSet,nbr);
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//check.first.came_from = ¤t;
check.first.g_score = tentative_g_score;
check.first.f_score = tentative_g_score + H(check.first,goal);
addNodeToSet(openSet,check.first);
#if DEBUG
printf("Updating node in open multiset %lld with new parent = %lld\n",check.first.id,current.id);
#endif
}
else if(check.second==false){
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//nbr.came_from = ¤t;
nbr.g_score = tentative_g_score;
nbr.f_score = tentative_g_score + H(nbr,goal);
addNodeToSet(openSet,nbr);
#if DEBUG
printf("Adding node= %lld to Open multiset with f_score %d \n", nbr.id,nbr.f_score);
#endif
}
}
#if DEBUG
cout<<"------------------------------------------------------------------------------------"<<endl;
#endif
}
printf("Solution Not Found. Goal is not reachable from start\n");
return false;
}
void AStar::update(Node current, Node goal){
removeMinimum(openSet,current);
addNodeToSet(closedSet,current);
#if DEBUG
printf("Adding node= %lld to Closed multiset\n", current.id);
#endif
vector<Node> nodeNbrs = getNeighbours(current);
for(int i=0;i<nodeNbrs.size();i++){
//nbr is a neighbouring node
Node nbr = nodeNbrs[i];
if(findInSet(closedSet,nbr)){
//TODO: remove the node from the closed and shift in open
#if DEBUG
cout<<"Node found in closed multiset "<< nbr.id<<endl;
#endif
continue;
}
int tentative_g_score = current.g_score + distance(current,nbr);
#if DEBUG
nbr.printData();
#endif
//TODO update this with the follwing logic.
// update if it is found in open multiset with higher g score
// or else it is not in open multiset.
pair<Node,bool> check = findInOpenSet(nbr);
if(check.second && tentative_g_score < check.first.g_score){
removeNodeFromSet(openSet,nbr);
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//check.first.came_from = ¤t;
check.first.g_score = tentative_g_score;
check.first.f_score = tentative_g_score + H(check.first,goal);
addNodeToSet(openSet,check.first);
#if DEBUG
printf("Updating node in open multiset %lld with new parent = %lld\n",check.first.id,current.id);
#endif
}
else if(check.second==false){
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//nbr.came_from = ¤t;
nbr.g_score = tentative_g_score;
nbr.f_score = tentative_g_score + H(nbr,goal);
addNodeToSet(openSet,nbr);
#if DEBUG
printf("Adding node= %lld to Open multiset with f_score %d \n", nbr.id,nbr.f_score);
#endif
}
}
#if DEBUG
cout<<"------------------------------------------------------------------------------------"<<endl;
#endif
return;
}
void startSearch()
{
int currentNodeID = 0; //Current node while starting is 0
init_Dfs(); //Initialize the stack spine
init_motors();
run();
int nextNodeID;
addNeigbourNodesToFrontier(currentNodeID);
addToSet(&markedNodes,currentNodeID);
push(&spine, currentNodeID);
nextNodeID = DecideNextNode(currentNodeID);
int cnt = 0;
while(1)
{
//scanAndSearch(); //openCV code
if(objectFound == true)
{
// Functionality here TBD
printf("Obj found: %d",cnt);
cnt ++;
rotate360();
objectFound = false;
/* while(isObjectClose() == 0)
moveForward(); // Provided by the motor library
if(distanceTravelledFromPrevNode == distanceToNextNode)
addToSet(&markedNodes,currentNodeID);
//?? Who updates the current position of the object
*/
//Keep track of the distance so that obj position is known
//Keep track of where we are in the graph
}
else
{
printf("Spine. \n");
printStack(&spine);
printf("\n\n");
printf("Next node to be traversed: %d\n", nextNodeID);
moveToNextNode(currentNodeID,nextNodeID); //Move to the next node
currentNodeID = nextNodeID;
addToSet(&markedNodes,currentNodeID); //Current Node has been traversed. Add to the set
addNeigbourNodesToFrontier(currentNodeID); //Adding frontier nodes from the new current node
removeNodeFromSet(&frontierNodes,currentNodeID); //Removing the current node from frontier
printf("Done nodes : \n");
listSet(markedNodes);
printf("Frontier Nodes : \n");
listSet(frontierNodes);
if (checkIfNeighborExistsInFrontier(currentNodeID)) //Check if neighbor exists in frontier
{ // If so visit there
push(&spine, currentNodeID); //Change
nextNodeID = DecideNextNode(currentNodeID);
}
else
{
if(isStackEmpty(&spine))
return ; // Done traversing. Stop
else
{
nextNodeID = pop(&spine);
}
}
}
}
}
