github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Using scipy.spatial.rotation matrix (#335)

Browse Source
This commit is contained in:
Atsushi Sakai
2020-06-07 20:28:29 +09:00
committed by GitHub
parent d1bde5835f
commit b8afdb10d8
26 changed files with 1002 additions and 931 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.travis.yml
View File

@@ -18,10 +18,9 @@ before_install:
- conda update -q conda
# Useful for debugging any issues with conda
- conda info -a
# - conda install python==3.6.8
install:
- conda install numpy==1.15
- conda install numpy
- conda install scipy
- conda install matplotlib
- conda install pandas

40
Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
View File

@@ -5,7 +5,8 @@ Ensemble Kalman Filter(EnKF) localization sample
author: Ryohei Sasaki(rsasaki0109)
Ref:
- [Ensemble Kalman filtering](https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
Ensemble Kalman filtering
(https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
"""
@@ -13,6 +14,7 @@ import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
# Simulation parameter
Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2
@@ -48,8 +50,8 @@ def observation(xTrue, xd, u, RFID):
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
anglen = angle + np.random.randn() * Q_sim[1, 1] ** 0.5 # add noise
zi = np.array([dn, anglen, RFID[i, 0], RFID[i, 1]])
angle_with_noise = angle + np.random.randn() * Q_sim[1, 1] ** 0.5
zi = np.array([dn, angle_with_noise, RFID[i, 0], RFID[i, 1]])
z = np.vstack((z, zi))
# add noise to input
@@ -79,10 +81,12 @@ def motion_model(x, u):
def observe_landmark_position(x, landmarks):
landmarks_pos = np.zeros((2 * landmarks.shape[0], 1))
for (i, lm) in enumerate(landmarks):
landmarks_pos[2 * i] = x[0, 0] + lm[0] * math.cos(x[2, 0] + lm[1]) + np.random.randn() * Q_sim[
0, 0] ** 0.5 / np.sqrt(2)
landmarks_pos[2 * i + 1] = x[1, 0] + lm[0] * math.sin(x[2, 0] + lm[1]) + np.random.randn() * Q_sim[
0, 0] ** 0.5 / np.sqrt(2)
index = 2 * i
q = Q_sim[0, 0] ** 0.5
landmarks_pos[index] = x[0, 0] + lm[0] * math.cos(
x[2, 0] + lm[1]) + np.random.randn() * q / np.sqrt(2)
landmarks_pos[index + 1] = x[1, 0] + lm[0] * math.sin(
x[2, 0] + lm[1]) + np.random.randn() * q / np.sqrt(2)
return landmarks_pos
@@ -148,8 +152,9 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative numbers extremely
# close to 0 (~10^-20), catch these cases and set the respective variable to 0
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative
# numbers extremely close to 0 (~10^-20), catch these cases and set
# the respective variable to 0
try:
a = math.sqrt(eig_val[big_ind])
except ValueError:
@@ -163,11 +168,11 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0])
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R.dot(np.array([[x, y]]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
fx = np.stack([x, y]).T @ rot
px = np.array(fx[:, 0] + xEst[0, 0]).flatten()
py = np.array(fx[:, 1] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
@@ -214,8 +219,9 @@ def main():
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
for i in range(len(z[:, 0])):
plt.plot([xTrue[0, 0], z[i, 2]], [xTrue[1, 0], z[i, 3]], "-k")
@@ -227,7 +233,7 @@ def main():
np.array(hxDR[1, :]).flatten(), "-k")
plt.plot(np.array(hxEst[0, :]).flatten(),
np.array(hxEst[1, :]).flatten(), "-r")
# plot_covariance_ellipse(xEst, PEst)
plot_covariance_ellipse(xEst, PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)

119
Localization/histogram_filter/histogram_filter.py
View File

@@ -28,11 +28,11 @@ MOTION_STD = 1.0 # standard deviation for motion gaussian distribution
RANGE_STD = 3.0 # standard deviation for observation gaussian distribution
# grid map param
XY_RESO = 0.5 # xy grid resolution
MINX = -15.0
MINY = -5.0
MAXX = 15.0
MAXY = 25.0
XY_RESOLUTION = 0.5 # xy grid resolution
MIN_X = -15.0
MIN_Y = -5.0
MAX_X = 15.0
MAX_Y = 25.0
# simulation parameters
NOISE_RANGE = 2.0 # [m] 1σ range noise parameter
@@ -41,17 +41,17 @@ NOISE_SPEED = 0.5 # [m/s] 1σ speed noise parameter
show_animation = True
class GridMap():
class GridMap:
def __init__(self):
self.data = None
self.xy_reso = None
self.minx = None
self.miny = None
self.maxx = None
self.maxx = None
self.xw = None
self.yw = None
self.xy_resolution = None
self.min_x = None
self.min_y = None
self.max_x = None
self.max_y = None
self.x_w = None
self.y_w = None
self.dx = 0.0 # movement distance
self.dy = 0.0 # movement distance
@@ -64,10 +64,10 @@ def histogram_filter_localization(grid_map, u, z, yaw):
return grid_map
def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
def calc_gaussian_observation_pdf(grid_map, z, iz, ix, iy, std):
# predicted range
x = ix * gmap.xy_reso + gmap.minx
y = iy * gmap.xy_reso + gmap.miny
x = ix * grid_map.xy_resolution + grid_map.min_x
y = iy * grid_map.xy_resolution + grid_map.min_y
d = math.hypot(x - z[iz, 1], y - z[iz, 2])
# likelihood
@@ -76,16 +76,16 @@ def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
return pdf
def observation_update(gmap, z, std):
def observation_update(grid_map, z, std):
for iz in range(z.shape[0]):
for ix in range(gmap.xw):
for iy in range(gmap.yw):
gmap.data[ix][iy] *= calc_gaussian_observation_pdf(
gmap, z, iz, ix, iy, std)
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
grid_map.data[ix][iy] *= calc_gaussian_observation_pdf(
grid_map, z, iz, ix, iy, std)
gmap = normalize_probability(gmap)
grid_map = normalize_probability(grid_map)
return gmap
return grid_map
def calc_input():
@@ -112,8 +112,8 @@ def motion_model(x, u):
def draw_heat_map(data, mx, my):
maxp = max([max(igmap) for igmap in data])
plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.get_cmap("Blues"))
max_value = max([max(i_data) for i_data in data])
plt.pcolor(mx, my, data, vmax=max_value, cmap=plt.cm.get_cmap("Blues"))
plt.axis("equal")
@@ -140,43 +140,47 @@ def observation(xTrue, u, RFID):
return xTrue, z, ud
def normalize_probability(gmap):
sump = sum([sum(igmap) for igmap in gmap.data])
def normalize_probability(grid_map):
sump = sum([sum(i_data) for i_data in grid_map.data])
for ix in range(gmap.xw):
for iy in range(gmap.yw):
gmap.data[ix][iy] /= sump
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
grid_map.data[ix][iy] /= sump
return gmap
return grid_map
def init_gmap(xy_reso, minx, miny, maxx, maxy):
def init_grid_map(xy_resolution, min_x, min_y, max_x, max_y):
grid_map = GridMap()
grid_map.xy_reso = xy_reso
grid_map.minx = minx
grid_map.miny = miny
grid_map.maxx = maxx
grid_map.maxy = maxy
grid_map.xw = int(round((grid_map.maxx - grid_map.minx) / grid_map.xy_reso))
grid_map.yw = int(round((grid_map.maxy - grid_map.miny) / grid_map.xy_reso))
grid_map.xy_resolution = xy_resolution
grid_map.min_x = min_x
grid_map.min_y = min_y
grid_map.max_x = max_x
grid_map.max_y = max_y
grid_map.x_w = int(round((grid_map.max_x - grid_map.min_x)
/ grid_map.xy_resolution))
grid_map.y_w = int(round((grid_map.max_y - grid_map.min_y)
/ grid_map.xy_resolution))
grid_map.data = [[1.0 for _ in range(grid_map.yw)] for _ in range(grid_map.xw)]
grid_map.data = [[1.0 for _ in range(grid_map.y_w)]
for _ in range(grid_map.x_w)]
grid_map = normalize_probability(grid_map)
return grid_map
def map_shift(grid_map, x_shift, y_shift):
tgmap = copy.deepcopy(grid_map.data)
tmp_grid_map = copy.deepcopy(grid_map.data)
for ix in range(grid_map.xw):
for iy in range(grid_map.yw):
for ix in range(grid_map.x_w):
for iy in range(grid_map.y_w):
nix = ix + x_shift
niy = iy + y_shift
if 0 <= nix < grid_map.xw and 0 <= niy < grid_map.yw:
grid_map.data[ix + x_shift][iy + y_shift] = tgmap[ix][iy]
if 0 <= nix < grid_map.x_w and 0 <= niy < grid_map.y_w:
grid_map.data[ix + x_shift][iy + y_shift] =\
tmp_grid_map[ix][iy]
return grid_map
@@ -185,22 +189,26 @@ def motion_update(grid_map, u, yaw):
grid_map.dx += DT * math.cos(yaw) * u[0]
grid_map.dy += DT * math.sin(yaw) * u[0]
x_shift = grid_map.dx // grid_map.xy_reso
y_shift = grid_map.dy // grid_map.xy_reso
x_shift = grid_map.dx // grid_map.xy_resolution
y_shift = grid_map.dy // grid_map.xy_resolution
if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0: # map should be shifted
grid_map = map_shift(grid_map, int(x_shift), int(y_shift))
grid_map.dx -= x_shift * grid_map.xy_reso
grid_map.dy -= y_shift * grid_map.xy_reso
grid_map.dx -= x_shift * grid_map.xy_resolution
grid_map.dy -= y_shift * grid_map.xy_resolution
grid_map.data = gaussian_filter(grid_map.data, sigma=MOTION_STD)
return grid_map
def calc_grid_index(gmap):
mx, my = np.mgrid[slice(gmap.minx - gmap.xy_reso / 2.0, gmap.maxx + gmap.xy_reso / 2.0, gmap.xy_reso),
slice(gmap.miny - gmap.xy_reso / 2.0, gmap.maxy + gmap.xy_reso / 2.0, gmap.xy_reso)]
def calc_grid_index(grid_map):
mx, my = np.mgrid[slice(grid_map.min_x - grid_map.xy_resolution / 2.0,
grid_map.max_x + grid_map.xy_resolution / 2.0,
grid_map.xy_resolution),
slice(grid_map.min_y - grid_map.xy_resolution / 2.0,
grid_map.max_y + grid_map.xy_resolution / 2.0,
grid_map.xy_resolution)]
return mx, my
@@ -217,7 +225,7 @@ def main():
time = 0.0
xTrue = np.zeros((4, 1))
grid_map = init_gmap(XY_RESO, MINX, MINY, MAXX, MAXY)
grid_map = init_grid_map(XY_RESOLUTION, MIN_X, MIN_Y, MAX_X, MAX_Y)
mx, my = calc_grid_index(grid_map) # for grid map visualization
while SIM_TIME >= time:
@@ -234,8 +242,9 @@ def main():
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
draw_heat_map(grid_map.data, mx, my)
plt.plot(xTrue[0, :], xTrue[1, :], "xr")
plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k")

16
Localization/particle_filter/particle_filter.py
View File

@@ -10,6 +10,7 @@ import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
# Estimation parameter of PF
Q = np.diag([0.2]) ** 2 # range error
@@ -172,8 +173,9 @@ def plot_covariance_ellipse(x_est, p_est): # pragma: no cover
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative numbers extremely
# close to 0 (~10^-20), catch these cases and set the respective variable to 0
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative
# numbers extremely close to 0 (~10^-20), catch these cases and set the
# respective variable to 0
try:
a = math.sqrt(eig_val[big_ind])
except ValueError:
@@ -187,9 +189,8 @@ def plot_covariance_ellipse(x_est, p_est): # pragma: no cover
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0])
Rot = np.array([[math.cos(angle), -math.sin(angle)],
[math.sin(angle), math.cos(angle)]])
fx = Rot.dot(np.array([[x, y]]))
rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
fx = rot.dot(np.array([[x, y]]))
px = np.array(fx[0, :] + x_est[0, 0]).flatten()
py = np.array(fx[1, :] + x_est[1, 0]).flatten()
plt.plot(px, py, "--r")
@@ -235,8 +236,9 @@ def main():
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
for i in range(len(z[:, 0])):
plt.plot([x_true[0, 0], z[i, 1]], [x_true[1, 0], z[i, 2]], "-k")

181
Mapping/lidar_to_grid_map/lidar_to_grid_map.py
View File

@@ -2,15 +2,16 @@
LIDAR to 2D grid map example
author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts (@Atsushi_twi)
author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts
"""
import math
import numpy as np
import matplotlib.pyplot as plt
from collections import deque
import matplotlib.pyplot as plt
import numpy as np
EXTEND_AREA = 1.0
@@ -44,19 +45,20 @@ def bresenham(start, end):
x2, y2 = end
dx = x2 - x1
dy = y2 - y1
is_steep = abs(dy) > abs(dx) # determine how steep the line is
if is_steep: # rotate line
is_steep = abs(dy) > abs(dx) # determine how steep the line is
if is_steep: # rotate line
x1, y1 = y1, x1
x2, y2 = y2, x2
swapped = False # swap start and end points if necessary and store swap state
# swap start and end points if necessary and store swap state
swapped = False
if x1 > x2:
x1, x2 = x2, x1
y1, y2 = y2, y1
swapped = True
dx = x2 - x1 # recalculate differentials
dy = y2 - y1 # recalculate differentials
error = int(dx / 2.0) # calculate error
ystep = 1 if y1 < y2 else -1
dx = x2 - x1 # recalculate differentials
dy = y2 - y1 # recalculate differentials
error = int(dx / 2.0) # calculate error
y_step = 1 if y1 < y2 else -1
# iterate over bounding box generating points between start and end
y = y1
points = []
@@ -65,26 +67,27 @@ def bresenham(start, end):
points.append(coord)
error -= abs(dy)
if error < 0:
y += ystep
y += y_step
error += dx
if swapped: # reverse the list if the coordinates were swapped
if swapped: # reverse the list if the coordinates were swapped
points.reverse()
points = np.array(points)
return points
def calc_grid_map_config(ox, oy, xyreso):
def calc_grid_map_config(ox, oy, xy_resolution):
"""
Calculates the size, and the maximum distances according to the the measurement center
Calculates the size, and the maximum distances according to the the
measurement center
"""
minx = round(min(ox) - EXTEND_AREA / 2.0)
miny = round(min(oy) - EXTEND_AREA / 2.0)
maxx = round(max(ox) + EXTEND_AREA / 2.0)
maxy = round(max(oy) + EXTEND_AREA / 2.0)
xw = int(round((maxx - minx) / xyreso))
yw = int(round((maxy - miny) / xyreso))
min_x = round(min(ox) - EXTEND_AREA / 2.0)
min_y = round(min(oy) - EXTEND_AREA / 2.0)
max_x = round(max(ox) + EXTEND_AREA / 2.0)
max_y = round(max(oy) + EXTEND_AREA / 2.0)
xw = int(round((max_x - min_x) / xy_resolution))
yw = int(round((max_y - min_y) / xy_resolution))
print("The grid map is ", xw, "x", yw, ".")
return minx, miny, maxx, maxy, xw, yw
return min_x, min_y, max_x, max_y, xw, yw
def atan_zero_to_twopi(y, x):
@@ -94,96 +97,110 @@ def atan_zero_to_twopi(y, x):
return angle
def init_floodfill(cpoint, opoints, xypoints, mincoord, xyreso):
def init_flood_fill(center_point, obstacle_points, xy_points, min_coord,
xy_resolution):
"""
cpoint: center point
opoints: detected obstacles points (x,y)
xypoints: (x,y) point pairs
center_point: center point
obstacle_points: detected obstacles points (x,y)
xy_points: (x,y) point pairs
"""
centix, centiy = cpoint
prev_ix, prev_iy = centix - 1, centiy
ox, oy = opoints
xw, yw = xypoints
minx, miny = mincoord
pmap = (np.ones((xw, yw))) * 0.5
center_x, center_y = center_point
prev_ix, prev_iy = center_x - 1, center_y
ox, oy = obstacle_points
xw, yw = xy_points
min_x, min_y = min_coord
occupancy_map = (np.ones((xw, yw))) * 0.5
for (x, y) in zip(ox, oy):
ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area
iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area
# x coordinate of the the occupied area
ix = int(round((x - min_x) / xy_resolution))
# y coordinate of the the occupied area
iy = int(round((y - min_y) / xy_resolution))
free_area = bresenham((prev_ix, prev_iy), (ix, iy))
for fa in free_area:
pmap[fa[0]][fa[1]] = 0 # free area 0.0
occupancy_map[fa[0]][fa[1]] = 0 # free area 0.0
prev_ix = ix
prev_iy = iy
return pmap
return occupancy_map
def flood_fill(cpoint, pmap):
def flood_fill(center_point, occupancy_map):
"""
cpoint: starting point (x,y) of fill
pmap: occupancy map generated from Bresenham ray-tracing
center_point: starting point (x,y) of fill
occupancy_map: occupancy map generated from Bresenham ray-tracing
"""
# Fill empty areas with queue method
sx, sy = pmap.shape
sx, sy = occupancy_map.shape
fringe = deque()
fringe.appendleft(cpoint)
fringe.appendleft(center_point)
while fringe:
n = fringe.pop()
nx, ny = n
# West
if nx > 0:
if pmap[nx - 1, ny] == 0.5:
pmap[nx - 1, ny] = 0.0
if occupancy_map[nx - 1, ny] == 0.5:
occupancy_map[nx - 1, ny] = 0.0
fringe.appendleft((nx - 1, ny))
# East
if nx < sx - 1:
if pmap[nx + 1, ny] == 0.5:
pmap[nx + 1, ny] = 0.0
if occupancy_map[nx + 1, ny] == 0.5:
occupancy_map[nx + 1, ny] = 0.0
fringe.appendleft((nx + 1, ny))
# North
if ny > 0:
if pmap[nx, ny - 1] == 0.5:
pmap[nx, ny - 1] = 0.0
if occupancy_map[nx, ny - 1] == 0.5:
occupancy_map[nx, ny - 1] = 0.0
fringe.appendleft((nx, ny - 1))
# South
if ny < sy - 1:
if pmap[nx, ny + 1] == 0.5:
pmap[nx, ny + 1] = 0.0
if occupancy_map[nx, ny + 1] == 0.5:
occupancy_map[nx, ny + 1] = 0.0
fringe.appendleft((nx, ny + 1))
def generate_ray_casting_grid_map(ox, oy, xyreso, breshen=True):
def generate_ray_casting_grid_map(ox, oy, xy_resolution, breshen=True):
"""
The breshen boolean tells if it's computed with bresenham ray casting (True) or with flood fill (False)
The breshen boolean tells if it's computed with bresenham ray casting
(True) or with flood fill (False)
"""
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
pmap = np.ones((xw, yw))/2 # default 0.5 -- [[0.5 for i in range(yw)] for i in range(xw)]
centix = int(round(-minx / xyreso)) # center x coordinate of the grid map
centiy = int(round(-miny / xyreso)) # center y coordinate of the grid map
min_x, min_y, max_x, max_y, x_w, y_w = calc_grid_map_config(
ox, oy, xy_resolution)
# default 0.5 -- [[0.5 for i in range(y_w)] for i in range(x_w)]
occupancy_map = np.ones((x_w, y_w)) / 2
center_x = int(
round(-min_x / xy_resolution)) # center x coordinate of the grid map
center_y = int(
round(-min_y / xy_resolution)) # center y coordinate of the grid map
# occupancy grid computed with bresenham ray casting
if breshen:
for (x, y) in zip(ox, oy):
ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area
iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area
laser_beams = bresenham((centix, centiy), (ix, iy)) # line form the lidar to the cooupied point
# x coordinate of the the occupied area
ix = int(round((x - min_x) / xy_resolution))
# y coordinate of the the occupied area
iy = int(round((y - min_y) / xy_resolution))
laser_beams = bresenham((center_x, center_y), (
ix, iy)) # line form the lidar to the occupied point
for laser_beam in laser_beams:
pmap[laser_beam[0]][laser_beam[1]] = 0.0 # free area 0.0
pmap[ix][iy] = 1.0 # occupied area 1.0
pmap[ix+1][iy] = 1.0 # extend the occupied area
pmap[ix][iy+1] = 1.0 # extend the occupied area
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
occupancy_map[laser_beam[0]][
laser_beam[1]] = 0.0 # free area 0.0
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
# occupancy grid computed with with flood fill
else:
pmap = init_floodfill((centix, centiy), (ox, oy), (xw, yw), (minx, miny), xyreso)
flood_fill((centix, centiy), pmap)
pmap = np.array(pmap, dtype=np.float)
occupancy_map = init_flood_fill((center_x, center_y), (ox, oy),
(x_w, y_w),
(min_x, min_y), xy_resolution)
flood_fill((center_x, center_y), occupancy_map)
occupancy_map = np.array(occupancy_map, dtype=np.float)
for (x, y) in zip(ox, oy):
ix = int(round((x - minx) / xyreso))
iy = int(round((y - miny) / xyreso))
pmap[ix][iy] = 1.0 # occupied area 1.0
pmap[ix+1][iy] = 1.0 # extend the occupied area
pmap[ix][iy+1] = 1.0 # extend the occupied area
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
return pmap, minx, maxx, miny, maxy, xyreso
ix = int(round((x - min_x) / xy_resolution))
iy = int(round((y - min_y) / xy_resolution))
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
return occupancy_map, min_x, max_x, min_y, max_y, xy_resolution
def main():
@@ -191,18 +208,20 @@ def main():
Example usage
"""
print(__file__, "start")
xyreso = 0.02 # x-y grid resolution
xy_resolution = 0.02 # x-y grid resolution
ang, dist = file_read("lidar01.csv")
ox = np.sin(ang) * dist
oy = np.cos(ang) * dist
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(ox, oy, xyreso, True)
xyres = np.array(pmap).shape
plt.figure(1, figsize=(10,4))
occupancy_map, min_x, max_x, min_y, max_y, xy_resolution = \
generate_ray_casting_grid_map(ox, oy, xy_resolution, True)
xy_res = np.array(occupancy_map).shape
plt.figure(1, figsize=(10, 4))
plt.subplot(122)
plt.imshow(pmap, cmap="PiYG_r") # cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r"
plt.imshow(occupancy_map, cmap="PiYG_r")
# cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r"
plt.clim(-0.4, 1.4)
plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor=True)
plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor=True)
plt.gca().set_xticks(np.arange(-.5, xy_res[1], 1), minor=True)
plt.gca().set_yticks(np.arange(-.5, xy_res[0], 1), minor=True)
plt.grid(True, which="minor", color="w", linewidth=0.6, alpha=0.5)
plt.colorbar()
plt.subplot(121)
@@ -210,8 +229,8 @@ def main():
plt.axis("equal")
plt.plot(0.0, 0.0, "ob")
plt.gca().set_aspect("equal", "box")
bottom, top = plt.ylim() # return the current ylim
plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
bottom, top = plt.ylim() # return the current y-lim
plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
plt.grid(True)
plt.show()

74
Mapping/rectangle_fitting/rectangle_fitting.py
View File

@@ -5,7 +5,10 @@ Object shape recognition with L-shape fitting
author: Atsushi Sakai (@Atsushi_twi)
Ref:
- [Efficient L\-Shape Fitting for Vehicle Detection Using Laser Scanners \- The Robotics Institute Carnegie Mellon University](https://www.ri.cmu.edu/publications/efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners/)
- Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners -
The Robotics Institute Carnegie Mellon University
https://www.ri.cmu.edu/publications/
efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners/
"""
@@ -13,13 +16,14 @@ import matplotlib.pyplot as plt
import numpy as np
import itertools
from enum import Enum
from scipy.spatial.transform import Rotation as Rot
from simulator import VehicleSimulator, LidarSimulator
show_animation = True
class LShapeFitting():
class LShapeFitting:
class Criteria(Enum):
AREA = 1
@@ -29,26 +33,27 @@ class LShapeFitting():
def __init__(self):
# Parameters
self.criteria = self.Criteria.VARIANCE
self.min_dist_of_closeness_crit = 0.01 # [m]
self.dtheta_deg_for_serarch = 1.0 # [deg]
self.min_dist_of_closeness_criteria = 0.01 # [m]
self.d_theta_deg_for_search = 1.0 # [deg]
self.R0 = 3.0 # [m] range segmentation param
self.Rd = 0.001 # [m] range segmentation param
def fitting(self, ox, oy):
# step1: Adaptive Range Segmentation
idsets = self._adoptive_range_segmentation(ox, oy)
id_sets = self._adoptive_range_segmentation(ox, oy)
# step2 Rectangle search
rects = []
for ids in idsets: # for each cluster
for ids in id_sets: # for each cluster
cx = [ox[i] for i in range(len(ox)) if i in ids]
cy = [oy[i] for i in range(len(oy)) if i in ids]
rects.append(self._rectangle_search(cx, cy))
return rects, idsets
return rects, id_sets
def _calc_area_criterion(self, c1, c2):
@staticmethod
def _calc_area_criterion(c1, c2):
c1_max = max(c1)
c2_max = max(c2)
c1_min = min(c1)
@@ -71,12 +76,13 @@ class LShapeFitting():
beta = 0
for i, _ in enumerate(D1):
d = max(min([D1[i], D2[i]]), self.min_dist_of_closeness_crit)
d = max(min([D1[i], D2[i]]), self.min_dist_of_closeness_criteria)
beta += (1.0 / d)
return beta
def _calc_variance_criterion(self, c1, c2):
@staticmethod
def _calc_variance_criterion(c1, c2):
c1_max = max(c1)
c2_max = max(c2)
c1_min = min(c1)
@@ -110,17 +116,17 @@ class LShapeFitting():
X = np.array([x, y]).T
dtheta = np.deg2rad(self.dtheta_deg_for_serarch)
minp = (-float('inf'), None)
for theta in np.arange(0.0, np.pi / 2.0 - dtheta, dtheta):
d_theta = np.deg2rad(self.d_theta_deg_for_search)
min_cost = (-float('inf'), None)
for theta in np.arange(0.0, np.pi / 2.0 - d_theta, d_theta):
e1 = np.array([np.cos(theta), np.sin(theta)])
e2 = np.array([-np.sin(theta), np.cos(theta)])
c1 = X @ e1.T
c2 = X @ e2.T
rot = Rot.from_euler('z', theta).as_matrix()[0:2, 0:2]
c = X @ rot
c1 = c[:, 0]
c2 = c[:, 1]
# Select criteria
cost = 0.0
if self.criteria == self.Criteria.AREA:
cost = self._calc_area_criterion(c1, c2)
elif self.criteria == self.Criteria.CLOSENESS:
@@ -128,12 +134,12 @@ class LShapeFitting():
elif self.criteria == self.Criteria.VARIANCE:
cost = self._calc_variance_criterion(c1, c2)
if minp[0] < cost:
minp = (cost, theta)
if min_cost[0] < cost:
min_cost = (cost, theta)
# calc best rectangle
sin_s = np.sin(minp[1])
cos_s = np.cos(minp[1])
sin_s = np.sin(min_cost[1])
cos_s = np.cos(min_cost[1])
c1_s = X @ np.array([cos_s, sin_s]).T
c2_s = X @ np.array([-sin_s, cos_s]).T
@@ -181,7 +187,7 @@ class LShapeFitting():
return S
class RectangleData():
class RectangleData:
def __init__(self):
self.a = [None] * 4
@@ -207,7 +213,8 @@ class RectangleData():
[self.a[3], self.a[0]], [self.b[3], self.b[0]], [self.c[3], self.c[0]])
self.rect_c_x[4], self.rect_c_y[4] = self.rect_c_x[0], self.rect_c_y[0]
def calc_cross_point(self, a, b, c):
@staticmethod
def calc_cross_point(a, b, c):
x = (b[0] * -c[1] - b[1] * -c[0]) / (a[0] * b[1] - a[1] * b[0])
y = (a[1] * -c[0] - a[0] * -c[1]) / (a[0] * b[1] - a[1] * b[0])
return x, y
@@ -216,42 +223,43 @@ class RectangleData():
def main():
# simulation parameters
simtime = 30.0 # simulation time
sim_time = 30.0 # simulation time
dt = 0.2 # time tick
angle_reso = np.deg2rad(3.0) # sensor angle resolution
angle_resolution = np.deg2rad(3.0) # sensor angle resolution
v1 = VehicleSimulator(-10.0, 0.0, np.deg2rad(90.0),
0.0, 50.0 / 3.6, 3.0, 5.0)
v2 = VehicleSimulator(20.0, 10.0, np.deg2rad(180.0),
0.0, 50.0 / 3.6, 4.0, 10.0)
lshapefitting = LShapeFitting()
l_shape_fitting = LShapeFitting()
lidar_sim = LidarSimulator()
time = 0.0
while time <= simtime:
while time <= sim_time:
time += dt
v1.update(dt, 0.1, 0.0)
v2.update(dt, 0.1, -0.05)
ox, oy = lidar_sim.get_observation_points([v1, v2], angle_reso)
ox, oy = lidar_sim.get_observation_points([v1, v2], angle_resolution)
rects, idsets = lshapefitting.fitting(ox, oy)
rects, id_sets = l_shape_fitting.fitting(ox, oy)
if show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.axis("equal")
plt.plot(0.0, 0.0, "*r")
v1.plot()
v2.plot()
# Plot range observation
for ids in idsets:
for ids in id_sets:
x = [ox[i] for i in range(len(ox)) if i in ids]
y = [oy[i] for i in range(len(ox)) if i in ids]

81
Mapping/rectangle_fitting/simulator.py
View File

@@ -10,15 +10,16 @@ import numpy as np
import matplotlib.pyplot as plt
import math
import random
from scipy.spatial.transform import Rotation as Rot
class VehicleSimulator():
class VehicleSimulator:
def __init__(self, ix, iy, iyaw, iv, max_v, w, L):
self.x = ix
self.y = iy
self.yaw = iyaw
self.v = iv
def __init__(self, i_x, i_y, i_yaw, i_v, max_v, w, L):
self.x = i_x
self.y = i_y
self.yaw = i_yaw
self.v = i_v
self.max_v = max_v
self.W = w
self.L = L
@@ -40,10 +41,10 @@ class VehicleSimulator():
plt.plot(gx, gy, "--b")
def calc_global_contour(self):
gx = [(ix * np.cos(self.yaw) + iy * np.sin(self.yaw)) +
self.x for (ix, iy) in zip(self.vc_x, self.vc_y)]
gy = [(ix * np.sin(self.yaw) - iy * np.cos(self.yaw)) +
self.y for (ix, iy) in zip(self.vc_x, self.vc_y)]
rot = Rot.from_euler('z', self.yaw).as_matrix()[0:2, 0:2]
gxy = np.stack([self.vc_x, self.vc_y]).T @ rot
gx = gxy[:, 0] + self.x
gy = gxy[:, 1] + self.y
return gx, gy
@@ -67,69 +68,71 @@ class VehicleSimulator():
self.vc_x.append(self.L / 2.0)
self.vc_y.append(self.W / 2.0)
self.vc_x, self.vc_y = self._interporate(self.vc_x, self.vc_y)
self.vc_x, self.vc_y = self._interpolate(self.vc_x, self.vc_y)
def _interporate(self, x, y):
@staticmethod
def _interpolate(x, y):
rx, ry = [], []
dtheta = 0.05
d_theta = 0.05
for i in range(len(x) - 1):
rx.extend([(1.0 - θ) * x[i] + θ * x[i + 1]
for θ in np.arange(0.0, 1.0, dtheta)])
ry.extend([(1.0 - θ) * y[i] + θ * y[i + 1]
for θ in np.arange(0.0, 1.0, dtheta)])
rx.extend([(1.0 - theta) * x[i] + theta * x[i + 1]
for theta in np.arange(0.0, 1.0, d_theta)])
ry.extend([(1.0 - theta) * y[i] + theta * y[i + 1]
for theta in np.arange(0.0, 1.0, d_theta)])
rx.extend([(1.0 - θ) * x[len(x) - 1] + θ * x[1]
for θ in np.arange(0.0, 1.0, dtheta)])
ry.extend([(1.0 - θ) * y[len(y) - 1] + θ * y[1]
for θ in np.arange(0.0, 1.0, dtheta)])
rx.extend([(1.0 - theta) * x[len(x) - 1] + theta * x[1]
for theta in np.arange(0.0, 1.0, d_theta)])
ry.extend([(1.0 - theta) * y[len(y) - 1] + theta * y[1]
for theta in np.arange(0.0, 1.0, d_theta)])
return rx, ry
class LidarSimulator():
class LidarSimulator:
def __init__(self):
self.range_noise = 0.01
def get_observation_points(self, vlist, angle_reso):
def get_observation_points(self, v_list, angle_resolution):
x, y, angle, r = [], [], [], []
# store all points
for v in vlist:
for v in v_list:
gx, gy = v.calc_global_contour()
for vx, vy in zip(gx, gy):
vangle = math.atan2(vy, vx)
v_angle = math.atan2(vy, vx)
vr = np.hypot(vx, vy) * random.uniform(1.0 - self.range_noise,
1.0 + self.range_noise)
x.append(vx)
y.append(vy)
angle.append(vangle)
angle.append(v_angle)
r.append(vr)
# ray casting filter
rx, ry = self.ray_casting_filter(x, y, angle, r, angle_reso)
rx, ry = self.ray_casting_filter(angle, r, angle_resolution)
return rx, ry
def ray_casting_filter(self, xl, yl, thetal, rangel, angle_reso):
@staticmethod
def ray_casting_filter(theta_l, range_l, angle_resolution):
rx, ry = [], []
rangedb = [float("inf") for _ in range(
int(np.floor((np.pi * 2.0) / angle_reso)) + 1)]
range_db = [float("inf") for _ in range(
int(np.floor((np.pi * 2.0) / angle_resolution)) + 1)]
for i in range(len(thetal)):
angleid = int(round(thetal[i] / angle_reso))
for i in range(len(theta_l)):
angle_id = int(round(theta_l[i] / angle_resolution))
if rangedb[angleid] > rangel[i]:
rangedb[angleid] = rangel[i]
if range_db[angle_id] > range_l[i]:
range_db[angle_id] = range_l[i]
for i in range(len(rangedb)):
t = i * angle_reso
if rangedb[i] != float("inf"):
rx.append(rangedb[i] * np.cos(t))
ry.append(rangedb[i] * np.sin(t))
for i in range(len(range_db)):
t = i * angle_resolution
if range_db[i] != float("inf"):
rx.append(range_db[i] * np.cos(t))
ry.append(range_db[i] * np.sin(t))
return rx, ry

130
PathPlanning/AStar/a_star.py
View File

@@ -18,39 +18,43 @@ show_animation = True
class AStarPlanner:
def __init__(self, ox, oy, reso, rr):
def __init__(self, ox, oy, resolution, 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]
resolution: grid resolution [m]
rr: robot radius[m]
"""
self.reso = reso
self.resolution = resolution
self.rr = rr
self.calc_obstacle_map(ox, oy)
self.min_x, self.min_y = 0, 0
self.max_x, self.max_y = 0, 0
self.obstacle_map = None
self.x_width, self.y_width = 0, 0
self.motion = self.get_motion_model()
self.calc_obstacle_map(ox, oy)
class Node:
def __init__(self, x, y, cost, pind):
def __init__(self, x, y, cost, parent_index):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.pind = pind
self.parent_index = parent_index
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(
self.cost) + "," + str(self.pind)
self.cost) + "," + str(self.parent_index)
def planning(self, sx, sy, gx, gy):
"""
A star path search
input:
sx: start x position [m]
sy: start y position [m]
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
@@ -59,13 +63,13 @@ class AStarPlanner:
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)
start_node = self.Node(self.calc_xy_index(sx, self.min_x),
self.calc_xy_index(sy, self.min_y), 0.0, -1)
goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
self.calc_xy_index(gy, self.min_y), 0.0, -1)
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(nstart)] = nstart
open_set[self.calc_grid_index(start_node)] = start_node
while 1:
if len(open_set) == 0:
@@ -74,15 +78,15 @@ class AStarPlanner:
c_id = min(
open_set,
key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal,
key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,
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")
plt.plot(self.calc_grid_position(current.x, self.min_x),
self.calc_grid_position(current.y, self.min_y), "xc")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(
@@ -90,10 +94,10 @@ class AStarPlanner:
if len(closed_set.keys()) % 10 == 0:
plt.pause(0.001)
if current.x == ngoal.x and current.y == ngoal.y:
if current.x == goal_node.x and current.y == goal_node.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
goal_node.parent_index = current.parent_index
goal_node.cost = current.cost
break
# Remove the item from the open set
@@ -123,20 +127,20 @@ class AStarPlanner:
# This path is the best until now. record it
open_set[n_id] = node
rx, ry = self.calc_final_path(ngoal, closed_set)
rx, ry = self.calc_final_path(goal_node, closed_set)
return rx, ry
def calc_final_path(self, ngoal, closedset):
def calc_final_path(self, goal_node, closed_set):
# 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
rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
self.calc_grid_position(goal_node.y, self.min_y)]
parent_index = goal_node.parent_index
while parent_index != -1:
n = closed_set[parent_index]
rx.append(self.calc_grid_position(n.x, self.min_x))
ry.append(self.calc_grid_position(n.y, self.min_y))
parent_index = n.parent_index
return rx, ry
@@ -146,69 +150,69 @@ class AStarPlanner:
d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
return d
def calc_grid_position(self, index, minp):
def calc_grid_position(self, index, min_position):
"""
calc grid position
:param index:
:param minp:
:param min_position:
:return:
"""
pos = index * self.reso + minp
pos = index * self.resolution + min_position
return pos
def calc_xyindex(self, position, min_pos):
return round((position - min_pos) / self.reso)
def calc_xy_index(self, position, min_pos):
return round((position - min_pos) / self.resolution)
def calc_grid_index(self, node):
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
def verify_node(self, node):
px = self.calc_grid_position(node.x, self.minx)
py = self.calc_grid_position(node.y, self.miny)
px = self.calc_grid_position(node.x, self.min_x)
py = self.calc_grid_position(node.y, self.min_y)
if px < self.minx:
if px < self.min_x:
return False
elif py < self.miny:
elif py < self.min_y:
return False
elif px >= self.maxx:
elif px >= self.max_x:
return False
elif py >= self.maxy:
elif py >= self.max_y:
return False
# collision check
if self.obmap[node.x][node.y]:
if self.obstacle_map[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.min_x = round(min(ox))
self.min_y = round(min(oy))
self.max_x = round(max(ox))
self.max_y = round(max(oy))
print("min_x:", self.min_x)
print("min_y:", self.min_y)
print("max_x:", self.max_x)
print("max_y:", self.max_y)
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)
self.x_width = round((self.max_x - self.min_x) / self.resolution)
self.y_width = round((self.max_y - self.min_y) / self.resolution)
print("x_width:", self.x_width)
print("y_width:", self.y_width)
# 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)
self.obstacle_map = [[False for _ in range(self.y_width)]
for _ in range(self.x_width)]
for ix in range(self.x_width):
x = self.calc_grid_position(ix, self.min_x)
for iy in range(self.y_width):
y = self.calc_grid_position(iy, self.min_y)
for iox, ioy in zip(ox, oy):
d = math.hypot(iox - x, ioy - y)
if d <= self.rr:
self.obmap[ix][iy] = True
self.obstacle_map[ix][iy] = True
break
@staticmethod
@@ -270,8 +274,8 @@ def main():
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.show()
plt.pause(0.001)
plt.show()
if __name__ == '__main__':

22
PathPlanning/BidirectionalAStar/bidirectional_a_star.py
View File

@@ -23,7 +23,7 @@ class BidirectionalAStarPlanner:
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
@@ -48,8 +48,8 @@ class BidirectionalAStarPlanner:
Bidirectional A star path search
input:
sx: start x position [m]
sy: start y position [m]
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
@@ -179,12 +179,12 @@ class BidirectionalAStarPlanner:
# generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)]
pind = ngoal.pind
pind = ngoal.parent_index
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
pind = n.parent_index
return rx, ry
@@ -252,15 +252,15 @@ class BidirectionalAStarPlanner:
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)
print("min_x:", self.minx)
print("min_y:", self.miny)
print("max_x:", self.maxx)
print("max_y:", 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)
print("x_width:", self.xwidth)
print("y_width:", self.ywidth)
# obstacle map generation
self.obmap = [[False for _ in range(self.ywidth)]

20
PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py
View File

@@ -23,7 +23,7 @@ class BidirectionalBreadthFirstSearchPlanner:
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
@@ -49,8 +49,8 @@ class BidirectionalBreadthFirstSearchPlanner:
Bidirectional Breadth First search based planning
input:
sx: start x position [m]
sy: start y position [m]
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
@@ -170,7 +170,7 @@ class BidirectionalBreadthFirstSearchPlanner:
# generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)]
n = closedset[ngoal.pind]
n = closedset[ngoal.parent_index]
while n is not None:
rx.append(self.calc_grid_position(n.x, self.minx))
ry.append(self.calc_grid_position(n.y, self.miny))
@@ -220,15 +220,15 @@ class BidirectionalBreadthFirstSearchPlanner:
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)
print("min_x:", self.minx)
print("min_y:", self.miny)
print("max_x:", self.maxx)
print("max_y:", 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)
print("x_width:", self.xwidth)
print("y_width:", self.ywidth)
# obstacle map generation
self.obmap = [[False for _ in range(self.ywidth)]

20
PathPlanning/BreadthFirstSearch/breadth_first_search.py
View File

@@ -23,7 +23,7 @@ class BreadthFirstSearchPlanner:
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
@@ -49,8 +49,8 @@ class BreadthFirstSearchPlanner:
Breadth First search based planning
input:
sx: start x position [m]
sy: start y position [m]
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
@@ -118,7 +118,7 @@ class BreadthFirstSearchPlanner:
# generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)]
n = closedset[ngoal.pind]
n = closedset[ngoal.parent_index]
while n is not None:
rx.append(self.calc_grid_position(n.x, self.minx))
ry.append(self.calc_grid_position(n.y, self.miny))
@@ -168,15 +168,15 @@ class BreadthFirstSearchPlanner:
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)
print("min_x:", self.minx)
print("min_y:", self.miny)
print("max_x:", self.maxx)
print("max_y:", 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)
print("x_width:", self.xwidth)
print("y_width:", self.ywidth)
# obstacle map generation
self.obmap = [[False for _ in range(self.ywidth)]

6
PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
View File

@@ -40,7 +40,7 @@ def pure_pursuit_control(state, cx, cy, pind):
if pind >= ind:
ind = pind
# print(pind, ind)
# print(parent_index, ind)
if ind < len(cx):
tx = cx[ind]
ty = cy[ind]
@@ -181,12 +181,12 @@ def set_stop_point(target_speed, cx, cy, cyaw):
speed_profile[i] = 0.0
forward = False
# plt.plot(cx[i], cy[i], "xb")
# print(iyaw, move_direction, dx, dy)
# print(i_yaw, move_direction, dx, dy)
elif not is_back and not forward:
speed_profile[i] = 0.0
forward = True
# plt.plot(cx[i], cy[i], "xb")
# print(iyaw, move_direction, dx, dy)
# print(i_yaw, move_direction, dx, dy)
speed_profile[0] = 0.0
if is_back:
speed_profile[-1] = -stop_speed

20
PathPlanning/DepthFirstSearch/depth_first_search.py
View File

@@ -23,7 +23,7 @@ class DepthFirstSearchPlanner:
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
@@ -49,8 +49,8 @@ class DepthFirstSearchPlanner:
Depth First search
input:
sx: start x position [m]
sy: start y position [m]
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
@@ -115,7 +115,7 @@ class DepthFirstSearchPlanner:
# generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)]
n = closedset[ngoal.pind]
n = closedset[ngoal.parent_index]
while n is not None:
rx.append(self.calc_grid_position(n.x, self.minx))
ry.append(self.calc_grid_position(n.y, self.miny))
@@ -165,15 +165,15 @@ class DepthFirstSearchPlanner:
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)
print("min_x:", self.minx)
print("min_y:", self.miny)
print("max_x:", self.maxx)
print("max_y:", 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)
print("x_width:", self.xwidth)
print("y_width:", self.ywidth)
# obstacle map generation
self.obmap = [[False for _ in range(self.ywidth)]

4
PathPlanning/Dijkstra/dijkstra.py
View File

@@ -53,8 +53,8 @@ class Dijkstra:
dijkstra path search
input:
sx: start x position [m]
sy: start y position [m]
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gx: goal x position [m]

96
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -9,6 +9,7 @@ import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
show_animation = True
@@ -136,7 +137,8 @@ def left_right_left(alpha, beta, d):
return t, p, q, mode
def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size):
def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature,
step_size):
dx = end_x
dy = end_y
D = math.hypot(dx, dy)
@@ -146,7 +148,8 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size
alpha = mod2pi(- theta)
beta = mod2pi(end_yaw - theta)
planners = [left_straight_left, right_straight_right, left_straight_right, right_straight_left, right_left_right,
planners = [left_straight_left, right_straight_right, left_straight_right,
right_straight_left, right_left_right,
left_right_left]
best_cost = float("inf")
@@ -163,13 +166,14 @@ def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size
best_cost = cost
lengths = [bt, bp, bq]
px, py, pyaw, directions = generate_local_course(
x_list, y_list, yaw_list, directions = generate_local_course(
sum(lengths), lengths, best_mode, curvature, step_size)
return px, py, pyaw, best_mode, best_cost
return x_list, y_list, yaw_list, best_mode, best_cost
def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions):
def interpolate(ind, length, mode, max_curvature, origin_x, origin_y,
origin_yaw, path_x, path_y, path_yaw, directions):
if mode == "S":
path_x[ind] = origin_x + length / max_curvature * math.cos(origin_yaw)
path_y[ind] = origin_y + length / max_curvature * math.sin(origin_yaw)
@@ -199,54 +203,49 @@ def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw
return path_x, path_y, path_yaw, directions
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, step_size=0.1):
def dubins_path_planning(s_x, s_y, s_yaw, g_x, g_y, g_yaw, c, step_size=0.1):
"""
Dubins path plannner
Dubins path planner
input:
sx x position of start point [m]
sy y position of start point [m]
syaw yaw angle of start point [rad]
ex x position of end point [m]
ey y position of end point [m]
eyaw yaw angle of end point [rad]
s_x x position of start point [m]
s_y y position of start point [m]
s_yaw yaw angle of start point [rad]
g_x x position of end point [m]
g_y y position of end point [m]
g_yaw yaw angle of end point [rad]
c curvature [1/m]
output:
px
py
pyaw
mode
"""
ex = ex - sx
ey = ey - sy
g_x = g_x - s_x
g_y = g_y - s_y
lex = math.cos(syaw) * ex + math.sin(syaw) * ey
ley = - math.sin(syaw) * ex + math.cos(syaw) * ey
leyaw = eyaw - syaw
l_rot = Rot.from_euler('z', s_yaw).as_matrix()[0:2, 0:2]
le_xy = np.stack([g_x, g_y]).T @ l_rot
le_yaw = g_yaw - s_yaw
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c, step_size)
lp_x, lp_y, lp_yaw, mode, lengths = dubins_path_planning_from_origin(
le_xy[0], le_xy[1], le_yaw, c, step_size)
px = [math.cos(-syaw) * x + math.sin(-syaw)
* y + sx for x, y in zip(lpx, lpy)]
py = [- math.sin(-syaw) * x + math.cos(-syaw)
* y + sy for x, y in zip(lpx, lpy)]
pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw]
rot = Rot.from_euler('z', -s_yaw).as_matrix()[0:2, 0:2]
converted_xy = np.stack([lp_x, lp_y]).T @ rot
x_list = converted_xy[:, 0] + s_x
y_list = converted_xy[:, 1] + s_y
yaw_list = [pi_2_pi(i_yaw + s_yaw) for i_yaw in lp_yaw]
return px, py, pyaw, mode, clen
return x_list, y_list, yaw_list, mode, lengths
def generate_local_course(total_length, lengths, mode, max_curvature, step_size):
def generate_local_course(total_length, lengths, mode, max_curvature,
step_size):
n_point = math.trunc(total_length / step_size) + len(lengths) + 4
path_x = [0.0 for _ in range(n_point)]
path_y = [0.0 for _ in range(n_point)]
path_yaw = [0.0 for _ in range(n_point)]
directions = [0.0 for _ in range(n_point)]
ind = 1
index = 1
if lengths[0] > 0.0:
directions[0] = 1
@@ -262,25 +261,28 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size)
d = -step_size
# set origin state
origin_x, origin_y, origin_yaw = path_x[ind], path_y[ind], path_yaw[ind]
origin_x, origin_y, origin_yaw = \
path_x[index], path_y[index], path_yaw[index]
ind -= 1
index -= 1
if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
pd = - d - ll
else:
pd = d - ll
while abs(pd) <= abs(l):
ind += 1
index += 1
path_x, path_y, path_yaw, directions = interpolate(
ind, pd, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions)
index, pd, m, max_curvature, origin_x, origin_y, origin_yaw,
path_x, path_y, path_yaw, directions)
pd += d
ll = l - pd - d # calc remain length
ind += 1
index += 1
path_x, path_y, path_yaw, directions = interpolate(
ind, l, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions)
index, l, m, max_curvature, origin_x, origin_y, origin_yaw,
path_x, path_y, path_yaw, directions)
if len(path_x) <= 1:
return [], [], [], []
@@ -295,14 +297,15 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size)
return path_x, path_y, path_yaw, directions
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r",
ec="k"): # pragma: no cover
"""
Plot arrow
"""
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
plot_arrow(ix, iy, iyaw)
for (i_x, i_y, i_yaw) in zip(x, y, yaw):
plot_arrow(i_x, i_y, i_yaw)
else:
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width)
@@ -322,11 +325,12 @@ def main():
curvature = 1.0
px, py, pyaw, mode, clen = dubins_path_planning(start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature)
path_x, path_y, path_yaw, mode, path_length = dubins_path_planning(
start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature)
if show_animation:
plt.plot(px, py, label="final course " + "".join(mode))
plt.plot(path_x, path_y, label="final course " + "".join(mode))
# plotting
plot_arrow(start_x, start_y, start_yaw)

36
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -41,11 +41,11 @@ class Config:
# robot parameter
self.max_speed = 1.0 # [m/s]
self.min_speed = -0.5 # [m/s]
self.max_yawrate = 40.0 * math.pi / 180.0 # [rad/s]
self.max_yaw_rate = 40.0 * math.pi / 180.0 # [rad/s]
self.max_accel = 0.2 # [m/ss]
self.max_dyawrate = 40.0 * math.pi / 180.0 # [rad/ss]
self.v_reso = 0.01 # [m/s]
self.yawrate_reso = 0.1 * math.pi / 180.0 # [rad/s]
self.max_delta_yaw_rate = 40.0 * math.pi / 180.0 # [rad/ss]
self.v_resolution = 0.01 # [m/s]
self.yaw_rate_resolution = 0.1 * math.pi / 180.0 # [rad/s]
self.dt = 0.1 # [s] Time tick for motion prediction
self.predict_time = 3.0 # [s]
self.to_goal_cost_gain = 0.15
@@ -93,15 +93,15 @@ def calc_dynamic_window(x, config):
# Dynamic window from robot specification
Vs = [config.min_speed, config.max_speed,
-config.max_yawrate, config.max_yawrate]
-config.max_yaw_rate, config.max_yaw_rate]
# Dynamic window from motion model
Vd = [x[3] - config.max_accel * config.dt,
x[3] + config.max_accel * config.dt,
x[4] - config.max_dyawrate * config.dt,
x[4] + config.max_dyawrate * config.dt]
x[4] - config.max_delta_yaw_rate * config.dt,
x[4] + config.max_delta_yaw_rate * config.dt]
# [vmin, vmax, yaw_rate min, yaw_rate max]
# [v_min, v_max, yaw_rate_min, yaw_rate_max]
dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
@@ -114,14 +114,14 @@ def predict_trajectory(x_init, v, y, config):
"""
x = np.array(x_init)
traj = np.array(x)
trajectory = np.array(x)
time = 0
while time <= config.predict_time:
x = motion(x, [v, y], config.dt)
traj = np.vstack((traj, x))
trajectory = np.vstack((trajectory, x))
time += config.dt
return traj
return trajectory
def calc_control_and_trajectory(x, dw, config, goal, ob):
@@ -135,8 +135,8 @@ def calc_control_and_trajectory(x, dw, config, goal, ob):
best_trajectory = np.array([x])
# evaluate all trajectory with sampled input in dynamic window
for v in np.arange(dw[0], dw[1], config.v_reso):
for y in np.arange(dw[2], dw[3], config.yawrate_reso):
for v in np.arange(dw[0], dw[1], config.v_resolution):
for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):
trajectory = predict_trajectory(x_init, v, y, config)
@@ -182,7 +182,7 @@ def calc_obstacle_cost(trajectory, ob, config):
np.logical_and(bottom_check, left_check))).any():
return float("Inf")
elif config.robot_type == RobotType.circle:
if (r <= config.robot_radius).any():
if np.array(r <= config.robot_radius).any():
return float("Inf")
min_r = np.min(r)
@@ -269,8 +269,9 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g")
plt.plot(x[0], x[1], "xr")
plt.plot(goal[0], goal[1], "xb")
@@ -296,4 +297,5 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
if __name__ == '__main__':
main(robot_type=RobotType.circle)
main(robot_type=RobotType.rectangle)
# main(robot_type=RobotType.circle)

20
PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py
View File

@@ -23,7 +23,7 @@ class BestFirstSearchPlanner:
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
@@ -49,8 +49,8 @@ class BestFirstSearchPlanner:
Greedy Best-First search
input:
sx: start x position [m]
sy: start y position [m]
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
@@ -132,7 +132,7 @@ class BestFirstSearchPlanner:
# generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)]
n = closedset[ngoal.pind]
n = closedset[ngoal.parent_index]
while n is not None:
rx.append(self.calc_grid_position(n.x, self.minx))
ry.append(self.calc_grid_position(n.y, self.miny))
@@ -188,15 +188,15 @@ class BestFirstSearchPlanner:
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)
print("min_x:", self.minx)
print("min_y:", self.miny)
print("max_x:", self.maxx)
print("max_y:", 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)
print("x_width:", self.xwidth)
print("y_width:", self.ywidth)
# obstacle map generation
self.obmap = [[False for _ in range(self.ywidth)]

187
PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
View File

@@ -11,6 +11,7 @@ from enum import IntEnum
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
sys.path.append(os.path.relpath("../../Mapping/grid_map_lib/"))
try:
@@ -36,52 +37,55 @@ class SweepSearcher:
self.sweep_direction = sweep_direction
self.turing_window = []
self.update_turning_window()
self.xinds_goaly = x_inds_goal_y
self.goaly = goal_y
self.x_indexes_goal_y = x_inds_goal_y
self.goal_y = goal_y
def move_target_grid(self, cxind, cyind, gmap):
nxind = self.moving_direction + cxind
nyind = cyind
def move_target_grid(self, c_x_index, c_y_index, grid_map):
n_x_index = self.moving_direction + c_x_index
n_y_index = c_y_index
# found safe grid
if not gmap.check_occupied_from_xy_index(nxind, nyind,
occupied_val=0.5):
return nxind, nyind
if not grid_map.check_occupied_from_xy_index(n_x_index, n_y_index,
occupied_val=0.5):
return n_x_index, n_y_index
else: # occupied
ncxind, ncyind = self.find_safe_turning_grid(cxind, cyind, gmap)
if (ncxind is None) and (ncyind is None):
next_c_x_index, next_c_y_index = self.find_safe_turning_grid(
c_x_index, c_y_index, grid_map)
if (next_c_x_index is None) and (next_c_y_index is None):
# moving backward
ncxind = -self.moving_direction + cxind
ncyind = cyind
if gmap.check_occupied_from_xy_index(ncxind, ncyind):
next_c_x_index = -self.moving_direction + c_x_index
next_c_y_index = c_y_index
if grid_map.check_occupied_from_xy_index(next_c_x_index,
next_c_y_index):
# moved backward, but the grid is occupied by obstacle
return None, None
else:
# keep moving until end
while not gmap.check_occupied_from_xy_index(
ncxind + self.moving_direction, ncyind,
occupied_val=0.5):
ncxind += self.moving_direction
while not grid_map.check_occupied_from_xy_index(
next_c_x_index + self.moving_direction,
next_c_y_index, occupied_val=0.5):
next_c_x_index += self.moving_direction
self.swap_moving_direction()
return ncxind, ncyind
return next_c_x_index, next_c_y_index
def find_safe_turning_grid(self, cxind, cyind, gmap):
def find_safe_turning_grid(self, c_x_index, c_y_index, grid_map):
for (d_x_ind, d_y_ind) in self.turing_window:
next_x_ind = d_x_ind + cxind
next_y_ind = d_y_ind + cyind
next_x_ind = d_x_ind + c_x_index
next_y_ind = d_y_ind + c_y_index
# found safe grid
if not gmap.check_occupied_from_xy_index(next_x_ind, next_y_ind,
occupied_val=0.5):
if not grid_map.check_occupied_from_xy_index(next_x_ind,
next_y_ind,
occupied_val=0.5):
return next_x_ind, next_y_ind
return None, None
def is_search_done(self, grid_map):
for ix in self.xinds_goaly:
if not grid_map.check_occupied_from_xy_index(ix, self.goaly,
for ix in self.x_indexes_goal_y:
if not grid_map.check_occupied_from_xy_index(ix, self.goal_y,
occupied_val=0.5):
return False
@@ -138,64 +142,54 @@ def find_sweep_direction_and_start_position(ox, oy):
return vec, sweep_start_pos
def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi):
tx = [ix - sweep_start_posi[0] for ix in ox]
ty = [iy - sweep_start_posi[1] for iy in oy]
def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_position):
tx = [ix - sweep_start_position[0] for ix in ox]
ty = [iy - sweep_start_position[1] for iy in oy]
th = math.atan2(sweep_vec[1], sweep_vec[0])
rot = Rot.from_euler('z', th).as_matrix()[0:2, 0:2]
converted_xy = np.stack([tx, ty]).T @ rot
c = np.cos(-th)
s = np.sin(-th)
rx = [ix * c - iy * s for (ix, iy) in zip(tx, ty)]
ry = [ix * s + iy * c for (ix, iy) in zip(tx, ty)]
return rx, ry
return converted_xy[:, 0], converted_xy[:, 1]
def convert_global_coordinate(x, y, sweep_vec, sweep_start_posi):
def convert_global_coordinate(x, y, sweep_vec, sweep_start_position):
th = math.atan2(sweep_vec[1], sweep_vec[0])
c = np.cos(th)
s = np.sin(th)
tx = [ix * c - iy * s for (ix, iy) in zip(x, y)]
ty = [ix * s + iy * c for (ix, iy) in zip(x, y)]
rx = [ix + sweep_start_posi[0] for ix in tx]
ry = [iy + sweep_start_posi[1] for iy in ty]
rot = Rot.from_euler('z', -th).as_matrix()[0:2, 0:2]
converted_xy = np.stack([x, y]).T @ rot
rx = [ix + sweep_start_position[0] for ix in converted_xy[:, 0]]
ry = [iy + sweep_start_position[1] for iy in converted_xy[:, 1]]
return rx, ry
def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
yind = None
xinds = []
y_index = None
x_indexes = []
if from_upper:
xrange = range(grid_map.height)[::-1]
yrange = range(grid_map.width)[::-1]
x_range = range(grid_map.height)[::-1]
y_range = range(grid_map.width)[::-1]
else:
xrange = range(grid_map.height)
yrange = range(grid_map.width)
x_range = range(grid_map.height)
y_range = range(grid_map.width)
for iy in xrange:
for ix in yrange:
for iy in x_range:
for ix in y_range:
if not grid_map.check_occupied_from_xy_index(ix, iy):
yind = iy
xinds.append(ix)
if yind:
y_index = iy
x_indexes.append(ix)
if y_index:
break
return xinds, yind
return x_indexes, y_index
def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=10):
width = math.ceil((max(ox) - min(ox)) / reso) + offset_grid
height = math.ceil((max(oy) - min(oy)) / reso) + offset_grid
center_x = (np.max(ox)+np.min(ox))/2.0
center_y = (np.max(oy)+np.min(oy))/2.0
def setup_grid_map(ox, oy, resolution, sweep_direction, offset_grid=10):
width = math.ceil((max(ox) - min(ox)) / resolution) + offset_grid
height = math.ceil((max(oy) - min(oy)) / resolution) + offset_grid
center_x = (np.max(ox) + np.min(ox)) / 2.0
center_y = (np.max(oy) + np.min(oy)) / 2.0
grid_map = GridMap(width, height, reso, center_x, center_y)
grid_map = GridMap(width, height, resolution, center_x, center_y)
grid_map.print_grid_map_info()
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
grid_map.expand_grid()
@@ -203,48 +197,51 @@ def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=10):
x_inds_goal_y = []
goal_y = 0
if sweep_direction == SweepSearcher.SweepDirection.UP:
x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(grid_map,
from_upper=True)
x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(
grid_map, from_upper=True)
elif sweep_direction == SweepSearcher.SweepDirection.DOWN:
x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(grid_map,
from_upper=False)
x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(
grid_map, from_upper=False)
return grid_map, x_inds_goal_y, goal_y
def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False):
# search start grid
cxind, cyind = sweep_searcher.search_start_grid(grid_map)
if not grid_map.set_value_from_xy_index(cxind, cyind, 0.5):
c_x_index, c_y_index = sweep_searcher.search_start_grid(grid_map)
if not grid_map.set_value_from_xy_index(c_x_index, c_y_index, 0.5):
print("Cannot find start grid")
return [], []
x, y = grid_map.calc_grid_central_xy_position_from_xy_index(cxind, cyind)
x, y = grid_map.calc_grid_central_xy_position_from_xy_index(c_x_index,
c_y_index)
px, py = [x], [y]
fig, ax = None, None
if grid_search_animation:
fig, ax = plt.subplots()
# for stopping simulation with the esc key.
fig.canvas.mpl_connect('key_release_event',
lambda event: [
exit(0) if event.key == 'escape' else None])
fig.canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
while True:
cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, grid_map)
c_x_index, c_y_index = sweep_searcher.move_target_grid(c_x_index,
c_y_index,
grid_map)
if sweep_searcher.is_search_done(grid_map) or (
cxind is None or cyind is None):
c_x_index is None or c_y_index is None):
print("Done")
break
x, y = grid_map.calc_grid_central_xy_position_from_xy_index(
cxind, cyind)
c_x_index, c_y_index)
px.append(x)
py.append(y)
grid_map.set_value_from_xy_index(cxind, cyind, 0.5)
grid_map.set_value_from_xy_index(c_x_index, c_y_index, 0.5)
if grid_search_animation:
grid_map.plot_grid_map(ax=ax)
@@ -255,32 +252,34 @@ def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False):
return px, py
def planning(ox, oy, reso,
def planning(ox, oy, resolution,
moving_direction=SweepSearcher.MovingDirection.RIGHT,
sweeping_direction=SweepSearcher.SweepDirection.UP,
):
sweep_vec, sweep_start_posi = find_sweep_direction_and_start_position(
sweep_vec, sweep_start_position = find_sweep_direction_and_start_position(
ox, oy)
rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi)
rox, roy = convert_grid_coordinate(ox, oy, sweep_vec,
sweep_start_position)
gmap, xinds_goaly, goaly = setup_grid_map(rox, roy, reso,
sweeping_direction)
grid_map, x_inds_goal_y, goal_y = setup_grid_map(rox, roy, resolution,
sweeping_direction)
sweep_searcher = SweepSearcher(moving_direction, sweeping_direction,
xinds_goaly, goaly)
x_inds_goal_y, goal_y)
px, py = sweep_path_search(sweep_searcher, gmap)
px, py = sweep_path_search(sweep_searcher, grid_map)
rx, ry = convert_global_coordinate(px, py, sweep_vec, sweep_start_posi)
rx, ry = convert_global_coordinate(px, py, sweep_vec,
sweep_start_position)
print("Path length:", len(rx))
return rx, ry
def planning_animation(ox, oy, reso): # pragma: no cover
px, py = planning(ox, oy, reso)
def planning_animation(ox, oy, resolution): # pragma: no cover
px, py = planning(ox, oy, resolution)
# animation
if do_animation:
@@ -311,18 +310,18 @@ def main(): # pragma: no cover
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0]
reso = 5.0
planning_animation(ox, oy, reso)
resolution = 5.0
planning_animation(ox, oy, resolution)
ox = [0.0, 50.0, 50.0, 0.0, 0.0]
oy = [0.0, 0.0, 30.0, 30.0, 0.0]
reso = 1.3
planning_animation(ox, oy, reso)
resolution = 1.3
planning_animation(ox, oy, resolution)
ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0]
oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0]
reso = 5.0
planning_animation(ox, oy, reso)
resolution = 5.0
planning_animation(ox, oy, resolution)
plt.show()
print("done!!")

147
PathPlanning/HybridAStar/a_star_heuristic.py
View File

@@ -8,8 +8,9 @@ See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
"""
import math
import heapq
import math
import matplotlib.pyplot as plt
show_animation = False
@@ -17,71 +18,73 @@ show_animation = False
class Node:
def __init__(self, x, y, cost, pind):
def __init__(self, x, y, cost, parent_index):
self.x = x
self.y = y
self.cost = cost
self.pind = pind
self.parent_index = parent_index
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
return str(self.x) + "," + str(self.y) + "," + str(
self.cost) + "," + str(self.parent_index)
def calc_final_path(ngoal, closedset, reso):
def calc_final_path(goal_node, closed_node_set, resolution):
# generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(n.x * reso)
ry.append(n.y * reso)
pind = n.pind
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 dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
def dp_planning(sx, sy, 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]
reso: grid resolution [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
ox = [iox / reso for iox in ox]
oy = [ioy / reso for ioy in oy]
start_node = Node(round(sx / resolution), round(sy / resolution), 0.0, -1)
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]
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
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()
openset, closedset = dict(), dict()
openset[calc_index(ngoal, xw, minx, miny)] = ngoal
pq = []
pq.append((0, calc_index(ngoal, xw, minx, miny)))
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 pq:
if not priority_queue:
break
cost, c_id = heapq.heappop(pq)
if c_id in openset:
current = openset[c_id]
closedset[c_id] = current
openset.pop(c_id)
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
# show graph
if show_animation: # pragma: no cover
plt.plot(current.x * reso, current.y * reso, "xc")
plt.plot(current.x * resolution, current.y * resolution, "xc")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if len(closedset.keys()) % 10 == 0:
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if len(closed_set.keys()) % 10 == 0:
plt.pause(0.001)
# Remove the item from the open set
@@ -91,82 +94,82 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
node = Node(current.x + motion[i][0],
current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
n_id = calc_index(node, x_w, min_x, min_y)
if n_id in closedset:
if n_id in closed_set:
continue
if not verify_node(node, obmap, minx, miny, maxx, maxy):
if not verify_node(node, obstacle_map, min_x, min_y, max_x, max_y):
continue
if n_id not in openset:
openset[n_id] = node # Discover a new node
if n_id not in open_set:
open_set[n_id] = node # Discover a new node
heapq.heappush(
pq, (node.cost, calc_index(node, xw, minx, miny)))
priority_queue,
(node.cost, calc_index(node, x_w, min_x, min_y)))
else:
if openset[n_id].cost >= node.cost:
if open_set[n_id].cost >= node.cost:
# This path is the best until now. record it!
openset[n_id] = node
open_set[n_id] = node
heapq.heappush(
pq, (node.cost, calc_index(node, xw, minx, miny)))
priority_queue,
(node.cost, calc_index(node, x_w, min_x, min_y)))
rx, ry = calc_final_path(closedset[calc_index(
nstart, xw, minx, miny)], closedset, reso)
rx, ry = calc_final_path(closed_set[calc_index(
start_node, x_w, min_x, min_y)], closed_set, resolution)
return rx, ry, closedset
return rx, ry, closed_set
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)
d = w * math.sqrt((n1.x - n2.x) ** 2 + (n1.y - n2.y) ** 2)
return d
def verify_node(node, obmap, minx, miny, maxx, maxy):
if node.x < minx:
def verify_node(node, obstacle_map, min_x, min_y, max_x, max_y):
if node.x < min_x:
return False
elif node.y < miny:
elif node.y < min_y:
return False
elif node.x >= maxx:
elif node.x >= max_x:
return False
elif node.y >= maxy:
elif node.y >= max_y:
return False
if obmap[node.x][node.y]:
if obstacle_map[node.x][node.y]:
return False
return True
def calc_obstacle_map(ox, oy, reso, vr):
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))
minx = round(min(ox))
miny = round(min(oy))
maxx = round(max(ox))
maxy = round(max(oy))
xwidth = round(maxx - minx)
ywidth = round(maxy - miny)
x_width = round(max_x - min_x)
y_width = round(max_y - min_y)
# obstacle map generation
obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
for ix in range(xwidth):
x = ix + minx
for iy in range(ywidth):
y = iy + miny
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 / reso:
obmap[ix][iy] = True
d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2)
if d <= vr / resolution:
obstacle_map[ix][iy] = True
break
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
return obstacle_map, min_x, min_y, max_x, max_y, x_width, y_width
def calc_index(node, xwidth, xmin, ymin):
return (node.y - ymin) * xwidth + (node.x - xmin)
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():

