Created
August 2, 2019 03:38
-
-
Save qiaoxu123/0cf45d8e2cc472dda91b75fbc1007aa5 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
""" | |
A* grid planning | |
author: Atsushi Sakai(@Atsushi_twi) | |
Nikos Kanargias (nkana@tee.gr) | |
See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm) | |
""" | |
import math | |
import matplotlib.pyplot as plt | |
show_animation = True | |
class AStarPlanner: | |
def __init__(self, ox, oy, reso, rr): | |
""" | |
Initialize grid map for a star planning | |
ox: x position list of Obstacles [m] | |
oy: y position list of Obstacles [m] | |
reso: grid resolution [m] | |
rr: robot radius[m] | |
""" | |
self.reso = reso | |
self.rr = rr | |
self.calc_obstacle_map(ox, oy) | |
self.motion = self.get_motion_model() | |
class Node: | |
def __init__(self, x, y, cost, pind): | |
self.x = x # index of grid | |
self.y = y # index of grid | |
self.cost = cost | |
self.pind = pind | |
def __str__(self): | |
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind) | |
def planning(self, sx, sy, gx, gy): | |
""" | |
A star path search | |
input: | |
sx: start x position [m] | |
sy: start y position [m] | |
gx: goal x position [m] | |
gx: goal x position [m] | |
output: | |
rx: x position list of the final path | |
ry: y position list of the final path | |
""" | |
nstart = self.Node(self.calc_xyindex(sx, self.minx), | |
self.calc_xyindex(sy, self.miny), 0.0, -1) | |
ngoal = self.Node(self.calc_xyindex(gx, self.minx), | |
self.calc_xyindex(gy, self.miny), 0.0, -1) | |
open_set, closed_set = dict(), dict() | |
open_set[self.calc_grid_index(nstart)] = nstart | |
while 1: | |
if len(open_set) == 0: | |
print("Open set is empty..") | |
break | |
c_id = min( | |
open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o])) | |
current = open_set[c_id] | |
# show graph | |
if show_animation: # pragma: no cover | |
plt.plot(self.calc_grid_position(current.x, self.minx), | |
self.calc_grid_position(current.y, self.miny), "xc") | |
if len(closed_set.keys()) % 10 == 0: | |
plt.pause(0.001) | |
if current.x == ngoal.x and current.y == ngoal.y: | |
print("Find goal") | |
ngoal.pind = current.pind | |
ngoal.cost = current.cost | |
break | |
# Remove the item from the open set | |
del open_set[c_id] | |
# Add it to the closed set | |
closed_set[c_id] = current | |
# expand_grid search grid based on motion model | |
for i, _ in enumerate(self.motion): | |
node = self.Node(current.x + self.motion[i][0], | |
current.y + self.motion[i][1], | |
current.cost + self.motion[i][2], c_id) | |
n_id = self.calc_grid_index(node) | |
# If the node is not safe, do nothing | |
if not self.verify_node(node): | |
continue | |
if n_id in closed_set: | |
continue | |
if n_id not in open_set: | |
open_set[n_id] = node # discovered a new node | |
else: | |
if open_set[n_id].cost > node.cost: | |
# This path is the best until now. record it | |
open_set[n_id] = node | |
rx, ry = self.calc_final_path(ngoal, closed_set) | |
return rx, ry | |
def calc_final_path(self, ngoal, closedset): | |
# generate final course | |
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ | |
self.calc_grid_position(ngoal.y, self.miny)] | |
pind = ngoal.pind | |
while pind != -1: | |
n = closedset[pind] | |
rx.append(self.calc_grid_position(n.x, self.minx)) | |
ry.append(self.calc_grid_position(n.y, self.miny)) | |
pind = n.pind | |
return rx, ry | |
@staticmethod | |
def calc_heuristic(n1, n2): | |
w = 1.0 # weight of heuristic | |
d = w * math.sqrt((n1.x - n2.x) ** 2 + (n1.y - n2.y) ** 2) | |
return d | |
def calc_grid_position(self, index, minp): | |
""" | |
calc grid position | |
:param index: | |
:param minp: | |
:return: | |
""" | |
pos = index * self.reso + minp | |
return pos | |
def calc_xyindex(self, position, min_pos): | |
return round((position - min_pos) / self.reso) | |
def calc_grid_index(self, node): | |
return (node.y - self.miny) * self.xwidth + (node.x - self.minx) | |
def verify_node(self, node): | |
px = self.calc_grid_position(node.x, self.minx) | |
py = self.calc_grid_position(node.y, self.miny) | |
if px < self.minx: | |
return False | |
elif py < self.miny: | |
return False | |
elif px >= self.maxx: | |
return False | |
elif py >= self.maxy: | |
return False | |
# collision check | |
if self.obmap[node.x][node.y]: | |
return False | |
return True | |
def calc_obstacle_map(self, ox, oy): | |
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("xwidth:", self.xwidth) | |
print("ywidth:", self.ywidth) | |
# obstacle map generation | |
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.sqrt((iox - x) ** 2 + (ioy - y) ** 2) | |
if d <= self.rr: | |
self.obmap[ix][iy] = True | |
break | |
@staticmethod | |
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 | |
def main(): | |
print(__file__ + " start!!") | |
# start and goal position | |
sx = 10.0 # [m] | |
sy = 10.0 # [m] | |
gx = 50.0 # [m] | |
gy = 50.0 # [m] | |
grid_size = 2.0 # [m] | |
robot_radius = 1.0 # [m] | |
# set obstable positions | |
ox, oy = [], [] | |
for i in range(-10, 60): | |
ox.append(i) | |
oy.append(-10.0) | |
for i in range(-10, 60): | |
ox.append(60.0) | |
oy.append(i) | |
for i in range(-10, 61): | |
ox.append(i) | |
oy.append(60.0) | |
for i in range(-10, 61): | |
ox.append(-10.0) | |
oy.append(i) | |
for i in range(-10, 40): | |
ox.append(20.0) | |
oy.append(i) | |
for i in range(0, 40): | |
ox.append(40.0) | |
oy.append(60.0 - i) | |
if show_animation: # pragma: no cover | |
plt.plot(ox, oy, ".k") | |
plt.plot(sx, sy, "og") | |
plt.plot(gx, gy, "xb") | |
plt.grid(True) | |
plt.axis("equal") | |
a_star = AStarPlanner(ox, oy, grid_size, robot_radius) | |
rx, ry = a_star.planning(sx, sy, gx, gy) | |
if show_animation: # pragma: no cover | |
plt.plot(rx, ry, "-r") | |
plt.show() | |
if __name__ == '__main__': | |
main() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment