keep coding

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -12,11 +12,12 @@ import random
 import math
 import copy
 import numpy as np
-import reeds_shepp_path_planning
 import pure_pursuit
-import unicycle_model
 import matplotlib.pyplot as plt
 
+import reeds_shepp_path_planning
+import unicycle_model
+
 show_animation = True
 
 
@@ -337,18 +338,14 @@ class RRT():
                 self.nodeList[i] = tNode
 
     def DrawGraph(self, rnd=None):
-        u"""
+        """
         Draw Graph
         """
-        #  plt.clf()
         if rnd is not None:
             plt.plot(rnd.x, rnd.y, "^k")
         for node in self.nodeList:
             if node.parent is not None:
                 plt.plot(node.path_x, node.path_y, "-g")
-                pass
-                #  plt.plot([node.x, self.nodeList[node.parent].x], [
-                #  node.y, self.nodeList[node.parent].y], "-g")
 
         for (ox, oy, size) in self.obstacleList:
             plt.plot(ox, oy, "ok", ms=30 * size)
@@ -362,9 +359,6 @@ class RRT():
         plt.grid(True)
         plt.pause(0.01)
 
-        #  plt.show()
-        #  input()
-
     def GetNearestListIndex(self, nodeList, rnd):
         dlist = [(node.x - rnd.x) ** 2 +
                  (node.y - rnd.y) ** 2 +
@@ -399,7 +393,7 @@ class RRT():
 
 
 class Node():
-    u"""
+    """
     RRT Node
     """
 
@@ -417,14 +411,6 @@ class Node():
 def main():
     print("Start rrt start planning")
     # ====Search Path with RRT====
-    obstacleList = [
-        (5, 5, 1),
-        (3, 6, 2),
-        (3, 8, 2),
-        (3, 10, 2),
-        (7, 5, 2),
-        (9, 5, 2)
-    ]  # [x,y,size(radius)]
     obstacleList = [
         (5, 5, 1),
         (4, 6, 1),

PathPlanning/ClosedLoopRRTStar/unicycle_model.py

@@ -1,15 +1,15 @@
-#! /usr/bin/python
-# -*- coding: utf-8 -*-
 """
 
+Unicycle model class
 
 author Atsushi Sakai
+
 """
 
 import math
 
 dt = 0.05  # [s]
-L = 2.9  # [m]
+L = 0.9  # [m]
 steer_max = math.radians(40.0)
 curvature_max = math.tan(steer_max) / L
 curvature_max = 1.0 / curvature_max + 1.0