Merge pull request AtsushiSakai#277 from AtsushiSakai/fix_dubins

fix the dubins path planner

Author: AtsushiSakai

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,41 +136,70 @@ 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)
         if t is None:
             continue
 
         cost = (abs(t) + abs(p) + abs(q))
-        if bcost > cost:
-            bt, bp, bq, bmode = t, p, q, mode
-            bcost = cost
-
-    #  print(bmode)
-    px, py, pyaw = generate_course([bt, bp, bq], bmode, c, D_ANGLE)
+        if best_cost > cost:
+            bt, bp, bq, best_mode = t, p, q, mode
+            best_cost = cost
+    lengths = [bt, bp, bq]
+
+    px, py, pyaw, directions = generate_local_course(
+        sum(lengths), lengths, best_mode, curvature, step_size)
+
+    return px, py, pyaw, best_mode, best_cost
+
+
+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 px, py, pyaw, bmode, bcost
+    return path_x, path_y, path_yaw, directions
 
 
-def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)):
+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,45 +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):
+def generate_local_course(total_length, lengths, mode, max_curvature, step_size):
+    n_point = math.trunc(total_length / step_size) + len(lengths) + 4
+
+    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
+
+    if lengths[0] > 0.0:
+        directions[0] = 1
+    else:
+        directions[0] = -1
+
+    ll = 0.0
 
-    px = [0.0]
-    py = [0.0]
-    pyaw = [0.0]
+    for (m, l, i) in zip(mode, lengths, range(len(mode))):
+        if l > 0.0:
+            d = step_size
+        else:
+            d = -step_size
 
-    for m, l in zip(mode, length):
-        pd = 0.0
-        if m == "S":
-            d = 1.0 * c
-        else:  # turning couse
-            d = D_ANGLE
+        # set origin state
+        origin_x, origin_y, origin_yaw = path_x[ind], path_y[ind], path_yaw[ind]
 
-        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]))
+        ind -= 1
+        if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
+            pd = - d - ll
+        else:
+            pd = d - ll
 
-            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)
+        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
+
+        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)
+
+    if len(path_x) <= 1:
+        return [], [], [], []
 
-        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
+    # 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 px, py, pyaw
+    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
@@ -289,54 +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()

PathPlanning/RRTDubins/rrt_dubins.py

@@ -137,6 +137,9 @@ def steer(self, from_node, to_node):
             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]

PathPlanning/RRTStarDubins/rrt_star_dubins.py

@@ -114,7 +114,7 @@ def draw_graph(self, rnd=None):
         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 @@ def steer(self, from_node, to_node):
             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]

tests/test_dubins_path_planning.py

@@ -7,6 +7,18 @@
 
 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,27 @@ def test1(self):
         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)