a*算法的python版

“““
A_star 2D
@author: huiming zhou
“““

import os
import sys
import math
import heapq

sys.path.append（os.path.dirname（os.path.abspath（__file__）） +
                “/../../Search_based_Planning/“）

from Search_2D import plotting env


class AStar:
    “““AStar set the cost + heuristics as the priority
    “““
    def __init__（self s_start s_goal heuristic_type）:
        self.s_start = s_start
        self.s_goal = s_goal
        self.heuristic_type = heuristic_type

        self.Env = env.Env（）  # class Env

        self.u_set = self.Env.motions  # feasible input set
        self.obs = self.Env.obs  # position of obstacles

        self.OPEN = []  # priority queue / OPEN set
        self.CLOSED = []  # CLOSED set / VISITED order
        self.PARENT = dict（）  # recorded parent
        self.g = dict（）  # cost to come

    def searching（self）:
        “““
        A_star Searching.
        :return: path visited order
        “““

        self.PARENT[self.s_start] = self.s_start
        self.g[self.s_start] = 0
        self.g[self.s_goal] = math.inf
        heapq.heappush（self.OPEN
                       （self.f_value（self.s_start） self.s_start））

        while self.OPEN:
            _ s = heapq.heappop（self.OPEN）
            self.CLOSED.append（s）

            if s == self.s_goal:  # stop condition
                break

            for s_n in self.get_neighbor（s）:
                new_cost = self.g[s] + self.cost（s s_n）

                if s_n not in self.g:
                    self.g[s_n] = math.inf

                if new_cost < self.g[s_n]:  # conditions for updating Cost
                    self.g[s_n] = new_cost
                    self.PARENT[s_n] = s
                    heapq.heappush（self.OPEN （self.f_value（s_n） s_n））

        return self.extract_path（self.PARENT） self.CLOSED

    def searching_repeated_astar（self e）:
        “““
        repeated A*.
        :param e: weight of A*
        :return: path and visited order
        “““

        path visited = [] []

        while e >= 1:
            p_k v_k = self.repeated_searching（self.s_start self.s_goal e）
            path.append（p_k）
            visited.append（v_k）
            e -= 0.5

        return path visited

    def repeated_searching（self s_start s_goal e）:
        “““
        run A* with weight e.
        :param s_start: starting state
        :param s_goal: goal state
        :param e: weight of a*
        :return: path and visited order.
        “““

        g = {s_start: 0 s_goal: float（“inf“）}
        PARENT = {s_start: s_start}
        OPEN = []
        CLOSED = []
        heapq.heappush（OPEN
                       （g[s_start] + e * self.heuristic（s_start） s_start））

        while OPEN:
            _ s = heapq.heappop（OPEN）
            CLOSED.append（s）

            if s == s_goal:
                break

            for s_n in self.get_neighbor（s）:
                new_cost = g[s] + self.cost（s s_n）

                if s_n not in g: