From 599ddf291af4523844ef2a50f1f1a37381f32571 Mon Sep 17 00:00:00 2001 From: Daniel Ingram Date: Mon, 13 Aug 2018 13:41:42 -0400 Subject: [PATCH] Draw goal vector on plot --- PathTracking/move_to_pose/move_to_pose.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/PathTracking/move_to_pose/move_to_pose.py b/PathTracking/move_to_pose/move_to_pose.py index a0fd6f11..2ac83d44 100644 --- a/PathTracking/move_to_pose/move_to_pose.py +++ b/PathTracking/move_to_pose/move_to_pose.py @@ -60,10 +60,10 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): x_traj.append(x) y_traj.append(y) - plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj) + plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, theta_goal, x_traj, y_traj) -def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj): +def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, theta_goal, x_traj, y_traj): T = transformation_matrix(x, y, theta) p1 = np.matmul(T, p1_i) p2 = np.matmul(T, p2_i) @@ -73,7 +73,7 @@ def plot_vehicle(x, y, theta, x_start, y_start, x_goal, y_goal, x_traj, y_traj): plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-') plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-') plt.plot(x_start, y_start, 'r*') - plt.plot(x_goal, y_goal, 'g*') + plt.arrow(x_goal, y_goal, cos(theta_goal), sin(theta_goal), color='g', width=0.1) plt.plot(x_traj, y_traj, 'b--') plt.xlim(0, 20) plt.ylim(0, 20)