release grid based_sweep_coverage_path_planner.py

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -1,19 +1,21 @@
 """
-Path Planning Sample Code with Closed loop RRT for car like robot.
+Path planning Sample Code with Closed loop RRT for car like robot.
 
 author: AtsushiSakai(@Atsushi_twi)
 
 """
 
-import random
-import math
 import copy
-import numpy as np
-import pure_pursuit
-import matplotlib.pyplot as plt
-
-import sys
+import math
 import os
+import random
+import sys
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+import pure_pursuit
+
 sys.path.append(os.path.dirname(
     os.path.abspath(__file__)) + "/../ReedsSheppPath/")
 
@@ -32,11 +34,11 @@ STEP_SIZE = 0.1
 
 class RRT():
     """
-    Class for RRT Planning
+    Class for RRT planning
     """
 
     def __init__(self, start, goal, obstacleList, randArea,
-                 maxIter=200):
+                 maxIter=50):
         """
         Setting Parameter
 
@@ -77,6 +79,7 @@ class RRT():
         self.try_goal_path()
 
         for i in range(self.maxIter):
+            print("loop:", i)
             rnd = self.get_random_point()
             nind = self.GetNearestListIndex(self.nodeList, rnd)
 
@@ -113,7 +116,7 @@ class RRT():
 
         best_time = float("inf")
 
-        fx = None
+        fx, fy, fyaw, fv, ft, fa, fd = None, None, None, None, None, None, None
 
         # pure pursuit tracking
         for ind in path_indexs:
@@ -246,7 +249,7 @@ class RRT():
         return node
 
     def get_best_last_indexs(self):
-        #  print("get_best_last_index")
+        #  print("search_finish_node")
 
         YAWTH = np.deg2rad(1.0)
         XYTH = 0.5
@@ -387,7 +390,7 @@ class Node():
         self.parent = None
 
 
-def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=500):
+def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=200):
     print("Start" + __file__)
     # ====Search Path with RRT====
     obstacleList = [

PathPlanning/ClosedLoopRRTStar/pure_pursuit.py

@@ -5,9 +5,11 @@ Path tracking simulation with pure pursuit steering control and PID speed contro
 author: Atsushi Sakai
 
 """
-import numpy as np
 import math
+
 import matplotlib.pyplot as plt
+import numpy as np
+
 import unicycle_model
 
 Kp = 2.0  # speed propotional gain
@@ -75,7 +77,7 @@ def calc_target_index(state, cx, cy):
 
     while Lf > L and (ind + 1) < len(cx):
         dx = cx[ind + 1] - cx[ind]
-        dy = cx[ind + 1] - cx[ind]
+        dy = cy[ind + 1] - cy[ind]
         L += math.sqrt(dx ** 2 + dy ** 2)
         ind += 1
 
@@ -153,6 +155,7 @@ def set_stop_point(target_speed, cx, cy, cyaw):
     forward = True
 
     d = []
+    is_back = False
 
     # Set stop point
     for i in range(len(cx) - 1):