Add speed limitation of the robot (AtsushiSakai#595)

* Added speed limitation of the robot

* Removed leading underscores for global vars

* Added unit test for robot speed limitation

* Modified x/abs(x) to np.sign(x); fixed code style

* Removed 'random' from test func header comment

Author: Muhammad-Yazdian

Control/move_to_pose/move_to_pose.py

@@ -19,6 +19,10 @@
 Kp_beta = -3
 dt = 0.01
 
+# Robot specifications
+MAX_LINEAR_SPEED = 15
+MAX_ANGULAR_SPEED = 7
+
 show_animation = True
 
 
@@ -63,6 +67,12 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
         if alpha > np.pi / 2 or alpha < -np.pi / 2:
             v = -v
 
+        if abs(v) > MAX_LINEAR_SPEED:
+            v = np.sign(v) * MAX_LINEAR_SPEED
+
+        if abs(w) > MAX_ANGULAR_SPEED:
+            w = np.sign(w) * MAX_ANGULAR_SPEED
+
         theta = theta + w * dt
         x = x + v * np.cos(theta) * dt
         y = y + v * np.sin(theta) * dt

tests/test_move_to_pose.py

@@ -7,5 +7,16 @@ def test_1():
     m.main()
 
 
+def test_2():
+    """
+    This unit test tests the move_to_pose.py program for a MAX_LINEAR_SPEED and
+    MAX_ANGULAR_SPEED
+    """
+    m.show_animation = False
+    m.MAX_LINEAR_SPEED = 11
+    m.MAX_ANGULAR_SPEED = 5
+    m.main()
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)