AStar

AStar

import math

class AStarPlannar:
    def __init__(self,ox,oy,reso,rr):
        """
        :param ox:障碍物的x的坐标列表
        :param oy: 障碍物的y的坐标列表
        :param reso: grid的分辨率（grid solution）
        :param rr: 机器人半径
        """
        self.reso = reso
        self.rr = rr
        self.cal_obstacle_map(ox,oy)
        self.motion = self.get_motion_model()

    class Node:
        """
        节点属性
        """
        def __init__(self,x,y,cost,pind):
            self.x = x
            self.y = y
            self.cost = cost
            self.pind = pind
        def __str__(self):
            """
            返回节点信息
            :return:
            """
            return str(self.x) + "," + str(self.y)+ "," + str(self.cost) + "," + str(self.pind)

    def calc_obstacle_map(self,ox,oy):
        """
        计算obstacle map
        :param ox:
        :param oy:
        :return:
        """
        self.minx = round(min(ox))
        self.miny = round(min(oy))
        self.maxx = round(max(ox))
        self.maxy = round(max(oy))
        print("minx:".self.minx)
        print("miny:".self.miny)
        print("maxx:".self.maxx)
        print("maxy:".self.maxy)
        self.xwidth = round((self.maxx-self.minx)/self.reso)
        self.ywidth = round((self.maxy-self.miny)/self.reso)
        print("x",self.xwidth)
        print("y",self.ywidth)

        self.obmap = [[False for i in range(self.ywidth)]
                      for i in range(self.xwidth)]
        for ix in range(self.xwidth):
            x = self.calc_grid_position(ix,self.minx)
            for iy in range(self.ywidth):
                y = self.calc_grid_position(iy,self.miny)
                for iox,ioy in zip(ox,oy):
                    d = math.hypot(iox-x,ioy-y)
                    if d<=self.rr:
                        self.obmap[ix][iy] = True
                        break
    def calc_grid_position(self,index,minp):
        """
        计算网格坐标
        :param index:网格索引号
        :param minp:min x
        :return:
        """
        pos = index*self.reso+minp
        return pos

    def calc_grid_index(self,node):
        """
        由节点计算index
        :param node:
        :return:
        """
        return (node.y - self.miny) * self.xwidth + (node.x -self.minx)

    @staticmethod
    def get_motion_model():
        """
        小车运动模型
        :return:
        """
        motion = [[1,0,1],
                  [0,1,1],
                  [-1,0,1],
                  [0,-1,1],
                  [-1,-1,math.sqrt(2)],
                  [-1,1,math.sqrt(2)],
                  [1,-1,math.sqrt(2)],
                  [1,1,math.sqrt(2)]]
        return motion

    @staticmethod
    def calc_heuristic(n1,n2):
        """
        计算两节点之间的欧拉距离
        :param n1:
        :param n2:
        :return:
        """
        w = 1.0
        d = w*math.hypot(n1.x-n2.x, n1.y-n2.y)
        return d