From 18ac00dced6448f12241c8d9d42d0e06709cc17a Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Tue, 28 Jan 2020 21:35:49 +0900 Subject: [PATCH] fix the dubins planning bug and code clean up --- .../DubinsPath/dubins_path_planning.py | 200 +++++++++--------- PathPlanning/RRTDubins/rrt_dubins.py | 3 + PathPlanning/RRTStarDubins/rrt_star_dubins.py | 5 +- tests/test_dubins_path_planning.py | 36 +++- 4 files changed, 139 insertions(+), 105 deletions(-) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index ce32a466..b72801a6 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -21,7 +21,7 @@ def pi_2_pi(angle): return (angle + math.pi) % (2 * math.pi) - math.pi -def LSL(alpha, beta, d): +def left_straight_left(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) @@ -38,12 +38,11 @@ def LSL(alpha, beta, d): t = mod2pi(-alpha + tmp1) p = math.sqrt(p_squared) q = mod2pi(beta - tmp1) - # print(np.rad2deg(t), p, np.rad2deg(q)) return t, p, q, mode -def RSR(alpha, beta, d): +def right_straight_right(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) @@ -63,7 +62,7 @@ def RSR(alpha, beta, d): return t, p, q, mode -def LSR(alpha, beta, d): +def left_straight_right(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) @@ -82,7 +81,7 @@ def LSR(alpha, beta, d): return t, p, q, mode -def RSL(alpha, beta, d): +def right_straight_left(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) @@ -101,7 +100,7 @@ def RSL(alpha, beta, d): return t, p, q, mode -def RLR(alpha, beta, d): +def right_left_right(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) @@ -119,7 +118,7 @@ def RLR(alpha, beta, d): return t, p, q, mode -def LRL(alpha, beta, d): +def left_right_left(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) ca = math.cos(alpha) @@ -137,23 +136,21 @@ def LRL(alpha, beta, d): return t, p, q, mode -def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE): - # normalize - dx = ex - dy = ey +def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature, step_size): + dx = end_x + dy = end_y D = math.hypot(dx, dy) - d = D * c - # print(dx, dy, D, d) + d = D * curvature theta = mod2pi(math.atan2(dy, dx)) alpha = mod2pi(- theta) - beta = mod2pi(eyaw - theta) - # print(theta, alpha, beta, d) + beta = mod2pi(end_yaw - theta) - planners = [LSL, RSR, LSR, RSL, RLR, LRL] + planners = [left_straight_left, right_straight_right, left_straight_right, right_straight_left, right_left_right, + left_right_left] - bcost = float("inf") - bt, bp, bq, bmode = None, None, None, None + best_cost = float("inf") + bt, bp, bq, best_mode = None, None, None, None for planner in planners: t, p, q, mode = planner(alpha, beta, d) @@ -161,17 +158,48 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE): continue cost = (abs(t) + abs(p) + abs(q)) - if bcost > cost: - bt, bp, bq, bmode = t, p, q, mode - bcost = cost + if best_cost > cost: + bt, bp, bq, best_mode = t, p, q, mode + best_cost = cost + lengths = [bt, bp, bq] - # print(bmode) - px, py, pyaw = generate_course([bt, bp, bq], bmode, c, D_ANGLE) + px, py, pyaw, directions = generate_local_course( + sum(lengths), lengths, best_mode, curvature, step_size) - return px, py, pyaw, bmode, bcost + return px, py, pyaw, best_mode, best_cost -def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)): +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 dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, step_size=0.1): """ Dubins path plannner @@ -200,7 +228,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0) leyaw = eyaw - syaw lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin( - lex, ley, leyaw, c, D_ANGLE) + lex, ley, leyaw, c, step_size) px = [math.cos(-syaw) * x + math.sin(-syaw) * y + sx for x, y in zip(lpx, lpy)] @@ -211,44 +239,60 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0) return px, py, pyaw, mode, clen -def generate_course(length, mode, c, D_ANGLE): - px = [0.0] - py = [0.0] - pyaw = [0.0] +def generate_local_course(total_length, lengths, mode, max_curvature, step_size): + n_point = math.trunc(total_length / step_size) + len(lengths) + 4 - for m, l in zip(mode, length): - pd = 0.0 - if m == "S": - d = 1.0 * c - else: # turning couse - d = D_ANGLE + path_x = [0.0 for _ in range(n_point)] + path_y = [0.0 for _ in range(n_point)] + path_yaw = [0.0 for _ in range(n_point)] + directions = [0.0 for _ in range(n_point)] + ind = 1 - while pd < abs(l - d): - # print(pd, l) - px.append(px[-1] + d / c * math.cos(pyaw[-1])) - py.append(py[-1] + d / c * math.sin(pyaw[-1])) + if lengths[0] > 0.0: + directions[0] = 1 + else: + directions[0] = -1 - if m == "L": # left turn - pyaw.append(pyaw[-1] + d) - elif m == "S": # Straight - pyaw.append(pyaw[-1]) - elif m == "R": # right turn - pyaw.append(pyaw[-1] - d) + 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 + origin_x, origin_y, origin_yaw = path_x[ind], path_y[ind], path_yaw[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 + path_x, path_y, path_yaw, directions = interpolate( + ind, pd, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) pd += d - d = l - pd - px.append(px[-1] + d / c * math.cos(pyaw[-1])) - py.append(py[-1] + d / c * math.sin(pyaw[-1])) + ll = l - pd - d # calc remain length - if m == "L": # left turn - pyaw.append(pyaw[-1] + d) - elif m == "S": # Straight - pyaw.append(pyaw[-1]) - elif m == "R": # right turn - pyaw.append(pyaw[-1] - d) - pd += d + ind += 1 + path_x, path_y, path_yaw, directions = interpolate( + ind, l, m, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions) - return px, py, pyaw + if len(path_x) <= 1: + return [], [], [], [] + + # remove unused data + while len(path_x) >= 1 and path_x[-1] == 0.0: + path_x.pop() + path_y.pop() + path_yaw.pop() + directions.pop() + + return path_x, path_y, path_yaw, directions def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover @@ -288,53 +332,11 @@ def main(): plot_arrow(start_x, start_y, start_yaw) plot_arrow(end_x, end_y, end_yaw) - # for (ix, iy, iyaw) in zip(px, py, pyaw): - # plot_arrow(ix, iy, iyaw, fc="b") - plt.legend() plt.grid(True) plt.axis("equal") plt.show() -def test(): - NTEST = 5 - - for i in range(NTEST): - start_x = (np.random.rand() - 0.5) * 10.0 # [m] - start_y = (np.random.rand() - 0.5) * 10.0 # [m] - start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] - - end_x = (np.random.rand() - 0.5) * 10.0 # [m] - end_y = (np.random.rand() - 0.5) * 10.0 # [m] - end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] - - curvature = 1.0 / (np.random.rand() * 5.0) - - px, py, pyaw, mode, clen = dubins_path_planning( - start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) - - if show_animation: - plt.cla() - # for stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - 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.xlim(-10, 10) - plt.ylim(-10, 10) - plt.pause(1.0) - - print("Test done") - - if __name__ == '__main__': - test() main() diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index 4599904c..55cea0bd 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -137,6 +137,9 @@ class RRTDubins(RRT): from_node.x, from_node.y, from_node.yaw, to_node.x, to_node.y, to_node.yaw, self.curvature) + if len(px) <= 1: # cannot find a dubins path + return None + new_node = copy.deepcopy(from_node) new_node.x = px[-1] new_node.y = py[-1] diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index 65fac54c..9cfd4e69 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -114,7 +114,7 @@ class RRTStarDubins(RRTStar): plt.clf() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) + lambda event: [exit(0) if event.key == 'escape' else None]) if rnd is not None: plt.plot(rnd.x, rnd.y, "^k") for node in self.node_list: @@ -143,6 +143,9 @@ class RRTStarDubins(RRTStar): from_node.x, from_node.y, from_node.yaw, to_node.x, to_node.y, to_node.yaw, self.curvature) + if len(px) <= 1: # cannot find a dubins path + return None + new_node = copy.deepcopy(from_node) new_node.x = px[-1] new_node.y = py[-1] diff --git a/tests/test_dubins_path_planning.py b/tests/test_dubins_path_planning.py index ad140b6c..72305530 100644 --- a/tests/test_dubins_path_planning.py +++ b/tests/test_dubins_path_planning.py @@ -7,6 +7,18 @@ from PathPlanning.DubinsPath import dubins_path_planning class Test(TestCase): + @staticmethod + def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw): + 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 test1(self): start_x = 1.0 # [m] start_y = 1.0 # [m] @@ -21,14 +33,28 @@ class Test(TestCase): px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning( start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) - assert (abs(px[-1] - end_x) <= 0.5) - assert (abs(py[-1] - end_y) <= 0.5) - assert(abs(pyaw[-1] - end_yaw) <= 0.1) + self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw) def test2(self): dubins_path_planning.show_animation = False dubins_path_planning.main() def test3(self): - dubins_path_planning.show_animation = False - dubins_path_planning.test() + N_TEST = 10 + + for i in range(N_TEST): + start_x = (np.random.rand() - 0.5) * 10.0 # [m] + start_y = (np.random.rand() - 0.5) * 10.0 # [m] + start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] + + end_x = (np.random.rand() - 0.5) * 10.0 # [m] + end_y = (np.random.rand() - 0.5) * 10.0 # [m] + end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] + + curvature = 1.0 / (np.random.rand() * 5.0) + + px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning( + start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) + + self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw) +