keep coding

Author: AtsushiSakai

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -20,6 +20,26 @@
 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