From 00f181621b108129df0217b4cf8cfe614fcf6629 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Mon, 2 Apr 2018 08:38:16 +0900
Subject: [PATCH] keep coding

---
 .../closed_loop_rrt_star_car.py               | 24 ++-----
 .../ClosedLoopRRTStar/unicycle_model.py       |  6 +-
 PathPlanning/HybridAStar/hybrid_a_star.py     | 62 +++++++++++++++++++
 3 files changed, 70 insertions(+), 22 deletions(-)

diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
index f840330a..1500abbf 100644
--- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
+++ b/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),
diff --git a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py
index f2a18ca9..fe6e05ae 100644
--- a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py
+++ b/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
diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py
index 2b28710d..e93ec83f 100644
--- a/PathPlanning/HybridAStar/hybrid_a_star.py
+++ b/PathPlanning/HybridAStar/hybrid_a_star.py
@@ -6,8 +6,70 @@ author: Atsushi Sakai (@Atsushi_twi)
 
 """
 
+import sys
+sys.path.append("../ReedsSheppPath/")
+
+#  import random
+import math
+#  import numpy as np
+import matplotlib.pyplot as plt
+import reeds_shepp_path_planning
+
+show_animation = True
+
+
+def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
+
+    rx, ry, ryaw = [], [], []
+
+    return rx, ry, ryaw
+
 
 def main():
+    print("Start rrt start planning")
+    # ====Search Path with RRT====
+
+    ox, oy = [], []
+
+    for i in range(60):
+        ox.append(i)
+        oy.append(0.0)
+    for i in range(60):
+        ox.append(60.0)
+        oy.append(i)
+    for i in range(61):
+        ox.append(i)
+        oy.append(60.0)
+    for i in range(61):
+        ox.append(0.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(20.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(40.0)
+        oy.append(60.0 - i)
+
+    # Set Initial parameters
+    start = [10.0, 10.0, math.radians(90.0)]
+    goal = [50.0, 50.0, math.radians(-90.0)]
+
+    xyreso = 2.0
+    yawreso = math.radians(15.0)
+
+    rx, ry, ryaw = hybrid_a_star_planning(
+        start, goal, ox, oy, xyreso, yawreso)
+
+    plt.plot(ox, oy, ".k")
+    reeds_shepp_path_planning.plot_arrow(
+        start[0], start[1], start[2])
+    reeds_shepp_path_planning.plot_arrow(
+        goal[0], goal[1], goal[2])
+
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()
+
     print(__file__ + " start!!")