A星算法的python实现

课上作业要去找A*算法的代码并解释，看了解释，大概知道了算法原理，找写的代码感觉都不是很清楚，就自己用python写了一下。
讲一下大致思路，我是用图片作为地图，以读入图片的灰度值，用来判断是否是障碍物，起始点与终点也是直接在图片上选取，看起来比较直观。
这里用来类定义了点（Point），地图（Map）和算法主体（DStar）。

# -*- coding = utf-8 -*-
# @Time : 2021/4/1 13:41
# @Software : PyCharm
import cv2

class Point(object):
    def __init__(self,loc,endPoint,g = 0,fatherPoint = None):
        self.loc = loc
        self.G = g
        self.fatherPoint = fatherPoint
        self.endPoint = endPoint

    def H(self):
        d = abs(self.endPoint[0] - self.loc[0]) + abs(self.endPoint[1] - self.loc[1])
        return d

    def F(self):
        f = self.G + self.H()
        return f

    def neighbers(self,step):
        a = []
        # b = point()
        add = [{'偏移量': [-step, 0], '代价': step}, {'偏移量': [-step, -step], '代价': 1.4 * step},
               {'偏移量': [0, -step], '代价': step},{'偏移量': [step, -step], '代价': 1.4 * step},
               {'偏移量': [step, 0], '代价': step},{'偏移量': [step, step], '代价': 1.4 * step},
               {'偏移量': [0, step], '代价': step}, {'偏移量': [-step, step], '代价': 1.4 * step}]
        for item in add:
            loc = [self.loc[0]+item['偏移量'][0],self.loc[1]+item['偏移量'][1]]
            G = self.G + item['代价']
            # 此处各个相邻点的父节点就是self点本身
            b = Point(loc, self.endPoint, G, self)
            a.append(b)
        return a


class Map(object):
    point = []
    def __init__(self,img):
        self.drawMap = cv2.imread(img)
        self.checkMap = cv2.imread(img, cv2.IMREAD_GRAYSCALE)
        self.width = self.checkMap.shape[1]
        self.height = self.checkMap.shape[0]

    def on_EVENT_LBUTTONDOWN(self,event, x, y, flags, param):
        if event == cv2.EVENT_LBUTTONDOWN:
            self.point.append([x, y])
            cv2.imshow('image', self.checkMap)

    def start_end(self):
        cv2.namedWindow('image')
        cv2.imshow('image', self.checkMap)
        cv2.setMouseCallback('image', self.on_EVENT_LBUTTONDOWN)
        cv2.waitKey(0)
        cv2.destroyAllWindows()
        print("起点：",self.point[0],"终点：",self.point[1])
        return self.point

    def is_block(self,point):
        a = point.loc
        # print(a[0],a[1])
        if self.checkMap[a[1],a[0]] == 0:
            return True
        else:
            return False

    def Route(self,point,v):
        a = point.loc
        b = point.fatherPoint.loc
        cv2.line(self.drawMap,tuple(a),tuple(b),(0,255,0),3)
        cv2.imshow('img', self.drawMap)
        cv2.waitKey(v)
        return point.fatherPoint

    def rectangular(self,point,color,step,v):
        img = self.drawMap
        x1 = int(point[0]-step/2)
        y1 = int(point[1]-step/2)
        x2 = int(point[0]+step/2)
        y2 = int(point[1]+step/2)
        cv2.rectangle(img, (x1, y1), (x2, y2), color)
        cv2.imshow('img', img)
        cv2.waitKey(v)
        return


class DStar(object):
    closeList = []
    openList = []
    def __init__(self,startPoint,endPoint,step,map,v):
        self.startPoint = startPoint
        self.endPoint = endPoint
        self.step = step
        self.map = map
        self.v = v

    def __minF(self,point):
        map = self.map
        v = self.v
        step = self.step
        closeList = self.closeList
        openList = self.openList

        # 将父节点以红色表示
        map.rectangular(point.loc,(0,0,255),step,v)

        if point == self.startPoint:
            openList.append(point)

        b = point.neighbers(step)
        for item in b:
            # 检查周围节点是否超过了地图边界
            if item.loc[0] >= map.width or item.loc[1] >= map.height:
                continue

            # 检查点周围画框的点是否超过边界或者是出于障碍上，应该先判断是否超过边界
            # 以tag为标志，以便跳出双重循环
            tag = False
            for i in item.neighbers(step//2):
                if i.loc[0] >= map.width or i.loc[1] >= map.height or map.is_block(i) :
                    tag = True
                    break
            if tag:
                continue

            # 检查周围节点是否已经位于clostList中
            check = []
            for i in closeList:
                check.append(i.loc)
            if item.loc in check:
                continue

            # 判断周围节点是否位于openList中
            tag = False
            for k in openList:
                if item.loc == k.loc :
                    if k.G > item.G:
                        k.fatherPoint = item
                    tag = True
                    break
            if tag:
                continue

            # 若与终点距离足够小，则减少step值，以保证精确度
            if item.H() < 5:
                self.step = 1
            openList.append(item)
            map.rectangular(item.loc, (0, 255, 0), step, v)

        openList.remove(point)
        closeList.append(point)

        a = []
        for item in openList:
            a.append(item.F())
        b = a.index(min(a))
        return openList[b]

    def route(self):
        map = self.map
        a = self.startPoint
        while a.H() != 0:
            a = self.__minF(a)
        p = self.closeList[-1]
        while True:
            p = map.Route(p,self.v)
            if p == self.startPoint:
                break

        cv2.imshow('img', map.drawMap)
        cv2.waitKey(0)

if __name__ == "__main__":

    map = Map("img_2.png")
    a = map.start_end()
    startPoint = a[0]
    endPoint = a[1]
    map.rectangular(startPoint,(0,0,255),3,5)
    map.rectangular(endPoint,(0,0,255),3,5)

    a = Point(startPoint, endPoint)
    b = endPoint

    c = DStar(a,b,3,map,1)
    c.route()