Fix reeds shepp path issue (#529)

* code clean up

* code clean up

* code clean up

* code clean up

* fix length handling issues

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.L = 0.0  # Total lengths of the path
-        self.x = []
+        self.x = []  # x positions
-        self.y = []
+        self.y = []  # y positions
-        self.yaw = []
+        self.yaw = []  # orientations [rad]
-        self.directions = []
+        self.directions = []  # directions (1:forward, -1:backward)
 
 
 def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
-    """
+    if isinstance(x, list):
-    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),
+        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), fc=fc,
-                  fc=fc, ec=ec, head_width=width, head_length=width)
+                  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:
+    for i_path in paths:
-        typeissame = (tpath.ctypes == path.ctypes)
+        type_is_same = (i_path.ctypes == path.ctypes)
-        if typeissame:
+        length_is_close = (sum(np.abs(i_path.lengths)) - path.L) <= step_size
-            if sum(np.abs(tpath.lengths)) - sum(np.abs(path.lengths)) <= 0.01:
+        if type_is_same and length_is_close:
-                return paths  # not insert path
+            return paths  # same path found, so do not insert path
 
-    path.L = sum([abs(i) for i in lengths])
+    # check path is long enough
-
+    if path.L <= step_size:
-    # Base.Test.@test path.L >= 0.01
+        return paths  # too short, so do not insert path
-    if path.L >= 0.01:
-        paths.append(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 = straight_curve_straight(x, y, dth, paths, step_size)
-    paths = curve_straight_curve(x, y, dth, paths)
+    paths = curve_straight_curve(x, y, dth, paths, step_size)
-    paths = curve_curve_curve(x, y, dth, paths)
+    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)
+        x = origin_x + dist / max_curvature * math.cos(origin_yaw)
-        path_y[ind] = origin_y + length / max_curvature * math.sin(origin_yaw)
+        y = origin_y + dist / max_curvature * math.sin(origin_yaw)
-        path_yaw[ind] = 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
+        x = origin_x + gdx
-        path_y[ind] = origin_y + gdy
+        y = origin_y + gdy
 
-    if mode == "L":  # left turn
+    return x, y, yaw, 1 if length > 0.0 else -1
-        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
 
 
 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(
+        xs, ys, yaws, directions = generate_local_course(path.lengths,
-            path.L, path.lengths, path.ctypes, maxc, step_size * maxc)
+                                                         path.ctypes, maxc,
+                                                         step_size * maxc)
 
         # convert global coordinate
-        path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2])
+        path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for
-                  * iy + q0[0] for (ix, iy) in zip(x, y)]
+                  (ix, iy) in zip(xs, ys)]
-        path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2])
+        path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for
-                  * iy + q0[1] for (ix, iy) in zip(x, y)]
+                  (ix, iy) in zip(xs, ys)]
-        path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw]
+        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,
+def reeds_shepp_path_planning(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=0.2):
-                              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")
+    # search minimum cost path
-    best_path_index = -1
+    best_path_index = paths.index(min(paths, key=lambda p: abs(p.L)))
-    for i, _ in enumerate(paths):
+    b_path = paths[best_path_index]
-        if paths[i].L <= minL:
-            minL = paths[i].L
-            best_path_index = i
 
-    bpath = paths[best_path_index]
+    return b_path.x, b_path.y, b_path.yaw, b_path.ctypes, b_path.lengths
-
-    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_x = -1.0  # [m]
-    # start_y = -4.0  # [m]
+    start_y = -4.0  # [m]
-    # start_yaw = np.deg2rad(-20.0)  # [rad]
+    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 = 0.0  # [m]
+    end_x = 5.0  # [m]
-    start_y = 0.0  # [m]
+    end_y = 5.0  # [m]
-    start_yaw = np.deg2rad(0.0)  # [rad]
+    end_yaw = np.deg2rad(25.0)  # [rad]
 
-    end_x = 0.0  # [m]
+    curvature = 0.1
-    end_y = 0.0  # [m]
+    step_size = 0.05
-    end_yaw = np.deg2rad(0.0)  # [rad]
 
-    curvature = 1.0
+    xs, ys, yaws, modes, lengths = reeds_shepp_path_planning(start_x, start_y,
-    step_size = 0.1
+                                                             start_yaw, end_x,
-
+                                                             end_y, end_yaw,
-    px, py, pyaw, mode, clen = reeds_shepp_path_planning(
+                                                             curvature,
-        start_x, start_y, start_yaw, end_x, end_y, end_yaw,
+                                                             step_size)
-        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"
 
 

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(
+        px, py, pyaw, mode, lengths = m.reeds_shepp_path_planning(
-            start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
+            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,
+        check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x,
-                             end_x, end_y, end_yaw)
+                             end_y, end_yaw)
+        check_path_length(px, py, lengths)
 
 
 if __name__ == '__main__':