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

fixed steer() in RRTstar

Browse Source
This commit is contained in:
luym11
2018-12-18 14:48:18 +01:00
parent a2830ba1f3
commit 39a918a202
1 changed files with 10 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
PathPlanning/RRTstar/rrt_star.py
View File

@@ -102,12 +102,16 @@ class RRT():
# expand tree
nearestNode = self.nodeList[nind]
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = copy.deepcopy(nearestNode)
newNode.x += self.expandDis * math.cos(theta)
newNode.y += self.expandDis * math.sin(theta)
newNode.cost += self.expandDis
newNode.parent = nind
newNode = Node(rnd[0], rnd[1])
currentDistance = math.sqrt( (rnd[1] - nearestNode.y) ** 2 + (rnd[0] - nearestNode.x) ** 2)
# Find a point within expandDis of nind, and closest to rnd
if currentDistance <= self.expandDis:
pass
else:
newNode.x = nearestNode.x + self.expandDis * math.cos(theta)
newNode.y = nearestNode.y + self.expandDis * math.sin(theta)
newNode.cost = float("inf")
newNode.parent = None
return newNode
def get_random_point(self):