開(kāi)源了！機(jī)器人技術(shù)常用的路徑規(guī)劃算法（含動(dòng)畫(huà)演示）

"""
Best-First?Searching
@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
from?Search_2D.Astar?import?AStar


class?BestFirst(AStar):
????"""BestFirst?set?the?heuristics?as?the?priority?
????"""
????def?searching(self):
????????"""
????????Breadth-first?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.heuristic(self.s_start),?self.s_start))

????????while?self.OPEN:
????????????_,?s?=?heapq.heappop(self.OPEN)
????????????self.CLOSED.append(s)

????????????if?s?==?self.s_goal:
????????????????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?#?conditions?for?updating?Cost
????????????????????self.g[s_n]?=?new_cost
????????????????????self.PARENT[s_n]?=?s

????????????????????#?best?first?set?the?heuristics?as?the?priority?
????????????????????heapq.heappush(self.OPEN,?(self.heuristic(s_n),?s_n))

????????return?self.extract_path(self.PARENT),?self.CLOSED


def?main():
????s_start?=?(5,?5)
????s_goal?=?(45,?25)

????BF?=?BestFirst(s_start,?s_goal,?'euclidean')
????plot?=?plotting.Plotting(s_start,?s_goal)

????path,?visited?=?BF.searching()
????plot.animation(path,?visited,?"Best-first?Searching")??#?animation


if?__name__?==?'__main__':
????main()