From ac066ee6049adc6315a859127344ee5de1e48b30 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Fri, 12 Apr 2019 22:15:58 +0900 Subject: [PATCH] remove unused code in pure_pursuit control --- PathTracking/pure_pursuit/pure_pursuit.py | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/PathTracking/pure_pursuit/pure_pursuit.py b/PathTracking/pure_pursuit/pure_pursuit.py index 26c3d848..af953e30 100644 --- a/PathTracking/pure_pursuit/pure_pursuit.py +++ b/PathTracking/pure_pursuit/pure_pursuit.py @@ -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")