From cd5eb797220af9b15e122e083f7364a0db758cd2 Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Wed, 4 Apr 2018 18:33:06 +0900
Subject: [PATCH] keep coding

---
 PathPlanning/HybridAStar/hybrid_a_star.py | 25 +++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py
index e25d1786..f4fe5460 100644
--- a/PathPlanning/HybridAStar/hybrid_a_star.py
+++ b/PathPlanning/HybridAStar/hybrid_a_star.py
@@ -20,6 +20,26 @@ EXTEND_AREA = 5.0  # [m]
 show_animation = True
 
 
+class Node:
+    """
+    Node
+    """
+
+    def __init__(self, xind, yind, yawind, direction, x, y, yaw, directions, steer, cost, pind):
+        # store kd-tree
+        self.xind = xind
+        self.yind = yind
+        self.yawind = yawind
+        self.direction = direction
+        self.xlist = x
+        self.ylist = y
+        self.yawlist = yaw
+        self.directionlist = directions
+        self.steer = steer
+        self.cost = cost
+        self.pind = pind
+
+
 class KDTree:
     """
     Nearest neighbor search class with KDTree
@@ -101,6 +121,11 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
 
     c = Config(tox, toy, xyreso, yawreso)
 
+    nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso),
+                  True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1)
+    ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso),
+                 True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1)
+
     rx, ry, ryaw = [], [], []
 
     return rx, ry, ryaw