From 6bd25b922159dfe6df77f03bd24d61a9ba78ae67 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Fri, 2 Apr 2021 21:43:29 +0900 Subject: [PATCH] fix state_lattice_planner.py coordinate conversion (#495) --- .../StateLatticePlanner/state_lattice_planner.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py index 51eeb000..b5fec697 100644 --- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py +++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py @@ -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)