A* 演算法 Python實作

最近考試周,沒時間封裝的很好

代碼分兩部分,定義和呼叫部分

定義部分

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import heapq

class AstarPoint(object):
    '''
    Astar點類
    ========== 輸入 ==========
    self_x,self_y : int, 點的坐標
    g: float, 點的g值
    f_value: float, 點的f值
    ParentPosition: tuple,父節點的坐標
    '''
    def __init__(self,self_x,self_y,g,f_value,ParentPosition=None):
        '''
        
        '''
        self.SelfPosition=(self_x,self_y)
        self.ParentPosition=ParentPosition
        self.f_value=f_value
        self.g=g
        
    def __hash__(self):
        return self.SelfPosition

    def __eq__(self,other):
        return self.SelfPosition==other.SelfPosition
    
    def __repr__(self):
        if self.ParentPosition!=None:
            return "AstarPoint(%d, %d, %d, %d"%(self.SelfPosition[0],self.SelfPosition[1],self.g,self.f_value)+\
    ", " +str(self.ParentPosition[0])+', '+str(self.ParentPosition[1])+")"
        else:
            return "AstarPoint(%d, %d, %d, %d"%(self.SelfPosition[0],self.SelfPosition[1],self.g,self.f_value)+\
    ", " +str(None)+', '+str(None)+")"
        
    def __str__(self):
        if self.ParentPosition==None:
            return "AstartPoint類 | 坐標為:       " + str(self.SelfPosition) + "\t\t f_value : "+str(self.f_value) + \
        "\n\t      | 當前節點無父節點"
        else:
            return "AstartPoint類 | 坐標為:       " + str(self.SelfPosition)+"\t\t f_value : " + str(self.f_value) + \
        "\n\t      | 父節點坐標為: " + str(self.ParentPosition)


class CostMap(object):
    '''地圖類'''
    def __init__(self,init_map):
        '''
        ========== 輸入 ==========
            init_map : np.ndarray, 初始化的CostMap,通常為np.zeros,可為二維,三維或者更高位
        '''
        self.map=init_map
        
    def obstacle_setter(self,start_x,end_x,start_y,end_y,is_clean=False):
        '''
        obstacle_adder用于添加障礙物
        ========== 輸入 ==========
        start_x, start_y : 起點的坐標
        end_x, end_y :終點的坐標
        is_clean : Ture表示清除所有障礙物
        '''
        if not is_clean:
            self.map[start_x:end_x,start_y:end_y]=np.inf
        else:
            self.map[:,:]=0


# 可視化函式
def ShowMap(CostMap,StartPoint=None,EndPoint=None,pandas=True,Path=None):
    '''
    ShowMap用于以表格形式或者以柵格圖形式展示地圖
    =========== 輸入 ==========
    CostMap : np.ndarray
    StartPoint, EndPoint : tuple, 默認為None
    Path : list
    =========== 輸出 ==========
    return None
    '''
    if pandas:
        pd.options.display.max_columns = None
        pd.options.display.max_rows = None
        shape=CostMap.shape
        CostMap=CostMap.T[::-1,:]
        return pd.DataFrame(CostMap,index=np.arange(shape[1]-1,-1,-1),columns=np.arange(0,shape[0],1))
    else:
        ColorPlan=list("rgbcwk")
        squareSpace=0.33333
        squareAera=3000
        
        #創建地圖
        Figure,Axes=plt.subplots(1)
        Figure.set_size_inches(CostMap.shape)
        
        #繪制圖形
        Obstacle=np.where(CostMap == np.inf)
        Suspects=np.where((CostMap > 0) & (CostMap < np.inf))
        
        #添加障礙物,起點,終點和路徑
        for (x,y) in zip(Obstacle[0],Obstacle[1]):
            Axes.scatter(x+0.5,y+0.5,marker='s',c=ColorPlan[-1],s=squareAera)
        for (x,y) in zip(Suspects[0],Suspects[1]):
            Axes.scatter(x+0.5,y+0.5,marker='s',c=ColorPlan[2],s=squareAera)
            style=dict(size=13,color='black')
            Axes.text(x,y,str(CostMap[x,y]),**style)
        
        if Path != None:
            for (x,y) in Path:
                Axes.scatter(x+0.5,y+0.5,marker='s',c=ColorPlan[3],s=squareAera)
            Axes.scatter([],[],marker='s',c=ColorPlan[3],label='Path Point')
        
        if StartPoint != None:
            Axes.scatter(StartPoint[0]+0.5,StartPoint[1]+0.5,marker='s',c=ColorPlan[1],s=squareAera)
            Axes.scatter([],[],marker='s',c=ColorPlan[0],label='Start Point')
        if EndPoint != None:
            Axes.scatter(EndPoint[0]+0.5,EndPoint[1]+0.5,marker='s',c=ColorPlan[0],s=squareAera)
            Axes.scatter([],[],marker='s',c=ColorPlan[1],label='End Point')
        

        #設定圖例
        Axes.scatter([],[],marker='s',c=ColorPlan[-1],label='Obstacle')
        Axes.scatter([],[],marker='s',c=ColorPlan[2],label='Suspect Point')
        
        
        #圖案設定
        Axes.set_xlim(0,CostMap.shape[0],1)
        Axes.set_ylim(0,CostMap.shape[1],1)
        Axes.set_xticks(np.arange(0,CostMap.shape[0]+1,1))
        Axes.set_yticks(np.arange(0,CostMap.shape[1]+1,1))
        Axes.grid(True)
        Axes.legend(bbox_to_anchor=(1.005, 1), loc='upper left', borderaxespad=0.,fontsize='xx-large')
        plt.show()

