github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 05:57:59 -05:00
Code Issues Packages Projects Releases Wiki Activity

keep coding

Browse Source
This commit is contained in:
Atsushi Sakai
2018-04-04 18:33:06 +09:00
parent 4018e6bdaa
commit cd5eb79722
1 changed files with 25 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
PathPlanning/HybridAStar/hybrid_a_star.py
View File

@@ -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