mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-02-10 16:25:31 -05:00
Fix reeds shepp path issue (#529)
* code clean up * code clean up * code clean up * code clean up * fix length handling issues
This commit is contained in:
Atsushi Sakai
@@ -14,28 +14,29 @@ show_animation = True
|
||||
|
||||
|
||||
class Path:
|
||||
"""
|
||||
Path data container
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
# course segment length (negative value is backward segment)
|
||||
self.lengths = []
|
||||
# course segment type char ("S": straight, "L": left, "R": right)
|
||||
self.ctypes = []
|
||||
self.L = 0.0
|
||||
self.x = []
|
||||
self.y = []
|
||||
self.yaw = []
|
||||
self.directions = []
|
||||
self.L = 0.0 # Total lengths of the path
|
||||
self.x = [] # x positions
|
||||
self.y = [] # y positions
|
||||
self.yaw = [] # orientations [rad]
|
||||
self.directions = [] # directions (1:forward, -1:backward)
|
||||
|
||||
|
||||
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
|
||||
"""
|
||||
Plot arrow
|
||||
"""
|
||||
|
||||
if not isinstance(x, float):
|
||||
if isinstance(x, list):
|
||||
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.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)
|
||||
|
||||
|
||||
@@ -68,35 +69,35 @@ def straight_left_straight(x, y, phi):
|
||||
return False, 0.0, 0.0, 0.0
|
||||
|
||||
|
||||
def set_path(paths, lengths, ctypes):
|
||||
def set_path(paths, lengths, ctypes, step_size):
|
||||
path = Path()
|
||||
path.ctypes = ctypes
|
||||
path.lengths = lengths
|
||||
path.L = sum(np.abs(lengths))
|
||||
|
||||
# check same path exist
|
||||
for tpath in paths:
|
||||
typeissame = (tpath.ctypes == path.ctypes)
|
||||
if typeissame:
|
||||
if sum(np.abs(tpath.lengths)) - sum(np.abs(path.lengths)) <= 0.01:
|
||||
return paths # not insert path
|
||||
for i_path in paths:
|
||||
type_is_same = (i_path.ctypes == path.ctypes)
|
||||
length_is_close = (sum(np.abs(i_path.lengths)) - path.L) <= step_size
|
||||
if type_is_same and length_is_close:
|
||||
return paths # same path found, so do 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)
|
||||
# check path is long enough
|
||||
if path.L <= step_size:
|
||||
return paths # too short, so do not insert path
|
||||
|
||||
paths.append(path)
|
||||
return paths
|
||||
|
||||
|
||||
def straight_curve_straight(x, y, phi, paths):
|
||||
def straight_curve_straight(x, y, phi, paths, step_size):
|
||||
flag, t, u, v = straight_left_straight(x, y, phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [t, u, v], ["S", "L", "S"])
|
||||
paths = set_path(paths, [t, u, v], ["S", "L", "S"], step_size)
|
||||
|
||||
flag, t, u, v = straight_left_straight(x, -y, -phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [t, u, v], ["S", "R", "S"])
|
||||
paths = set_path(paths, [t, u, v], ["S", "R", "S"], step_size)
|
||||
|
||||
return paths
|
||||
|
||||
@@ -131,22 +132,22 @@ def left_right_left(x, y, phi):
|
||||
return False, 0.0, 0.0, 0.0
|
||||
|
||||
|
||||
def curve_curve_curve(x, y, phi, paths):
|
||||
def curve_curve_curve(x, y, phi, paths, step_size):
|
||||
flag, t, u, v = left_right_left(x, y, phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [t, u, v], ["L", "R", "L"])
|
||||
paths = set_path(paths, [t, u, v], ["L", "R", "L"], step_size)
|
||||
|
||||
flag, t, u, v = left_right_left(-x, y, -phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"])
|
||||
paths = set_path(paths, [-t, -u, -v], ["L", "R", "L"], step_size)
|
||||
|
||||
flag, t, u, v = left_right_left(x, -y, -phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [t, u, v], ["R", "L", "R"])
|
||||
paths = set_path(paths, [t, u, v], ["R", "L", "R"], step_size)
|
||||
|
||||
flag, t, u, v = left_right_left(-x, -y, phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"])
|
||||
paths = set_path(paths, [-t, -u, -v], ["R", "L", "R"], step_size)
|
||||
|
||||
# backwards
|
||||
xb = x * math.cos(phi) + y * math.sin(phi)
|
||||
@@ -154,55 +155,55 @@ def curve_curve_curve(x, y, phi, paths):
|
||||
|
||||
flag, t, u, v = left_right_left(xb, yb, phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [v, u, t], ["L", "R", "L"])
|
||||
paths = set_path(paths, [v, u, t], ["L", "R", "L"], step_size)
|
||||
|
||||
flag, t, u, v = left_right_left(-xb, yb, -phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"])
|
||||
paths = set_path(paths, [-v, -u, -t], ["L", "R", "L"], step_size)
|
||||
|
||||
flag, t, u, v = left_right_left(xb, -yb, -phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [v, u, t], ["R", "L", "R"])
|
||||
paths = set_path(paths, [v, u, t], ["R", "L", "R"], step_size)
|
||||
|
||||
flag, t, u, v = left_right_left(-xb, -yb, phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"])
|
||||
paths = set_path(paths, [-v, -u, -t], ["R", "L", "R"], step_size)
|
||||
|
||||
return paths
|
||||
|
||||
|
||||
def curve_straight_curve(x, y, phi, paths):
|
||||
def curve_straight_curve(x, y, phi, paths, step_size):
|
||||
flag, t, u, v = left_straight_left(x, y, phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [t, u, v], ["L", "S", "L"])
|
||||
paths = set_path(paths, [t, u, v], ["L", "S", "L"], step_size)
|
||||
|
||||
flag, t, u, v = left_straight_left(-x, y, -phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"])
|
||||
paths = set_path(paths, [-t, -u, -v], ["L", "S", "L"], step_size)
|
||||
|
||||
flag, t, u, v = left_straight_left(x, -y, -phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [t, u, v], ["R", "S", "R"])
|
||||
paths = set_path(paths, [t, u, v], ["R", "S", "R"], step_size)
|
||||
|
||||
flag, t, u, v = left_straight_left(-x, -y, phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"])
|
||||
paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"], step_size)
|
||||
|
||||
flag, t, u, v = left_straight_right(x, y, phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [t, u, v], ["L", "S", "R"])
|
||||
paths = set_path(paths, [t, u, v], ["L", "S", "R"], step_size)
|
||||
|
||||
flag, t, u, v = left_straight_right(-x, y, -phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"])
|
||||
paths = set_path(paths, [-t, -u, -v], ["L", "S", "R"], step_size)
|
||||
|
||||
flag, t, u, v = left_straight_right(x, -y, -phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [t, u, v], ["R", "S", "L"])
|
||||
paths = set_path(paths, [t, u, v], ["R", "S", "L"], step_size)
|
||||
|
||||
flag, t, u, v = left_straight_right(-x, -y, phi)
|
||||
if flag:
|
||||
paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"])
|
||||
paths = set_path(paths, [-t, -u, -v], ["R", "S", "L"], step_size)
|
||||
|
||||
return paths
|
||||
|
||||
@@ -222,7 +223,7 @@ def left_straight_right(x, y, phi):
|
||||
return False, 0.0, 0.0, 0.0
|
||||
|
||||
|
||||
def generate_path(q0, q1, max_curvature):
|
||||
def generate_path(q0, q1, max_curvature, step_size):
|
||||
dx = q1[0] - q0[0]
|
||||
dy = q1[1] - q0[1]
|
||||
dth = q1[2] - q0[2]
|
||||
@@ -232,98 +233,70 @@ def generate_path(q0, q1, 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)
|
||||
paths = straight_curve_straight(x, y, dth, paths, step_size)
|
||||
paths = curve_straight_curve(x, y, dth, paths, step_size)
|
||||
paths = curve_curve_curve(x, y, dth, paths, step_size)
|
||||
|
||||
return paths
|
||||
|
||||
|
||||
def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions):
|
||||
def calc_interpolate_dists_list(lengths, step_size):
|
||||
interpolate_dists_list = []
|
||||
for length in lengths:
|
||||
d_dist = step_size if length >= 0.0 else -step_size
|
||||
interp_dists = np.arange(0.0, length, d_dist)
|
||||
interp_dists = np.append(interp_dists, length)
|
||||
interpolate_dists_list.append(interp_dists)
|
||||
|
||||
return interpolate_dists_list
|
||||
|
||||
|
||||
def generate_local_course(lengths, modes, max_curvature, step_size):
|
||||
interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size)
|
||||
|
||||
origin_x, origin_y, origin_yaw = 0.0, 0.0, 0.0
|
||||
|
||||
xs, ys, yaws, directions = [], [], [], []
|
||||
for (interp_dists, mode, length) in zip(interpolate_dists_list, modes,
|
||||
lengths):
|
||||
|
||||
for dist in interp_dists:
|
||||
x, y, yaw, direction = interpolate(dist, length, mode,
|
||||
max_curvature, origin_x,
|
||||
origin_y, origin_yaw)
|
||||
xs.append(x)
|
||||
ys.append(y)
|
||||
yaws.append(yaw)
|
||||
directions.append(direction)
|
||||
origin_x = xs[-1]
|
||||
origin_y = ys[-1]
|
||||
origin_yaw = yaws[-1]
|
||||
|
||||
return xs, ys, yaws, directions
|
||||
|
||||
|
||||
def interpolate(dist, length, mode, max_curvature, origin_x, origin_y,
|
||||
origin_yaw):
|
||||
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
|
||||
x = origin_x + dist / max_curvature * math.cos(origin_yaw)
|
||||
y = origin_y + dist / max_curvature * math.sin(origin_yaw)
|
||||
yaw = origin_yaw
|
||||
else: # curve
|
||||
ldx = math.sin(length) / max_curvature
|
||||
ldx = math.sin(dist) / max_curvature
|
||||
ldy = 0.0
|
||||
yaw = None
|
||||
if mode == "L": # left turn
|
||||
ldy = (1.0 - math.cos(length)) / max_curvature
|
||||
ldy = (1.0 - math.cos(dist)) / max_curvature
|
||||
yaw = origin_yaw + dist
|
||||
elif mode == "R": # right turn
|
||||
ldy = (1.0 - math.cos(length)) / -max_curvature
|
||||
ldy = (1.0 - math.cos(dist)) / -max_curvature
|
||||
yaw = origin_yaw - dist
|
||||
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
|
||||
x = origin_x + gdx
|
||||
y = 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, length, i) in zip(mode, lengths, range(len(mode))):
|
||||
if length == 0.0:
|
||||
continue
|
||||
elif length > 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(length):
|
||||
ind += 1
|
||||
px, py, pyaw, directions = interpolate(
|
||||
ind, pd, m, max_curvature, ox, oy, oyaw,
|
||||
px, py, pyaw, directions)
|
||||
pd += d
|
||||
|
||||
ll = length - pd - d # calc remain length
|
||||
|
||||
ind += 1
|
||||
px, py, pyaw, directions = interpolate(
|
||||
ind, length, 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
|
||||
return x, y, yaw, 1 if length > 0.0 else -1
|
||||
|
||||
|
||||
def pi_2_pi(angle):
|
||||
@@ -334,17 +307,18 @@ 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)
|
||||
paths = generate_path(q0, q1, maxc, step_size)
|
||||
for path in paths:
|
||||
x, y, yaw, directions = generate_local_course(
|
||||
path.L, path.lengths, path.ctypes, maxc, step_size * maxc)
|
||||
xs, ys, yaws, directions = generate_local_course(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.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for
|
||||
(ix, iy) in zip(xs, ys)]
|
||||
path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for
|
||||
(ix, iy) in zip(xs, ys)]
|
||||
path.yaw = [pi_2_pi(yaw + q0[2]) for yaw in yaws]
|
||||
path.directions = directions
|
||||
path.lengths = [length / maxc for length in path.lengths]
|
||||
path.L = path.L / maxc
|
||||
@@ -352,54 +326,42 @@ def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
|
||||
return paths
|
||||
|
||||
|
||||
def reeds_shepp_path_planning(sx, sy, syaw,
|
||||
gx, gy, gyaw, maxc, step_size=0.2):
|
||||
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
|
||||
return None, None, None, None, None # could not generate any path
|
||||
|
||||
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
|
||||
# search minimum cost path
|
||||
best_path_index = paths.index(min(paths, key=lambda p: abs(p.L)))
|
||||
b_path = paths[best_path_index]
|
||||
|
||||
bpath = paths[best_path_index]
|
||||
|
||||
return bpath.x, bpath.y, bpath.yaw, bpath.ctypes, bpath.lengths
|
||||
return b_path.x, b_path.y, b_path.yaw, b_path.ctypes, b_path.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]
|
||||
start_x = -1.0 # [m]
|
||||
start_y = -4.0 # [m]
|
||||
start_yaw = np.deg2rad(-20.0) # [rad]
|
||||
|
||||
start_x = 0.0 # [m]
|
||||
start_y = 0.0 # [m]
|
||||
start_yaw = np.deg2rad(0.0) # [rad]
|
||||
end_x = 5.0 # [m]
|
||||
end_y = 5.0 # [m]
|
||||
end_yaw = np.deg2rad(25.0) # [rad]
|
||||
|
||||
end_x = 0.0 # [m]
|
||||
end_y = 0.0 # [m]
|
||||
end_yaw = np.deg2rad(0.0) # [rad]
|
||||
curvature = 0.1
|
||||
step_size = 0.05
|
||||
|
||||
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)
|
||||
xs, ys, yaws, modes, lengths = 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))
|
||||
plt.plot(xs, ys, label="final course " + str(modes))
|
||||
print(f"{lengths=}")
|
||||
|
||||
# plotting
|
||||
plot_arrow(start_x, start_y, start_yaw)
|
||||
@@ -410,7 +372,7 @@ def main():
|
||||
plt.axis("equal")
|
||||
plt.show()
|
||||
|
||||
if not px:
|
||||
if not xs:
|
||||
assert False, "No path"
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user