#This is an example of the A* pathfinding algorithm I wrote in Python. This
#program is primarly based on Abraham L. Howell's ER1 Mapper made specifically
#for the ER1 robot. His program was written in Visual Basic, so I tried to
#"translate" it to Python as best I can. All credit goes to him for the basic
#code I used as a general guide to create this. You are free to use, edit and
#distribute this as long as credit is given where appropriate.
#
#The code is very basic right now. You create a map in the grid below; it can
#be any size as long as if it stays a rectangle or square (it does not have to
#be one, but it is significantly easier if it is). This grid is basically a
#list within a list: lists a through e are the rows and they are in list "map".
#The bottom-left cell is reached by typing "map[0][0]". The first x and y
#coordinates are 0 (so if you have 5 rows, the bottom is row 0 and the top is
#row 4). You enter the x and y coordinates of the start and goal cells to the
#"start" and "target" variables appropriately (see below). You then run the
#program and it finds a path to the start to goal cell. It then prints the
#entire map again with the path marked. That is all it does for now; it finds
#a path. Other details about this code can be discovered by reading this code
#or Howell's original code/documentation.
#
#It has some bugs I cannot figure out though. It treats blocked cells as normal
#cells, when it is supposed to ignore them. It also marks every analyzed cell
#as a parent cell, so an actual path cannot be found.
a = [0, 0, 0, 0, 0, 0, 0, 0]
b = [1, 1, 1, 1, 1, 1, 1, 0] #0 means cell is blocked and 1 means cell is
c = [1, 1, 1, 1, 1, 1, 1, 0] #unblocked. When accessing a cell, type y-
d = [0, 0, 1, 1, 1, 1, 1, 0] #coordinate first, then the x-coordinate. like:
e = [0, 0, 0, 0, 0, 0, 0, 0] #"map[4][0]" to access the top left cell in a.
map = [e, #list 0
d, #list 1
c, #list 2
b, #list 3
a] #list 4
len(map)
openlist = [] #open list is empty
closedlist = [] #closed list is empty
P_list = [] #list of parent cells is empty
targetX = 6 #goal cell X coordinate
targetY = 1 #goal cell Y coordinate
startX = 0 #start X coordiante
startY = 3 #start Y coordinate
Dg = 1 #cell heuristic constants; Dg>Dh, then
Dh = 1 #the algrithm searches more cells for
#a more optimal path. If Dh>Dg, then it searches less cells, finds a path
#quicker, but it is much less optimal. Adjust these values to see which work best.
maxValX = 8 #x value of a cell cannot be greater
maxValY = 5 #y value of a cell cannot be greater
#--------------------------------------------------------------------
#this part of the code establishes the telnet connection to the RCC and is
#independent and unrelated to the A* algorithm or finding a path (it is only
#used to physically move the robot after the path is found).
import telnetlib
import time
import string
HOST = "localhost"
print "\nOpening Telnet session to ER1 GUI."
tn = telnetlib.Telnet(HOST, 9000)
print "Opened session.\n"
def evocon (cmd, timeout=1):
a=''
if cmd=='events':
tn.write("%s\n" % cmd)
time.sleep(.1)
a=tn.read_until("\n", timeout)
print a
if a != '':
while a!= '':
a=tn.read_until("\n", timeout)
print a
tn.write("%s\n" % cmd)
return tn.read_until("\n", timeout)
#---------------------------------------------------------------------
#These are the mathamatical heuristics, used to calculate the so-called G,
#H and F scores of a cell.
def G(a):
answerG = Dg * (abs(a[0] - startX) + abs(a[1] - startY))
return answerG
def H(a):
Hscore = Dh * (abs(a[0] - targetX) + abs(a[1] - targetY))
return Hscore
def F(a):
Fscore = G(a) + H(a)
return Fscore
#----------------------------------------------------------------------
#When the goal cell is found, print map with marked cells. I'm having trouble
#with parent cells, so it just prints a map with all the cells analyzed.
def ShowPath():
print "Path found"
for j in P_list: #mark each parent cell with integer 4.
map[j[1]][j[0]] = 4 #The trail of 4's is the found path.
map.reverse() #reverse list "map", so when printed in the
for i in map: #for: loop, it comes out right-side up
print i
def NoSolutionFound():
print "No path found, the goal cell is unreachable"
#----------------------------------------------------------------------
#This part of the code is the actual A* algorithm.
curCellX = startX #start cell is the current cell
curCellY = startY
closedlist.append([startX, startY]) #add start cell to closed list
rest = 0 #start search. I set up a dummy variable,
while not rest: #so it would loop (I will add a variable
#that counts how many loops passed later)
NcellX = curCellX
NcellY = curCellY + 1
ScellX = curCellX
ScellY = curCellY - 1
EcellX = curCellX + 1
EcellY = curCellY
WcellX = curCellX -1
WcellY = curCellY
#check if north cell is in range, that is, not under 0 or over max value
if NcellX > -1 or NcellY > -1 or NcellX < maxValX or NcellY < maxValY:
#check if it's on the closed list or blocked. Its blocked if the cell
#NcellX and NcellY represent is = 0, open is = 1
if map[NcellY][NcellX] > 0 or closedlist.count([NcellX, NcellY]) != 1:
#if it's not on the openlist, add it and set parent.
if openlist.count([NcellX, NcellY]) != 1:
openlist.append([NcellX, NcellY])
P_list.append([curCellX, curCellY]) #P of N-cell is cur-cell
else:
#check if path from current cell has a lower G value
if G([curCellX, curCellY]) + Dg < G([NcellX, NcellY]):
#if it is, set parent to current cell and recalculate G&F scores
#for this cell
P_list.append([curCellX, curCellY])
G([NcellX, NcellY]) == G([curCellX, curCellY]) + Dg
F([NcellX, NcellY]) == G([NcellX, NcellY]) + H([NcellX, NcellY])
#repeat above, only for south, east and west cells.
if ScellX > -1 or ScellY > -1 or ScellX < maxValX or ScellY < maxValY:
if map[ScellY][ScellX] > 0 or closedlist.count([ScellX, ScellY]) != 1:
if openlist.count([ScellX, ScellY]) != 1:
openlist.append([ScellX, ScellY])
P_list.append([curCellX, curCellY])
else:
if G([curCellX, curCellY]) + Dg < G([ScellX, ScellY]):
P_list.append([curCellX, curCellY])
G([ScellX, ScellY]) == G([curCellX, curCellY]) + Dg
F([ScellX, ScellY]) == G([ScellX, ScellY]) + H([ScellX, ScellY])
if EcellX > -1 or EcellY > -1 or EcellX < maxValX or EcellY < maxValY:
if map[EcellY][EcellX] > 0 or closedlist.count([EcellX, EcellY]) != 1:
if openlist.count([EcellX, EcellY]) != 1:
openlist.append([EcellX, EcellY])
P_list.append([curCellX, curCellY])
else:
if G([curCellX, curCellY]) + Dg < G([EcellX, EcellY]):
P_list.append([curCellX, curCellY])
G([EcellX, EcellY]) == G([curCellX, curCellY]) + Dg
F([EcellX, EcellY]) == G([EcellX, EcellY]) + H([EcellX, EcellY])
if WcellX > -1 or WcellY > -1 or WcellX < maxValX or WcellY < maxValY:
if map[WcellY][WcellX] > 0 or closedlist.count([WcellX, WcellY]) != 1:
if openlist.count([WcellX, WcellY]) != 1:
openlist.append([WcellX, WcellY])
P_list.append([curCellX, curCellY])
else:
if G([curCellX, curCellY]) + Dg < G([WcellX, WcellY]):
P_list.append([curCellX, curCellY])
G([WcellX, WcellY]) == G([curCellX, curCellY]) + Dg
F([WcellX, WcellY]) == G([WcellX, WcellY]) + H([WcellX, WcellY])
if openlist == []: #if openlist is empty, exit
NoSolutionFound() #loop; there is no solution.
break
#now we find the cell on the openlist with the lowest F score and set it
#as the current cell.
for r in openlist:
if F(r) <= F([curCellX, curCellY]):
curCellX = r[0]
curCellY = r[1]
break #I use break here because the F scores of the cells
#are often equal. It picks the first one that it meets.
#add the cell you just selected to the closed list and remove it from the
#open list.
closedlist.append([curCellX, curCellY])
openlist.remove([curCellX, curCellY])
#Find out if the cell you put in the closed list is the target cell
#else, mark the current cell, so when you print "map", you can see it
#was analyzed (analyzed cells are marked with the number 3)
if curCellX == targetX and curCellY == targetY:
ShowPath()
break
else:
map[curCellY][curCellX] = 3