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