first release CLRRT simulation

PathPlanning/CRRRTStar/cr_rrt_star_car.py

@@ -1,7 +1,7 @@
 #!/usr/bin/python
 # -*- coding: utf-8 -*-
 """
-@brief: Path Planning Sample Code with Closed roop RRT for car like robot.
+@brief: Path Planning Sample Code with Closed loop RRT for car like robot.
 
 @author: AtsushiSakai(@Atsushi_twi)
 
@@ -20,10 +20,6 @@ import unicycle_model
 
 
 target_speed = 10.0 / 3.6
-accel = 0.1
-
-# TODO
-# 制約条件をいれる
 
 
 class RRT():
@@ -32,7 +28,7 @@ class RRT():
     """
 
     def __init__(self, start, goal, obstacleList, randArea,
-                 maxIter=100):
+                 maxIter=200):
         u"""
         Setting Parameter
 
@@ -109,9 +105,6 @@ class RRT():
             flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(
                 path)
 
-            #  print(t[-1])
-            #  print(ind)
-
             if flag and best_time >= t[-1]:
                 print("feasible path is found")
                 best_time = t[-1]
@@ -169,13 +162,15 @@ class RRT():
         cx, cy, cyaw = pure_pursuit.extend_path(cx, cy, cyaw)
 
         speed_profile = pure_pursuit.calc_speed_profile(
-            cx, cy, cyaw, target_speed, accel)
+            cx, cy, cyaw, target_speed)
 
         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 not find_goal:
+            print("cannot reach goal")
+
         if abs(yaw[-1] - goal[2]) >= math.pi / 4.0:
             print("final angle is bad")
             find_goal = False
@@ -194,12 +189,12 @@ class RRT():
             print("This path is collision")
             find_goal = False
 
-        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
 
@@ -430,12 +425,24 @@ if __name__ == '__main__':
         (7, 5, 2),
         (9, 5, 2)
     ]  # [x,y,size(radius)]
+    obstacleList = [
+        (5, 5, 1),
+        (4, 6, 1),
+        (4, 8, 1),
+        (4, 10, 1),
+        (6, 5, 1),
+        (7, 5, 1),
+        (8, 6, 1),
+        (8, 8, 1),
+        (8, 10, 1)
+    ]  # [x,y,size(radius)]
 
     # Set Initial parameters
     start = [0.0, 0.0, math.radians(0.0)]
-    goal = [10.0, 10.0, math.radians(0.0)]
+    #  goal = [10.0, 10.0, math.radians(0.0)]
+    goal = [6.0, 7.0, math.radians(90.0)]
 
-    rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
+    rrt = RRT(start, goal, randArea=[-2.0, 20.0], obstacleList=obstacleList)
     flag, x, y, yaw, v, t, a, d = rrt.Planning(animation=False)
 
     if not flag:

PathPlanning/CRRRTStar/pure_pursuit.py

@@ -16,7 +16,7 @@ Kp = 2.0  # speed propotional gain
 Lf = 0.5  # look-ahead distance
 T = 100.0  # max simulation time
 goal_dis = 0.5
-stop_speed = 0.1
+stop_speed = 0.5
 
 #  animation = True
 animation = False
@@ -35,7 +35,7 @@ def PIDControl(target, current):
 
 def pure_pursuit_control(state, cx, cy, pind):
 
-    ind = calc_target_index(state, cx, cy)
+    ind, dis = calc_target_index(state, cx, cy)
 
     if pind >= ind:
         ind = pind
@@ -53,10 +53,6 @@ def pure_pursuit_control(state, cx, cy, pind):
 
     if state.v <= 0.0:  # back
         alpha = math.pi - alpha
-        #  if alpha > 0:
-        #  alpha = math.pi - alpha
-        #  else:
-        #  alpha = math.pi + alpha
 
     delta = math.atan2(2.0 * unicycle_model.L * math.sin(alpha) / Lf, 1.0)
 
@@ -65,7 +61,7 @@ def pure_pursuit_control(state, cx, cy, pind):
     elif delta < - unicycle_model.steer_max:
         delta = -unicycle_model.steer_max
 
-    return delta, ind
+    return delta, ind, dis
 
 
 def calc_target_index(state, cx, cy):
@@ -73,8 +69,9 @@ def calc_target_index(state, cx, cy):
     dy = [state.y - icy for icy in cy]
 
     d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
+    mindis = min(d)
 
-    ind = d.index(min(d))
+    ind = d.index(mindis)
 
     L = 0.0
 
@@ -84,7 +81,8 @@ def calc_target_index(state, cx, cy):
         L += math.sqrt(dx ** 2 + dy ** 2)
         ind += 1
 
-    return ind
+    #  print(mindis)
+    return ind, mindis
 
 
 def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
@@ -100,15 +98,22 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
     t = [0.0]
     a = [0.0]
     d = [0.0]
-    target_ind = calc_target_index(state, cx, cy)
+    target_ind, mindis = calc_target_index(state, cx, cy)
     find_goal = False
 
+    maxdis = 0.5
+
     while T >= time:
-        di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
-        ai = PIDControl(speed_profile[target_ind], state.v)
+        di, target_ind, dis = pure_pursuit_control(state, cx, cy, target_ind)
+
+        target_speed = speed_profile[target_ind]
+        target_speed = target_speed * \
+            (maxdis - min(dis, maxdis - 0.1)) / maxdis
+
+        ai = PIDControl(target_speed, state.v)
         state = unicycle_model.update(state, ai, di)
 
-        if abs(state.v) <= stop_speed:
+        if abs(state.v) <= stop_speed and target_ind <= len(cx) - 2:
             target_ind += 1
 
         time = time + unicycle_model.dt
@@ -139,6 +144,9 @@ def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
                       "tind:" + str(target_ind))
             plt.pause(0.0001)
 
+    else:
+        print("Time out!!")
+
     return t, x, y, yaw, v, a, d, find_goal
 
 
@@ -176,61 +184,32 @@ def set_stop_point(target_speed, cx, cy, cyaw):
             #  plt.plot(cx[i], cy[i], "xb")
             #  print(iyaw, move_direction, dx, dy)
     speed_profile[0] = 0.0
-    speed_profile[-1] = 0.0
+    if is_back:
+        speed_profile[-1] = -stop_speed
+    else:
+        speed_profile[-1] = stop_speed
 
     d.append(d[-1])
 
     return speed_profile, d
 
 
-def calc_speed_profile(cx, cy, cyaw, target_speed, a):
+def calc_speed_profile(cx, cy, cyaw, target_speed):
 
     speed_profile, d = set_stop_point(target_speed, cx, cy, cyaw)
 
-    #  nsp = len(speed_profile)
-
     if animation:
         plt.plot(speed_profile, "xb")
 
-    # forward 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, "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
-
-    #  if animation:
-        #  plt.plot(speed_profile, "-r")
-        #  plt.show()
-
     return speed_profile
 
 
 def extend_path(cx, cy, cyaw):
 
     dl = 0.1
-    dl_list = [dl] * (int(Lf / dl) + 10)
+    dl_list = [dl] * (int(Lf / dl) + 1)
 
-    move_direction = math.atan2(cy[-1] - cy[-2], cx[-1] - cx[-2])
+    move_direction = math.atan2(cy[-1] - cy[-3], cx[-1] - cx[-3])
     is_back = abs(move_direction - cyaw[-1]) >= math.pi / 2.0
 
     for idl in dl_list:
@@ -317,13 +296,12 @@ def main2():
     cyaw = np.array(data["yaw"])
 
     target_speed = 10.0 / 3.6
-    a = 2.0
 
     goal = [cx[-1], cy[-1]]
 
     cx, cy, cyaw = extend_path(cx, cy, cyaw)
 
-    speed_profile = calc_speed_profile(cx, cy, cyaw, target_speed, a)
+    speed_profile = calc_speed_profile(cx, cy, cyaw, target_speed)
 
     t, x, y, yaw, v, a, d, flag = closed_loop_prediction(
         cx, cy, cyaw, speed_profile, goal)

PathPlanning/CRRRTStar/unicycle_model.py

@@ -12,8 +12,7 @@ dt = 0.05  # [s]
 L = 2.9  # [m]
 steer_max = math.radians(40.0)
 curvature_max = math.tan(steer_max) / L
-curvature_max = 1.0 / curvature_max
-#  print(curvature_max)
+curvature_max = 1.0 / curvature_max + 1.0
 
 accel_max = 5.0