Always getting the same path with A* implementation

I'm trying to implementing A* from the pseudo code from wikipedia however I'm getting some weird results.

The implementation finds what at first looks like a good path, but with a further look it always produces the same path!

Can anyone spot anything wrong? The code is written in python 3.1 and uses pygame.

import pygame
import sys, traceback
import random
import math

TILE_WIDTH =  30
TILE_HEIGHT = 30
NUM_TILES_X = 30
NUM_TILES_Y = 30
NUM_TILES = NUM_TILES_X * NUM_TILES_Y
GRID_WIDTH = TILE_WIDTH * NUM_TILES_X
GRID_HEIGHT = TILE_HEIGHT * NUM_TILES_Y



# h(x,y)
def heuristic_dist(source,dest):
    return int(( (source.x - dest.x)**2 + (source.y - dest.y)**2 ) **0.5)

def a_star(nodes,start,goal):

     # Set up data structures
     closedset = []
     openset = [start]
     came_from={}
     g_score = {}
     g_score[start.index] = 0
     h_score = {}
     h_score[start.index] = heuristic_dist(start,goal)
     f_score = {}
     f_score[start.index] = h_score[start.index]


     while len(openset) > 0:

        # Find node with least f_score in openset
        x = min(openset,key=lambda el:f_score[el.index])


        # We have reached our goal!
        if x.index == goal.index:

            path = reconstruct_path(came_from,goal.index)

            # Mark the path with green color
            for node in path:
                nodes[node].color=(0,255,0)
            print( "Yihaaa!" )
            return True

        # Filter out x from openset and add it to closedset
        openset = list(filter(lambda y:y.index!=x.index,openset))
        closedset.append(x)
        # Go through all neighbours
        for y in x.get_neighbours():

            # If this neighbour has been closed, skip it
            if y in closedset: continue

            # Not sure that this is correct.
            tentative_g_score = g_score[x.index] + heuristic_dist(x,y)

            if y not in openset:
                openset.append(y)
                tentative_is_better = True
            elif tentative_g_score < g_score[y.index]:
                 tentative_is_better = True
            else:
                tentative_is_better = False
            if tentative_is_better:
                if y.index in came_from:
                    if f_score[x.index] < f_score[came_from[y].index]:
                        came_from[y.index] = x
                else:
                    came_from[y.index] = x
                g_score[y.index] = tentative_g_score
                h_score[y.index] = heuristic_dist(y, goal)
                f_score[y.index] = g_score[y.index] + h_score[y.index]
     print("Couldn't find a path!")
     return False





# Traverse the path backwards
def reconstruct_path(came_from,current_node,depth=0):
    if current_node in came_from:
        p = reconstruct_path(came_from,came_from[current_node].index)
        return p + [current_node]
    else:
        return [current_node]



def draw_string(surface,string,x,y):
    s = font.render(string,True,(0,0,0))
    surface.blit(s,(x,y))

# Tile or Node that has a cuple of attributes: color, cost and x,y
class Tile:
    def __init__(self,x,y,cost,index):
        self.x=x
        self.y=y
        self.cost=cost
        self.index=index
        self.color = (255,255,255)
    def draw(self,surface):
        surface.fill(self.color,pygame.Rect(self.x*TILE_WIDTH,self.y*TILE_HEIGHT,TILE_WIDTH,TILE_HEIGHT))
        pygame.draw.rect(surface,(255, 180, 180),pygame.Rect(self.x*TILE_WIDTH,self.y*TILE_HEIGHT,TILE_WIDTH,TILE_HEIGHT),2)
        draw_string(surface,str(self.cost),self.x*TILE_WIDTH+TILE_WIDTH//3,self.y*TILE_HEIGHT+TILE_HEIGHT//3)
    def get_neighbours(self):
        nbs = []
        # Where are our neighbours?
        offsets = [(0,-1),(-1,0),(1,0),(0,1)]
        for offset in offsets:
            x = self.x + offset[0]
            y = self.y + offset[1]
            try: # coord_to_tile throws exception if no such neighbour exists (out of bounds for example)
                nbs.append(coord_to_tile(x,y))
   开发者_如何学C         except Exception as e:
                pass
        return nbs
    def __eq__(self,other):
        return self.x == other.x and self.y==other.y




# Small helper function to convert x,y coords to a tile instance
nodes_lookup={}
def coord_to_tile(x,y):
    return nodes_lookup[(x,y)]

def main():
    global nodes_lookup

    screen = pygame.display.set_mode((GRID_WIDTH, GRID_HEIGHT))


    tiles = []
    for x in range(NUM_TILES_X):
        for y in range(NUM_TILES_Y):
            # Create a random distribution where max grows
            cost = random.randint(1,min(x*y,98)+1)

            # Let the bottom line cost 1 as well
            if y == NUM_TILES_Y-1: cost = 1

            t = Tile(x,y,cost,len(tiles))
            nodes_lookup[(x,y)] = t
            tiles.append(t)


    # Do a*
    a_star(tiles,tiles[0],tiles[len(tiles)-1])



    while True:
        event = pygame.event.wait()
        if event.type == pygame.QUIT:
            break

        for tile in tiles:
            tile.draw(screen)

        pygame.display.flip()


pygame.init()
font = pygame.font.SysFont("Times New Roman",18)
try:
    main()
except Exception as e:
    tb = sys.exc_info()[2]
    traceback.print_exception(e.__class__, e, tb)
pygame.quit()

I really have no clue, since I think I have pretty much implemented the pseudo code statement by statement.

Here's a screenshot as well: http://andhen.mine.nu/uploads/astar.dib

Thanks!

You access came_from on time with y, and one time with y.index in

     if tentative_is_better:
            if y.index in came_from:
                if f_score[x.index] < f_score[came_from[y].index]: // index by y
                    came_from[y.index] = x // index by y.index
            else:

You probably meant

if f_score[x.index] < f_score[came_from[y.index].index]:

in the first line.

Besides that, the code looks ok.

Anyway, what do you mean by always produces the same path? The algorithm is supposed to return the optimal path which should always be the same... (or did you mean, it always produces the same path independently of start and goal?)`

EDIT:

You don't use your random cost anywhere in the algorithm. The 'costs' the algorithm is using are always the distance between two adjacent nodes: They are defined in heuristic_distance and used in the line

tentative_g_score = g_score[x.index] + heuristic_dist(x,y)

If you want to define random costs, you must first realize that this algorithm assigns costs to edges, not to vertices. You'll have to define some function real_costs(x,y) which calculates the costs for going from node x to node y and use this cost function instead of heuristic_dist in the above line.