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"]))