github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-11 17:36:56 -05:00
Code Issues Packages Projects Releases Wiki Activity

change RS path code to correct one

Browse Source
This commit is contained in:
Atsushi Sakai
2018-02-07 13:46:13 -08:00
parent d700425402
commit 0b1ca74408
3 changed files with 7 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
View File

@@ -5,6 +5,9 @@ author: AtsushiSakai(@Atsushi_twi)
"""
import sys
sys.path.append("../ReedsSheppPath/")
import random
import math
import copy
@@ -18,6 +21,7 @@ show_animation = True
target_speed = 10.0 / 3.6
STEP_SIZE = 0.5
class RRT():
@@ -217,7 +221,7 @@ class RRT():
px, py, pyaw, mode, clen = reeds_shepp_path_planning.reeds_shepp_path_planning(
nearestNode.x, nearestNode.y, nearestNode.yaw,
rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max)
rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max, STEP_SIZE)
newNode = copy.deepcopy(nearestNode)
newNode.x = px[-1]
@@ -227,7 +231,7 @@ class RRT():
newNode.path_x = px
newNode.path_y = py
newNode.path_yaw = pyaw
newNode.cost += clen
newNode.cost += sum(clen)
newNode.parent = nind
return newNode