Esempio n. 1
0
def expand(x,y,Cnode):
    global nodeHeap
    global nodeGrid
    global grid
    if x> 0 and y > 0 and x < len(grid) and y < len(grid[0]):
        if grid[x][y] == 0 and nodeGrid[x][y] != 1:
            nodeGrid[x][y] = 1
#            if x  == 20  and y == 20:

#                print "a"
#                print goal.x
#                print goal.y
#                print x
#                print y
#            for j in nodeGrid:
#                for l in j:
#                    if l == 1 :
#                        print l ,   
#                    else:
#                        print " ", 
#                print " "
#            for j in grid:
#                for l in j:
#                    if l == 1 :
#                        print "#" ,   
#                    else:
#                        print " ", 
#                print " "
            
            Nnode = node(x, y , goal, Cnode,Cnode.robot)
            heapq.heappush(nodeHeap,Nnode)
Esempio n. 2
0
def astar(map,bot,Realgoal,path):
    global grid
    grid = map.grid
    global robot
    global nodeHeap
    global goal
    global nodeGrid
    goal = Realgoal
    nodeHeap = list()
    robot = bot
    fill = 0
    nodeGrid = grid.copy()  * 0
    
    #setup 
    Cnode = node(robot.x,robot.y,goal,0,robot)
    NodesOpened=0
    heapq.heappush(nodeHeap, Cnode)
    while(Cnode.x != goal.x or Cnode.y != goal.y):
        NodesOpened=NodesOpened+1
        expandSequence(Cnode)
        if len(nodeHeap) > 0:
            Cnode = heapq.heappop(nodeHeap)
#            for k in nodeHeap:
#                print k.cost
        else:
            print nodeHeap
            print "nodeHeap died :( sad. Astar Failed"
            sys.exit()
#        print goal.x
#        print goal.y
#        print Cnode.x
#        print Cnode.y
    zero = 0
    while Cnode.parent:
        path.add(Cnode.x,Cnode.y,zero)
        zero = zero+1
        Cnode = Cnode.parent
        
    print "COUNT",count