add test for move_to_pose sample

Author: AtsushiSakai

PathTracking/move_to_pose/move_to_pose.py

@@ -1,32 +1,26 @@
 """
+
 Move to specified pose
+
 Author: Daniel Ingram (daniel-s-ingram)
+        Atsushi Sakai(@Atsushi_twi)
 
 P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7
-"""
 
-from __future__ import print_function, division
+"""
 
 import matplotlib.pyplot as plt
 import numpy as np
-
-from math import cos, sin, sqrt, atan2, pi
 from random import random
 
+# simulation parameters
 Kp_rho = 9
 Kp_alpha = 15
 Kp_beta = -3
 dt = 0.01
 
-#Corners of triangular vehicle when pointing to the right (0 radians)
-p1_i = np.array([0.5, 0, 1]).T
-p2_i = np.array([-0.5, 0.25, 1]).T
-p3_i = np.array([-0.5, -0.25, 1]).T
-
-x_traj = []
-y_traj = []
+show_animation = True
 
-plt.ion()
 
 def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
     """
@@ -44,7 +38,9 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
     x_diff = x_goal - x
     y_diff = y_goal - y
 
-    rho = sqrt(x_diff**2 + y_diff**2)
+    x_traj, y_traj = [], []
+
+    rho = np.sqrt(x_diff**2 + y_diff**2)
     while rho > 0.001:
         x_traj.append(x)
         y_traj.append(y)
@@ -54,63 +50,78 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
 
         """
         Restrict alpha and beta (angle differences) to the range
-        [-pi, pi] to prevent unstable behavior e.g. difference going 
+        [-pi, pi] to prevent unstable behavior e.g. difference going
         from 0 rad to 2*pi rad with slight turn
-        """ 
+        """
 
-        rho = sqrt(x_diff**2 + y_diff**2)
-        alpha = (atan2(y_diff, x_diff) - theta + pi)%(2*pi) - pi
-        beta = (theta_goal - theta - alpha + pi)%(2*pi) - pi
+        rho = np.sqrt(x_diff**2 + y_diff**2)
+        alpha = (np.arctan2(y_diff, x_diff) -
+                 theta + np.pi) % (2 * np.pi) - np.pi
+        beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
 
-        v = Kp_rho*rho
-        w = Kp_alpha*alpha + Kp_beta*beta
+        v = Kp_rho * rho
+        w = Kp_alpha * alpha + Kp_beta * beta
 
-        if alpha > pi/2 or alpha < -pi/2: 
+        if alpha > np.pi / 2 or alpha < -np.pi / 2:
             v = -v
 
-        theta = theta + w*dt
-        x = x + v*cos(theta)*dt
-        y = y + v*sin(theta)*dt
+        theta = theta + w * dt
+        x = x + v * np.cos(theta) * dt
+        y = y + v * np.sin(theta) * dt
 
-        plot_vehicle(x, y, theta, x_traj, y_traj)
+        if show_animation:
+            plt.cla()
+            plt.arrow(x_start, y_start, np.cos(theta_start),
+                      np.sin(theta_start), color='r', width=0.1)
+            plt.arrow(x_goal, y_goal, np.cos(theta_goal),
+                      np.sin(theta_goal), color='g', width=0.1)
+            plot_vehicle(x, y, theta, x_traj, y_traj)
 
 
 def plot_vehicle(x, y, theta, x_traj, y_traj):
+    # Corners of triangular vehicle when pointing to the right (0 radians)
+    p1_i = np.array([0.5, 0, 1]).T
+    p2_i = np.array([-0.5, 0.25, 1]).T
+    p3_i = np.array([-0.5, -0.25, 1]).T
+
     T = transformation_matrix(x, y, theta)
     p1 = np.matmul(T, p1_i)
     p2 = np.matmul(T, p2_i)
     p3 = np.matmul(T, p3_i)
 
-    plt.cla()
-
     plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-')
     plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-')
     plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-')
 
-    plt.arrow(x_start, y_start, cos(theta_start), sin(theta_start), color='r', width=0.1)
-    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)
 
-    plt.show()
     plt.pause(dt)
 
+
 def transformation_matrix(x, y, theta):
     return np.array([
-        [cos(theta), -sin(theta), x],
-        [sin(theta), cos(theta), y],
+        [np.cos(theta), -np.sin(theta), x],
+        [np.sin(theta), np.cos(theta), y],
         [0, 0, 1]
-        ])
+    ])
+
+
+def main():
+    x_start = 20 * random()
+    y_start = 20 * random()
+    theta_start = 2 * np.pi * random() - np.pi
+    x_goal = 20 * random()
+    y_goal = 20 * random()
+    theta_goal = 2 * np.pi * random() - np.pi
+    print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
+          (x_start, y_start, theta_start))
+    print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
+          (x_goal, y_goal, theta_goal))
+    move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
+
 
 if __name__ == '__main__':
-    x_start = 20*random()
-    y_start = 20*random()
-    theta_start = 2*pi*random() - pi
-    x_goal = 20*random()
-    y_goal = 20*random()
-    theta_goal = 2*pi*random() - pi
-    print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" % (x_start, y_start, theta_start))
-    print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" % (x_goal, y_goal, theta_goal))
-    move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
+    main()

tests/test_move_to_pose.py

@@ -0,0 +1,12 @@
+from unittest import TestCase
+
+from PathTracking.move_to_pose import move_to_pose as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()