A*寻路算法 python实现

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

# -*- 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()