Merge branch 'release/0.7.0'

[hubacji1/iamcar.git] / plot.py

diff --git a/plot.py b/plot.py
index bbe565c8cf2027ca003231405f38aa903dd6d33d..c9f1eb13aad7f300ec73e2ca8e6d13a78883c1db 100644 (file)
--- a/plot.py
+++ b/plot.py
@@ -129,13 +129,13 @@ def plot_segments(segments=[]):
 
 if __name__ == "__main__":
     SCEN_FILE = argv[1]
-    TRAJ_FILE = argv[2]
 
     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
@@ -221,29 +221,37 @@ if __name__ == "__main__":
         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")
@@ -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()