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

fix state_lattice_planner.py coordinate conversion (#495)

Browse Source
This commit is contained in:
Atsushi Sakai
2021-04-02 21:43:29 +09:00
committed by GitHub
parent 3abc9c69e3
commit 6bd25b9221
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
PathPlanning/StateLatticePlanner/state_lattice_planner.py
View File

@@ -156,8 +156,8 @@ def calc_lane_states(l_center, l_heading, l_width, v_width, d, nxy):
:param nxy: sampling number
:return: state list
"""
xc = math.cos(l_heading) * d + math.sin(l_heading) * l_center
yc = math.sin(l_heading) * d + math.cos(l_heading) * l_center
xc = d
yc = l_center
states = []
for i in range(nxy):
@@ -301,7 +301,7 @@ def lane_state_sampling_test1():
k0 = 0.0
l_center = 10.0
l_heading = np.deg2rad(90.0)
l_heading = np.deg2rad(0.0)
l_width = 3.0
v_width = 1.0
d = 10
@@ -309,6 +309,9 @@ def lane_state_sampling_test1():
states = calc_lane_states(l_center, l_heading, l_width, v_width, d, nxy)
result = generate_path(states, k0)
if show_animation:
plt.close("all")
for table in result:
xc, yc, yawc = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)