python宽度优先搜索算法并输出路径

[hadoop@iZ25s7cmfyrZ test]$ cat uda_city.py
# ----------

# ----------

# Grid format:
#   0 = Navigable space
#   1 = Occupied space
import numpy as np

grid = [[0, 0, 1, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 1, 1, 1, 0],
[0, 0, 0, 0, 1, 0]]
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1

delta = [[-1, 0], # go up
[ 0,-1], # go left
[ 0, 1], # go right
[ 1, 0]] # go down

#delta_name = ['^', '<', 'v', '>']
delta_name = ['^', '<', '>', 'v']

def search(grid,init,goal,cost):
# ----------------------------------------
# insert code here
# ----------------------------------------
closed=[[0 for _ in range(len(grid[0]))] for _ in range(len(grid))]
action=[[-1 for row in range(len(grid[0]))] for col in range(len(grid))]

x=init[0]
y=init[1]
g=0
closed[x][y]=1
open=[[g,x,y]]
found=False
resign=False
grid[x][y]=2
n=0

while found is False and resign is False:
#print open
n=n+1
#print "*"*20
#print n
#print np.array(grid)
if len(open)==0:
resign=True
print "fail"
else:
##remove node from list
open.sort()
open.reverse()
next=open.pop()
## take
x=next[1]
y=next[2]
g=next[0]
#print g
if x==goal[0] and y==goal[1]:
found=True
print next
else:
for i in range(len(delta)):
x2=x+delta[i][0]
y2=y+delta[i][1]
#print i, delta[i], x2, y2
#print x2, y2
if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]):
if grid[x2][y2]==0 and closed[x2][y2]==0:
g2=g+cost
open.append([g2, x2, y2])
grid[x2][y2]=2
action[x2][y2]=i
closed[x2][y2]=1
return action
print np.array(grid)
action=search(grid,init,goal,cost)   ##注意action中存储的在上一时刻位置 t-1 经过何种动作到达本位置
print np.array(action)
policy=[[" " for row in range(len(grid[0]))] for col in range(len(grid))]  ## policy 中存储的是在当前位置经过何种动作到达下一位置；即由t到t+1
x=goal[0]
y=goal[1]
policy[x][y]="*"
while x!=init[0] or y!=init[1]:
x2=x-delta[action[x][y]][0]
y2=y-delta[aciton[x][y]][1]
policy[x2][y2]=delta_name[action[x][y]]
x=x2
y=y2
print np.array(policy)