python寻路_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: ', 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()