堆优化的A*算法-Python实现

堆优化的A*算法-Python实现

原理参考博客地址
代码借鉴地址
A*算法解决二维网格地图中的寻路问题

• 输入：图片（白色区域代表可行，深色区域代表不行可行）
• 输出：路径（在图中绘制）

""" 方格地图中的A*算法 (openList进行了堆优化)
A* 算法：  F = G+H
F: 总移动代价
G: 起点到当前点的移动代价  直:1, 斜:1.4
H: 当前点到终点的预估代价  曼哈顿距离
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1.把起点加入 openList中
2.While True:
    a.遍历openList，查找F值最小的节点，作为current
    b.current是终点:
        ========结束========
    c.从openList中弹出，放入closeList中
    d.对八个方位的格点:
        if 越界 or 是障碍物 or 在closeList中:
            continue
        if 不在openList中：
            设置父节点,F,G,H
            加入openList中
        else:
            if 这条路径更好:
                设置父节点,F,G
                更新openList中的对应节点
3.生成路径path
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
堆优化:
    openList：作为最小堆，按F值排序存储坐标 （不更新只增加）
    openDict：坐标:点详细信息 （既更新又增加）
    get_minfNode() 从openList中弹出坐标，去openDict中取点 （但由于不更新只增加，坐标可能冗余）
    in_openList() 判断坐标是否在openDict中即可 

"""
import math
from PIL import Image,ImageDraw 
import numpy as np
import heapq # 堆

STAT_OBSTACLE='#'
STAT_NORMAL='.'

class Node():
    """
    节点元素，parent用来在成功的时候回溯路径
    """
    def __init__(self, x, y,parent=None, g=0, h=0):
        self.parent = parent
        self.x = x
        self.y = y
        self.g = g
        self.h = h
        self.update()
    
    def update(self):
        self.f = self.g+self.h

class RoadMap():
    """ 读进一张图片，二值化成为有障碍物的二维网格化地图，并提供相关操作
    """
    def __init__(self,img_file):
        """图片变二维数组"""
        test_map = []
        img = Image.open(img_file)
#        img = img.resize((100,100))  ### resize图片尺寸
        img_gray = img.convert('L')  # 地图灰度化
        img_arr = np.array(img_gray)
        img_binary = np.where(img_arr<127,0,255)
        for x in range(img_binary.shape[0]):
            temp_row = []
            for y in range(img_binary.shape[1]):
                status = STAT_OBSTACLE if img_binary[x,y]==0 else STAT_NORMAL 
                temp_row.append(status)
            test_map.append(temp_row)
            
        self.map = test_map
        self.cols = len(self.map[0])
        self.rows = len(self.map)
        
    def is_valid_xy(self, x,y):
        if x < 0 or x >= self.rows or y < 0 or y >= self.cols:
            return False
        return True

    def