#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"]: