github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-10 20:25:01 -05: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 - conda update -q conda
# Useful for debugging any issues with conda # Useful for debugging any issues with conda
- conda info -a - conda info -a
# - conda install python==3.6.8
install: install:
- conda install numpy==1.15 - conda install numpy
- conda install scipy - conda install scipy
- conda install matplotlib - conda install matplotlib
- conda install pandas - conda install pandas

38
Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
View File

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

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

14
Localization/particle_filter/particle_filter.py
View File

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

165
Mapping/lidar_to_grid_map/lidar_to_grid_map.py
View File

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

72
Mapping/rectangle_fitting/rectangle_fitting.py
View File

@@ -5,7 +5,10 @@ Object shape recognition with L-shape fitting
author: Atsushi Sakai (@Atsushi_twi) author: Atsushi Sakai (@Atsushi_twi)
Ref: 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 numpy as np
import itertools import itertools
from enum import Enum from enum import Enum
from scipy.spatial.transform import Rotation as Rot
from simulator import VehicleSimulator, LidarSimulator from simulator import VehicleSimulator, LidarSimulator
show_animation = True show_animation = True
class LShapeFitting(): class LShapeFitting:
class Criteria(Enum): class Criteria(Enum):
AREA = 1 AREA = 1
@@ -29,26 +33,27 @@ class LShapeFitting():
def __init__(self): def __init__(self):
# Parameters # Parameters
self.criteria = self.Criteria.VARIANCE self.criteria = self.Criteria.VARIANCE
self.min_dist_of_closeness_crit = 0.01 # [m] self.min_dist_of_closeness_criteria = 0.01 # [m]
self.dtheta_deg_for_serarch = 1.0 # [deg] self.d_theta_deg_for_search = 1.0 # [deg]
self.R0 = 3.0 # [m] range segmentation param self.R0 = 3.0 # [m] range segmentation param
self.Rd = 0.001 # [m] range segmentation param self.Rd = 0.001 # [m] range segmentation param
def fitting(self, ox, oy): def fitting(self, ox, oy):
# step1: Adaptive Range Segmentation # step1: Adaptive Range Segmentation
idsets = self._adoptive_range_segmentation(ox, oy) id_sets = self._adoptive_range_segmentation(ox, oy)
# step2 Rectangle search # step2 Rectangle search
rects = [] 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] 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] cy = [oy[i] for i in range(len(oy)) if i in ids]
rects.append(self._rectangle_search(cx, cy)) 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) c1_max = max(c1)
c2_max = max(c2) c2_max = max(c2)
c1_min = min(c1) c1_min = min(c1)
@@ -71,12 +76,13 @@ class LShapeFitting():
beta = 0 beta = 0
for i, _ in enumerate(D1): 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) beta += (1.0 / d)
return beta return beta
def _calc_variance_criterion(self, c1, c2): @staticmethod
def _calc_variance_criterion(c1, c2):
c1_max = max(c1) c1_max = max(c1)
c2_max = max(c2) c2_max = max(c2)
c1_min = min(c1) c1_min = min(c1)
@@ -110,17 +116,17 @@ class LShapeFitting():
X = np.array([x, y]).T X = np.array([x, y]).T
dtheta = np.deg2rad(self.dtheta_deg_for_serarch) d_theta = np.deg2rad(self.d_theta_deg_for_search)
minp = (-float('inf'), None) min_cost = (-float('inf'), None)
for theta in np.arange(0.0, np.pi / 2.0 - dtheta, dtheta): for theta in np.arange(0.0, np.pi / 2.0 - d_theta, d_theta):
e1 = np.array([np.cos(theta), np.sin(theta)]) rot = Rot.from_euler('z', theta).as_matrix()[0:2, 0:2]
e2 = np.array([-np.sin(theta), np.cos(theta)]) c = X @ rot
c1 = c[:, 0]
c1 = X @ e1.T c2 = c[:, 1]
c2 = X @ e2.T
# Select criteria # Select criteria
cost = 0.0
if self.criteria == self.Criteria.AREA: if self.criteria == self.Criteria.AREA:
cost = self._calc_area_criterion(c1, c2) cost = self._calc_area_criterion(c1, c2)
elif self.criteria == self.Criteria.CLOSENESS: elif self.criteria == self.Criteria.CLOSENESS:
@@ -128,12 +134,12 @@ class LShapeFitting():
elif self.criteria == self.Criteria.VARIANCE: elif self.criteria == self.Criteria.VARIANCE:
cost = self._calc_variance_criterion(c1, c2) cost = self._calc_variance_criterion(c1, c2)
if minp[0] < cost: if min_cost[0] < cost:
minp = (cost, theta) min_cost = (cost, theta)
# calc best rectangle # calc best rectangle
sin_s = np.sin(minp[1]) sin_s = np.sin(min_cost[1])
cos_s = np.cos(minp[1]) cos_s = np.cos(min_cost[1])
c1_s = X @ np.array([cos_s, sin_s]).T c1_s = X @ np.array([cos_s, sin_s]).T
c2_s = X @ np.array([-sin_s, cos_s]).T c2_s = X @ np.array([-sin_s, cos_s]).T
@@ -181,7 +187,7 @@ class LShapeFitting():
return S return S
class RectangleData(): class RectangleData:
def __init__(self): def __init__(self):
self.a = [None] * 4 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.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] 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]) 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]) y = (a[1] * -c[0] - a[0] * -c[1]) / (a[0] * b[1] - a[1] * b[0])
return x, y return x, y
@@ -216,34 +223,35 @@ class RectangleData():
def main(): def main():
# simulation parameters # simulation parameters
simtime = 30.0 # simulation time sim_time = 30.0 # simulation time
dt = 0.2 # time tick 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), v1 = VehicleSimulator(-10.0, 0.0, np.deg2rad(90.0),
0.0, 50.0 / 3.6, 3.0, 5.0) 0.0, 50.0 / 3.6, 3.0, 5.0)
v2 = VehicleSimulator(20.0, 10.0, np.deg2rad(180.0), v2 = VehicleSimulator(20.0, 10.0, np.deg2rad(180.0),
0.0, 50.0 / 3.6, 4.0, 10.0) 0.0, 50.0 / 3.6, 4.0, 10.0)
lshapefitting = LShapeFitting() l_shape_fitting = LShapeFitting()
lidar_sim = LidarSimulator() lidar_sim = LidarSimulator()
time = 0.0 time = 0.0
while time <= simtime: while time <= sim_time:
time += dt time += dt
v1.update(dt, 0.1, 0.0) v1.update(dt, 0.1, 0.0)
v2.update(dt, 0.1, -0.05) 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 if show_animation: # pragma: no cover
plt.cla() plt.cla()
# for stopping simulation with the esc key. # for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event', plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None]) lambda event: [exit(0) if event.key == 'escape' else None])
plt.axis("equal") plt.axis("equal")
plt.plot(0.0, 0.0, "*r") plt.plot(0.0, 0.0, "*r")
@@ -251,7 +259,7 @@ def main():
v2.plot() v2.plot()
# Plot range observation # 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] 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] 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 matplotlib.pyplot as plt
import math import math
import random 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): def __init__(self, i_x, i_y, i_yaw, i_v, max_v, w, L):
self.x = ix self.x = i_x
self.y = iy self.y = i_y
self.yaw = iyaw self.yaw = i_yaw
self.v = iv self.v = i_v
self.max_v = max_v self.max_v = max_v
self.W = w self.W = w
self.L = L self.L = L
@@ -40,10 +41,10 @@ class VehicleSimulator():
plt.plot(gx, gy, "--b") plt.plot(gx, gy, "--b")
def calc_global_contour(self): def calc_global_contour(self):
gx = [(ix * np.cos(self.yaw) + iy * np.sin(self.yaw)) + rot = Rot.from_euler('z', self.yaw).as_matrix()[0:2, 0:2]
self.x for (ix, iy) in zip(self.vc_x, self.vc_y)] gxy = np.stack([self.vc_x, self.vc_y]).T @ rot
gy = [(ix * np.sin(self.yaw) - iy * np.cos(self.yaw)) + gx = gxy[:, 0] + self.x
self.y for (ix, iy) in zip(self.vc_x, self.vc_y)] gy = gxy[:, 1] + self.y
return gx, gy return gx, gy
@@ -67,69 +68,71 @@ class VehicleSimulator():
self.vc_x.append(self.L / 2.0) self.vc_x.append(self.L / 2.0)
self.vc_y.append(self.W / 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 = [], [] rx, ry = [], []
dtheta = 0.05 d_theta = 0.05
for i in range(len(x) - 1): for i in range(len(x) - 1):
rx.extend([(1.0 - θ) * x[i] + θ * x[i + 1] rx.extend([(1.0 - theta) * x[i] + theta * x[i + 1]
for θ in np.arange(0.0, 1.0, dtheta)]) for theta in np.arange(0.0, 1.0, d_theta)])
ry.extend([(1.0 - θ) * y[i] + θ * y[i + 1] ry.extend([(1.0 - theta) * y[i] + theta * y[i + 1]
for θ in np.arange(0.0, 1.0, dtheta)]) for theta in np.arange(0.0, 1.0, d_theta)])
rx.extend([(1.0 - θ) * x[len(x) - 1] + θ * x[1] rx.extend([(1.0 - theta) * x[len(x) - 1] + theta * x[1]
for θ in np.arange(0.0, 1.0, dtheta)]) for theta in np.arange(0.0, 1.0, d_theta)])
ry.extend([(1.0 - θ) * y[len(y) - 1] + θ * y[1] ry.extend([(1.0 - theta) * y[len(y) - 1] + theta * y[1]
for θ in np.arange(0.0, 1.0, dtheta)]) for theta in np.arange(0.0, 1.0, d_theta)])
return rx, ry return rx, ry
class LidarSimulator(): class LidarSimulator:
def __init__(self): def __init__(self):
self.range_noise = 0.01 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 = [], [], [], [] x, y, angle, r = [], [], [], []
# store all points # store all points
for v in vlist: for v in v_list:
gx, gy = v.calc_global_contour() gx, gy = v.calc_global_contour()
for vx, vy in zip(gx, gy): 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, vr = np.hypot(vx, vy) * random.uniform(1.0 - self.range_noise,
1.0 + self.range_noise) 1.0 + self.range_noise)
x.append(vx) x.append(vx)
y.append(vy) y.append(vy)
angle.append(vangle) angle.append(v_angle)
r.append(vr) r.append(vr)
# ray casting filter # 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 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 = [], [] rx, ry = [], []
rangedb = [float("inf") for _ in range( range_db = [float("inf") for _ in range(
int(np.floor((np.pi * 2.0) / angle_reso)) + 1)] int(np.floor((np.pi * 2.0) / angle_resolution)) + 1)]
for i in range(len(thetal)): for i in range(len(theta_l)):
angleid = int(round(thetal[i] / angle_reso)) angle_id = int(round(theta_l[i] / angle_resolution))
if rangedb[angleid] > rangel[i]: if range_db[angle_id] > range_l[i]:
rangedb[angleid] = rangel[i] range_db[angle_id] = range_l[i]
for i in range(len(rangedb)): for i in range(len(range_db)):
t = i * angle_reso t = i * angle_resolution
if rangedb[i] != float("inf"): if range_db[i] != float("inf"):
rx.append(rangedb[i] * np.cos(t)) rx.append(range_db[i] * np.cos(t))
ry.append(rangedb[i] * np.sin(t)) ry.append(range_db[i] * np.sin(t))
return rx, ry return rx, ry

130
PathPlanning/AStar/a_star.py
View File

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

4
PathPlanning/Dijkstra/dijkstra.py
View File

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

94
PathPlanning/DubinsPath/dubins_path_planning.py
View File

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

34
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -41,11 +41,11 @@ class Config:
# robot parameter # robot parameter
self.max_speed = 1.0 # [m/s] self.max_speed = 1.0 # [m/s]
self.min_speed = -0.5 # [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_accel = 0.2 # [m/ss]
self.max_dyawrate = 40.0 * math.pi / 180.0 # [rad/ss] self.max_delta_yaw_rate = 40.0 * math.pi / 180.0 # [rad/ss]
self.v_reso = 0.01 # [m/s] self.v_resolution = 0.01 # [m/s]
self.yawrate_reso = 0.1 * math.pi / 180.0 # [rad/s] self.yaw_rate_resolution = 0.1 * math.pi / 180.0 # [rad/s]
self.dt = 0.1 # [s] Time tick for motion prediction self.dt = 0.1 # [s] Time tick for motion prediction
self.predict_time = 3.0 # [s] self.predict_time = 3.0 # [s]
self.to_goal_cost_gain = 0.15 self.to_goal_cost_gain = 0.15
@@ -93,15 +93,15 @@ def calc_dynamic_window(x, config):
# Dynamic window from robot specification # Dynamic window from robot specification
Vs = [config.min_speed, config.max_speed, 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 # Dynamic window from motion model
Vd = [x[3] - config.max_accel * config.dt, Vd = [x[3] - config.max_accel * config.dt,
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_delta_yaw_rate * config.dt,
x[4] + config.max_dyawrate * 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]), dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
max(Vs[2], Vd[2]), min(Vs[3], Vd[3])] 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) x = np.array(x_init)
traj = np.array(x) trajectory = np.array(x)
time = 0 time = 0
while time <= config.predict_time: while time <= config.predict_time:
x = motion(x, [v, y], config.dt) x = motion(x, [v, y], config.dt)
traj = np.vstack((traj, x)) trajectory = np.vstack((trajectory, x))
time += config.dt time += config.dt
return traj return trajectory
def calc_control_and_trajectory(x, dw, config, goal, ob): 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]) best_trajectory = np.array([x])
# evaluate all trajectory with sampled input in dynamic window # evaluate all trajectory with sampled input in dynamic window
for v in np.arange(dw[0], dw[1], config.v_reso): for v in np.arange(dw[0], dw[1], config.v_resolution):
for y in np.arange(dw[2], dw[3], config.yawrate_reso): for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):
trajectory = predict_trajectory(x_init, v, y, config) 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(): np.logical_and(bottom_check, left_check))).any():
return float("Inf") return float("Inf")
elif config.robot_type == RobotType.circle: elif config.robot_type == RobotType.circle:
if (r <= config.robot_radius).any(): if np.array(r <= config.robot_radius).any():
return float("Inf") return float("Inf")
min_r = np.min(r) min_r = np.min(r)
@@ -269,7 +269,8 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
if show_animation: if show_animation:
plt.cla() plt.cla()
# for stopping simulation with the esc key. # for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event', plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None]) lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g") plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g")
plt.plot(x[0], x[1], "xr") plt.plot(x[0], x[1], "xr")
@@ -296,4 +297,5 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
if __name__ == '__main__': 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] ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m] oy: y position list of Obstacles [m]
reso: grid resolution [m] resolution: grid resolution [m]
rr: robot radius[m] rr: robot radius[m]
""" """
@@ -49,8 +49,8 @@ class BestFirstSearchPlanner:
Greedy Best-First search Greedy Best-First search
input: input:
sx: start x position [m] s_x: start x position [m]
sy: start y position [m] s_y: start y position [m]
gx: goal x position [m] gx: goal x position [m]
gy: goal y position [m] gy: goal y position [m]
@@ -132,7 +132,7 @@ class BestFirstSearchPlanner:
# generate final course # generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [ rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)] self.calc_grid_position(ngoal.y, self.miny)]
n = closedset[ngoal.pind] n = closedset[ngoal.parent_index]
while n is not None: while n is not None:
rx.append(self.calc_grid_position(n.x, self.minx)) rx.append(self.calc_grid_position(n.x, self.minx))
ry.append(self.calc_grid_position(n.y, self.miny)) ry.append(self.calc_grid_position(n.y, self.miny))
@@ -188,15 +188,15 @@ class BestFirstSearchPlanner:
self.miny = round(min(oy)) self.miny = round(min(oy))
self.maxx = round(max(ox)) self.maxx = round(max(ox))
self.maxy = round(max(oy)) self.maxy = round(max(oy))
print("minx:", self.minx) print("min_x:", self.minx)
print("miny:", self.miny) print("min_y:", self.miny)
print("maxx:", self.maxx) print("max_x:", self.maxx)
print("maxy:", self.maxy) print("max_y:", self.maxy)
self.xwidth = round((self.maxx - self.minx) / self.reso) self.xwidth = round((self.maxx - self.minx) / self.reso)
self.ywidth = round((self.maxy - self.miny) / self.reso) self.ywidth = round((self.maxy - self.miny) / self.reso)
print("xwidth:", self.xwidth) print("x_width:", self.xwidth)
print("ywidth:", self.ywidth) print("y_width:", self.ywidth)
# obstacle map generation # obstacle map generation
self.obmap = [[False for _ in range(self.ywidth)] self.obmap = [[False for _ in range(self.ywidth)]

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

141
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 heapq
import math
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
show_animation = False show_animation = False
@@ -17,71 +18,73 @@ show_animation = False
class Node: class Node:
def __init__(self, x, y, cost, pind): def __init__(self, x, y, cost, parent_index):
self.x = x self.x = x
self.y = y self.y = y
self.cost = cost self.cost = cost
self.pind = pind self.parent_index = parent_index
def __str__(self): 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 # generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso] rx, ry = [goal_node.x * resolution], [goal_node.y * resolution]
pind = ngoal.pind parent_index = goal_node.parent_index
while pind != -1: while parent_index != -1:
n = closedset[pind] n = closed_node_set[parent_index]
rx.append(n.x * reso) rx.append(n.x * resolution)
ry.append(n.y * reso) ry.append(n.y * resolution)
pind = n.pind parent_index = n.parent_index
return rx, ry 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]
gx: goal x position [m] gx: goal x position [m]
ox: x position list of Obstacles [m] ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m] oy: y position list of Obstacles [m]
reso: grid resolution [m] resolution: grid resolution [m]
rr: robot radius[m] rr: robot radius[m]
""" """
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1) start_node = Node(round(sx / resolution), round(sy / resolution), 0.0, -1)
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1) goal_node = Node(round(gx / resolution), round(gy / resolution), 0.0, -1)
ox = [iox / reso for iox in ox] ox = [iox / resolution for iox in ox]
oy = [ioy / reso for ioy in oy] 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() motion = get_motion_model()
openset, closedset = dict(), dict() open_set, closed_set = dict(), dict()
openset[calc_index(ngoal, xw, minx, miny)] = ngoal open_set[calc_index(goal_node, x_w, min_x, min_y)] = goal_node
pq = [] priority_queue = [(0, calc_index(goal_node, x_w, min_x, min_y))]
pq.append((0, calc_index(ngoal, xw, minx, miny)))
while 1: while 1:
if not pq: if not priority_queue:
break break
cost, c_id = heapq.heappop(pq) cost, c_id = heapq.heappop(priority_queue)
if c_id in openset: if c_id in open_set:
current = openset[c_id] current = open_set[c_id]
closedset[c_id] = current closed_set[c_id] = current
openset.pop(c_id) open_set.pop(c_id)
else: else:
continue continue
# show graph # show graph
if show_animation: # pragma: no cover 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. # for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event', plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None]) lambda event: [exit(0) if event.key == 'escape' else None])
if len(closedset.keys()) % 10 == 0: if len(closed_set.keys()) % 10 == 0:
plt.pause(0.001) plt.pause(0.001)
# Remove the item from the open set # Remove the item from the open set
@@ -91,29 +94,31 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr):
node = Node(current.x + motion[i][0], node = Node(current.x + motion[i][0],
current.y + motion[i][1], current.y + motion[i][1],
current.cost + motion[i][2], c_id) 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 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 continue
if n_id not in openset: if n_id not in open_set:
openset[n_id] = node # Discover a new node open_set[n_id] = node # Discover a new node
heapq.heappush( 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: 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! # This path is the best until now. record it!
openset[n_id] = node open_set[n_id] = node
heapq.heappush( 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( rx, ry = calc_final_path(closed_set[calc_index(
nstart, xw, minx, miny)], closedset, reso) 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): def calc_heuristic(n1, n2):
@@ -122,51 +127,49 @@ def calc_heuristic(n1, n2):
return d return d
def verify_node(node, obmap, minx, miny, maxx, maxy): def verify_node(node, obstacle_map, min_x, min_y, max_x, max_y):
if node.x < min_x:
if node.x < minx:
return False return False
elif node.y < miny: elif node.y < min_y:
return False return False
elif node.x >= maxx: elif node.x >= max_x:
return False return False
elif node.y >= maxy: elif node.y >= max_y:
return False return False
if obmap[node.x][node.y]: if obstacle_map[node.x][node.y]:
return False return False
return True 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)) x_width = round(max_x - min_x)
miny = round(min(oy)) y_width = round(max_y - min_y)
maxx = round(max(ox))
maxy = round(max(oy))
xwidth = round(maxx - minx)
ywidth = round(maxy - miny)
# obstacle map generation # obstacle map generation
obmap = [[False for i in range(ywidth)] for i in range(xwidth)] obstacle_map = [[False for _ in range(y_width)] for _ in range(x_width)]
for ix in range(xwidth): for ix in range(x_width):
x = ix + minx x = ix + min_x
for iy in range(ywidth): for iy in range(y_width):
y = iy + miny y = iy + min_y
# print(x, y) # print(x, y)
for iox, ioy in zip(ox, oy): for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2) d = math.sqrt((iox - x) ** 2 + (ioy - y) ** 2)
if d <= vr / reso: if d <= vr / resolution:
obmap[ix][iy] = True obstacle_map[ix][iy] = True
break 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): def calc_index(node, x_width, x_min, y_min):
return (node.y - ymin) * xwidth + (node.x - xmin) return (node.y - y_min) * x_width + (node.x - x_min)
def get_motion_model(): 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 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 WB = 3. # rear to front wheel
W = 2. # width of car 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 LB = 1.0 # distance from rear to vehicle back end
MAX_STEER = 0.6 # [rad] maximum steering angle MAX_STEER = 0.6 # [rad] maximum steering angle
WBUBBLE_DIST = (LF - LB) / 2.0 W_BUBBLE_DIST = (LF - LB) / 2.0
WBUBBLE_R = sqrt(((LF + LB) / 2.0)**2 + 1) W_BUBBLE_R = sqrt(((LF + LB) / 2.0) ** 2 + 1)
# vehicle rectangle verticles # vehicle rectangle vertices
VRX = [LF, LF, -LB, -LB, LF] VRX = [LF, LF, -LB, -LB, LF]
VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2] VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2]
def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): def check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):
for x, y, yaw in zip(xlist, ylist, yawlist): for i_x, i_y, i_yaw in zip(x_list, y_list, yaw_list):
cx = x + WBUBBLE_DIST * cos(yaw) cx = i_x + W_BUBBLE_DIST * cos(i_yaw)
cy = y + WBUBBLE_DIST * sin(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: if not ids:
continue 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]): [ox[i] for i in ids], [oy[i] for i in ids]):
return False # collision 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): def rectangle_check(x, y, yaw, ox, oy):
# transform obstacles to base link frame # 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): for iox, ioy in zip(ox, oy):
tx = iox - x tx = iox - x
ty = ioy - y ty = ioy - y
rx = c * tx - s * ty converted_xy = np.stack([tx, ty]).T @ rot
ry = s * tx + c * ty 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): if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0):
return False # no collision 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"): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
"""Plot arrow.""" """Plot arrow."""
if not isinstance(x, float): if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw): for (i_x, i_y, i_yaw) in zip(x, y, yaw):
plot_arrow(ix, iy, iyaw) plot_arrow(i_x, i_y, i_yaw)
else: else:
plt.arrow(x, y, length * cos(yaw), length * sin(yaw), plt.arrow(x, y, length * cos(yaw), length * sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4) fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4)
# plt.plot(x, y)
def plot_car(x, y, yaw): def plot_car(x, y, yaw):
car_color = '-k' car_color = '-k'
c, s = cos(yaw), sin(yaw) c, s = cos(yaw), sin(yaw)
rot = Rot.from_euler('z', yaw).as_matrix()[0:2, 0:2]
car_outline_x, car_outline_y = [], [] car_outline_x, car_outline_y = [], []
for rx, ry in zip(VRX, VRY): for rx, ry in zip(VRX, VRY):
tx = c * rx - s * ry + x converted_xy = np.stack([rx, ry]).T @ rot
ty = s * rx + c * ry + y car_outline_x.append(converted_xy[0]+x)
car_outline_x.append(tx) car_outline_y.append(converted_xy[1]+y)
car_outline_y.append(ty)
arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw
plot_arrow(arrow_x, arrow_y, arrow_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 return x, y, yaw
if __name__ == '__main__': def main():
x, y, yaw = 0., 0., 1. x, y, yaw = 0., 0., 1.
plt.axis('equal') plt.axis('equal')
plot_car(x, y, yaw) plot_car(x, y, yaw)
plt.show() plt.show()
if __name__ == '__main__':
main()

320
PathPlanning/HybridAStar/hybrid_a_star.py
View File

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

77
PathPlanning/InformedRRTStar/informed_rrt_star.py
View File

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

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

4
PathPlanning/VoronoiRoadMap/dijkstra_search.py
View File

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

96
SLAM/GraphBasedSLAM/graph_based_slam.py
View File

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