AStar寻路算法的Python实现

AStar寻路算法的Python实现

人工智能课老师让整的，简单的python，加上matplotlib生成了一个散点图，矩阵点生成有10以内的误差，红色点是障碍物，百分之十的几率变成障碍物，绿色的点是要走的道路，初始化时可以设置地图大小，默认为10*20.

``''' 
    A-star寻路算法
'''
import random
import math
import queue
import matplotlib.pyplot as plt


# 点
class Point:
    def __init__(self, row_index, col_index, access, index):
        self.index = index
        self.row_index = row_index
        self.col_index = col_index
        self.access = access
        self.h = 0
        self.color = "b"
        self.head = None
        self.temp_distance = 0

# 地图
class Map:
    def __init__(self, max_col, max_row):
        self.max_col = max_col
        self.max_row = max_row
        self.point = []

    # 生成地图
    def generate_map(self):
        index_count = 0
        # 生成坐标
        for i in range(0, self.max_col * 10, 10):
            for j in range(0, self.max_row * 10, 10):
                col_ran = random.randint(1, 10)
                row_ran = random.randint(1, 10)
                point = Point(i + col_ran, j + row_ran, 0, index_count)
                isAccessNum = random.randint(1, 10)
                if isAccessNum == 5:
                    point.access = 1
                    point.color = 'r'
                self.point.append(point)
                index_count = index_count + 1
        point_len = len(self.point)
        # 求每个点的k值（到终点的预估距离，欧几里得距离）
        final_x = self.point[point_len - 1].col_index
        final_y = self.point[point_len - 1].row_index
        for p in self.point:
            p.h = math.sqrt(math.pow(final_x - p.col_index, 2) + math.pow(final_y - p.row_index, 2))


# AStar
class AStar:
    def __init__(self, max_col, max_row):
        self.max_col = max_col
        self.max_row = max_row
        self.map = Map(max_col, max_row)
        self.map.generate_map()
        self.g = 0
        self.open_set = queue.PriorityQueue()
        self.close_set = []
        self.max_size = max_col * max_row - 1

    # 输出地图
    def print_map(self):
        count = 0
        for i in range(self.max_col * self.max_row):
            point = self.map.point[i]
            print("row: {}, col: {}, k:{}, {}".format(point.row_index, point.col_index, point.k, point.access), end=' ')
            if count == self.max_row:
                count = 0
                print("\n")
            count += 1
    
    # 输出散点地图
    def draw_map(self):
        x = [i.col_index for i in self.map.point]
        y = [i.row_index for i in self.map.point]
        color = [i.color for i in self.map.point]
        plt.scatter(x, y, c=color)
        plt.show()

    def a_star(self):
        
        points = self.map.point
        # 将首节点加入优先队列
        self.open_set.put((points[0].h, points[0]))
        head = None
        while self.open_set.empty() != True:
            # 取出优先队列最小数
            X = self.open_set.get()[1]
            self.g = self.g + X.temp_distance
            X.head = head
            head = X
            X.color = 'g'
            # 如果到达终点
            if X == points[len(points) -1]:
                print("已找到路线============")
                break
            # 获取邻居点
            y_list = []
            
            if X.index % self.max_row != (self.max_row - 1) and X.index + 1 >= 0 and X.index + 1 <= self.max_size:
                y_list.append(points[X.index + 1])
            if X.index % self.max_row != 0 and X.index - 1 >= 0 and X.index - 1 <= self.max_size:
                y_list.append(points[X.index - 1])
            if X.index - self.max_row >= 0 and X.index - self.max_row <= self.max_size:
                y_list.append(points[X.index - self.max_row])
            if X.index + self.max_row >= 0 and X.index + self.max_row <= self.max_size:
                y_list.append(points[X.index + self.max_row])
            if X.index % self.max_row != (self.max_row - 1) and X.index + self.max_row + 1 >= 0 and X.index + self.max_row + 1 <= self.max_size:
                y_list.append(points[X.index + self.max_row + 1])
            if X.index % self.max_row != 0 and X.index - 1 >= 0 and X.index + self.max_row - 1 >= 0 and X.index + self.max_row - 1 <= self.max_size:
                y_list.append(points[X.index + self.max_row - 1])
            if X.index % self.max_row != (self.max_row - 1) and X.index - self.max_row + 1 >= 0 and X.index - self.max_row + 1 <= self.max_size:
                y_list.append(points[X.index - self.max_row + 1])
            if X.index % self.max_row != 0 and X.index - self.max_row - 1 >= 0 and X.index - self.max_row - 1 <= self.max_size:
                y_list.append(points[X.index - self.max_row - 1])
                
            
            print('----------------')
            # 遍历所有可达节点
            # 如果Y在openset中，则判断
            new_open_queue = queue.PriorityQueue()
            for Y in y_list:
                if Y.access == 1:
                    continue
                # 计算到该点的距离和到终点的价值
                distance = math.sqrt(math.pow(X.col_index - Y.col_index, 2) + math.pow(X.row_index - Y.row_index, 2))
                Y.temp_distance = distance
                
                value = self.g + distance + Y.h
                print("index:{}, self.g: {}, distance: {}, Y.h: {}, value: {}".format(Y.index, self.g, distance, Y.h, value))
                not_in = True
                for opens in self.open_set.queue:
                    if Y == opens[1]:
                        if value < opens[0]:
                            new_open_queue.put((value, Y))
                            not_in = False
                
                for closes in self.close_set:
                    if Y == closes[1]:
                        if value < closes[0]:
                            self.close_set.remove(closes)
                            new_open_queue.put((value, Y))
                        not_in = False

                if not_in:
                    new_open_queue.put((value, Y))
            self.open_set = new_open_queue
            self.close_set.append((value, X))
        
        # 输出路径
        print('path:  ')
        while head != None:
            print("col_index: {}, row_index: {}, index: {}, h: {}".format(head.col_index, head.row_index, head.index, head.h))
            head.color = 'g'
            head = head.head        

if __name__ == '__main__':
    star = AStar(10, 20)
    star.a_star()
    star.draw_map()