A*寻路算法 python实现

# -*- coding: utf-8 -*-

import math
import cv2 as cv


class Point(object):
    def __init__(self, position, parent):
        self.position = position
        self.parent = parent
        self.F = 0
        self.G = 0
        self.H = 0


# 全局阈值
def threshold_demo(image):
    gray = cv.cvtColor(image, cv.COLOR_RGB2GRAY)  # 把输入图像灰度化
    # 直接阈值化是对输入的单通道矩阵逐像素进行阈值分割。
    ret, binary = cv.threshold(gray, 0, 255, cv.THRESH_BINARY | cv.THRESH_TRIANGLE)
    # print("threshold value %s" % ret)
    # cv.imshow("binary0", binary)
    return binary


src = cv.imread('C:/tensor/map.jpg')
# cv.imshow('input_image', src)
bi = threshold_demo(src)


def estimate_distance(from_point, target_point):
    return math.sqrt(math.pow(target_point.position[0] - from_point.position[0], 2) + math.pow(
        target_point.position[1] - from_point.position[1], 2))


def is_same_node(point, target_point):
    if point.position[0] == target_point.position[0] and point.position[1] == target_point.position[1]:
        return True
    return False


def is_point_in_list(point, p_list):
    for p in p_list:
        if is_same_node(p, point):
            return True
    return False


def get_point_from_list(point, p_list):
    for p in p_list:
        if is_same_node(p, point):
            return p
    return None


def display_path(last_point):
    point_path = [last_point]
    last_point = last_point.parent
    while last_point is not None:
        point_path.append(last_point)
        last_point = last_point.parent

    point_path.reverse()
    path_str = ''
    for p in point_path:
        path_str += '[' + str(p.position[0]) + ',' + str(p.position[1]) + ']-->'
    print(path_str)

    image = src
    for point in point_path:
        cv.circle(image, (point.position[1], point.position[0]), 1, (0, 0, 255), 1)
    image = cv.resize(image, (bi.shape[1]*4, bi.shape[0]*4))
    cv.imshow("final", image)


def filter_not_reachables(map, points):
    new_points = []

    for point in points:
        if map[point.position[0]][point.position[1]] == 255:
            new_points.append(point)

    return new_points


def get_periphery_points(map, point):
    points = []

    x = point.position[0]
    y = point.position[1]

    points.append(Point([x - 1, y - 1], None))
    points.append(Point([x, y - 1], None))
    points.append(Point([x + 1, y - 1], None))

    points.append(Point([x - 1, y], None))
    points.append(Point([x + 1, y], None))

    points.append(Point([x - 1, y + 1], None))
    points.append(Point([x, y + 1], None))
    points.append(Point([x + 1, y + 1], None))

    valid_points = []

    for p in points:
        if 0 <= p.position[0] < map.shape[0] and 0 <= p.position[1] < map.shape[1]:
            valid_points.append(p)

    return valid_points


def pick_one_min_F_point(p_list):
    if len(p_list) == 0:
        return None

    if len(p_list) == 1:
        return p_list[0]

    min_F = p_list[0].F
    min_idx = 0

    for idx, p in enumerate(p_list[1:]):
        if p.F < min_F:
            min_F = p.F
            min_idx = idx + 1

    return p_list[min_idx]


def filter_ignored(points):
    new_points = []

    if len(points) <= 0:
        return new_points

    for p in points:
        if p.ignore:
            continue
        new_points.append(p)

    return new_points


def a_star(map):
    width, height = map.shape
    print(' ', width, 'height: ', height)
    print(width * height)

    target_point = Point([width - 1, height - 1], None)

    from_point = Point([0, 0], None)
    from_point.G = 0
    from_point.H = estimate_distance(from_point, target_point)
    from_point.F = from_point.G + from_point.H

    open_list = []
    close_list = []
    open_list.append(from_point)

    while len(open_list) > 0:
        cur_point = pick_one_min_F_point(open_list)
        if cur_point is None:
            raise ValueError('无法找到可达路径')

        points = get_periphery_points(map, cur_point)
        points = filter_not_reachables(map, points)

        for point in points:
            if is_point_in_list(point, open_list):
                point.new_added = False
                point.ignore = False
                p = get_point_from_list(point, open_list)
                point.parent = p.parent
                point.F = p.F
                point.G = p.G
                point.H = p.H
            elif is_point_in_list(point, close_list):
                point.new_added = False
                point.ignore = True
                p = get_point_from_list(point, close_list)
                point.parent = p.parent
                point.F = p.F
                point.G = p.G
                point.H = p.H
            else:
                point.new_added = True
                point.ignore = False
                open_list.append(point)

        points = filter_ignored(points)

        for point in points:
            if point.new_added:
                point.parent = cur_point
                # 计算FGH
                point.G = cur_point.G + 1
                point.H = estimate_distance(point, target_point)
                point.F = point.G + point.H
            else:
                # 计算FGH
                old_f = point.G + point.H
                new_f = cur_point.G + 1 + point.H

                # 比较新的和老的F值哪个大
                if new_f < old_f:
                    # 覆盖新的FGH/PARENT
                    point.parent = cur_point
                    point.G = cur_point.G + 1
                    point.F = point.G + point.H

        for point in points:
            if is_same_node(point, target_point):
                display_path(point)
                return

        open_list.remove(cur_point)
        close_list.append(cur_point)


a_star(bi)

cv.waitKey(0)
cv.destroyAllWindows()