def run(self, src, start, end, NDIR=4, steps=-1): """ src = ndarray start = point tuple (y,x) end = point tuple (y,x) NDIR = 4/8, 8 includes diagonals steps = Limit number of steps """ iDir = [0] * NDIR jDir = [0] * NDIR four = 4 eight = 8 if(NDIR == four): yDir = [1, 0, -1, 0] xDir = [0, 1, 0, -1] for i in range(0, NDIR): iDir[i] = yDir[i] jDir[i] = xDir[i] if(NDIR == eight): yDir = [1, 1, 0, -1, -1, -1, 0, 1] xDir = [0, 1, 1, 1, 0, -1, -1, -1] for i in range(0, NDIR): iDir[i] = yDir[i] jDir[i] = xDir[i] openNodes = np.zeros(src.shape, np.int) closeNodes = np.zeros(src.shape, np.int) # map of directions (0: Right, 1: Up, 2: Left, 3: Down) dirMap = np.zeros(src.shape, np.int) pq1 = PriorityQueue() pq2 = PriorityQueue() q = [pq1, pq2] qi = 0 row = col = 0 results = cv2.cvtColor(src,cv2.COLOR_GRAY2BGR) results[start] = (0,0,255) results[end] = (255,0,0) self.pointVec = [] self.pointVec.append(start) self.pathFound = False # create the start node and push into list of open nodes pNode1 = Node(start, 0, 0) pNode1.calculateFValue(end) q[qi].put(pNode1) while not q[qi].empty(): # get the current node w/ the lowest FValue # from the list of open nodes pNode1 = Node( q[qi].queue[0].getPos(), q[qi].queue[0].getGValue(), q[qi].queue[0].getFValue()) row = pNode1.getPos()[0] col = pNode1.getPos()[1] #print "{}) {},{} - {},{} - {}, {}".format(i,col,row,jNext,iNext,pNode1.getGValue(),pNode1.getFValue()) if(pNode1.getGValue()==steps): return results # mark the node on the open list q[qi].get() openNodes[row][col] = 0 # mark it on the closed nodes list closeNodes[row][col] = 1 # stop searching when the goal state is reached if(row == end[0] and col == end[1]): # generate the path from finish to start from dirMap while( not (row == start[0] and col == start[1]) ): j = dirMap[row][col] row += iDir[j] col += jDir[j] if(row!=start[0] and col!=start[1]): results[row,col] = (0,255,0) self.pointVec.append((row,col)) self.pointVec.append(end) # empty the leftover nodes while not q[qi].empty(): q[qi].get() self.pathFound = True return results # generate moves in all possible directions for i in range(0, NDIR): iNext = row + iDir[i] jNext = col + jDir[i] # if not wall (obstacle) nor in the closed list if( not (iNext<0 or iNext>src.shape[0]-1 or jNext<0 or jNext>src.shape[1]-1 or src[iNext,jNext]==0 or closeNodes[iNext][jNext] == 1) ): # generate a child node pNode2 = Node( (iNext, jNext), pNode1.getGValue(), pNode1.getFValue()) pNode2.updateGValue(i) pNode2.calculateFValue(end) # if it is not in the open list then add into that if(openNodes[iNext][jNext] == 0): openNodes[iNext][jNext] = pNode2.getFValue() q[qi].put(pNode2) # mark its parent node direction dirMap[iNext][jNext] = (i + NDIR/2) % NDIR # already in the open list elif(openNodes[iNext][jNext] > pNode2.getFValue()): # update the FValue info openNodes[iNext][jNext] = pNode2.getFValue() # update the parent direction info, mark its parent node direction dirMap[iNext][jNext] = (i + NDIR/2) % NDIR # replace the node by emptying one q to the other one # except the node to be replaced will be ignored # and the new node will be pushed in instead while( not (q[qi].queue[0].getPos()[0] == iNext and q[qi].queue[0].getPos()[1] == jNext)): q[1-qi].put(q[qi].queue[0]) q[qi].get() # remove the wanted node q[qi].get() # empty the larger size q to the smaller one if(len(q[qi].queue) > len(q[1 - qi].queue)): qi = 1 - qi while not q[qi].empty(): q[1 - qi].put(q[qi].queue[0]) q[qi].get() qi = 1 - qi # add the better node instead q[qi].put(pNode2) # end if not obstacle or closed list # end for i < NDIR # end while not empty return results