Use slot in main, plot

[hubacji1/iamcar.git] / plot.py

diff --git a/plot.py b/plot.py
index 43f6ee8093c618140c3e2535007820246aeb0f5e..d56c095da29033d91182d262cdc5c176c92a5f1e 100644 (file)
--- a/plot.py
+++ b/plot.py
@@ -24,6 +24,7 @@ PLOT = {
         "frame" : False,
         "path" : False,
         "traj" : True,
+        "slot" : True,
         }
 COLOR = {
         "node": "lightgrey",
@@ -36,6 +37,7 @@ COLOR = {
         "trajectory-frame": "lightblue",
         "obstacle": "black",
         "log": ("gold", "orange", "blueviolet", "blue", "navy", "black"),
+        "slot": "red"
         }
 
 def car_frame(pose):
@@ -213,6 +215,12 @@ if __name__ == "__main__":
         except:
             print("No trajectory")
 
+    if PLOT["slot"]:
+        try:
+            plt.plot(*plot_nodes(s["slot"]["polygon"]), 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)
@@ -318,6 +326,12 @@ if __name__ == "__main__":
         except:
             print("No trajectory")
 
+    if PLOT["slot"]:
+        try:
+            plt.plot(*plot_nodes(s["slot"]["polygon"]), 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)