From 08a69bf445ebc29576a9cb17e5d4f0e2aa779c51 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 4 Feb 2018 09:29:09 -0800 Subject: [PATCH] try implement my reeds shepp path --- .../reeds_shepp_path_planning.py | 224 +++++++++++++++++- 1 file changed, 219 insertions(+), 5 deletions(-) diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 3757aca7..bbee64c0 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -6,12 +6,25 @@ author Atsushi Sakai(@Atsushi_twi) """ import reeds_shepp +import numpy as np import math import matplotlib.pyplot as plt 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 @@ -26,6 +39,203 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): 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 SLS(x, y, phi): + # println(x,",", y,",", phi, ",", mod2pi(phi)) + phi = mod2pi(phi) + if y > 0.0 and phi > 0.0 and 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) + # println("1,",t,",",u,",",v) + return True, t, u, v + elif y < 0.0 and phi > 0.0 and 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) + # println("2,",t,",",u,",",v) + 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 - 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 SCS(x, y, phi, paths): + flag, t, u, v = SLS(x, y, phi) + if flag: + paths = set_path(paths, [t, u, v], ["S", "L", "S"]) + + flag, t, u, v = SLS(x, -y, -phi) + if flag: + paths = set_path(paths, [t, u, v], ["S", "R", "S"]) + + return paths + + +def generate_path(q0, q1, maxc): + 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) * maxc + y = (-s * dx + c * dy) * maxc + + paths = [] + paths = SCS(x, y, dth, paths) + # paths = CSC(x, y, dth, paths) + # paths = CCC(x, y, dth, paths) + + return paths + + +# def generate_local_course(L: : Float64, + # lengths: : Array{Float64}, + # mode: : Array{String}, + # maxc: : Float64, + # step_size: : Float64) + # npoint = trunc(Int64, L / step_size) + length(lengths) + 3 + # # println(npoint, ",", L, ",", step_size, ",", L/step_size) + + # px = fill(0.0, npoint) + # py = fill(0.0, npoint) + # pyaw = fill(0.0, npoint) + # directions = fill(0, npoint) + # ind = 2 + + # if lengths[1] > 0.0 + # directions[1] = 1 + # else + # directions[1] = -1 + # end + + # if lengths[1] > 0.0 + # d = step_size + # else + # d = -step_size + # end + + # pd = d + # ll = 0.0 + + # for (m, l, i) in zip(mode, lengths, 1: length(mode)) + + # if l > 0.0 + # d = step_size + # else + # d = -step_size + # end + + # # set prigin state + # ox, oy, oyaw = px[ind], py[ind], pyaw[ind] + + # ind -= 1 + # if i >= 2 & & (lengths[i - 1] * lengths[i]) > 0 + # pd = - d - ll + # else + # pd = d - ll + # end + + # while abs(pd) <= abs(l) + # ind += 1 + # px, py, pyaw, directions = interpolate( + # ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) + # pd += d + # end + + # ll = l - pd - d # calc remain length + + # ind += 1 + # px, py, pyaw, directions = interpolate( + # ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) + # end + + # # remove unused data + # while px[end] == 0.0 + # pop!(px) + # pop!(py) + # pop!(pyaw) + # pop!(directions) + # end + + # return px, py, pyaw, directions + + +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) + pass + + # # convert global coordinate + # path.x = [cos(-q0[3]) * ix + sin(-q0[3]) * iy + q0[1] for (ix, iy) in zip(x, y)] + # path.y = [-sin(-q0[3]) * ix + cos(-q0[3]) * iy + q0[2] for (ix, iy) in zip(x, y)] + # path.yaw = common_func.pi_2_pi.([iyaw + q0[3] for iyaw in yaw]) + # path.directions = directions + # path.lengths = [l/maxc for l in path.lengths] + # path.L = path.L/maxc + + return paths + + +def reeds_shepp_path_planning2(sx, sy, syaw, + gx, gy, gyaw, maxc, step_size): + + paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size) + + minL = float("Inf") + best_path_index = -1 + for i in range(len(paths)): + if paths[i].L <= minL: + minL = paths[i].L + best_path_index = i + + bpath = paths[best_path_index] + + xs = bpath.x + ys = bpath.y + yaw = bpath.yaw + ptype = bpath.ctypes + clen = bpath.lengths + return xs, ys, yaw, ptype, clen + + def reeds_shepp_path_planning(start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature): step_size = 0.1 @@ -62,14 +272,18 @@ def main(): start_y = 1.0 # [m] start_yaw = math.radians(0.0) # [rad] - end_x = -0.0 # [m] - end_y = -3.0 # [m] - end_yaw = math.radians(-45.0) # [rad] + end_x = 5.0 # [m] + end_y = 10.0 # [m] + end_yaw = math.radians(45.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) + px, py, pyaw, mode, clen = reeds_shepp_path_planning2( + start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size) + + # px, py, pyaw, mode, clen = reeds_shepp_path_planning( + # start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) if show_animation: plt.plot(px, py, label="final course " + str(mode))