A-start寻路算法 Python 实现

A-start寻路算法 Python 实现

# -*- coding:utf-8 -*-
import heapq

import time
from typing import List, Optional


class SearchNode:

    def __init__(self, x: int, y: int):
        self.x = x
        self.y = y
        # search property
        self.h = 0.0  # Manhattan Distance
        self.g = 0.0  # 实际代价
        self.f = 0.0  # 优先级
        self.parent: Optional[SearchNode] = None
        # 是否关闭
        self.closed = False

    def __hash__(self):
        return hash((self.x, self.y))

    def __lt__(self, other):
        return self.f < other.f

    def __repr__(self):
        return f"({self.x},{self.y})"

    def __eq__(self, other):
        return self.x == other.x and self.y == other.y

    def __sub__(self, other):
        # 符合向量减法
        x = other.x - self.x
        y = other.y - self.y
        return SearchNode(x, y)


class AStarDetector:

    def __init__(self, step=None):
        """

        Args:
            step: 步长
        """

        if not step:
            self.step = 1
        else:
            self.step = step

    def find_shortest_path(self, start_node: SearchNode, end_node: SearchNode) -> List[SearchNode]:
        """
        返回最短路径
        Args:
            start_node:
            end_node:

        Returns:
        """

        if start_node == end_node:
            return [start_node, end_node]

        # 初始化开放列表和关闭列表
        open_list = [start_node]

        heapq.heapify(open_list)
        start_time = time.time()
        closes = []
        while open_list and time.time() - start_time < 0.03:
            # while open_list:
            # 选择当前f值最小的节点进行探索
            current_node = heapq.heappop(open_list)
            current_node.closed = True
            closes.append(current_node)
            if current_node == end_node:
                # 如果当前节点是目标节点，重构路径并返回
                result_path = []
                while current_node:
                    result_path.append(current_node)
                    current_node = current_node.parent
                return result_path[::-1]
            # 获取相邻节点
            neighbors = self.get_neighbors(current_node)

            for neighbor in neighbors:
                if neighbor.closed:
                    continue

                tentative_g = current_node.g + self.heuristics_calc_cost(current_node, neighbor)
                if neighbor not in open_list or tentative_g < neighbor.g:
                    # 更新节点信息
                    neighbor.g = tentative_g
                    neighbor.h = self.calculate_distance(neighbor, end_node)
                    neighbor.f = neighbor.g + neighbor.h
                    neighbor.parent = current_node

                    if neighbor not in open_list:
                        heapq.heappush(open_list, neighbor)

        return []

    def get_neighbors(self, current_node: SearchNode) -> List[SearchNode]:
        """
         实现获取相邻节点的逻辑, 避让在这里处理，如果需要避让就不要返回那个点
        Args:
            current_node:
        Returns:

        """
        neighbors = []
        x = current_node.x
        y = current_node.y

        def check(x, y):
            neighbors.append(SearchNode(x, y))

        # 上
        check(x, y + self.step)
        # 右
        check(x + self.step, y)
        # 下
        check(x, y - self.step)
        # 左
        check(x - self.step, y)

        return neighbors

    @staticmethod
    def calculate_distance(node1: SearchNode, node2: SearchNode) -> float:
        """
        实现计算两个节点之间的曼哈顿距离，启发函数
        Args:
            node1:
            node2:

        Returns:

        """
        dx = abs(node1.x - node2.x)
        dy = abs(node1.y - node2.y)

        return dx + dy

    @staticmethod
    def heuristics_calc_cost(current_node: SearchNode, neighbor: SearchNode):
        """
        代价函数
        Args:
            current_node:
            neighbor:

        Returns:

        """
        # 同向优先保持不会一直转弯
        a = neighbor - current_node

        # TODO 考虑格点距离
        if current_node.parent:
            b = current_node - current_node.parent
        else:
            return 1
        if a == b:
            return 1
        else:
            return 2


if __name__ == '__main__':
    a = AStarDetector(step=1)
    s = SearchNode(0, 0)
    e = SearchNode(4, 4)

    print(a.find_shortest_path(s, e))