Pathfinding: A*-related question

Here is working code. Barrier map is 2D grid saved as grayscale 8-bit PNG image. Requires additional module (baseconvert.py, posted after the main code). Works only with rectangular maps. Currently, only maps with flat (even) ground are suitable, but I am going to add support for uneven terrain.

import direct.directbase.DirectStart
from pandac.PandaModules import *
from direct.task.Task import Task
from baseconvert import *
import sys

# Set the minimal distance between objects,
# which allows actor to move in between.
cellSize = 1.0
# Set x, y coordinates of the top-left corner of the map.
topLeft = (-10, 10)
# Set x, y coordinates of the lower-right corner of the map.
lowerRight = (10, -10)
# Lift collision spheres above the ground by ... units.
liftBy = 0.1

# Do not change.
mXSize = lowerRight[0] - topLeft[0]
mYSize = topLeft[1] - lowerRight[1]
bmXSize = mXSize / cellSize
bmYSize = mYSize / cellSize

class Map():
    def __init__(self):
        '''self.barrier = loader.loadModel("collisionBox")
        self.barrier.reparentTo(render)
        self.barrier.setCollideMask(BitMask32.bit(1))'''
        self.barrier = render.attachNewNode("barrier")
        self.barrierCN = CollisionNode("barrierCN")
        self.barrierSolid = CollisionTube(-2, 2, 0.5, 2, -2, 0.5, 1)
        self.barrierCN.addSolid(self.barrierSolid)
        self.barrierCN.setIntoCollideMask(BitMask32.bit(1))
        self.barrierCNP = self.barrier.attachNewNode(self.barrierCN)

class Traveller():
    def __init__(self):
        # First, setup collisions.
        self.setupCollisions()
        # Cell row/column.
        self.cellCoord = [0, 0]
        # Define the first position of the cell.
        self.cellPos = [topLeft[0]+self.radius, topLeft[1]-self.radius]
        # Create an image for the barrier map.
        self.mapImage = PNMImage(bmXSize, bmYSize)
        self.mapImage.makeGrayscale()
        # Analyze the cell.
        taskMgr.add(self.analyzeCell, "analyzeCellTask")

    def setupCollisions(self):
        self.cTrav = CollisionTraverser()
        self.handler = CollisionHandlerQueue()

        self.cell = render.attachNewNode("cell")
        self.radius = cellSize / 2.0

        self.cn_c = self.cell.attachNewNode(CollisionNode("cn_c"))
        self.cn_c.setPos(0, 0, 0)
        self.cn_c.node().addSolid(CollisionSphere(0, 0, 0, self.radius))
        self.setCMaskAddCollider(self.cn_c)

        self.cn_tl = self.cell.attachNewNode(CollisionNode("cn_tl"))
        self.cn_tl.setPos(-self.radius, self.radius, 0)
        self.cn_tl.node().addSolid(CollisionSphere(0, 0, 0, self.radius))
        self.setCMaskAddCollider(self.cn_tl)

        self.cn_tr = self.cell.attachNewNode(CollisionNode("cn_tr"))
        self.cn_tr.setPos(self.radius, self.radius, 0)
        self.cn_tr.node().addSolid(CollisionSphere(0, 0, 0, self.radius))
        self.setCMaskAddCollider(self.cn_tr)

        self.cn_lr = self.cell.attachNewNode(CollisionNode("cn_lr"))
        self.cn_lr.setPos(self.radius, -self.radius, 0)
        self.cn_lr.node().addSolid(CollisionSphere(0, 0, 0, self.radius))
        self.setCMaskAddCollider(self.cn_lr)

        self.cn_ll = self.cell.attachNewNode(CollisionNode("cn_ll"))
        self.cn_ll.setPos(-self.radius, -self.radius, 0)
        self.cn_ll.node().addSolid(CollisionSphere(0, 0, 0, self.radius))
        self.setCMaskAddCollider(self.cn_ll)

    def setCMaskAddCollider(self, cn):
        cn.node().setFromCollideMask(BitMask32.bit(1))
        cn.node().setIntoCollideMask(BitMask32.allOff())
        self.cTrav.addCollider(cn, self.handler)

    def analyzeCell(self, task):
        # Set the position of the self.cell.
        self.cell.setPos(self.cellPos[0], self.cellPos[1], self.radius+liftBy)
        # Run traverser.
        self.cTrav.traverse(render)
        self.recordCollisions()
        self.plotPixel()
        # Define row/column of the next cell.
        self.cellCoord[0] += 1
        if self.cellCoord[0] == bmXSize:
            self.cellCoord[0] = 0
            self.cellCoord[1] += 1
        # Define position of the next cell.
        self.cellPos[0] += cellSize
        if self.cellPos[0] > lowerRight[0]:
            self.cellPos[0] = topLeft[0]+self.radius
            self.cellPos[1] -= cellSize
        # If the next cell is within the map, continue the task,
        # if not - finish the task.
        if self.cellPos[1] > lowerRight[1]:
            return Task.cont
        else:
            self.saveAndExit()

    def recordCollisions(self):
        self.entries = []
        for i in range(self.handler.getNumEntries()):
            self.entry = self.handler.getEntry(i).getFromNodePath()
            self.entries.append(self.entry)

        self.cellParamBinary = ["1"]*8
        if self.entries.__contains__(self.cn_c):
            # 0 means blocked
            self.cellParamBinary[0] = "0"
        else:
            # 1 means free
            self.cellParamBinary[0] = "1"
        if self.entries.__contains__(self.cn_tl):
            self.cellParamBinary[1] = "0"
        else:
            self.cellParamBinary[1] = "1"
        if self.entries.__contains__(self.cn_tr):
            self.cellParamBinary[2] = "0"
        else:
            self.cellParamBinary[2] = "1"
        if self.entries.__contains__(self.cn_lr):
            self.cellParamBinary[3] = "0"
        else:
            self.cellParamBinary[3] = "1"
        if self.entries.__contains__(self.cn_ll):
            self.cellParamBinary[4] = "0"
        else:
            self.cellParamBinary[4] = "1"

        delimiter = ""
        self.cellParamBinary = int(delimiter.join(self.cellParamBinary))
        self.cellParamDecimal = baseconvert(self.cellParamBinary, BASE2, BASE10)
        self.cellParamDecimal = int(self.cellParamDecimal)
        print self.cellCoord, ":", self.cellParamBinary, self.cellParamDecimal

    def plotPixel(self):
        self.mapImage.setGrayVal(self.cellCoord[0],
                                 self.cellCoord[1],
                                 self.cellParamDecimal)
    def saveAndExit(self):
        self.mapImage.write(Filename("barrierMap.png"))
        sys.exit()

