add some corse type

scripts/PathPlanning/DubinsPath/dubins_path_planning.py

@@ -12,19 +12,6 @@ Liscense MIT
 import math
 
 
-def pi_2_pi(angle):
-    """
-    """
-
-    while(angle >= math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle <= -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
-
-
 def fmodr(x, y):
     return x - y * math.floor(x / y)
 
@@ -50,7 +37,100 @@ def LSL(alpha, beta, d):
     q = mod2pi(beta - tmp1)
     #  print(math.degrees(t), p, math.degrees(q))
 
-    return t, p, q
+    mode = ["L", "S", "L"]
+
+    return t, p, q, mode
+
+
+def RSR(alpha, beta, d):
+    sa = math.sin(alpha)
+    sb = math.sin(beta)
+    ca = math.cos(alpha)
+    cb = math.cos(beta)
+    c_ab = math.cos(alpha - beta)
+
+    tmp0 = d - sa + sb
+    p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa))
+    if p_squared < 0:
+        return 0.0
+    tmp1 = math.atan2((ca - cb), tmp0)
+    t = mod2pi(alpha - tmp1)
+    p = math.sqrt(p_squared)
+    q = mod2pi(-beta + tmp1)
+
+    mode = ["R", "S", "R"]
+
+    return t, p, q, mode
+
+
+def LSR(alpha, beta, d):
+    sa = math.sin(alpha)
+    sb = math.sin(beta)
+    ca = math.cos(alpha)
+    cb = math.cos(beta)
+    c_ab = math.cos(alpha - beta)
+
+    p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb))
+    p = math.sqrt(p_squared)
+    tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p)
+    t = mod2pi(-alpha + tmp2)
+    q = mod2pi(-mod2pi(beta) + tmp2)
+
+    mode = ["L", "S", "R"]
+    return t, p, q, mode
+
+
+def RSL(alpha, beta, d):
+    sa = math.sin(alpha)
+    sb = math.sin(beta)
+    ca = math.cos(alpha)
+    cb = math.cos(beta)
+    c_ab = math.cos(alpha - beta)
+
+    p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb))
+    p = math.sqrt(p_squared)
+    tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p)
+    t = mod2pi(alpha - tmp2)
+    q = mod2pi(beta - tmp2)
+
+    mode = ["R", "S", "L"]
+    return t, p, q, mode
+
+
+def RLR(alpha, beta, d):
+    sa = math.sin(alpha)
+    sb = math.sin(beta)
+    ca = math.cos(alpha)
+    cb = math.cos(beta)
+    c_ab = math.cos(alpha - beta)
+
+    mode = ["R", "L", "R"]
+    tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0
+    if abs(tmp_rlr) > 1.0:
+        return None, None, None, mode
+
+    p = mod2pi(2 * math.pi - math.acos(tmp_rlr))
+    t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0))
+    q = mod2pi(alpha - beta - t + mod2pi(p))
+    return t, p, q, mode
+
+
+def LRL(alpha, beta, d):
+    sa = math.sin(alpha)
+    sb = math.sin(beta)
+    ca = math.cos(alpha)
+    cb = math.cos(beta)
+    c_ab = math.cos(alpha - beta)
+
+    mode = ["L", "R", "L"]
+    tmp_lrl = (6. - d * d + 2 * c_ab + 2 * d * (- sa + sb)) / 8.
+    if abs(tmp_lrl) > 1:
+        return None, None, None, mode
+    p = mod2pi(2 * math.pi - math.acos(tmp_lrl))
+    t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.)
+    q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))
+
+    return t, p, q, mode
 
 
 def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
@@ -81,16 +161,22 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
     theta = mod2pi(math.atan2(dy, dx))
     alpha = mod2pi(syaw - theta)
     beta = mod2pi(eyaw - theta)
-    print(theta, alpha, beta, d)
+    #  print(theta, alpha, beta, d)
 
-    t, p, q = LSL(alpha, beta, d)
+    planners = [LSL, RSR, LSR, RSL, RLR, LRL]
 
-    px, py, pyaw = generate_course(t, p, q)
+    for planner in planners:
+        t, p, q, mode = planner(alpha, beta, d)
+        if t is None:
+            print("".join(mode) + " cannot generate path")
+            continue
+        px, py, pyaw = generate_course([t, p, q], mode, c)
+        plt.plot(px, py, label=("".join(mode)))
 
     return px, py, pyaw
 
 
-def generate_course(t, p, q):
+def generate_course(length, mode, c):
 
     px = [0.0]
     py = [0.0]
@@ -98,29 +184,22 @@ def generate_course(t, p, q):
 
     d = 0.001
 
-    pd = 0.0
-    while pd <= abs(t):
+    for m, l in zip(mode, length):
+        pd = 0.0
+        while pd <= abs(l):
+            #  print(pd, l)
+            px.append(px[-1] + d * c * math.cos(pyaw[-1]))
+            py.append(py[-1] + d * c * math.sin(pyaw[-1]))
 
-        px.append(px[-1] + d * math.cos(pyaw[-1]))
-        py.append(py[-1] + d * math.sin(pyaw[-1]))
-        pyaw.append(pyaw[-1] + d * 1.0)
+            if m is "L":  # left turn
+                pyaw.append(pyaw[-1] + d)
+            elif m is "S":  # Straight
+                pyaw.append(pyaw[-1])
+            elif m is "R":  # right turn
+                pyaw.append(pyaw[-1] - d)
+            pd += d
 
-        pd += d
-
-    pd = 0.0
-    while pd <= abs(p):
-        px.append(px[-1] + d * math.cos(pyaw[-1]))
-        py.append(py[-1] + d * math.sin(pyaw[-1]))
-        pyaw.append(pyaw[-1])
-
-        pd += d
-
-    pd = 0.0
-    while pd <= abs(q):
-        px.append(px[-1] + d * math.cos(pyaw[-1]))
-        py.append(py[-1] + d * math.sin(pyaw[-1]))
-        pyaw.append(pyaw[-1] + d * 1.0)
-        pd += d
+    #  print(px, py, pyaw)
 
     return px, py, pyaw
 
@@ -147,21 +226,21 @@ if __name__ == '__main__':
     start_y = 0.0  # [m]
     start_yaw = math.radians(0.0)  # [rad]
 
-    end_x = -1.0  # [m]
+    end_x = -10.0  # [m]
     end_y = 10.0  # [m]
-    end_yaw = math.radians(135.0)  # [rad]
+    end_yaw = math.radians(35.0)  # [rad]
 
-    curvature = 1.0
+    curvature = 2.0
 
     px, py, pyaw = dubins_path_planning(start_x, start_y, start_yaw,
                                         end_x, end_y, end_yaw, curvature)
 
     #  print(px, py)
-    plt.plot(px, py, "-r")
     # 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.show()