github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

add straight test

Browse Source
This commit is contained in:
Atsushi Sakai
2018-07-15 18:24:13 +09:00
parent f642f21802
commit d43cd19786
1 changed files with 13 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
View File

@@ -478,9 +478,11 @@ def smooth_yaw(yaw):
for i in range(len(yaw) - 1):
dyaw = yaw[i + 1] - yaw[i]
while dyaw >= math.pi / 2.0:
yaw[i + 1] -= math.pi * 2.0
dyaw = yaw[i + 1] - yaw[i]
while dyaw <= -math.pi / 2.0:
yaw[i + 1] += math.pi * 2.0
dyaw = yaw[i + 1] - yaw[i]
@@ -488,6 +490,15 @@ def smooth_yaw(yaw):
return yaw
def get_straight_course(dl):
ax = [0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0]
ay = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
ax, ay, ds=dl)
return cx, cy, cyaw, ck
def get_forward_course(dl):
ax = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0]
ay = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0]
@@ -519,7 +530,8 @@ def main():
print(__file__ + " start!!")
dl = 1.0 # course tick
# cx, cy, cyaw, ck = get_forward_course(dl)
# cx, cy, cyaw, ck = get_straight_course(dl)
# cx, cy, cyaw, ck = get_forward_course(dl)
cx, cy, cyaw, ck = get_switch_back_course(dl)
sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED)