mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
405 lines
10 KiB
Python
405 lines
10 KiB
Python
"""
|
|
|
|
Reeds Shepp path planner sample code
|
|
|
|
author Atsushi Sakai(@Atsushi_twi)
|
|
|
|
"""
|
|
import math
|
|
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
show_animation = True
|
|
|
|
|
|
class Path:
|
|
|
|
def __init__(self):
|
|
self.lengths = []
|
|
self.ctypes = []
|
|
self.L = 0.0
|
|
self.x = []
|
|
self.y = []
|
|
self.yaw = []
|
|
self.directions = []
|
|
|
|
|
|
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
|
|
"""
|
|
Plot arrow
|
|
"""
|
|
|
|
if not isinstance(x, float):
|
|
for (ix, iy, iyaw) in zip(x, y, yaw):
|
|
plot_arrow(ix, iy, iyaw)
|
|
else:
|
|
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
|
|
fc=fc, ec=ec, head_width=width, head_length=width)
|
|
plt.plot(x, y)
|
|
|
|
|
|
def mod2pi(x):
|
|
v = np.mod(x, 2.0 * math.pi)
|
|
if v < -math.pi:
|
|
v += 2.0 * math.pi
|
|
else:
|
|
if v > math.pi:
|
|
v -= 2.0 * math.pi
|
|
return v
|
|
|
|
|
|
def straight_left_straight(x, y, phi):
|
|
phi = mod2pi(phi)
|
|
if y > 0.0 and 0.0 < phi < math.pi * 0.99:
|
|
xd = - y / math.tan(phi) + x
|
|
t = xd - math.tan(phi / 2.0)
|
|
u = phi
|
|
v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
|
|
return True, t, u, v
|
|
elif y < 0.0 < phi < math.pi * 0.99:
|
|
xd = - y / math.tan(phi) + x
|
|
t = xd - math.tan(phi / 2.0)
|
|
u = phi
|
|
v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0)
|
|
return True, t, u, v
|
|
|
|
return False, 0.0, 0.0, 0.0
|
|
|
|
|
|
def set_path(paths, lengths, ctypes):
|
|
path = Path()
|
|
path.ctypes = ctypes
|
|
path.lengths = lengths
|
|
|
|
# check same path exist
|
|
for tpath in paths:
|
|
typeissame = (tpath.ctypes == path.ctypes)
|
|
if typeissame:
|
|
if sum(tpath.lengths) - sum(path.lengths) <= 0.01:
|
|
return paths # not insert path
|
|
|
|
path.L = sum([abs(i) for i in lengths])
|
|
|
|
# Base.Test.@test path.L >= 0.01
|
|
if path.L >= 0.01:
|
|
paths.append(path)
|
|
|
|
return paths
|
|
|
|
|
|
def straight_curve_straight(x, y, phi, paths):
|
|
flag, t, u, v = straight_left_straight(x, y, phi)
|
|
if flag:
|
|
paths = set_path(paths, [t, u, v], ["S", "L", "S"])
|
|
|
|
flag, t, u, v = straight_left_straight(x, -y, -phi)
|
|
if flag:
|
|
paths = set_path(paths, [t, u, v], ["S", "R", "S"])
|
|
|
|
return paths
|
|
|
|
|
|
def polar(x, y):
|
|
r = math.sqrt(x ** 2 + y ** 2)
|
|
theta = math.atan2(y, x)
|
|
return r, theta
|
|
|
|
|
|
def left_straight_left(x, y, phi):
|
|
u, t = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
|
|
if t >= 0.0:
|
|
v = mod2pi(phi - t)
|
|
if v >= 0.0:
|
|
return True, t, u, v
|
|
|
|
return False, 0.0, 0.0, 0.0
|
|
|
|
|
|
def left_right_left(x, y, phi):
|
|
u1, t1 = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
|
|
|
|
if u1 <= 4.0:
|
|
u = -2.0 * math.asin(0.25 * u1)
|
|
t = mod2pi(t1 + 0.5 * u + math.pi)
|
|
v = mod2pi(phi - t + u)
|
|
|
|
if t >= 0.0 >= u:
|
|
return True, t, u, v
|
|
|
|
return False, 0.0, 0.0, 0.0
|
|
|
|
|
|
def curve_curve_curve(x, y, phi, paths):
|
|
flag, t, u, v = left_right_left(x, y, phi)
|
|
if flag:
|
|
paths = set_path(paths, [t, u, v], ["L", "R", "L"])
|
|
|
|
flag, t, u, v = left_right_left(-x, y, -phi)
|
|
if flag:
|
|
paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"])
|
|
|
|
flag, t, u, v = left_right_left(x, -y, -phi)
|
|
if flag:
|
|
paths = set_path(paths, [t, u, v], ["R", "L", "R"])
|
|
|
|
flag, t, u, v = left_right_left(-x, -y, phi)
|
|
if flag:
|
|
paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"])
|
|
|
|
# backwards
|
|
xb = x * math.cos(phi) + y * math.sin(phi)
|
|
yb = x * math.sin(phi) - y * math.cos(phi)
|
|
|
|
flag, t, u, v = left_right_left(xb, yb, phi)
|
|
if flag:
|
|
paths = set_path(paths, [v, u, t], ["L", "R", "L"])
|
|
|
|
flag, t, u, v = left_right_left(-xb, yb, -phi)
|
|
if flag:
|
|
paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"])
|
|
|
|
flag, t, u, v = left_right_left(xb, -yb, -phi)
|
|
if flag:
|
|
paths = set_path(paths, [v, u, t], ["R", "L", "R"])
|
|
|
|
flag, t, u, v = left_right_left(-xb, -yb, phi)
|
|
if flag:
|
|
paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"])
|
|
|
|
return paths
|
|
|
|
|
|
def curve_straight_curve(x, y, phi, paths):
|
|
flag, t, u, v = left_straight_left(x, y, phi)
|
|
if flag:
|
|
paths = set_path(paths, [t, u, v], ["L", "S", "L"])
|
|
|
|
flag, t, u, v = left_straight_left(-x, y, -phi)
|
|
if flag:
|
|
paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"])
|
|
|
|
flag, t, u, v = left_straight_left(x, -y, -phi)
|
|
if flag:
|
|
paths = set_path(paths, [t, u, v], ["R", "S", "R"])
|
|
|
|
flag, t, u, v = left_straight_left(-x, -y, phi)
|
|
if flag:
|
|
paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"])
|
|
|
|
flag, t, u, v = left_straight_right(x, y, phi)
|
|
if flag:
|
|
paths = set_path(paths, [t, u, v], ["L", "S", "R"])
|
|
|
|
flag, t, u, v = left_straight_right(-x, y, -phi)
|
|
if flag:
|
|
paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"])
|
|
|
|
flag, t, u, v = left_straight_right(x, -y, -phi)
|
|
if flag:
|
|
paths = set_path(paths, [t, u, v], ["R", "S", "L"])
|
|
|
|
flag, t, u, v = left_straight_right(-x, -y, phi)
|
|
if flag:
|
|
paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"])
|
|
|
|
return paths
|
|
|
|
|
|
def left_straight_right(x, y, phi):
|
|
u1, t1 = polar(x + math.sin(phi), y - 1.0 - math.cos(phi))
|
|
u1 = u1 ** 2
|
|
if u1 >= 4.0:
|
|
u = math.sqrt(u1 - 4.0)
|
|
theta = math.atan2(2.0, u)
|
|
t = mod2pi(t1 + theta)
|
|
v = mod2pi(t - phi)
|
|
|
|
if t >= 0.0 and v >= 0.0:
|
|
return True, t, u, v
|
|
|
|
return False, 0.0, 0.0, 0.0
|
|
|
|
|
|
def generate_path(q0, q1, max_curvature):
|
|
dx = q1[0] - q0[0]
|
|
dy = q1[1] - q0[1]
|
|
dth = q1[2] - q0[2]
|
|
c = math.cos(q0[2])
|
|
s = math.sin(q0[2])
|
|
x = (c * dx + s * dy) * max_curvature
|
|
y = (-s * dx + c * dy) * max_curvature
|
|
|
|
paths = []
|
|
paths = straight_curve_straight(x, y, dth, paths)
|
|
paths = curve_straight_curve(x, y, dth, paths)
|
|
paths = curve_curve_curve(x, y, dth, paths)
|
|
|
|
return paths
|
|
|
|
|
|
def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions):
|
|
if mode == "S":
|
|
path_x[ind] = origin_x + length / max_curvature * math.cos(origin_yaw)
|
|
path_y[ind] = origin_y + length / max_curvature * math.sin(origin_yaw)
|
|
path_yaw[ind] = origin_yaw
|
|
else: # curve
|
|
ldx = math.sin(length) / max_curvature
|
|
ldy = 0.0
|
|
if mode == "L": # left turn
|
|
ldy = (1.0 - math.cos(length)) / max_curvature
|
|
elif mode == "R": # right turn
|
|
ldy = (1.0 - math.cos(length)) / -max_curvature
|
|
gdx = math.cos(-origin_yaw) * ldx + math.sin(-origin_yaw) * ldy
|
|
gdy = -math.sin(-origin_yaw) * ldx + math.cos(-origin_yaw) * ldy
|
|
path_x[ind] = origin_x + gdx
|
|
path_y[ind] = origin_y + gdy
|
|
|
|
if mode == "L": # left turn
|
|
path_yaw[ind] = origin_yaw + length
|
|
elif mode == "R": # right turn
|
|
path_yaw[ind] = origin_yaw - length
|
|
|
|
if length > 0.0:
|
|
directions[ind] = 1
|
|
else:
|
|
directions[ind] = -1
|
|
|
|
return path_x, path_y, path_yaw, directions
|
|
|
|
|
|
def generate_local_course(total_length, lengths, mode, max_curvature, step_size):
|
|
n_point = math.trunc(total_length / step_size) + len(lengths) + 4
|
|
|
|
px = [0.0 for _ in range(n_point)]
|
|
py = [0.0 for _ in range(n_point)]
|
|
pyaw = [0.0 for _ in range(n_point)]
|
|
directions = [0.0 for _ in range(n_point)]
|
|
ind = 1
|
|
|
|
if lengths[0] > 0.0:
|
|
directions[0] = 1
|
|
else:
|
|
directions[0] = -1
|
|
|
|
ll = 0.0
|
|
|
|
for (m, l, i) in zip(mode, lengths, range(len(mode))):
|
|
if l > 0.0:
|
|
d = step_size
|
|
else:
|
|
d = -step_size
|
|
|
|
# set origin state
|
|
ox, oy, oyaw = px[ind], py[ind], pyaw[ind]
|
|
|
|
ind -= 1
|
|
if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
|
|
pd = - d - ll
|
|
else:
|
|
pd = d - ll
|
|
|
|
while abs(pd) <= abs(l):
|
|
ind += 1
|
|
px, py, pyaw, directions = interpolate(
|
|
ind, pd, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions)
|
|
pd += d
|
|
|
|
ll = l - pd - d # calc remain length
|
|
|
|
ind += 1
|
|
px, py, pyaw, directions = interpolate(
|
|
ind, l, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions)
|
|
|
|
# remove unused data
|
|
while px[-1] == 0.0:
|
|
px.pop()
|
|
py.pop()
|
|
pyaw.pop()
|
|
directions.pop()
|
|
|
|
return px, py, pyaw, directions
|
|
|
|
|
|
def pi_2_pi(angle):
|
|
return (angle + math.pi) % (2 * math.pi) - math.pi
|
|
|
|
|
|
def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
|
|
q0 = [sx, sy, syaw]
|
|
q1 = [gx, gy, gyaw]
|
|
|
|
paths = generate_path(q0, q1, maxc)
|
|
for path in paths:
|
|
x, y, yaw, directions = generate_local_course(
|
|
path.L, path.lengths, path.ctypes, maxc, step_size * maxc)
|
|
|
|
# convert global coordinate
|
|
path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2])
|
|
* iy + q0[0] for (ix, iy) in zip(x, y)]
|
|
path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2])
|
|
* iy + q0[1] for (ix, iy) in zip(x, y)]
|
|
path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw]
|
|
path.directions = directions
|
|
path.lengths = [length / maxc for length in path.lengths]
|
|
path.L = path.L / maxc
|
|
|
|
return paths
|
|
|
|
|
|
def reeds_shepp_path_planning(sx, sy, syaw,
|
|
gx, gy, gyaw, maxc, step_size=0.2):
|
|
paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size)
|
|
|
|
if not paths:
|
|
return None, None, None, None, None
|
|
|
|
minL = float("Inf")
|
|
best_path_index = -1
|
|
for i, _ in enumerate(paths):
|
|
if paths[i].L <= minL:
|
|
minL = paths[i].L
|
|
best_path_index = i
|
|
|
|
bpath = paths[best_path_index]
|
|
|
|
return bpath.x, bpath.y, bpath.yaw, bpath.ctypes, bpath.lengths
|
|
|
|
|
|
def main():
|
|
print("Reeds Shepp path planner sample start!!")
|
|
|
|
start_x = -1.0 # [m]
|
|
start_y = -4.0 # [m]
|
|
start_yaw = np.deg2rad(-20.0) # [rad]
|
|
|
|
end_x = 5.0 # [m]
|
|
end_y = 5.0 # [m]
|
|
end_yaw = np.deg2rad(25.0) # [rad]
|
|
|
|
curvature = 1.0
|
|
step_size = 0.1
|
|
|
|
px, py, pyaw, mode, clen = reeds_shepp_path_planning(
|
|
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
|
|
|
|
if show_animation: # pragma: no cover
|
|
plt.cla()
|
|
plt.plot(px, py, label="final course " + str(mode))
|
|
|
|
# plotting
|
|
plot_arrow(start_x, start_y, start_yaw)
|
|
plot_arrow(end_x, end_y, end_yaw)
|
|
|
|
plt.legend()
|
|
plt.grid(True)
|
|
plt.axis("equal")
|
|
plt.show()
|
|
|
|
if not px:
|
|
assert False, "No path"
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|