github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 02:47:54 -05:00
Code Issues Packages Projects Releases Wiki Activity

remove unused code in pure_pursuit control

Browse Source
This commit is contained in:
Atsushi Sakai
2019-04-12 22:15:58 +09:00
parent 98ba4dd15a
commit ac066ee604
1 changed files with 16 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
PathTracking/pure_pursuit/pure_pursuit.py
View File

@@ -61,9 +61,6 @@ def pure_pursuit_control(state, cx, cy, pind):
alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
if state.v < 0: # back
alpha = math.pi - alpha
Lf = k * state.v + Lfc
delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
@@ -92,6 +89,20 @@ def calc_target_index(state, cx, cy):
return ind
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
"""
Plot arrow
"""
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
plot_arrow(ix, iy, iyaw)
else:
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width)
plt.plot(x, y)
def main():
# target course
cx = np.arange(0, 50, 0.1)
@@ -128,7 +139,8 @@ def main():
if show_animation: # pragma: no cover
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plot_arrow(state.x, state.y, state.yaw)
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")