first release

PathPlanning/CRRRTStar/cr_rrt_star_car.py

@@ -16,6 +16,7 @@ import numpy as np
 import reeds_shepp_path_planning
 import pure_pursuit
 import pandas as pd
+import unicycle_model
 
 
 target_speed = 10.0 / 3.6
@@ -133,18 +134,30 @@ class RRT():
         t, x, y, yaw, v, a, d, find_goal = pure_pursuit.closed_loop_prediction(
             cx, cy, cyaw, speed_profile, goal)
 
+        yaw = [self.pi_2_pi(iyaw) for iyaw in yaw]
+
         if abs(yaw[-1] - goal[2]) >= math.pi / 2.0:
-            print(yaw[-1], goal[2])
             find_goal = False
+
+        travel = sum([abs(iv) * unicycle_model.dt for iv in v])
+        #  print(travel)
+        origin_travel = sum([math.sqrt(dx ** 2 + dy ** 2)
+                             for (dx, dy) in zip(np.diff(cx), np.diff(cy))])
+        #  print(origin_travel)
+
+        if (travel / origin_travel) >= 2.0:
+            print("path is too long")
+            #  find_goal = False
+
         if not find_goal:
             print("This path is bad")
 
-        plt.clf
-        plt.plot(x, y, '-r')
-        plt.plot(path[:, 0], path[:, 1], '-g')
-        plt.grid(True)
-        plt.axis("equal")
-        plt.show()
+        #  plt.clf
+        #  plt.plot(x, y, '-r')
+        #  plt.plot(path[:, 0], path[:, 1], '-g')
+        #  plt.grid(True)
+        #  plt.axis("equal")
+        #  plt.show()
 
         return find_goal, x, y, yaw, v, t, a, d
 

PathPlanning/CRRRTStar/pure_pursuit.py

@@ -14,7 +14,7 @@ import unicycle_model
 
 Kp = 2.0  # speed propotional gain
 Lf = 1.0  # look-ahead distance
-T = 1000.0  # max simulation time
+T = 100.0  # max simulation time
 goal_dis = 0.5
 stop_speed = 0.1
 
@@ -47,11 +47,11 @@ def pure_pursuit_control(state, cx, cy, pind):
     alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
 
     if state.v <= 0.0:  # back
+        alpha = math.pi - alpha
+        #  if alpha > 0:
         #  alpha = math.pi - alpha
-        if alpha > 0:
-            alpha = math.pi - alpha
-        else:
-            alpha = math.pi + alpha
+        #  else:
+        #  alpha = math.pi + alpha
 
     delta = math.atan2(2.0 * unicycle_model.L * math.sin(alpha) / Lf, 1.0)
 
@@ -118,7 +118,7 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
         a.append(ai)
         d.append(di)
 
-        if target_ind % 20 == 0 and animation:
+        if target_ind % 1 == 0 and animation:
             plt.cla()
             plt.plot(cx, cy, "-r", label="course")
             plt.plot(x, y, "ob", label="trajectory")
@@ -177,40 +177,40 @@ def calc_speed_profile(cx, cy, cyaw, target_speed, a):
 
     speed_profile, d = set_stop_point(target_speed, cx, cy, cyaw)
 
-    nsp = len(speed_profile)
+    #  nsp = len(speed_profile)
 
     if animation:
         plt.plot(speed_profile, "xb")
 
     # forward integration
-    for i in range(nsp - 1):
+    #  for i in range(nsp - 1):
 
-        if speed_profile[i + 1] >= 0:  # forward
-            tspeed = speed_profile[i] + a * d[i]
-            if tspeed <= speed_profile[i + 1]:
-                speed_profile[i + 1] = tspeed
-        else:
-            tspeed = speed_profile[i] - a * d[i]
-            if tspeed >= speed_profile[i + 1]:
-                speed_profile[i + 1] = tspeed
+        #  if speed_profile[i + 1] >= 0:  # forward
+        #  tspeed = speed_profile[i] + a * d[i]
+        #  if tspeed <= speed_profile[i + 1]:
+        #  speed_profile[i + 1] = tspeed
+        #  else:
+        #  tspeed = speed_profile[i] - a * d[i]
+        #  if tspeed >= speed_profile[i + 1]:
+        #  speed_profile[i + 1] = tspeed
 
-    if animation:
-        plt.plot(speed_profile, "ok")
+    #  if animation:
+        #  plt.plot(speed_profile, "ok")
 
-    # back integration
-    for i in range(nsp - 1):
-        if speed_profile[- i - 1] >= 0:  # forward
-            tspeed = speed_profile[-i] + a * d[-i]
-            if tspeed <= speed_profile[-i - 1]:
-                speed_profile[-i - 1] = tspeed
-        else:
-            tspeed = speed_profile[-i] - a * d[-i]
-            if tspeed >= speed_profile[-i - 1]:
-                speed_profile[-i - 1] = tspeed
+    #  # back integration
+    #  for i in range(nsp - 1):
+        #  if speed_profile[- i - 1] >= 0:  # forward
+        #  tspeed = speed_profile[-i] + a * d[-i]
+        #  if tspeed <= speed_profile[-i - 1]:
+        #  speed_profile[-i - 1] = tspeed
+        #  else:
+        #  tspeed = speed_profile[-i] - a * d[-i]
+        #  if tspeed >= speed_profile[-i - 1]:
+        #  speed_profile[-i - 1] = tspeed
 
-    if animation:
-        plt.plot(speed_profile, "-r")
-        plt.show()
+    #  if animation:
+        #  plt.plot(speed_profile, "-r")
+        #  plt.show()
 
     return speed_profile
 

PathPlanning/CRRRTStar/unicycle_model.py

@@ -26,11 +26,22 @@ def update(state, a, delta):
     state.x = state.x + state.v * math.cos(state.yaw) * dt
     state.y = state.y + state.v * math.sin(state.yaw) * dt
     state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
+    state.yaw = pi_2_pi(state.yaw)
     state.v = state.v + a * dt
 
     return state
 
 
+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
+
+
 if __name__ == '__main__':
     print("start unicycle simulation")
     import matplotlib.pyplot as plt