fix state_lattice_planner.py coordinate conversion (#495)

Author: superdev7

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,14 +301,17 @@ 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
     nxy = 5
     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)