m = Map()
t = Traveller()

run()

baseconvert module:

BASE2 = "01"
BASE10 = "0123456789"
BASE16 = "0123456789ABCDEF"

def baseconvert(number, fromdigits, todigits):
    """ converts a "number" between two bases of arbitrary digits

    The input number is assumed to be a string of digits from the
    fromdigits string (which is in order of smallest to largest
    digit). The return value is a string of elements from todigits
    (ordered in the same way). The input and output bases are
    determined from the lengths of the digit strings. Negative
    signs are passed through.

    Source: http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/111286

    decimal to binary
    >>> baseconvert(555,BASE10,BASE2)
    '1000101011'

    binary to decimal
    >>> baseconvert('1000101011',BASE2,BASE10)
    '555'

    integer interpreted as binary and converted to decimal (!)
    >>> baseconvert(1000101011,BASE2,BASE10)
    '555'

    base10 to base4
    >>> baseconvert(99,BASE10,"0123")
    '1203'

    base4 to base5 (with alphabetic digits)
    >>> baseconvert(1203,"0123","abcde")
    'dee'

    base5, alpha digits back to base 10
    >>> baseconvert('dee',"abcde",BASE10)
    '99'

    decimal to a base that uses A-Z0-9a-z for its digits
    >>> baseconvert(257938572394L,BASE10,BASE62)
    'E78Lxik'

    ..convert back
    >>> baseconvert('E78Lxik',BASE62,BASE10)
    '257938572394'

    binary to a base with words for digits (the function cannot convert this back)
    >>> baseconvert('1101',BASE2,('Zero','One'))
    'OneOneZeroOne'

    """
    if str(number)[0]=='-':
        number = str(number)[1:]
        neg=1
    else:
        neg=0

    # make an integer out of the number
    x=long(0)
    for digit in str(number):
       x = x*len(fromdigits) + fromdigits.index(digit)

    # create the result in base 'len(todigits)'
    res=""
    while x>0:
        digit = x % len(todigits)
        res = todigits[digit] + res
        x /= len(todigits)
    if neg:
        res = "-"+res

    return res