Plot traj

[hubacji1/iamcar2.git] / scripts / plot_json_objects_scenario.py

diff --git a/scripts/plot_json_objects_scenario.py b/scripts/plot_json_objects_scenario.py
index 76e29cf7ef54fae86b6628c96e3e2d1d47d60748..804275a016fd79522d6b442fc3250a8f7942c021 100755 (executable)
--- a/scripts/plot_json_objects_scenario.py
+++ b/scripts/plot_json_objects_scenario.py
@@ -316,7 +316,8 @@ if __name__ == "__main__":
     MINY = scenario["init"][1]
     MAXX = scenario["init"][0]
     MAXY = scenario["init"][1]
-    if "obst" in scenario and  len(scenario["obst"]) > 0:
+    if ("obst" in scenario and isinstance(scenario["obst"], list)
+            and len(scenario["obst"]) > 0):
         for o in scenario["obst"]:
             if not o:
                 continue
@@ -542,7 +543,8 @@ if __name__ == "__main__":
             lw=0,
         )
     # Plot obstacles, slot.
-    if "obst" in scenario and len(scenario["obst"]) > 0:
+    if ("obst" in scenario and isinstance(scenario["obst"], list)
+            and len(scenario["obst"]) > 0):
         for o in scenario["obst"]:
             if not o:
                 continue
@@ -726,7 +728,17 @@ if __name__ == "__main__":
     if ("path" in scenario and isinstance(scenario["path"], list)
             and len(scenario["path"]) > 0):
         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=".")
+            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)
@@ -734,6 +746,31 @@ if __name__ == "__main__":
             #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)
+    if ("traj" in scenario and isinstance(scenario["traj"], list)
+            and len(scenario["traj"]) > 0):
+        print(len(scenario["traj"]))
+        # print(scenario["traj"])
+        print(len(plot_nodes(scenario["traj"])[0]))
+        # print(plot_nodes(scenario["traj"]))
+        plt.plot(*plot_nodes(scenario["traj"]), color="red")
+        plt.plot(*plot_car(scenario["traj"][-1]), color="red")
+        for p in scenario["traj"]:
+            if len(p) > 4:
+                if p[3] > 0:
+                    plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker="+")
+                elif p[3] < 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=".")
+            else:
+                plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker=".")
+            #plt.plot(*plot_car(p), color="red")
+            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)
     if "ispath" in scenario and len(scenario["ispath"]) > 0:
         plt.plot(*plot_nodes(scenario["ispath"]), color="green")
         for p in scenario["ispath"]: