python搜索路径

代码一：

#create grid
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]
#up left down right
dir = [[0, -1],
      [-1, 0],
      [0, 1],
      [1, 0]]
gird_row=len(grid)
gird_col=len(grid[0])


def search(gird, init, goal):
    if init == goal:
        return [0, init[0], init[1]]
    #未扩展的标记0，已经扩展的标记1
    expanded_grid=[[0,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0]]
    #初始节点标记
    expanded_grid[init[0]][init[1]] = 2
    # expanded node
    open_list = [[0,init[0],init[1]]]
    # cost,list of(x,y)
    path = []
    cost = 1
    #loop
    while open_list != []:
            g = open_list[0][0]
            x = open_list[0][1]
            y = open_list[0][2]
            #弹出最小的g
            path.append(open_list[0])
            del open_list[0]
            if x != goal[0] or y != goal[1]:
                for i in dir:
                    x2 = x+i[0]
                    y2 = y+i[1]
                    # 移动的时候越界判断&障碍物判断&已经扩展过的判断
                    if (x2 <= gird_row-1) and (x2 >= 0) and (y2 <= gird_col-1) and (y2 >= 0) and (gird[x2][y2] == 0) and (expanded_grid[x2][y2] == 0):
                        g2 = g+cost
                        open_list.append([g2, x2, y2])
                        expanded_grid[x2][y2] = 2
                        #将g按从小到大的顺序排列
                        open_list.sort()
            elif open_list == []:
                return path
            else:
                pass
    # print("There is no vaild path!")
    return path
path=(search(grid,init,goal))
for i in path:
    print(i)

 代码二：

#create grid
grid=[[0,0,1,0,0,0],
      [0,0,1,0,0,0],
      [0,0,1,0,0,0],
      [0,0,1,0,0,0],
      [0,0,0,0,0,0]]
init = [0, 0]
goal = [len(grid)-1,len(grid[0])-1]
#up left down right
dir = [[0, -1],
      [-1, 0],
      [0, 1],
      [1, 0]]
gird_row=len(grid)
gird_col=len(grid[0])


def search(gird, init, goal):
    step = 1
    if init == goal:
        return 0
    #未扩展的标记0，已经扩展的标记1
    expanded_grid=[[0, 0, 0, 0, 0, 0],
                   [0, 0, 0, 0, 0, 0],
                   [0, 0, 0, 0, 0, 0],
                   [0, 0, 0, 0, 0, 0],
                   [0, 0, 0, 0, 0, 0]]
    for row in range(len(grid)):
        for col in range(len(grid[0])):
            if grid[row][col]==1:
                expanded_grid[row][col]='x'
    #初始节点标记
    expanded_grid[init[0]][init[1]] = 1
    # expanded node
    open_list = [[0,init[0],init[1]]]
    # cost,list of(x,y)
    path = []
    cost = 1
    #loop
    while open_list != []:
            g = open_list[0][0]
            x = open_list[0][1]
            y = open_list[0][2]
            #弹出最小得g
            path.append(open_list[0])
            del open_list[0]
            if x != goal[0] or y != goal[1]:
                for i in dir:
                    x2 = x+i[0]
                    y2 = y+i[1]
                    # 移动的时候越界判断&障碍物判断&已经扩展过的判断
                    if (x2 <= gird_row-1) and (x2 >= 0) and (y2 <= gird_col-1) and (y2 >= 0) and (gird[x2][y2] == 0) and (expanded_grid[x2][y2] == 0):
                        g2 = g+cost
                        open_list.append([g2, x2, y2])
                        step += 1
                        expanded_grid[x2][y2] = step
                        #将g按从小到大得顺序排列
                        open_list.sort()
            elif open_list == []:
                return expanded_grid
            else:
                pass
    # print("There is no vaild path!")
    return expanded_grid
expanded=(search(grid,init,goal))
for i in expanded:
    print(i)

 代码三：

#create grid
grid=[[0,0,0,0,0,0],
      [0,1,1,1,1,0],
      [0,0,1,0,0,0],
      [0,0,1,1,0,1],
      [0,0,0,0,0,0]]

init = [0, 0]
goal = [len(grid)-1,len(grid[0])-1]
#up left down right
dir = [[0, -1],
      [-1, 0],
      [0, 1],
      [1, 0]]
dir_pic=['<','^','>','v']
gird_row=len(grid)
gird_col=len(grid[0])


def search(gird, init, goal):
    if init == goal:
        return [0, init[0], init[1]]
    #未扩展的标记0，已经扩展的标记1
    expanded_grid=[[0,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0],
                   [0,0,0,0,0,0]]
    #标记动作的地图
    action_grid = [[-1, -1, -1, -1, -1, -1],
                   [-1, -1, -1, -1, -1, -1],
                   [-1, -1, -1, -1, -1, -1],
                   [-1, -1, -1, -1, -1, -1],
                   [-1, -1, -1, -1, -1, -1]]
    #初始节点标记
    expanded_grid[init[0]][init[1]] = 2
    # expanded node
    open_list = [[0,init[0],init[1]]]
    # cost,list of(x,y)
    path = []
    cost = 1
    #loop
    while open_list != []:
            g = open_list[0][0]
            x = open_list[0][1]
            y = open_list[0][2]
            #弹出最小得g
            path.append(open_list[0])
            del open_list[0]
            if x != goal[0] or y != goal[1]:
                for i in range(len(dir)):
                    x2 = x+dir[i][0]
                    y2 = y+dir[i][1]
                    # 移动的时候越界判断&障碍物判断&已经扩展过的判断
                    if (x2 <= gird_row-1) and (x2 >= 0) and (y2 <= gird_col-1) and (y2 >= 0) and (gird[x2][y2] == 0) and (expanded_grid[x2][y2] == 0):
                        g2 = g+cost
                        open_list.append([g2, x2, y2])
                        expanded_grid[x2][y2] = 2
                        action_grid[x2][y2]=i
                        #将g按从小到大得顺序排列
                        open_list.sort()
            elif open_list == []:
                return action_grid

    # print("There is no vaild path!")
    return action_grid


def print_path():
    action_res = (search(grid, init, goal))
    policy = [[' ' for col in range(len(grid[0]))] for row in range(len(grid))]
    x = goal[0]
    y = goal[1]
    policy[x][y] = '*'
    while x!=init[0] or y!=init[1]:
        x2=x-dir[action_res[x][y]][0]
        y2=y-dir[action_res[x][y]][1]
        policy[x2][y2]=dir_pic[action_res[x][y]]
        x=x2
        y=y2
    return policy
#test
policy=print_path()
for i in range(len(policy)):
    print(policy[i])