Plot traj

diff --git a/scripts/plot_json_objects_scenario.py b/scripts/plot_json_objects_scenario.py
index ee51993600f435cd454040dd728607e59b16a422..804275a016fd79522d6b442fc3250a8f7942c021 100755 (executable)
--- a/scripts/plot_json_objects_scenario.py
+++ b/scripts/plot_json_objects_scenario.py
@@ -746,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"]: