shortest path length | Python Fiddle

# ----------

# The code implements Dijkstra's algorithm to find the shortest path length
# If there is no valid path from the start point
# to the goal, the result displays 'fail'
#
# Hao Zhong, 2015, www.zh-anse.com
# ----------

# Grid format:
#   0 = Navigable space
#   1 = Occupied space
# test input
grid = [[0, 0, 1, 0, 0, 0],
        [0, 0, 1, 0, 0, 0],
        [0, 0, 1, 0, 1, 0],
        [0, 0, 0, 0, 1, 0],
        [0, 0, 1, 0, 1, 0]]

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

# delta is the movement the agent can take
delta = [[-1, 0 ], # go up
        [ 0, -1], # go left
        [ 1, 0 ], # go down
        [ 0, 1 ]] # go right

cost = 1 # cost of one movement

def search(): 
    # find the length of the shortest path
     
    MAX_COST = float('inf')
    
    visited=[] # A grid to show if the locations are visited
    for i in range(len(grid)):
        visited.append([])
        for j in range(len(grid[0])):
            visited[i].append([False, MAX_COST])

curpos = init # current position
    visited[init[0]][init[1]] = [True,0]
    
    templist = [] # a list containing future position to explore
    
    while(curpos!=goal):
        curcost = visited[curpos[0]][curpos[1]][1]
        nextcost = curcost+cost
        
        for k in range(len(delta)):
            newx = curpos[0]+delta[k][0]
            newy = curpos[1]+delta[k][1]
            
            # check if out of range
            if (newx < 0 or newx>=len(grid)):
                continue
            if (newy < 0 or newy>=len(grid[0])):
                continue
            if (grid[newx][newy]==1):
                continue
            # check if visited position
            if (visited[newx][newy][0]==True):
                continue
            # if the new location has never been visited, add it to the templist
            if (visited[newx][newy][1]==MAX_COST):
                templist.append([newx, newy])
            # update the cost
            if (visited[newx][newy][1]>nextcost):
                visited[newx][newy][1] = nextcost
        
        # if no where to explore before reaching the goal, return fail
        if(len(templist)==0):
            return 'fail'
        
        # find the next position with the lowest cost and visit
        lowestcost = MAX_COST
        lcId = -1
        for i, elem in enumerate(templist):
              if visited[elem[0]][elem[1]][1] <=lowestcost:
                   lcId=i
                   lowestcost = visited[elem[0]][elem[1]][1]
        curpos = templist.pop(lcId)
        # mark the position as being visited 
        visited[curpos[0]][curpos[1]][0] = True

return visited[curpos[0]][curpos[1]][1] # return the path length
    
print search()

Python Fiddle

Python Cloud IDE