48
PathPlanning/HybridAStar/car.py
View File

@@ -6,9 +6,11 @@ author: Zheng Zh (@Zhengzh)
"""
from math import sqrt, cos, sin, tan, pi
import matplotlib.pyplot as plt
from math import sqrt, cos, sin, tan, pi
import numpy as np
from scipy.spatial.transform import Rotation as Rot
WB = 3. # rear to front wheel
W = 2. # width of car
@@ -16,25 +18,25 @@ LF = 3.3 # distance from rear to vehicle front end
LB = 1.0 # distance from rear to vehicle back end
MAX_STEER = 0.6 # [rad] maximum steering angle
WBUBBLE_DIST = (LF - LB) / 2.0
WBUBBLE_R = sqrt(((LF + LB) / 2.0)**2 + 1)
W_BUBBLE_DIST = (LF - LB) / 2.0
W_BUBBLE_R = sqrt(((LF + LB) / 2.0) ** 2 + 1)
# vehicle rectangle verticles
# vehicle rectangle vertices
VRX = [LF, LF, -LB, -LB, LF]
VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2]
def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
for x, y, yaw in zip(xlist, ylist, yawlist):
cx = x + WBUBBLE_DIST * cos(yaw)
cy = y + WBUBBLE_DIST * sin(yaw)
def check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):
for i_x, i_y, i_yaw in zip(x_list, y_list, yaw_list):
cx = i_x + W_BUBBLE_DIST * cos(i_yaw)
cy = i_y + W_BUBBLE_DIST * sin(i_yaw)
ids = kdtree.search_in_distance([cx, cy], WBUBBLE_R)
ids = kd_tree.search_in_distance([cx, cy], W_BUBBLE_R)
if not ids:
continue
if not rectangle_check(x, y, yaw,
if not rectangle_check(i_x, i_y, i_yaw,
[ox[i] for i in ids], [oy[i] for i in ids]):
return False # collision
@@ -43,12 +45,12 @@ def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
def rectangle_check(x, y, yaw, ox, oy):
# transform obstacles to base link frame
c, s = cos(-yaw), sin(-yaw)
rot = Rot.from_euler('z', yaw).as_matrix()[0:2, 0:2]
for iox, ioy in zip(ox, oy):
tx = iox - x
ty = ioy - y
rx = c * tx - s * ty
ry = s * tx + c * ty
converted_xy = np.stack([tx, ty]).T @ rot
rx, ry = converted_xy[0], converted_xy[1]
if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0):
return False # no collision
@@ -59,24 +61,22 @@ def rectangle_check(x, y, yaw, ox, oy):
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
"""Plot arrow."""
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
plot_arrow(ix, iy, iyaw)
for (i_x, i_y, i_yaw) in zip(x, y, yaw):
plot_arrow(i_x, i_y, i_yaw)
else:
plt.arrow(x, y, length * cos(yaw), length * sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4)
# plt.plot(x, y)
def plot_car(x, y, yaw):
car_color = '-k'
c, s = cos(yaw), sin(yaw)
rot = Rot.from_euler('z', yaw).as_matrix()[0:2, 0:2]
car_outline_x, car_outline_y = [], []
for rx, ry in zip(VRX, VRY):
tx = c * rx - s * ry + x
ty = s * rx + c * ry + y
car_outline_x.append(tx)
car_outline_y.append(ty)
converted_xy = np.stack([rx, ry]).T @ rot
car_outline_x.append(converted_xy[0]+x)
car_outline_y.append(converted_xy[1]+y)
arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw
plot_arrow(arrow_x, arrow_y, arrow_yaw)
@@ -96,8 +96,12 @@ def move(x, y, yaw, distance, steer, L=WB):
return x, y, yaw
if __name__ == '__main__':
def main():
x, y, yaw = 0., 0., 1.
plt.axis('equal')
plot_car(x, y, yaw)
plt.show()
if __name__ == '__main__':
main()

322
PathPlanning/HybridAStar/hybrid_a_star.py
View File

@@ -7,28 +7,27 @@ author: Zheng Zh (@Zhengzh)
"""
import heapq
import scipy.spatial
import numpy as np
import math
import matplotlib.pyplot as plt
import sys
import os
import sys
import matplotlib.pyplot as plt
import numpy as np
import scipy.spatial
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../ReedsSheppPath")
try:
from a_star_heuristic import dp_planning # , calc_obstacle_map
from a_star_heuristic import dp_planning
import reeds_shepp_path_planning as rs
from car import move, check_car_collision, MAX_STEER, WB, plot_car
except Exception:
raise
XY_GRID_RESOLUTION = 2.0 # [m]
YAW_GRID_RESOLUTION = np.deg2rad(15.0) # [rad]
MOTION_RESOLUTION = 0.1 # [m] path interporate resolution
MOTION_RESOLUTION = 0.1 # [m] path interpolate resolution
N_STEER = 20 # number of steer command
H_COST = 1.0
VR = 1.0 # robot radius
SB_COST = 100.0 # switch back penalty cost
@@ -42,29 +41,29 @@ show_animation = True
class Node:
def __init__(self, xind, yind, yawind, direction,
xlist, ylist, yawlist, directions,
steer=0.0, pind=None, cost=None):
self.xind = xind
self.yind = yind
self.yawind = yawind
def __init__(self, x_ind, y_ind, yaw_ind, direction,
x_list, y_list, yaw_list, directions,
steer=0.0, parent_index=None, cost=None):
self.x_index = x_ind
self.y_index = y_ind
self.yaw_index = yaw_ind
self.direction = direction
self.xlist = xlist
self.ylist = ylist
self.yawlist = yawlist
self.x_list = x_list
self.y_list = y_list
self.yaw_list = yaw_list
self.directions = directions
self.steer = steer
self.pind = pind
self.parent_index = parent_index
self.cost = cost
class Path:
def __init__(self, xlist, ylist, yawlist, directionlist, cost):
self.xlist = xlist
self.ylist = ylist
self.yawlist = yawlist
self.directionlist = directionlist
def __init__(self, x_list, y_list, yaw_list, direction_list, cost):
self.x_list = x_list
self.y_list = y_list
self.yaw_list = yaw_list
self.direction_list = direction_list
self.cost = cost
@@ -88,9 +87,9 @@ class KDTree:
dist = []
for i in inp.T:
idist, iindex = self.tree.query(i, k=k)
index.append(iindex)
dist.append(idist)
i_dist, i_index = self.tree.query(i, k=k)
index.append(i_index)
dist.append(i_dist)
return index, dist
@@ -108,7 +107,7 @@ class KDTree:
class Config:
def __init__(self, ox, oy, xyreso, yawreso):
def __init__(self, ox, oy, xy_resolution, yaw_resolution):
min_x_m = min(ox)
min_y_m = min(oy)
max_x_m = max(ox)
@@ -119,94 +118,93 @@ class Config:
ox.append(max_x_m)
oy.append(max_y_m)
self.minx = round(min_x_m / xyreso)
self.miny = round(min_y_m / xyreso)
self.maxx = round(max_x_m / xyreso)
self.maxy = round(max_y_m / xyreso)
self.min_x = round(min_x_m / xy_resolution)
self.min_y = round(min_y_m / xy_resolution)
self.max_x = round(max_x_m / xy_resolution)
self.max_y = round(max_y_m / xy_resolution)
self.xw = round(self.maxx - self.minx)
self.yw = round(self.maxy - self.miny)
self.x_w = round(self.max_x - self.min_x)
self.y_w = round(self.max_y - self.min_y)
self.minyaw = round(- math.pi / yawreso) - 1
self.maxyaw = round(math.pi / yawreso)
self.yaww = round(self.maxyaw - self.minyaw)
self.min_yaw = round(- math.pi / yaw_resolution) - 1
self.max_yaw = round(math.pi / yaw_resolution)
self.yaw_w = round(self.max_yaw - self.min_yaw)
def calc_motion_inputs():
for steer in np.concatenate((np.linspace(-MAX_STEER, MAX_STEER,
N_STEER), [0.0])):
for d in [1, -1]:
yield [steer, d]
def get_neighbors(current, config, ox, oy, kdtree):
def get_neighbors(current, config, ox, oy, kd_tree):
for steer, d in calc_motion_inputs():
node = calc_next_node(current, steer, d, config, ox, oy, kdtree)
node = calc_next_node(current, steer, d, config, ox, oy, kd_tree)
if node and verify_index(node, config):
yield node
def calc_next_node(current, steer, direction, config, ox, oy, kdtree):
x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1]
def calc_next_node(current, steer, direction, config, ox, oy, kd_tree):
x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1]
arc_l = XY_GRID_RESOLUTION * 1.5
xlist, ylist, yawlist = [], [], []
x_list, y_list, yaw_list = [], [], []
for _ in np.arange(0, arc_l, MOTION_RESOLUTION):
x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer)
xlist.append(x)
ylist.append(y)
yawlist.append(yaw)
x_list.append(x)
y_list.append(y)
yaw_list.append(yaw)
if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):
return None
d = direction == 1
xind = round(x / XY_GRID_RESOLUTION)
yind = round(y / XY_GRID_RESOLUTION)
yawind = round(yaw / YAW_GRID_RESOLUTION)
x_ind = round(x / XY_GRID_RESOLUTION)
y_ind = round(y / XY_GRID_RESOLUTION)
yaw_ind = round(yaw / YAW_GRID_RESOLUTION)
addedcost = 0.0
added_cost = 0.0
if d != current.direction:
addedcost += SB_COST
added_cost += SB_COST
# steer penalty
addedcost += STEER_COST * abs(steer)
added_cost += STEER_COST * abs(steer)
# steer change penalty
addedcost += STEER_CHANGE_COST * abs(current.steer - steer)
added_cost += STEER_CHANGE_COST * abs(current.steer - steer)
cost = current.cost + addedcost + arc_l
cost = current.cost + added_cost + arc_l
node = Node(xind, yind, yawind, d, xlist,
ylist, yawlist, [d],
pind=calc_index(current, config),
node = Node(x_ind, y_ind, yaw_ind, d, x_list,
y_list, yaw_list, [d],
parent_index=calc_index(current, config),
cost=cost, steer=steer)
return node
def is_same_grid(n1, n2):
if n1.xind == n2.xind and n1.yind == n2.yind and n1.yawind == n2.yawind:
if n1.x_index == n2.x_index \
and n1.y_index == n2.y_index \
and n1.yaw_index == n2.yaw_index:
return True
return False
def analytic_expantion(current, goal, c, ox, oy, kdtree):
def analytic_expansion(current, goal, ox, oy, kd_tree):
start_x = current.x_list[-1]
start_y = current.y_list[-1]
start_yaw = current.yaw_list[-1]
sx = current.xlist[-1]
sy = current.ylist[-1]
syaw = current.yawlist[-1]
gx = goal.xlist[-1]
gy = goal.ylist[-1]
gyaw = goal.yawlist[-1]
goal_x = goal.x_list[-1]
goal_y = goal.y_list[-1]
goal_yaw = goal.yaw_list[-1]
max_curvature = math.tan(MAX_STEER) / WB
paths = rs.calc_paths(sx, sy, syaw, gx, gy, gyaw,
paths = rs.calc_paths(start_x, start_y, start_yaw,
goal_x, goal_y, goal_yaw,
max_curvature, step_size=MOTION_RESOLUTION)
if not paths:
@@ -215,7 +213,7 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree):
best_path, best = None, None
for path in paths:
if check_car_collision(path.x, path.y, path.yaw, ox, oy, kdtree):
if check_car_collision(path.x, path.y, path.yaw, ox, oy, kd_tree):
cost = calc_rs_path_cost(path)
if not best or best > cost:
best = cost
@@ -224,99 +222,105 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree):
return best_path
def update_node_with_analystic_expantion(current, goal,
c, ox, oy, kdtree):
apath = analytic_expantion(current, goal, c, ox, oy, kdtree)
def update_node_with_analytic_expansion(current, goal,
c, ox, oy, kd_tree):
path = analytic_expansion(current, goal, ox, oy, kd_tree)
if apath:
if path:
if show_animation:
plt.plot(apath.x, apath.y)
fx = apath.x[1:]
fy = apath.y[1:]
fyaw = apath.yaw[1:]
plt.plot(path.x, path.y)
f_x = path.x[1:]
f_y = path.y[1:]
f_yaw = path.yaw[1:]
fcost = current.cost + calc_rs_path_cost(apath)
fpind = calc_index(current, c)
f_cost = current.cost + calc_rs_path_cost(path)
f_parent_index = calc_index(current, c)
fd = []
for d in apath.directions[1:]:
for d in path.directions[1:]:
fd.append(d >= 0)
fsteer = 0.0
fpath = Node(current.xind, current.yind, current.yawind,
current.direction, fx, fy, fyaw, fd,
cost=fcost, pind=fpind, steer=fsteer)
return True, fpath
f_steer = 0.0
f_path = Node(current.x_index, current.y_index, current.yaw_index,
current.direction, f_x, f_y, f_yaw, fd,
cost=f_cost, parent_index=f_parent_index, steer=f_steer)
return True, f_path
return False, None
def calc_rs_path_cost(rspath):
def calc_rs_path_cost(reed_shepp_path):
cost = 0.0
for l in rspath.lengths:
if l >= 0: # forward
cost += l
for length in reed_shepp_path.lengths:
if length >= 0: # forward
cost += length
else: # back
cost += abs(l) * BACK_COST
cost += abs(length) * BACK_COST
# swich back penalty
for i in range(len(rspath.lengths) - 1):
if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0: # switch back
# switch back penalty
for i in range(len(reed_shepp_path.lengths) - 1):
# switch back
if reed_shepp_path.lengths[i] * reed_shepp_path.lengths[i + 1] < 0.0:
cost += SB_COST
# steer penalyty
for ctype in rspath.ctypes:
if ctype != "S": # curve
# steer penalty
for course_type in reed_shepp_path.ctypes:
if course_type != "S": # curve
cost += STEER_COST * abs(MAX_STEER)
# ==steer change penalty
# calc steer profile
nctypes = len(rspath.ctypes)
ulist = [0.0] * nctypes
for i in range(nctypes):
if rspath.ctypes[i] == "R":
ulist[i] = - MAX_STEER
elif rspath.ctypes[i] == "L":
ulist[i] = MAX_STEER
n_ctypes = len(reed_shepp_path.ctypes)
u_list = [0.0] * n_ctypes
for i in range(n_ctypes):
if reed_shepp_path.ctypes[i] == "R":
u_list[i] = - MAX_STEER
elif reed_shepp_path.ctypes[i] == "L":
u_list[i] = MAX_STEER
for i in range(len(rspath.ctypes) - 1):
cost += STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i])
for i in range(len(reed_shepp_path.ctypes) - 1):
cost += STEER_CHANGE_COST * abs(u_list[i + 1] - u_list[i])
return cost
def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
"""
start
goal
start: start node
goal: goal node
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
xyreso: grid resolution [m]
yawreso: yaw angle resolution [rad]
xy_resolution: grid resolution [m]
yaw_resolution: yaw angle resolution [rad]
"""
start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
tox, toy = ox[:], oy[:]
obkdtree = KDTree(np.vstack((tox, toy)).T)
obstacle_kd_tree = KDTree(np.vstack((tox, toy)).T)
config = Config(tox, toy, xyreso, yawreso)
config = Config(tox, toy, xy_resolution, yaw_resolution)
nstart = Node(round(start[0] / xyreso), round(start[1] / xyreso), round(start[2] / yawreso),
True, [start[0]], [start[1]], [start[2]], [True], cost=0)
ngoal = Node(round(goal[0] / xyreso), round(goal[1] / xyreso), round(goal[2] / yawreso),
True, [goal[0]], [goal[1]], [goal[2]], [True])
start_node = Node(round(start[0] / xy_resolution),
round(start[1] / xy_resolution),
round(start[2] / yaw_resolution), True,
[start[0]], [start[1]], [start[2]], [True], cost=0)
goal_node = Node(round(goal[0] / xy_resolution),
round(goal[1] / xy_resolution),
round(goal[2] / yaw_resolution), True,
[goal[0]], [goal[1]], [goal[2]], [True])
openList, closedList = {}, {}
_, _, h_dp = dp_planning(nstart.xlist[-1], nstart.ylist[-1],
ngoal.xlist[-1], ngoal.ylist[-1], ox, oy, xyreso, VR)
_, _, h_dp = dp_planning(start_node.x_list[-1], start_node.y_list[-1],
goal_node.x_list[-1], goal_node.y_list[-1],
ox, oy, xy_resolution, VR)
pq = []
openList[calc_index(nstart, config)] = nstart
heapq.heappush(pq, (calc_cost(nstart, h_dp, ngoal, config),
calc_index(nstart, config)))
openList[calc_index(start_node, config)] = start_node
heapq.heappush(pq, (calc_cost(start_node, h_dp, config),
calc_index(start_node, config)))
final_path = None
while True:
if not openList:
@@ -331,83 +335,85 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
continue
if show_animation: # pragma: no cover
plt.plot(current.xlist[-1], current.ylist[-1], "xc")
plt.plot(current.x_list[-1], current.y_list[-1], "xc")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if len(closedList.keys()) % 10 == 0:
plt.pause(0.001)
isupdated, fpath = update_node_with_analystic_expantion(
current, ngoal, config, ox, oy, obkdtree)
is_updated, final_path = update_node_with_analytic_expansion(
current, goal_node, config, ox, oy, obstacle_kd_tree)
if isupdated:
if is_updated:
print("path found")
break
for neighbor in get_neighbors(current, config, ox, oy, obkdtree):
for neighbor in get_neighbors(current, config, ox, oy,
obstacle_kd_tree):
neighbor_index = calc_index(neighbor, config)
if neighbor_index in closedList:
continue
if neighbor not in openList \
or openList[neighbor_index].cost > neighbor.cost:
heapq.heappush(
pq, (calc_cost(neighbor, h_dp, ngoal, config),
pq, (calc_cost(neighbor, h_dp, config),
neighbor_index))
openList[neighbor_index] = neighbor
path = get_final_path(closedList, fpath, nstart, config)
path = get_final_path(closedList, final_path)
return path
def calc_cost(n, h_dp, goal, c):
ind = (n.yind - c.miny) * c.xw + (n.xind - c.minx)
def calc_cost(n, h_dp, c):
ind = (n.y_index - c.min_y) * c.x_w + (n.x_index - c.min_x)
if ind not in h_dp:
return n.cost + 999999999 # collision cost
return n.cost + H_COST * h_dp[ind].cost
def get_final_path(closed, ngoal, nstart, config):
rx, ry, ryaw = list(reversed(ngoal.xlist)), list(
reversed(ngoal.ylist)), list(reversed(ngoal.yawlist))
direction = list(reversed(ngoal.directions))
nid = ngoal.pind
finalcost = ngoal.cost
def get_final_path(closed, goal_node):
reversed_x, reversed_y, reversed_yaw = \
list(reversed(goal_node.x_list)), list(reversed(goal_node.y_list)), \
list(reversed(goal_node.yaw_list))
direction = list(reversed(goal_node.directions))
nid = goal_node.parent_index
final_cost = goal_node.cost
while nid:
n = closed[nid]
rx.extend(list(reversed(n.xlist)))
ry.extend(list(reversed(n.ylist)))
ryaw.extend(list(reversed(n.yawlist)))
reversed_x.extend(list(reversed(n.x_list)))
reversed_y.extend(list(reversed(n.y_list)))
reversed_yaw.extend(list(reversed(n.yaw_list)))
direction.extend(list(reversed(n.directions)))
nid = n.pind
nid = n.parent_index
rx = list(reversed(rx))
ry = list(reversed(ry))
ryaw = list(reversed(ryaw))
reversed_x = list(reversed(reversed_x))
reversed_y = list(reversed(reversed_y))
reversed_yaw = list(reversed(reversed_yaw))
direction = list(reversed(direction))
# adjust first direction
direction[0] = direction[1]
path = Path(rx, ry, ryaw, direction, finalcost)
path = Path(reversed_x, reversed_y, reversed_yaw, direction, final_cost)
return path
def verify_index(node, c):
xind, yind = node.xind, node.yind
if xind >= c.minx and xind <= c.maxx and yind >= c.miny \
and yind <= c.maxy:
x_ind, y_ind = node.x_index, node.y_index
if c.min_x <= x_ind <= c.max_x and c.min_y <= y_ind <= c.max_y:
return True
return False
def calc_index(node, c):
ind = (node.yawind - c.minyaw) * c.xw * c.yw + \
(node.yind - c.miny) * c.xw + (node.xind - c.minx)
ind = (node.yaw_index - c.min_yaw) * c.x_w * c.y_w + \
(node.y_index - c.min_y) * c.x_w + (node.x_index - c.min_x)
if ind <= 0:
print("Error(calc_index):", ind)
@@ -457,18 +463,18 @@ def main():
path = hybrid_a_star_planning(
start, goal, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION)
x = path.xlist
y = path.ylist
yaw = path.yawlist
x = path.x_list
y = path.y_list
yaw = path.yaw_list
if show_animation:
for ix, iy, iyaw in zip(x, y, yaw):
for i_x, i_y, i_yaw in zip(x, y, yaw):
plt.cla()
plt.plot(ox, oy, ".k")
plt.plot(x, y, "-r", label="Hybrid A* path")
plt.grid(True)
plt.axis("equal")
plot_car(ix, iy, iyaw)
plot_car(i_x, i_y, i_yaw)
plt.pause(0.0001)
print(__file__ + " done!!")

91
PathPlanning/InformedRRTStar/informed_rrt_star.py
View File

@@ -5,7 +5,8 @@ author: Karan Chawla
Atsushi Sakai(@Atsushi_twi)
Reference: Informed RRT*: Optimal Sampling-based Path planning Focused via
Direct Sampling of an Admissible Ellipsoidal Heuristichttps://arxiv.org/pdf/1404.2334.pdf
Direct Sampling of an Admissible Ellipsoidal Heuristic
https://arxiv.org/pdf/1404.2334.pdf
"""
@@ -14,6 +15,7 @@ import math
import random
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as Rot
import numpy as np
show_animation = True
@@ -38,7 +40,8 @@ class InformedRRTStar:
def informed_rrt_star_search(self, animation=True):
self.node_list = [self.start]
# max length we expect to find in our 'informed' sample space, starts as infinite
# max length we expect to find in our 'informed' sample space,
# starts as infinite
cBest = float('inf')
solutionSet = set()
path = None
@@ -51,13 +54,14 @@ class InformedRRTStar:
a1 = np.array([[(self.goal.x - self.start.x) / cMin],
[(self.goal.y - self.start.y) / cMin], [0]])
etheta = math.atan2(a1[1], a1[0])
# first column of idenity matrix transposed
e_theta = math.atan2(a1[1], a1[0])
# first column of identity matrix transposed
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
M = a1 @ id1_t
U, S, Vh = np.linalg.svd(M, True, True)
C = np.dot(np.dot(U, np.diag(
[1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh)
[1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),
Vh)
for i in range(self.max_iter):
# Sample space is defined by cBest
@@ -66,11 +70,11 @@ class InformedRRTStar:
# cBest changes when a new path is found
rnd = self.informed_sample(cBest, cMin, xCenter, C)
nind = self.get_nearest_list_index(self.node_list, rnd)
nearestNode = self.node_list[nind]
n_ind = self.get_nearest_list_index(self.node_list, rnd)
nearestNode = self.node_list[n_ind]
# steer
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = self.get_new_node(theta, nind, nearestNode)
newNode = self.get_new_node(theta, n_ind, nearestNode)
d = self.line_cost(nearestNode, newNode)
noCollision = self.check_collision(nearestNode, theta, d)
@@ -83,7 +87,8 @@ class InformedRRTStar:
self.rewire(newNode, nearInds)
if self.is_near_goal(newNode):
if self.check_segment_collision(newNode.x, newNode.y, self.goal.x , self.goal.y):
if self.check_segment_collision(newNode.x, newNode.y,
self.goal.x, self.goal.y):
solutionSet.add(newNode)
lastIndex = len(self.node_list) - 1
tempPath = self.get_final_course(lastIndex)
@@ -94,7 +99,7 @@ class InformedRRTStar:
if animation:
self.draw_graph(xCenter=xCenter,
cBest=cBest, cMin=cMin,
etheta=etheta, rnd=rnd)
e_theta=e_theta, rnd=rnd)
return path
@@ -117,7 +122,7 @@ class InformedRRTStar:
minInd = nearInds[dList.index(minCost)]
if minCost == float('inf'):
print("mincost is inf")
print("min cost is inf")
return newNode
newNode.cost = minCost
@@ -126,12 +131,12 @@ class InformedRRTStar:
return newNode
def find_near_nodes(self, newNode):
nnode = len(self.node_list)
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
dlist = [(node.x - newNode.x) ** 2
+ (node.y - newNode.y) ** 2 for node in self.node_list]
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
return nearinds
n_node = len(self.node_list)
r = 50.0 * math.sqrt((math.log(n_node) / n_node))
d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2
for node in self.node_list]
near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]
return near_inds
def informed_sample(self, cMax, cMin, xCenter, C):
if cMax < float('inf'):
@@ -192,14 +197,14 @@ class InformedRRTStar:
minIndex = dList.index(min(dList))
return minIndex
def get_new_node(self, theta, nind, nearestNode):
def get_new_node(self, theta, n_ind, nearestNode):
newNode = copy.deepcopy(nearestNode)
newNode.x += self.expand_dis * math.cos(theta)
newNode.y += self.expand_dis * math.sin(theta)
newNode.cost += self.expand_dis
newNode.parent = nind
newNode.parent = n_ind
return newNode
def is_near_goal(self, node):
@@ -216,28 +221,29 @@ class InformedRRTStar:
d = math.sqrt((nearNode.x - newNode.x) ** 2
+ (nearNode.y - newNode.y) ** 2)
scost = newNode.cost + d
s_cost = newNode.cost + d
if nearNode.cost > scost:
if nearNode.cost > s_cost:
theta = math.atan2(newNode.y - nearNode.y,
newNode.x - nearNode.x)
if self.check_collision(nearNode, theta, d):
nearNode.parent = n_node - 1
nearNode.cost = scost
nearNode.cost = s_cost
@staticmethod
def distance_squared_point_to_segment(v, w, p):
# Return minimum distance between line segment vw and point p
if (np.array_equal(v, w)):
return (p-v).dot(p-v) # v == w case
l2 = (w-v).dot(w-v) # i.e. |w-v|^2 - avoid a sqrt
# Consider the line extending the segment, parameterized as v + t (w - v).
if np.array_equal(v, w):
return (p - v).dot(p - v) # v == w case
l2 = (w - v).dot(w - v) # i.e. |w-v|^2 - avoid a sqrt
# Consider the line extending the segment,
# parameterized as v + t (w - v).
# We find projection of point p onto the line.
# It falls where t = [(p-v) . (w-v)] / |w-v|^2
# We clamp t from [0,1] to handle points outside the segment vw.
t = max(0, min(1, (p - v).dot(w - v) / l2))
projection = v + t * (w - v) # Projection falls on the segment
return (p-projection).dot(p-projection)
projection = v + t * (w - v) # Projection falls on the segment
return (p - projection).dot(p - projection)
def check_segment_collision(self, x1, y1, x2, y2):
for (ox, oy, size) in self.obstacle_list:
@@ -245,16 +251,15 @@ class InformedRRTStar:
np.array([x1, y1]),
np.array([x2, y2]),
np.array([ox, oy]))
if dd <= size**2:
if dd <= size ** 2:
return False # collision
return True
def check_collision(self, nearNode, theta, d):
tmpNode = copy.deepcopy(nearNode)
endx = tmpNode.x + math.cos(theta)*d
endy = tmpNode.y + math.sin(theta)*d
return self.check_segment_collision(tmpNode.x, tmpNode.y, endx, endy)
end_x = tmpNode.x + math.cos(theta) * d
end_y = tmpNode.y + math.sin(theta) * d
return self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y)
def get_final_course(self, lastIndex):
path = [[self.goal.x, self.goal.y]]
@@ -265,15 +270,17 @@ class InformedRRTStar:
path.append([self.start.x, self.start.y])
return path
def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None):
def draw_graph(self, xCenter=None, cBest=None, cMin=None, e_theta=None,
rnd=None):
plt.clf()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^k")
if cBest != float('inf'):
self.plot_ellipse(xCenter, cBest, cMin, etheta)
self.plot_ellipse(xCenter, cBest, cMin, e_theta)
for node in self.node_list:
if node.parent is not None:
@@ -291,20 +298,18 @@ class InformedRRTStar:
plt.pause(0.01)
@staticmethod
def plot_ellipse(xCenter, cBest, cMin, etheta): # pragma: no cover
def plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no cover
a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
b = cBest / 2.0
angle = math.pi / 2.0 - etheta
angle = math.pi / 2.0 - e_theta
cx = xCenter[0]
cy = xCenter[1]
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R @ np.array([x, y])
rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2]
fx = rot @ np.array([x, y])
px = np.array(fx[0, :] + cx).flatten()
py = np.array(fx[1, :] + cy).flatten()
plt.plot(cx, cy, "xc")

128
PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
View File

@@ -1,6 +1,6 @@
"""
Probablistic Road Map (PRM) Planner
Probabilistic Road Map (PRM) Planner
author: Atsushi Sakai (@Atsushi_twi)
@@ -25,14 +25,15 @@ class Node:
Node class for dijkstra search
"""
def __init__(self, x, y, cost, pind):
def __init__(self, x, y, cost, parent_index):
self.x = x
self.y = y
self.cost = cost
self.pind = pind
self.parent_index = parent_index
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
return str(self.x) + "," + str(self.y) + "," +\
str(self.cost) + "," + str(self.parent_index)
class KDTree:
@@ -57,9 +58,9 @@ class KDTree:
dist = []
for i in inp.T:
idist, iindex = self.tree.query(i, k=k)
index.append(iindex)
dist.append(idist)
i_dist, i_index = self.tree.query(i, k=k)
index.append(i_index)
dist.append(i_dist)
return index, dist
@@ -75,23 +76,24 @@ class KDTree:
return index
def PRM_planning(sx, sy, gx, gy, ox, oy, rr):
def prm_planning(sx, sy, gx, gy, ox, oy, rr):
obkdtree = KDTree(np.vstack((ox, oy)).T)
obstacle_kd_tree = KDTree(np.vstack((ox, oy)).T)
sample_x, sample_y = sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree)
sample_x, sample_y = sample_points(sx, sy, gx, gy,
rr, ox, oy, obstacle_kd_tree)
if show_animation:
plt.plot(sample_x, sample_y, ".b")
road_map = generate_roadmap(sample_x, sample_y, rr, obkdtree)
road_map = generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree)
rx, ry = dijkstra_planning(
sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y)
sx, sy, gx, gy, road_map, sample_x, sample_y)
return rx, ry
def is_collision(sx, sy, gx, gy, rr, okdtree):
def is_collision(sx, sy, gx, gy, rr, obstacle_kd_tree):
x = sx
y = sy
dx = gx - sx
@@ -103,41 +105,41 @@ def is_collision(sx, sy, gx, gy, rr, okdtree):
return True
D = rr
nstep = round(d / D)
n_step = round(d / D)
for i in range(nstep):
idxs, dist = okdtree.search(np.array([x, y]).reshape(2, 1))
for i in range(n_step):
_, dist = obstacle_kd_tree.search(np.array([x, y]).reshape(2, 1))
if dist[0] <= rr:
return True # collision
x += D * math.cos(yaw)
y += D * math.sin(yaw)
# goal point check
idxs, dist = okdtree.search(np.array([gx, gy]).reshape(2, 1))
_, dist = obstacle_kd_tree.search(np.array([gx, gy]).reshape(2, 1))
if dist[0] <= rr:
return True # collision
return False # OK
def generate_roadmap(sample_x, sample_y, rr, obkdtree):
def generate_road_map(sample_x, sample_y, rr, obstacle_kd_tree):
"""
Road map generation
sample_x: [m] x positions of sampled points
sample_y: [m] y positions of sampled points
rr: Robot Radius[m]
obkdtree: KDTree object of obstacles
obstacle_kd_tree: KDTree object of obstacles
"""
road_map = []
nsample = len(sample_x)
skdtree = KDTree(np.vstack((sample_x, sample_y)).T)
n_sample = len(sample_x)
sample_kd_tree = KDTree(np.vstack((sample_x, sample_y)).T)
for (i, ix, iy) in zip(range(nsample), sample_x, sample_y):
for (i, ix, iy) in zip(range(n_sample), sample_x, sample_y):
index, dists = skdtree.search(
np.array([ix, iy]).reshape(2, 1), k=nsample)
index, dists = sample_kd_tree.search(
np.array([ix, iy]).reshape(2, 1), k=n_sample)
inds = index[0]
edge_id = []
# print(index)
@@ -146,7 +148,7 @@ def generate_roadmap(sample_x, sample_y, rr, obkdtree):
nx = sample_x[inds[ii]]
ny = sample_y[inds[ii]]
if not is_collision(ix, iy, nx, ny, rr, obkdtree):
if not is_collision(ix, iy, nx, ny, rr, obstacle_kd_tree):
edge_id.append(inds[ii])
if len(edge_id) >= N_KNN:
@@ -159,10 +161,10 @@ def generate_roadmap(sample_x, sample_y, rr, obkdtree):
return road_map
def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
def dijkstra_planning(sx, sy, gx, gy, road_map, sample_x, sample_y):
"""
sx: start x position [m]
sy: start y position [m]
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
ox: x position list of Obstacles [m]
@@ -175,41 +177,42 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
@return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found
"""
nstart = Node(sx, sy, 0.0, -1)
ngoal = Node(gx, gy, 0.0, -1)
start_node = Node(sx, sy, 0.0, -1)
goal_node = Node(gx, gy, 0.0, -1)
openset, closedset = dict(), dict()
openset[len(road_map) - 2] = nstart
open_set, closed_set = dict(), dict()
open_set[len(road_map) - 2] = start_node
path_found = True
while True:
if not openset:
if not open_set:
print("Cannot find path")
path_found = False
break
c_id = min(openset, key=lambda o: openset[o].cost)
current = openset[c_id]
c_id = min(open_set, key=lambda o: open_set[o].cost)
current = open_set[c_id]
# show graph
if show_animation and len(closedset.keys()) % 2 == 0:
if show_animation and len(closed_set.keys()) % 2 == 0:
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(current.x, current.y, "xg")
plt.pause(0.001)
if c_id == (len(road_map) - 1):
print("goal is found!")
ngoal.pind = current.pind
ngoal.cost = current.cost
goal_node.parent_index = current.parent_index
goal_node.cost = current.cost
break
# Remove the item from the open set
del openset[c_id]
del open_set[c_id]
# Add it to the closed set
closedset[c_id] = current
closed_set[c_id] = current
# expand search grid based on motion model
for i in range(len(road_map[c_id])):
@@ -220,27 +223,27 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
node = Node(sample_x[n_id], sample_y[n_id],
current.cost + d, c_id)
if n_id in closedset:
if n_id in closed_set:
continue
# Otherwise if it is already in the open set
if n_id in openset:
if openset[n_id].cost > node.cost:
openset[n_id].cost = node.cost
openset[n_id].pind = c_id
if n_id in open_set:
if open_set[n_id].cost > node.cost:
open_set[n_id].cost = node.cost
open_set[n_id].parent_index = c_id
else:
openset[n_id] = node
open_set[n_id] = node
if path_found is False:
return [], []
# generate final course
rx, ry = [ngoal.x], [ngoal.y]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx, ry = [goal_node.x], [goal_node.y]
parent_index = goal_node.parent_index
while parent_index != -1:
n = closed_set[parent_index]
rx.append(n.x)
ry.append(n.y)
pind = n.pind
parent_index = n.parent_index
return rx, ry
@@ -255,19 +258,19 @@ def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover
[sample_y[i], sample_y[ind]], "-k")
def sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree):
maxx = max(ox)
maxy = max(oy)
minx = min(ox)
miny = min(oy)
def sample_points(sx, sy, gx, gy, rr, ox, oy, obstacle_kd_tree):
max_x = max(ox)
max_y = max(oy)
min_x = min(ox)
min_y = min(oy)
sample_x, sample_y = [], []
while len(sample_x) <= N_SAMPLE:
tx = (random.random() * (maxx - minx)) + minx
ty = (random.random() * (maxy - miny)) + miny
tx = (random.random() * (max_x - min_x)) + min_x
ty = (random.random() * (max_y - min_y)) + min_y
index, dist = obkdtree.search(np.array([tx, ty]).reshape(2, 1))
index, dist = obstacle_kd_tree.search(np.array([tx, ty]).reshape(2, 1))
if dist[0] >= rr:
sample_x.append(tx)
@@ -320,12 +323,13 @@ def main():
plt.grid(True)
plt.axis("equal")
rx, ry = PRM_planning(sx, sy, gx, gy, ox, oy, robot_size)
rx, ry = prm_planning(sx, sy, gx, gy, ox, oy, robot_size)
assert rx, 'Cannot found path'
if show_animation:
plt.plot(rx, ry, "-r")
plt.pause(0.001)
plt.show()

6
PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
View File

@@ -71,9 +71,9 @@ def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_
quintic polynomial planner
input
sx: start x position [m]
sy: start y position [m]
syaw: start yaw angle [rad]
s_x: start x position [m]
s_y: start y position [m]
s_yaw: start yaw angle [rad]
sa: start accel [m/ss]
gx: goal x position [m]
gy: goal y position [m]

4
PathPlanning/VoronoiRoadMap/dijkstra_search.py
View File

@@ -35,8 +35,8 @@ class DijkstraSearch:
"""
Search shortest path
sx: start x positions [m]
sy: start y positions [m]
s_x: start x positions [m]
s_y: start y positions [m]
gx: goal x position [m]
gx: goal x position [m]
node_x: node x position

104
SLAM/GraphBasedSLAM/graph_based_slam.py
View File

@@ -6,20 +6,22 @@ author: Atsushi Sakai (@Atsushi_twi)
Ref
[A Tutorial on Graph-Based SLAM](http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf)
[A Tutorial on Graph-Based SLAM]
(http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf)
"""
import numpy as np
import math
import copy
import itertools
import matplotlib.pyplot as plt
import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
# Simulation parameter
Qsim = np.diag([0.2, np.deg2rad(1.0)])**2
Rsim = np.diag([0.1, np.deg2rad(10.0)])**2
Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2
R_sim = np.diag([0.1, np.deg2rad(10.0)]) ** 2
DT = 2.0 # time tick [s]
SIM_TIME = 100.0 # simulation time [s]
@@ -33,11 +35,11 @@ C_SIGMA3 = np.deg2rad(1.0)
MAX_ITR = 20 # Maximum iteration
show_graph_dtime = 20.0 # [s]
show_graph_d_time = 20.0 # [s]
show_animation = True
class Edge():
class Edge:
def __init__(self):
self.e = np.zeros((3, 1))
@@ -52,26 +54,21 @@ class Edge():
self.id2 = 0
def cal_observation_sigma(d):
def cal_observation_sigma():
sigma = np.zeros((3, 3))
sigma[0, 0] = C_SIGMA1**2
sigma[1, 1] = C_SIGMA2**2
sigma[2, 2] = C_SIGMA3**2
sigma[0, 0] = C_SIGMA1 ** 2
sigma[1, 1] = C_SIGMA2 ** 2
sigma[2, 2] = C_SIGMA3 ** 2
return sigma
def calc_rotational_matrix(angle):
Rt = np.array([[math.cos(angle), -math.sin(angle), 0],
[math.sin(angle), math.cos(angle), 0],
[0, 0, 1.0]])
return Rt
return Rot.from_euler('z', angle).as_matrix()
def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,
angle1, phi1, d2, angle2, phi2, t1, t2):
angle1, d2, angle2, t1, t2):
edge = Edge()
tangle1 = pi_2_pi(yaw1 + angle1)
@@ -88,8 +85,8 @@ def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,
Rt1 = calc_rotational_matrix(tangle1)
Rt2 = calc_rotational_matrix(tangle2)
sig1 = cal_observation_sigma(d1)
sig2 = cal_observation_sigma(d2)
sig1 = cal_observation_sigma()
sig2 = cal_observation_sigma()
edge.omega = np.linalg.inv(Rt1 @ sig1 @ Rt1.T + Rt2 @ sig2 @ Rt2.T)
@@ -101,34 +98,33 @@ def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,
return edge
def calc_edges(xlist, zlist):
def calc_edges(x_list, z_list):
edges = []
cost = 0.0
zids = list(itertools.combinations(range(len(zlist)), 2))
z_ids = list(itertools.combinations(range(len(z_list)), 2))
for (t1, t2) in zids:
x1, y1, yaw1 = xlist[0, t1], xlist[1, t1], xlist[2, t1]
x2, y2, yaw2 = xlist[0, t2], xlist[1, t2], xlist[2, t2]
for (t1, t2) in z_ids:
x1, y1, yaw1 = x_list[0, t1], x_list[1, t1], x_list[2, t1]
x2, y2, yaw2 = x_list[0, t2], x_list[1, t2], x_list[2, t2]
if zlist[t1] is None or zlist[t2] is None:
if z_list[t1] is None or z_list[t2] is None:
continue # No observation
for iz1 in range(len(zlist[t1][:, 0])):
for iz2 in range(len(zlist[t2][:, 0])):
if zlist[t1][iz1, 3] == zlist[t2][iz2, 3]:
d1 = zlist[t1][iz1, 0]
angle1, phi1 = zlist[t1][iz1, 1], zlist[t1][iz1, 2]
d2 = zlist[t2][iz2, 0]
angle2, phi2 = zlist[t2][iz2, 1], zlist[t2][iz2, 2]
for iz1 in range(len(z_list[t1][:, 0])):
for iz2 in range(len(z_list[t2][:, 0])):
if z_list[t1][iz1, 3] == z_list[t2][iz2, 3]:
d1 = z_list[t1][iz1, 0]
angle1, phi1 = z_list[t1][iz1, 1], z_list[t1][iz1, 2]
d2 = z_list[t2][iz2, 0]
angle2, phi2 = z_list[t2][iz2, 1], z_list[t2][iz2, 2]
edge = calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,
angle1, phi1, d2, angle2, phi2, t1, t2)
angle1, d2, angle2, t1, t2)
edges.append(edge)
cost += (edge.e.T @ (edge.omega) @ edge.e)[0, 0]
cost += (edge.e.T @ edge.omega @ edge.e)[0, 0]
print("cost:", cost, ",nedge:", len(edges))
print("cost:", cost, ",n_edge:", len(edges))
return edges
@@ -147,7 +143,6 @@ def calc_jacobian(edge):
def fill_H_and_b(H, b, edge):
A, B = calc_jacobian(edge)
id1 = edge.id1 * STATE_SIZE
@@ -167,14 +162,14 @@ def fill_H_and_b(H, b, edge):
def graph_based_slam(x_init, hz):
print("start graph based slam")
zlist = copy.deepcopy(hz)
z_list = copy.deepcopy(hz)
x_opt = copy.deepcopy(x_init)
nt = x_opt.shape[1]
n = nt * STATE_SIZE
for itr in range(MAX_ITR):
edges = calc_edges(x_opt, zlist)
edges = calc_edges(x_opt, z_list)
H = np.zeros((n, n))
b = np.zeros((n, 1))
@@ -200,13 +195,12 @@ def graph_based_slam(x_init, hz):
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([[v, yawrate]]).T
yaw_rate = 0.1 # [rad/s]
u = np.array([[v, yaw_rate]]).T
return u
def observation(xTrue, xd, u, RFID):
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
@@ -220,16 +214,16 @@ def observation(xTrue, xd, u, RFID):
angle = pi_2_pi(math.atan2(dy, dx)) - xTrue[2, 0]
phi = pi_2_pi(math.atan2(dy, dx))
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0] # add noise
angle_noise = np.random.randn() * Qsim[1, 1]
dn = d + np.random.randn() * Q_sim[0, 0] # add noise
angle_noise = np.random.randn() * Q_sim[1, 1]
angle += angle_noise
phi += angle_noise
zi = np.array([dn, angle, phi, i])
z = np.vstack((z, zi))
# add noise to input
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0]
ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1]
ud = np.array([[ud1, ud2]]).T
xd = motion_model(xd, ud)
@@ -238,7 +232,6 @@ def observation(xTrue, xd, u, RFID):
def motion_model(x, u):
F = np.array([[1.0, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])
@@ -277,7 +270,7 @@ def main():
hxTrue = []
hxDR = []
hz = []
dtime = 0.0
d_time = 0.0
init = False
while SIM_TIME >= time:
@@ -290,22 +283,23 @@ def main():
hxTrue = np.hstack((hxTrue, xTrue))
time += DT
dtime += DT
d_time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
hz.append(z)
if dtime >= show_graph_dtime:
if d_time >= show_graph_d_time:
x_opt = graph_based_slam(hxDR, hz)
dtime = 0.0
d_time = 0.0
if show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(hxTrue[0, :].flatten(),