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

fix pure_pursuit sample (#445)

Browse Source
This commit is contained in:
Atsushi Sakai
2020-12-20 20:29:36 +09:00
committed by GitHub
parent 2d5024dc38
commit 4264cb8b10
1 changed files with 2 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
PathPlanning/ClosedLoopRRTStar/pure_pursuit.py
View File

@@ -250,7 +250,7 @@ def main(): # pragma: no cover
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
target_ind, dis = calc_target_index(state, cx, cy)
while T >= time and lastIndex > target_ind:
ai = PIDControl(target_speed, state.v)
@@ -291,43 +291,6 @@ def main(): # pragma: no cover
plt.show()
def main2(): # pragma: no cover
import pandas as pd
data = pd.read_csv("rrt_course.csv")
cx = np.array(data["x"])
cy = np.array(data["y"])
cyaw = np.array(data["yaw"])
target_speed = 10.0 / 3.6
goal = [cx[-1], cy[-1]]
cx, cy, cyaw = extend_path(cx, cy, cyaw)
speed_profile = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v, a, d, flag = closed_loop_prediction(
cx, cy, cyaw, speed_profile, goal)
plt.subplots(1)
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(goal[0], goal[1], "xg", label="goal")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
if __name__ == '__main__': # pragma: no cover
print("Pure pursuit path tracking simulation start")
# main()
main2()
main()