1 """Plot JSON formatted scenario."""
3 from math import cos, pi, sin
4 from matplotlib import pyplot as plt
5 from sys import argv, exit
15 def get_scenario(fname):
16 """Load scenario from file."""
18 raise ValueError("File name as argument needed")
19 with open(fname, "r") as f:
20 scenario = loads(f.read())
23 def plot_nodes(nodes=[]):
24 """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
27 nodes -- The list of nodes to plot.
34 return (xcoords, ycoords)
37 """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
40 pose -- The pose of a car.
43 lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
44 lfx += BCAR_DF * cos(pose[2])
45 lfx += BCAR_SD * cos(pose[2])
48 lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
49 lrx += -BCAR_DR * cos(pose[2])
50 lrx += -BCAR_SD * cos(pose[2])
53 rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
54 rrx += -BCAR_DR * cos(pose[2])
55 rrx += -BCAR_SD * cos(pose[2])
58 rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
59 rfx += BCAR_DF * cos(pose[2])
60 rfx += BCAR_SD * cos(pose[2])
63 lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
64 lfy += BCAR_DF * sin(pose[2])
65 lfy += BCAR_SD * sin(pose[2])
68 lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
69 lry += -BCAR_DR * sin(pose[2])
70 lry += -BCAR_SD * sin(pose[2])
73 rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
74 rry += -BCAR_DR * sin(pose[2])
75 rry += -BCAR_SD * sin(pose[2])
78 rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
79 rfy += BCAR_DF * sin(pose[2])
80 rfy += BCAR_SD * sin(pose[2])
82 xcoords = (lfx, lrx, rrx, rfx)
83 ycoords = (lfy, lry, rry, rfy)
84 return (xcoords, ycoords)
86 if __name__ == "__main__":
92 scenario = get_scenario(SCEN_FILE)
94 plt.rcParams["font.size"] = 24
95 plt.rcParams['hatch.linewidth'] = 1.0
99 ax = fig.add_subplot(111)
100 ax.set_aspect("equal")
101 ax.set_title("Scenario")
102 ax.set_xlabel("x [m]")
103 ax.set_ylabel("y [m]")
105 if "obst" in scenario and len(scenario["obst"]) > 0:
106 for o in scenario["obst"]:
107 plt.plot(*plot_nodes(o), color="blue")
108 if "path" in scenario and len(scenario["path"]) > 0:
109 for n in scenario["path"]:
110 plt.plot(*plot_car(n), color="orange")
111 if "slot" in scenario and len(scenario["slot"]) > 0:
112 for s in scenario["slot"]:
113 plt.plot(*plot_nodes(s), color="gray")
114 if "init" in scenario and len(scenario["init"]) == 3:
115 plt.plot(*plot_car(scenario["init"]), color="red")
116 if "goal" in scenario and len(scenario["goal"]) == 3:
117 plt.plot(*plot_car(scenario["goal"]), color="green")
118 print("possible starts:")
119 if "starts" in scenario and len(scenario["starts"]) > 0:
120 for p in scenario["starts"]:
121 plt.plot(*p, color="red", marker="+", ms=12)
122 print(" {}".format(p))
124 handles, labels = ax.get_legend_handles_labels()