def run(self, start, goal): self.pathCost[functions.getVertexID(start[0], start[1])]=0 #start following algorithm self.parent[functions.getVertexID(start[0], start[1])]=functions.getVertexID(start[0], start[1]) #push start into the queue --> heap assign an array as a value to each item. The array has the f(item), and the item's vertex id self.fringe.push([self.pathCost[functions.getVertexID(start[0], start[1])]+functions.getDistanceToGoal(start[0], start[1], goal),self.pathCost[functions.getVertexID(start[0], start[1])], functions.getVertexID(start[0], start[1]), True]) #mark it as "added to self.fringe" by changing thrid # to 1 VertexInfo=self.graph.getVertex(functions.getVertexID(start[0], start[1])) VertexInfo[3]=1 self.graph.setVertex(functions.getVertexID(start[0], start[1]), VertexInfo) #start the A* loop print "\nStarting A*" startTimeA=time.time() while (self.fringe.len)>0: #pop first value of queue --> checkout the beginning of "making a self.graph" to find out what's in the array vrtx=self.fringe.pop() #print "vrtx: " + str(vrtx) if(vrtx==-1): self.pathFound=False break #if it's goal, then break if(vrtx[2]==functions.getVertexID(goal[0], goal[1])): print "A* found path!" print "Time taken: " +str(time.time()-startTimeA) vrtInfo=self.graph.getVertex(vrtx[2]) print "Total cost: " +str(vrtInfo[4]) self.pathFound=True #self.write_results(time.time()-startTimeA, vrtInfo[4], False) break #mark vertex as closed VertexInfo=self.graph.getVertex(vrtx[2]) VertexInfo[2]=0 self.graph.setVertex(vrtx[2], VertexInfo) #get the neighbors ngbrs=[] ngbrs=self.graph.neighbourOf(vrtx[2]) #print "neighbors: " + str(ngbrs) #loop through neighbors for i in range (len(ngbrs)): checkNeighbor=self.graph.getVertex(ngbrs[i]) if(checkNeighbor[2]==1): if(checkNeighbor[3]==0): self.pathCost[ngbrs[i]]=1000000000 self.parent[ngbrs[i]]=-1 cpyfringe=self.fringe self.fringe=self.UpdateVertex(vrtx[2],ngbrs[i], cpyfringe, goal) if(self.pathFound!=True): print "A* did not find path!" return [-1 for x in range(self.graph.getNumVertices())] else: return self.parent
parentTheta=[-1 for x in range(graph.getNumVertices())] parentV=[-1 for x in range(graph.getNumVertices())] #run A* a_star=A_star.A_star(graph,[]) parent=a_star.run(start, goal) #run Theta* theta_star=Theta_star.Theta_star(graph,obstacles) parentTheta=theta_star.run(start, goal) #check if anyone found path child=functions.getVertexID(goal[0], goal[1]) parents=parent[child] parentT=parentTheta[child] if (parents==-1 and parentT==-1): print "No paths found!" sys.exit() ############################################################################# ############################################################################# ############################################################################# #run A* on visibility graph corners=[start,goal] for i in range (len(obstacles)): value=obstacles[i]
def lineOfSight(self, s, sPrime, obstacles): #print "\nstarting line of sight" lineOk=True temp=self.graph.getVertex(s) s_x=functions.convertColToPixel(temp[0]) s_y=functions.convertRowToPixel(temp[1]) temp=self.graph.getVertex(sPrime) sPrime_x=functions.convertColToPixel(temp[0]) sPrime_y=functions.convertRowToPixel(temp[1]) #print "s's x,y location: " +str(s_x) +", " + str(s_y) #print "sprim's x,y location: " +str(sPrime_x) +", " + str(sPrime_y) if(s<sPrime): loc_x=float(s_x) loc_y=float(s_y) end_x=float(sPrime_x) end_y=float(sPrime_y) else: loc_x=float(sPrime_x) loc_y=float(sPrime_y) end_x=float(s_x) end_y=float(s_y) if(end_x>loc_x): whichWay=True else: whichWay=False if((sPrime_y-s_y)!=0 and (sPrime_x-s_x)!=0): #print "slope!" slope=float((sPrime_y-s_y))/(float(sPrime_x-s_x)) b=s_y-(slope*s_x) #print "b: " +str(b) + " slope: " +str(slope) while(not(round(loc_x,10)==round(end_x,10) and round(loc_y, 10)==round(end_y, 10))): #print "Pix_x: "+str(loc_x) + " Pix_y: " + str(loc_y) #print "Pix_x: "+str(end_x) + " Pix_y: " + str(end_y) #print "Pix_x: "+str(round(loc_x,10)==round(end_x,10)) + " Pix_y: " + str(round(loc_y, 10)==round(end_y, 10)) if(((loc_x%1.0)==0.0 and (loc_x%1.0)==0.0) and int(loc_x)==int(end_x) and int(loc_y)==int(end_y)): break if(float(loc_x)<0 or float(loc_y)<0): print "ERROR:Pixels in negative" sys.exit() x=functions.convertPixelToCol(loc_x) y=functions.convertPixelToRow(loc_y) checkX=x checkY=y #check for blocked if (not((checkX%1.0)==0.0 or (checkY%1.0)==0.0) and (checkX+1)<(colLength-1) and (checkY+1)<(rowLength-1)): #print "x: "+str(x) + " y: " + str(y) + " not whole" #print str(checkX%1.0)+ " " + str(checkY%1.0) #print "x: "+str(math.floor(checkX)) + " y: " + str(math.floor(checkY)) + " not whole" if not(self.graph.getEdge(functions.getVertexID(int(math.floor(checkX)), int(math.floor(checkY))), functions.getVertexID(int(math.floor(checkX+1)), int(math.floor(checkY+1))))>0): #print "doesn't work" return False elif (not((checkX%1.0)==0.0 and (checkY%1.0)==0.0)): for i in range (len(obstacles)): value=[int(math.floor(checkX)),int(math.floor(checkY))] if obstacles[i]==[int(math.floor(checkX)),int(math.floor(checkY))]: return False #update values if(loc_x!=end_x): if(whichWay): loc_x=loc_x+.25 else: loc_x=loc_x-.25 loc_y=(loc_x*slope)+b elif(loc_y!=end_y): loc_y=loc_y+.25 loc_x=(loc_y-b)/slope elif((sPrime_x-s_x)==0): #print "vertical line" while(not(round(loc_x,10)==round(end_x,10) and round(loc_y, 10)==round(end_y, 10))): #print "Pix_x: "+str(loc_x) + " Pix_y: " + str(loc_y) x=functions.convertPixelToCol(loc_x) y=functions.convertPixelToRow(loc_y) checkX=x checkY=y if(((loc_x%1.0)==0.0 and (loc_x%1.0)==0.0) and int(loc_x)==int(end_x) and int(loc_y)==int(end_y)): break if (not((checkX%1.0)==0.0 or (checkY%1.0)==0.0) and (checkX+1)<(colLength-1) and (checkY+1)<(rowLength-1)): #print "x: "+str(x) + " y: " + str(y) + " not whole" #print str(x%1.0)+ " " + str(y%1.0) #print "x: "+str(math.floor(checkX)) + " y: " + str(math.floor(checkY)) + " not whole" if not(self.graph.getEdge(functions.getVertexID(int(math.floor(checkX)), int(math.floor(checkY))), functions.getVertexID(int(math.floor(checkX)+1), int(math.floor(checkY)+1)))>0): #print "doesn't work" return False elif (not((checkX%1.0)==0.0 and (checkY%1.0)==0.0)): for i in range (len(obstacles)): value=[int(math.floor(checkX)),int(math.floor(checkY))] if obstacles[i]==[int(math.floor(checkX)),int(math.floor(checkY))]: return False #update next value loc_y=loc_y+.25 elif((sPrime_y-sPrime_y)==0): #print "horizontal line" while(not(round(loc_x,10)==round(end_x,10) and round(loc_y, 10)==round (end_y, 10))): #print "Pix_x: "+str(loc_x) + " Pix_y: " + str(loc_y) x=functions.convertPixelToCol(loc_x) y=functions.convertPixelToRow(loc_y) checkX=x checkY=y if(((loc_x%1.0)==0.0 and (loc_x%1.0)==0.0) and int(loc_x)==int(end_x) and int(loc_y)==int(end_y)): break #check for blocked if (not((checkX%1.0)==0.0 or (checkY%1.0)==0.0) and (checkX+1)<(colLength-1) and (checkY+1)<(rowLength-1)): #print "x: "+str(x) + " y: " + str(y) + " not whole" #print str(x%1.0)+ " " + str(y%1.0) #print "x: "+str(math.floor(checkX)) + " y: " + str(math.floor(checkY)) + " not whole" if not(self.graph.getEdge(functions.getVertexID(int(math.floor (checkX)), int(math.floor(checkY))), functions.getVertexID (int(math.floor(checkX)+1), int(math.floor(checkY)+1)))>0): #print "doesn't work" return False elif (not((checkX%1.0)==0.0 and (checkY%1.0)==0.0)): for i in range (len(obstacles)): value=[int(math.floor(checkX)),int(math.floor(checkY))] if obstacles[i]==[int(math.floor(checkX)),int(math.floor(checkY))]: return False #update next value loc_x=loc_x+.25 #print "end of line of sight\n" return lineOk