Add pose heading method to SlotPlanner

[hubacji1/iamcar.git] / cplot.py

diff --git a/cplot.py b/cplot.py
index 4e0bc4c3eb276706781558f699fbdb71df8d254f..8bcc7b9742e94bd4b330a35e4dbcb049de75fa91 100644 (file)
--- a/cplot.py
+++ b/cplot.py
@@ -11,6 +11,11 @@ from matplotlib import pyplot as plt
 from os import listdir
 from sys import argv, exit
 
+YAX = "cost"
+YAX_LOG = False
+YAX_ASP = False
+ELAP_HIST_MAX = 10
+
 if len(argv) != 3:
     print("Exactly 2 arguments needed.")
     exit(1)
@@ -42,20 +47,24 @@ def plot_nodes(nodes=[]):
 if __name__ == "__main__":
     nodes = []
     podes = []
+    nhist = []
+    phist = []
     # blue nodes (not parallel)
     try:
         for f in listdir(TRAJ_FILE):
             try:
                 t = load_trajectory("{}/{}".format(TRAJ_FILE, f))
-                for p in zip(t["secs"], t["cost"]):
+                for p in zip(t["secs"], t[YAX]):
                     nodes.append(p)
+                nhist.append(t["elap"])
             except:
                 pass
     except:
         try:
             t = load_trajectory(TRAJ_FILE) # fixed to trajectories
-            for p in zip(t["secs"], t["cost"]):
+            for p in zip(t["secs"], t[YAX]):
                 nodes.append(p)
+            nhist.append(t["elap"])
         except:
             print("Failed loading 1st")
             exit(1)
@@ -64,25 +73,33 @@ if __name__ == "__main__":
         for f in listdir(TRAJ_FILE2):
             try:
                 t = load_trajectory("{}/{}".format(TRAJ_FILE2, f))
-                for p in zip(t["secs"], t["cost"]):
+                for p in zip(t["secs"], t[YAX]):
                     podes.append(p)
+                phist.append(t["elap"])
             except:
                 pass
     except:
         try:
             t = load_trajectory(TRAJ_FILE2) # fixed to trajectories
-            for p in zip(t["secs"], t["cost"]):
+            for p in zip(t["secs"], t[YAX]):
                 podes.append(p)
+            phist.append(t["elap"])
         except:
             print("Failed loading 2nd")
             exit(1)
 
     fig = plt.figure()
     ax = fig.add_subplot(111)
-    ax.set_aspect("equal")
+    if YAX_ASP:
+        ax.set_aspect("equal")
+    if YAX_LOG:
+        ax.set_yscale("log")
     ax.set_title("SCENARIO")
     ax.set_xlabel("Time [s]")
-    ax.set_ylabel("Cost (Euclidean length) [m]")
+    if YAX is "cost":
+        ax.set_ylabel("Cost (Euclidean length) [m]")
+    elif YAX is "node":
+        ax.set_ylabel("Number of nodes [-]")
     #ax.set_xlim(0, 60)
     #ax.set_ylim(0, 100)
 
@@ -99,3 +116,9 @@ if __name__ == "__main__":
     plt.show()
     #plt.savefig("{}_{}.png".format(argv[1], argv[2]))
     plt.close(fig)
+
+if ELAP_HIST_MAX > 0:
+    plt.hist(nhist, log=True, range=[0, ELAP_HIST_MAX], bins=100)
+    plt.show()
+    plt.hist(phist, log=True, range=[0, ELAP_HIST_MAX], bins=100)
+    plt.show()