Correct plotting bugs

Author: ChaoqinRobotics

README.md

@@ -55,7 +55,7 @@ python3 togt_refine.py
 cd ../plots
 python3 plot_togt_refined_traj.py
 ```
-![Alt Text](documents/images/togt_refined_traj.png)
+![Alt Text](documents/images/fig_togt_refined_traj.png)
 
 ## Visualize the AOS trajectory for waypoint flight
 
@@ -66,7 +66,7 @@ Another example is the Split-S track provided in [this](https://www.science.org/
 cd ../scripts/plots
 python3 plot_aos_traj.py
 ```
-![Alt Text](documents/images/aos_traj.png)
+![Alt Text](documents/images/fig_aos_traj.png)
 
 ## Visualize the AOS-TOGT trajectory
 
@@ -77,7 +77,7 @@ cd ../scripts/plots
 python3 plot_aos_togt_traj.py
 ```
 
-![Alt Text](documents/images/aos_togt_traj.png)
+![Alt Text](documents/images/fig_aos_togt_traj.png)
 
 ## Acknowledgements
 

documents/images/aos_refined_traj.png

@@ -1,16 +1,17 @@
 import numpy as np
 import matplotlib
 import matplotlib.pyplot as plt
-from track import plot_track
+from scipy.interpolate import UnivariateSpline
 
 import sys, os
 BASEPATH = os.path.abspath(__file__).split("plots/", 1)[0]+"plots/"
 ROOTPATH = os.path.abspath(__file__).split("plots/", 1)[0]+".."
 sys.path += [BASEPATH]
 sys.path += [ROOTPATH]
 
-track_file = ROOTPATH + "/resources/racetrack/race_uzh_7g_multiprisma.yaml"
-traj_file = ROOTPATH + "/resources/trajectory/togt_traj.csv"
+directory = ROOTPATH + "/resources/trajectory"
+
+traj_file = directory + "/aos_refined_traj.csv"
 
 data_ocp = np.genfromtxt(traj_file, dtype=float, delimiter=',', names=True)
 
@@ -64,15 +65,44 @@
 
 plt.scatter(ps[:, 0], ps[:, 1], s=5,
             c=vt, cmap=plt.cm.autumn.reversed())
-plt.colorbar(pad=0.01).ax.set_ylabel('Speed [m/s]')
+plt.colorbar(pad=0.0).ax.set_ylabel('Speed [m/s]')
+
+# Plot gates
+
+orientations = {
+    'Gate1': [0.0, -90.0, -0.0],
+    'Gate2': [0.0, -90.0, -20.0],
+    'Gate3': [0.0, -90.0, 50.0],
+    'Gate4': [0.0, -90.0, 0.0],
+    'Gate5': [0.0, -90.0, 0.0],
+    'Gate6': [0.0, -90.0, 70.0],
+    'Gate7': [0.0, -90.0, 20.0]
+}
+    
+positions = {
+    'Gate1': (-1.1, -1.6, 3.6),
+    'Gate2': (9.2, 6.6, 1.0),
+    'Gate3': (9.2, -4.0, 1.2),
+    'Gate4': (-4.5, -6.0, 3.5),
+    'Gate5': (-4.5, -6.0, 0.8),
+    'Gate6': [4.75, -0.9, 1.2],
+    'Gate7': [-2.8, 6.8, 1.2]
+}
+
 
-plot_track(plt.gca(), track_file)
+orders = ['Gate1', 'Gate2', 'Gate3', 'Gate4', 'Gate5', 'Gate6', 'Gate7']
+for g in orders:
+    rpy = np.array(orientations[g]) * np.pi/180
+    y = rpy[2]
+    r = 0.8
+    plt.plot([positions[g][0]+r*np.sin(y), positions[g][0]-r*np.sin(y)],
+            [positions[g][1]-r*np.cos(y), positions[g][1]+r*np.cos(y)], 'k-', linewidth=4.0)
 
 
 plt.xlabel('x [m]')
 plt.ylabel('y [m]')
 plt.axis('equal')
 plt.grid()
-plt.savefig('aos_refined_traj.png', bbox_inches='tight')
+plt.savefig('aos_refined_traj.png')
 
 plt.show()

documents/images/aos_togt_traj.png

@@ -1,16 +1,17 @@
 import numpy as np
 import matplotlib
 import matplotlib.pyplot as plt
-from track import plot_track
+from scipy.interpolate import UnivariateSpline
 
 import sys, os
 BASEPATH = os.path.abspath(__file__).split("plots/", 1)[0]+"plots/"
 ROOTPATH = os.path.abspath(__file__).split("plots/", 1)[0]+".."
 sys.path += [BASEPATH]
 sys.path += [ROOTPATH]
 
-track_file = ROOTPATH + "/resources/racetrack/race_uzh_7g_multiprisma.yaml"
-traj_file = ROOTPATH + "/resources/trajectory/togt_traj.csv"
+directory = ROOTPATH + "/resources/trajectory"
+
+traj_file = directory + "/aos_togt_traj.csv"
 
 data_ocp = np.genfromtxt(traj_file, dtype=float, delimiter=',', names=True)
 
@@ -64,15 +65,44 @@
 
 plt.scatter(ps[:, 0], ps[:, 1], s=5,
             c=vt, cmap=plt.cm.summer.reversed())
-plt.colorbar(pad=0.01).ax.set_ylabel('Speed [m/s]')
+plt.colorbar(pad=0.0).ax.set_ylabel('Speed [m/s]')
+
+# Plot gates
+
+orientations = {
+    'Gate1': [0.0, -90.0, -0.0],
+    'Gate2': [0.0, -90.0, -20.0],
+    'Gate3': [0.0, -90.0, 50.0],
+    'Gate4': [0.0, -90.0, 0.0],
+    'Gate5': [0.0, -90.0, 0.0],
+    'Gate6': [0.0, -90.0, 70.0],
+    'Gate7': [0.0, -90.0, 20.0]
+}
+    
+positions = {
+    'Gate1': (-1.1, -1.6, 3.6),
+    'Gate2': (9.2, 6.6, 1.0),
+    'Gate3': (9.2, -4.0, 1.2),
+    'Gate4': (-4.5, -6.0, 3.5),
+    'Gate5': (-4.5, -6.0, 0.8),
+    'Gate6': [4.75, -0.9, 1.2],
+    'Gate7': [-2.8, 6.8, 1.2]
+}
+
 
-plot_track(plt.gca(), track_file)
+orders = ['Gate1', 'Gate2', 'Gate3', 'Gate4', 'Gate5', 'Gate6', 'Gate7']
+for g in orders:
+    rpy = np.array(orientations[g]) * np.pi/180
+    y = rpy[2]
+    r = 0.8
+    plt.plot([positions[g][0]+r*np.sin(y), positions[g][0]-r*np.sin(y)],
+            [positions[g][1]-r*np.cos(y), positions[g][1]+r*np.cos(y)], 'k-', linewidth=4.0)
 
 
 plt.xlabel('x [m]')
 plt.ylabel('y [m]')
 plt.axis('equal')
 plt.grid()
-plt.savefig('aos_togt_traj.png', bbox_inches='tight')
+plt.savefig('aos_togt_traj.png')
 
 plt.show()

documents/images/aos_traj.png

@@ -1,16 +1,17 @@
 import numpy as np
 import matplotlib
 import matplotlib.pyplot as plt
-from track import plot_track
+from scipy.interpolate import UnivariateSpline
 
 import sys, os
 BASEPATH = os.path.abspath(__file__).split("plots/", 1)[0]+"plots/"
 ROOTPATH = os.path.abspath(__file__).split("plots/", 1)[0]+".."
 sys.path += [BASEPATH]
 sys.path += [ROOTPATH]
 
-track_file = ROOTPATH + "/resources/racetrack/race_uzh_7g_multiprisma.yaml"
-traj_file = ROOTPATH + "/resources/trajectory/togt_traj.csv"
+directory = ROOTPATH + "/resources/trajectory"
+
+traj_file = directory + "/aos_traj.csv"
 
 data_ocp = np.genfromtxt(traj_file, dtype=float, delimiter=',', names=True)
 
@@ -64,15 +65,44 @@
 
 plt.scatter(ps[:, 0], ps[:, 1], s=5,
             c=vt, cmap=plt.cm.autumn.reversed())
-plt.colorbar(pad=0.01).ax.set_ylabel('Speed [m/s]')
+plt.colorbar(pad=0.0).ax.set_ylabel('Speed [m/s]')
+
+# Plot gates
+
+orientations = {
+    'Gate1': [0.0, -90.0, -0.0],
+    'Gate2': [0.0, -90.0, -20.0],
+    'Gate3': [0.0, -90.0, 50.0],
+    'Gate4': [0.0, -90.0, 0.0],
+    'Gate5': [0.0, -90.0, 0.0],
+    'Gate6': [0.0, -90.0, 70.0],
+    'Gate7': [0.0, -90.0, 20.0]
+}
+    
+positions = {
+    'Gate1': (-1.1, -1.6, 3.6),
+    'Gate2': (9.2, 6.6, 1.0),
+    'Gate3': (9.2, -4.0, 1.2),
+    'Gate4': (-4.5, -6.0, 3.5),
+    'Gate5': (-4.5, -6.0, 0.8),
+    'Gate6': [4.75, -0.9, 1.2],
+    'Gate7': [-2.8, 6.8, 1.2]
+}
+
 
-plot_track(plt.gca(), track_file)
+orders = ['Gate1', 'Gate2', 'Gate3', 'Gate4', 'Gate5', 'Gate6', 'Gate7']
+for g in orders:
+    rpy = np.array(orientations[g]) * np.pi/180
+    y = rpy[2]
+    r = 0.8
+    plt.plot([positions[g][0]+r*np.sin(y), positions[g][0]-r*np.sin(y)],
+            [positions[g][1]-r*np.cos(y), positions[g][1]+r*np.cos(y)], 'k-', linewidth=4.0)
 
 
 plt.xlabel('x [m]')
 plt.ylabel('y [m]')
 plt.axis('equal')
 plt.grid()
-plt.savefig('aos_traj.png', bbox_inches='tight')
+plt.savefig('aos_traj.png')
 
 plt.show()

documents/images/fig_aos_refined_traj.png

@@ -10,7 +10,7 @@
 sys.path += [ROOTPATH]
 
 track_file = ROOTPATH + "/resources/racetrack/race_uzh_7g_multiprisma.yaml"
-traj_file = ROOTPATH + "/resources/trajectory/togt_traj.csv"
+traj_file = ROOTPATH + "/resources/trajectory/togt_refined_traj.csv"
 
 data_ocp = np.genfromtxt(traj_file, dtype=float, delimiter=',', names=True)