Merge branch 'release/0.7.0'

[hubacji1/iamcar.git] / plot.py

diff --git a/plot.py b/plot.py
index 43f6ee8093c618140c3e2535007820246aeb0f5e..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,
@@ -24,6 +21,7 @@ PLOT = {
         "frame" : False,
         "path" : False,
         "traj" : True,
+        "slot" : True,
         }
 COLOR = {
         "node": "lightgrey",
@@ -36,6 +34,7 @@ COLOR = {
         "trajectory-frame": "lightblue",
         "obstacle": "black",
         "log": ("gold", "orange", "blueviolet", "blue", "navy", "black"),
+        "slot": "red"
         }
 
 def car_frame(pose):
@@ -129,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
@@ -213,24 +215,43 @@ if __name__ == "__main__":
         except:
             print("No trajectory")
 
-    plt.plot(*car_frame(s["init"]), color="red", lw=2)
-    plt.plot(*car_frame(s["goal"]), color="red", lw=2)
-    plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
-    plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
+    if PLOT["slot"]:
+        try:
+            plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
+        except:
+            print("No slot")
 
-    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)
+    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.text(1, 0.2, s="1", color="red")
     #plt.text(2, 12.2, s="2", color="red")
@@ -318,24 +339,30 @@ if __name__ == "__main__":
         except:
             print("No trajectory")
 
+    if PLOT["slot"]:
+        try:
+            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(s["goal"]), color="red", lw=2)
     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()