-
Notifications
You must be signed in to change notification settings - Fork 0
/
A_star_search.py
162 lines (123 loc) · 4.43 KB
/
A_star_search.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
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
"""
A* grid search
"""
import heapq
import math
class Node:
def __init__(self, x, y, cost, parent_index):
self.x = x
self.y = y
self.cost = cost
self.parent_index = parent_index
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(
self.cost) + "," + str(self.parent_index)
def calc_final_path(goal_node, closed_node_set, resolution):
# generate final course
rx, ry = [goal_node.x * resolution], [goal_node.y * resolution]
parent_index = goal_node.parent_index
while parent_index != -1:
n = closed_node_set[parent_index]
rx.append(n.x * resolution)
ry.append(n.y * resolution)
parent_index = n.parent_index
return rx, ry
def calc_distance_heuristic(gx, gy, ox, oy, resolution, rr):
"""
gx: goal x position [m]
gx: goal x position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
resolution: grid resolution [m]
rr: radius[m]
"""
# Map the obstacle positions to the grid
goal_node = Node(round(gx / resolution), round(gy / resolution), 0.0, -1)
ox = [iox / resolution for iox in ox]
oy = [ioy / resolution for ioy in oy]
obstacle_map, min_x, min_y, max_x, max_y, x_w, y_w = calc_obstacle_map(
ox, oy, resolution, rr)
motion = get_motion_model()
open_set, closed_set = dict(), dict()
open_set[calc_index(goal_node, x_w, min_x, min_y)] = goal_node
priority_queue = [(0, calc_index(goal_node, x_w, min_x, min_y))]
while 1:
if not priority_queue:
break
cost, c_id = heapq.heappop(priority_queue)
if c_id in open_set:
current = open_set[c_id]
closed_set[c_id] = current
open_set.pop(c_id)
else:
continue
# Remove the item from the open set
# expand search grid based on motion model
for i, _ in enumerate(motion):
node = Node(current.x + motion[i][0],
current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, x_w, min_x, min_y)
if n_id in closed_set:
continue
if not verify_node(node, obstacle_map, min_x, min_y, max_x, max_y):
continue
if n_id not in open_set:
open_set[n_id] = node # Discover a new node
heapq.heappush(
priority_queue,
(node.cost, calc_index(node, x_w, min_x, min_y)))
else:
if open_set[n_id].cost >= node.cost:
# This path is the best until now. record it!
open_set[n_id] = node
heapq.heappush(
priority_queue,
(node.cost, calc_index(node, x_w, min_x, min_y)))
return closed_set
def verify_node(node, obstacle_map, min_x, min_y, max_x, max_y):
if node.x < min_x:
return False
elif node.y < min_y:
return False
elif node.x >= max_x:
return False
elif node.y >= max_y:
return False
# if obstacle_map[node.x][node.y]:
# return False
return True
# Use true and false to represent the obstacle map.
def calc_obstacle_map(ox, oy, resolution, vr):
min_x = round(min(ox))
min_y = round(min(oy))
max_x = round(max(ox))
max_y = round(max(oy))
x_width = round(max_x - min_x)
y_width = round(max_y - min_y)
# obstacle map generation
obstacle_map = [[False for _ in range(y_width)] for _ in range(x_width)]
for ix in range(x_width):
x = ix + min_x
for iy in range(y_width):
y = iy + min_y
# print(x, y)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2)
if d <= vr / resolution:
obstacle_map[ix][iy] = True
break
return obstacle_map, min_x, min_y, max_x, max_y, x_width, y_width
def calc_index(node, x_width, x_min, y_min):
return (node.y - y_min) * x_width + (node.x - x_min)
def get_motion_model():
# dx, dy, cost
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