add quadrotor test

Author: AtsushiSakai

AerialNavigation/Quadrotor.py renamed to AerialNavigation/drone_3d_trajectory_following/Quadrotor.py

@@ -11,7 +11,7 @@
 
 
 class Quadrotor():
-    def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25):
+    def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True):
         self.p1 = np.array([size / 2, 0, 0, 1]).T
         self.p2 = np.array([-size / 2, 0, 0, 1]).T
         self.p3 = np.array([0, size / 2, 0, 1]).T
@@ -20,6 +20,7 @@ def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25):
         self.x_data = []
         self.y_data = []
         self.z_data = []
+        self.show_animation = show_animation
 
         plt.ion()
 
@@ -38,7 +39,9 @@ def update_pose(self, x, y, z, roll, pitch, yaw):
         self.x_data.append(x)
         self.y_data.append(y)
         self.z_data.append(z)
-        self.plot()
+
+        if self.show_animation:
+            self.plot()
 
     def transformation_matrix(self):
         x = self.x

AerialNavigation/TrajectoryGenerator.py renamed to AerialNavigation/drone_3d_trajectory_following/TrajectoryGenerator.py

@@ -9,6 +9,8 @@
 from Quadrotor import Quadrotor
 from TrajectoryGenerator import TrajectoryGenerator
 
+show_animation = True
+
 # Simulation parameters
 g = 9.81
 m = 0.2
@@ -59,7 +61,7 @@ def quad_sim(x_c, y_c, z_c):
     t = 0
 
     q = Quadrotor(x=x_pos, y=y_pos, z=z_pos, roll=roll,
-                  pitch=pitch, yaw=yaw, size=1)
+                  pitch=pitch, yaw=yaw, size=1, show_animation=show_animation)
 
     i = 0
     n_run = 8

AerialNavigation/animation.gif renamed to AerialNavigation/drone_3d_trajectory_following/animation.gif

@@ -0,0 +1,14 @@
+from unittest import TestCase
+
+import sys
+sys.path.append("./AerialNavigation/drone_3d_trajectory_following/")
+
+from AerialNavigation.drone_3d_trajectory_following import drone_3d_trajectory_following as m
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()