# 獲得最小f值的函式
def GetMinimunFValueIndex(OpenList):
    FValueList=[]
    FValueHeap=[]
    for point in OpenList:
        heapq.heappush(FValueHeap,point.f_value)
        FValueList.append(point.f_value)
    return FValueList.index(heapq.nsmallest(1,FValueHeap)[0])

def A_star_path_planning(cost_map,start_point,goal_point):
    """
    A star 路徑規劃
    ========== 輸入 ==========
    cost_map    : CostMap的一個實體
    start_point : tuple，表示起點坐標，例如 (1, 1) 或者 (1, 1, 1)
    goal_point  : tuple，表示終點坐標，例如 (19, 19) 或者 (19, 19, 19)
    ========== 輸出 ==========
    path : 點list，例如 [(1, 1, 1), (2, 2, 2), ..., (19, 19, 19)]
    """
    
    OpenList=[
    AstarPoint(start_point[0],start_point[1],0,np.sqrt(np.sum((start_point[0]-goal_point[0])**2+(start_point[1]-goal_point[1])**2)))
    ]
    CloseList=[]
    Path=[]
    
    
    isDone=False
    k=1
    while (len(OpenList)>0) and (not isDone) :
        
        # 獲得最小f值的點
        index=GetMinimunFValueIndex(OpenList)           # 獲得f值最小的點的索引
    
        # OpenList中彈出f值最小的點,加入CloseList中
        tempPoint=OpenList.pop(index)
        CloseList.append(tempPoint)
    
        # 將周圍的點加入OpenList中
        for i in [-1,0,1]:
            for j in [-1,0,1]:
            
                x_next=tempPoint.SelfPosition[0]+i
                y_next=tempPoint.SelfPosition[1]+j
            
                # 首先排除一些不可能的情況
                if i==0 and j==0:                                # 1. 點是本身
                    continue
                if(x_next<0 or x_next>=cost_map.shape[0] or y_next<0 or y_next>=cost_map.shape[1]):      # 2. 點在地圖外           
                    continue
                if cost_map[x_next,y_next] == np.inf:        # 3. 點是障礙物
                    continue
            
                if  (AstarPoint(x_next,y_next,0,0) in CloseList):        # 4. 點已經在OpenList或者在CloseList中
                    continue
            
                if (AstarPoint(x_next,y_next,0,0) not in OpenList):
                    g_next=tempPoint.g+1
                    f_value_next=g_next + np.sqrt(np.sum((x_next-goal_point[0])**2+(y_next-goal_point[1])**2))
            
                    parent_x=x_next-i
                    parent_y=y_next-j
                    
                    OpenList.append(
                        AstarPoint(x_next, y_next, g_next, f_value_next, ParentPosition=(parent_x,parent_y))
                    )
            
                    AstarMap.map[x_next,y_next]=g_next
                    k+=1
                else:
                    # 如果當前點已經在OpenList中,且G更小,更新父節點
                    if (tempPoint.g > OpenList[OpenList.index(AstarPoint(x_next,y_next,0,0))].g):
                        parent_x=x_next
                        parent_y=x_next
    
        # 如果終點已經在CloseList中
        if AstarPoint(goal_point[0],goal_point[1],0,0) in CloseList:
            isDone=True
    
    #回溯路徑
    son_index=CloseList.index(AstarPoint(goal_point[0],goal_point[1],0,0))
    while start_point not in Path:
        try:
            Path.append(CloseList[son_index].SelfPosition)
            parent_x,parent_y=CloseList[son_index].ParentPosition
            son_index=CloseList.index(AstarPoint(parent_x,parent_y,0,0))
        except TypeError:
            print("已回溯到起點")
            return Path

呼叫部分

# 初始化地圖和起點終點
shape=tuple([int(x) for x in input("請輸入地圖大小(x_length,y_length): ").split(',')])
startPoint=tuple([int(x) for x in input('請輸入起點坐標(x,y): ').split(',') if (int(x) <shape[0] and int(x)<shape[1])])
endPoint=tuple([int(x) for x in input('請輸入終點坐標(x,y): ').split(',') if (int(x) <shape[0] and int(x)<shape[1])])

AstarMap=CostMap(np.zeros(shape))

# 添加障礙物
AstarMap.obstacle_setter(None,None,None,None,is_clean=True)
AstarMap.obstacle_setter(1,8,2,4)
AstarMap.obstacle_setter(6,10,6,7)
AstarMap.obstacle_setter(9,10,6,18)
AstarMap.obstacle_setter(6,10,17,18)
AstarMap.obstacle_setter(12,13,11,14)
AstarMap.obstacle_setter(12,18,13,14)
AstarMap.obstacle_setter(17,18,11,14)
AstarMap.obstacle_setter(11,20,15,16)
ShowMap(AstarMap.map,StartPoint=startPoint,EndPoint=endPoint,pandas=False)

# 路徑規劃
Path=A_star_path_planning(AstarMap.map,startPoint,endPoint)

# 可視化
ShowMap(AstarMap.map,StartPoint=startPoint,EndPoint=endPoint,pandas=False,Path=Path)

運行結果