Merge branch 'release/0.7.0'

[hubacji1/iamcar.git] / plot.py

diff --git a/plot.py b/plot.py
index c0c3245f31f60b3862477db5a6d351e98f8ae2e3..c9f1eb13aad7f300ec73e2ca8e6d13a78883c1db 100644 (file)
--- a/plot.py
+++ b/plot.py
@@ -13,9 +13,6 @@ SAFETY_DIST = 0
 WHEEL_BASE = 2.450
 WIDTH = 1.625
 
-SCEN_FILE = argv[1]
-TRAJ_FILE = argv[2]
-
 PLOT = {
         "enable" : True,
         "node" : False,
@@ -131,11 +128,14 @@ def plot_segments(segments=[]):
     pass
 
 if __name__ == "__main__":
+    SCEN_FILE = argv[1]
+
     s = load_scenario(SCEN_FILE)
     try:
+        TRAJ_FILE = argv[2]
         t = load_trajectory(TRAJ_FILE) # fixed to trajectories
     except:
-        pass
+        t = {"elap": 0}
 
     plt.rcParams["font.size"] = 24
     plt.rcParams['hatch.linewidth'] = 1.0
@@ -217,33 +217,41 @@ if __name__ == "__main__":
 
     if PLOT["slot"]:
         try:
-            plt.plot(*plot_nodes(s["slot"]["polygon"]), COLOR["slot"])
+            plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
         except:
             print("No slot")
 
-    plt.plot(*car_frame(s["init"]), color="red", lw=2)
-    plt.plot(*car_frame(t["goal"]), color="red", lw=2)
-    plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
-    plt.plot(*plot_nodes([t["goal"]]), color="red", marker="+", ms=12)
+    try: # init
+        plt.plot(*car_frame(t["init"]), color="red", lw=2)
+        plt.plot(*plot_nodes([t["init"]]), color="red", marker="+", ms=12)
+    except:
+        plt.plot(*car_frame(s["init"]), color="red", lw=2)
+        plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
+    try: # goal
+        plt.plot(*plot_nodes([t["goal"]]), color="red", marker="+", ms=12)
+        plt.plot(*car_frame(t["goal"]), color="red", lw=2)
+    except:
+        plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
+        plt.plot(*car_frame(s["goal"]), color="red", lw=2)
     try: # middle
         plt.plot(*car_frame(t["midd"]), color="red", lw=2)
         plt.plot(*plot_nodes([t["midd"]]), color="red", marker="+", ms=12)
     except:
         pass
 
-    plt.fill_between(
-                [-2.7, 3.25, 3.25, -2.7],
-                [-0.5, -0.5, 0, 0],
-                [26, 26, 26.5, 26.5],
-                facecolor="none", hatch="//", edgecolor="black", linewidth=0)
-    plt.fill_between(
-                [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
-                [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
-                facecolor="none", hatch="//", edgecolor="black", linewidth=0)
-    plt.fill_between(
-                [3.25, 3.25, 2.75, 2.75, 3.25],
-                [-0.5, 26.5, 26.5, -0.5, -0.5],
-                facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+    #plt.fill_between(
+    #            [-2.7, 3.25, 3.25, -2.7],
+    #            [-0.5, -0.5, 0, 0],
+    #            [26, 26, 26.5, 26.5],
+    #            facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+    #plt.fill_between(
+    #            [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
+    #            [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
+    #            facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+    #plt.fill_between(
+    #            [3.25, 3.25, 2.75, 2.75, 3.25],
+    #            [-0.5, 26.5, 26.5, -0.5, -0.5],
+    #            facecolor="none", hatch="//", edgecolor="black", linewidth=0)
 
     #plt.text(1, 0.2, s="1", color="red")
     #plt.text(2, 12.2, s="2", color="red")
@@ -333,7 +341,7 @@ if __name__ == "__main__":
 
     if PLOT["slot"]:
         try:
-            plt.plot(*plot_nodes(s["slot"]["polygon"]), COLOR["slot"])
+            plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
         except:
             print("No slot")
 
@@ -342,19 +350,19 @@ if __name__ == "__main__":
     plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
     plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
 
-    plt.fill_between(
-                [-2.7, 3.25, 3.25, -2.7],
-                [-0.5, -0.5, 0, 0],
-                [26, 26, 26.5, 26.5],
-                facecolor="none", hatch="//", edgecolor="black", linewidth=0)
-    plt.fill_between(
-                [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
-                [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
-                facecolor="none", hatch="//", edgecolor="black", linewidth=0)
-    plt.fill_between(
-                [3.25, 3.25, 2.75, 2.75, 3.25],
-                [-0.5, 26.5, 26.5, -0.5, -0.5],
-                facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+    #plt.fill_between(
+    #            [-2.7, 3.25, 3.25, -2.7],
+    #            [-0.5, -0.5, 0, 0],
+    #            [26, 26, 26.5, 26.5],
+    #            facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+    #plt.fill_between(
+    #            [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
+    #            [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
+    #            facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+    #plt.fill_between(
+    #            [3.25, 3.25, 2.75, 2.75, 3.25],
+    #            [-0.5, 26.5, 26.5, -0.5, -0.5],
+    #            facecolor="none", hatch="//", edgecolor="black", linewidth=0)
 
     handles, labels = ax.get_legend_handles_labels()