A*算法。

一、A*算法原理

代码来自CHH3213，注释仅个人理解

import math
import matplotlib.pyplot as plt
show_animation=True

class AStarPlanner:
    def __init__(self,ox,oy,resolution,rr):
        self.resolution=resolution
        self.rr=rr
        self.min_x,self.min_y=0,0
        self.max_x,self.max_y=0,0
        self.obstacle_map=None
        self.x_width,self.y_width=0,0
        self.motion=self.get_motion_model()
        self.calc_obstacle_map(ox,oy)

    class Node:
        # 定义搜索区域节点类, 每个Node都包含坐标x和y, 移动代价cost和父节点索引。
        def __init__(self,x,y,cost,parent_index):
            self.x=x
            self.y=y
            self.cost=cost  #cost代表g(n)值
            self.parent_index=parent_index

        def __str__(self):
            return str(self.x) +&#