“““
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]?=?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:
????????????????????