Plot scenario with pose of 5 elements

diff --git a/scripts/plot_scenario.py b/scripts/plot_scenario.py
index 4d8761ef654397215eaab79c5b17b0cf19e22501..77ae54fd732a0063490bd35df4a101e062ce36a8 100755 (executable)
--- a/scripts/plot_scenario.py
+++ b/scripts/plot_scenario.py
@@ -730,25 +730,19 @@ if __name__ == "__main__":
         plt.plot(*plot_nodes(scenario["path"]), color="blue")
         i = 0
         for p in scenario["path"]:
-            if False and len(p) > 4:
-                if p[4] > 0:
-                    plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker="+")
-                elif p[4] < 0:
-                    plt.plot(p[0] - MINX, p[1] - MINY, color="green", marker="x")
-                else:
-                    plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker=".")
+            if len(p) > 5:
+                pc = {1: "red", 0: "blue", -1: "green"}
+                pm = "o" if p[5] else "+"
+                plt.plot(p[0] - MINX, p[1] - MINY, color=pc[p[4]], marker=pm)
+                #plt.plot(*plot_car(p), color="blue")
+                pass
+                #cc = plot_car_corners(p)
+                #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
+                #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
+                #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
+                #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
             else:
-                if len(p) > 5 and p[5]:  # parent is cusp
-                    plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="o")
-                else:
-                    plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="+")
-            #plt.plot(*plot_car(p), color="blue")
-            pass
-            #cc = plot_car_corners(p)
-            #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
-            #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
-            #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
-            #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
+                plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="+")
     if ("traj" in scenario and isinstance(scenario["traj"], list)
             and len(scenario["traj"]) > 0):
         print(len(scenario["traj"]))