diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 0b31e737..5bc04c8e 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -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" diff --git a/tests/test_reeds_shepp_path_planning.py b/tests/test_reeds_shepp_path_planning.py index f1ce82d4..34ccfe77 100644 --- a/tests/test_reeds_shepp_path_planning.py +++ b/tests/test_reeds_shepp_path_planning.py @@ -1,6 +1,7 @@ +import numpy as np + import conftest # Add root path to sys.path from PathPlanning.ReedsSheppPath import reeds_shepp_path_planning as m -import numpy as np def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, @@ -8,14 +9,21 @@ def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, assert (abs(px[0] - start_x) <= 0.01) assert (abs(py[0] - start_y) <= 0.01) assert (abs(pyaw[0] - start_yaw) <= 0.01) - print("x", px[-1], end_x) assert (abs(px[-1] - end_x) <= 0.01) - print("y", py[-1], end_y) assert (abs(py[-1] - end_y) <= 0.01) - print("yaw", pyaw[-1], end_yaw) assert (abs(pyaw[-1] - end_yaw) <= 0.01) +def check_path_length(px, py, lengths): + sum_len = sum(abs(length) for length in lengths) + dpx = np.diff(px) + dpy = np.diff(py) + actual_len = sum( + np.hypot(dx, dy) for (dx, dy) in zip(dpx, dpy)) + diff_len = sum_len - actual_len + assert (diff_len <= 0.01) + + def test1(): m.show_animation = False m.main() @@ -23,6 +31,7 @@ def test1(): def test2(): N_TEST = 10 + np.random.seed(1234) for i in range(N_TEST): start_x = (np.random.rand() - 0.5) * 10.0 # [m] @@ -35,11 +44,13 @@ def test2(): curvature = 1.0 / (np.random.rand() * 5.0) - px, py, pyaw, mode, clen = m.reeds_shepp_path_planning( - start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) + px, py, pyaw, mode, lengths = m.reeds_shepp_path_planning( + start_x, start_y, start_yaw, + end_x, end_y, end_yaw, curvature) - check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, - end_x, end_y, end_yaw) + check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, + end_y, end_yaw) + check_path_length(px, py, lengths) if __name__ == '__main__':