WHEEL_BASE = 2.450
WIDTH = 1.625
-SCEN_FILE = argv[1]
-TRAJ_FILE = argv[2]
-
PLOT = {
"enable" : True,
"node" : False,
pass
if __name__ == "__main__":
+ SCEN_FILE = argv[1]
+
s = load_scenario(SCEN_FILE)
try:
+ TRAJ_FILE = argv[2]
t = load_trajectory(TRAJ_FILE) # fixed to trajectories
except:
- pass
+ t = {"elap": 0}
plt.rcParams["font.size"] = 24
plt.rcParams['hatch.linewidth'] = 1.0
if PLOT["slot"]:
try:
- plt.plot(*plot_nodes(s["slot"]["polygon"]), COLOR["slot"])
+ plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
except:
print("No slot")
- plt.plot(*car_frame(s["init"]), color="red", lw=2)
- plt.plot(*car_frame(s["goal"]), color="red", lw=2)
- plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
- plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
+ try: # init
+ plt.plot(*car_frame(t["init"]), color="red", lw=2)
+ plt.plot(*plot_nodes([t["init"]]), color="red", marker="+", ms=12)
+ except:
+ plt.plot(*car_frame(s["init"]), color="red", lw=2)
+ plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
+ try: # goal
+ plt.plot(*plot_nodes([t["goal"]]), color="red", marker="+", ms=12)
+ plt.plot(*car_frame(t["goal"]), color="red", lw=2)
+ except:
+ plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
+ plt.plot(*car_frame(s["goal"]), color="red", lw=2)
+ try: # middle
+ plt.plot(*car_frame(t["midd"]), color="red", lw=2)
+ plt.plot(*plot_nodes([t["midd"]]), color="red", marker="+", ms=12)
+ except:
+ pass
- plt.fill_between(
- [-2.7, 3.25, 3.25, -2.7],
- [-0.5, -0.5, 0, 0],
- [26, 26, 26.5, 26.5],
- facecolor="none", hatch="//", edgecolor="black", linewidth=0)
- plt.fill_between(
- [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
- [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
- facecolor="none", hatch="//", edgecolor="black", linewidth=0)
- plt.fill_between(
- [3.25, 3.25, 2.75, 2.75, 3.25],
- [-0.5, 26.5, 26.5, -0.5, -0.5],
- facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [-2.7, 3.25, 3.25, -2.7],
+ # [-0.5, -0.5, 0, 0],
+ # [26, 26, 26.5, 26.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
+ # [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [3.25, 3.25, 2.75, 2.75, 3.25],
+ # [-0.5, 26.5, 26.5, -0.5, -0.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
#plt.text(1, 0.2, s="1", color="red")
#plt.text(2, 12.2, s="2", color="red")
if PLOT["slot"]:
try:
- plt.plot(*plot_nodes(s["slot"]["polygon"]), COLOR["slot"])
+ plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
except:
print("No slot")
plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
- plt.fill_between(
- [-2.7, 3.25, 3.25, -2.7],
- [-0.5, -0.5, 0, 0],
- [26, 26, 26.5, 26.5],
- facecolor="none", hatch="//", edgecolor="black", linewidth=0)
- plt.fill_between(
- [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
- [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
- facecolor="none", hatch="//", edgecolor="black", linewidth=0)
- plt.fill_between(
- [3.25, 3.25, 2.75, 2.75, 3.25],
- [-0.5, 26.5, 26.5, -0.5, -0.5],
- facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [-2.7, 3.25, 3.25, -2.7],
+ # [-0.5, -0.5, 0, 0],
+ # [26, 26, 26.5, 26.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
+ # [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [3.25, 3.25, 2.75, 2.75, 3.25],
+ # [-0.5, 26.5, 26.5, -0.5, -0.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
handles, labels = ax.get_legend_handles_labels()