1 # -*- coding: utf-8 -*-
2 """This scipt loads scenario and result trajectory from files and plots it."""
4 from math import copysign, cos, pi, sin
5 from matplotlib import pyplot as plt
6 from sys import argv, exit
8 sign = lambda x: copysign(1, x)
36 "trajectory-frame": "lightblue",
38 "log": ("gold", "orange", "blueviolet", "blue", "navy", "black"),
42 """Return `xcoords`, `ycoords` arrays of car frame.
45 pose -- The pose of a car.
47 dr = (LENGTH - WHEEL_BASE) / 2
51 lfx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
52 lfx += df * cos(pose[2])
53 lfx += SAFETY_DIST * cos(pose[2])
56 lrx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
57 lrx += -dr * cos(pose[2])
58 lrx += -SAFETY_DIST * cos(pose[2])
61 rrx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
62 rrx += -dr * cos(pose[2])
63 rrx += -SAFETY_DIST * cos(pose[2])
66 rfx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
67 rfx += df * cos(pose[2])
68 rfx += SAFETY_DIST * cos(pose[2])
71 lfy += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
72 lfy += df * sin(pose[2])
73 lfy += SAFETY_DIST * sin(pose[2])
76 lry += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
77 lry += -dr * sin(pose[2])
78 lry += -SAFETY_DIST * sin(pose[2])
81 rry += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
82 rry += -dr * sin(pose[2])
83 rry += -SAFETY_DIST * sin(pose[2])
86 rfy += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
87 rfy += df * sin(pose[2])
88 rfy += SAFETY_DIST * sin(pose[2])
90 xcoords = (lfx, lrx, rrx, rfx)
91 ycoords = (lfy, lry, rry, rfy)
92 return (xcoords, ycoords)
94 def load_scenario(fname):
95 """Load scenario from file."""
97 raise ValueError("File name as argument needed")
98 with open(fname, "r") as f:
99 scenario = loads(f.read())
102 def load_trajectory(fname):
103 """Load trajectory from file."""
105 raise ValueError("File name as argument needed")
106 with open(fname, "r") as f:
107 trajectory = loads(f.read())
110 def plot_nodes(nodes=[]):
111 """Return xcoords, ycoords of nodes to plot.
114 nodes -- The list of nodes to plot.
121 return (xcoords, ycoords)
123 def plot_segments(segments=[]):
124 """Return xcoords, ycoords of segments to plot.
127 segments -- The list of segments to plot.
131 if __name__ == "__main__":
132 s = load_scenario(SCEN_FILE)
134 t = load_trajectory(TRAJ_FILE) # fixed to trajectories
138 plt.rcParams["font.size"] = 24
139 plt.rcParams['hatch.linewidth'] = 1.0
145 if "edge" in t.keys():
146 ax = fig.add_subplot(121)
148 ax = fig.add_subplot(111)
149 ax.set_aspect("equal")
150 ax.set_title("Final path")
151 ax.set_xlabel("x [m]")
152 ax.set_ylabel("y [m]")
156 plt.plot(*plot_nodes(o["segment"]), color="black")
160 ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
162 color="black", fill=False, hatch="//"))
167 plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
168 marker=".", linestyle = "None")
173 for edges in t["edge"]:
175 plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
183 plt.plot(*car_frame(i), color=COLOR["sample"])
185 plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
186 marker=".", linestyle = "None")
188 print("No RRTSample")
191 for path in range(len(t["path"])):
192 plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
197 for traj in range(len(t["traj"])):
199 for i in t["traj"][traj]:
200 plt.plot(*car_frame(i), color=COLOR["log"][traj],
201 label=t["cost"][traj], lw=1)
205 *plot_nodes(t["traj"][traj]),
206 color=COLOR["log"][traj],
207 label=t["cost"][traj])
210 *plot_nodes(t["traj"][traj]),
212 label=t["cost"][traj])
214 print("No trajectory")
216 plt.plot(*car_frame(s["init"]), color="red", lw=2)
217 plt.plot(*car_frame(s["goal"]), color="red", lw=2)
218 plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
219 plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
222 [-2.7, 3.25, 3.25, -2.7],
224 [26, 26, 26.5, 26.5],
225 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
227 [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
228 [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
229 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
231 [3.25, 3.25, 2.75, 2.75, 3.25],
232 [-0.5, 26.5, 26.5, -0.5, -0.5],
233 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
235 #plt.text(1, 0.2, s="1", color="red")
236 #plt.text(2, 12.2, s="2", color="red")
237 #plt.text(3.6, 10, s="3", color="black")
238 #plt.text(-0.5, 14.3, s="4", color="black")
239 #plt.text(3, 18, s="5", color="blue")
240 #plt.text(-4.7, 0.8, s="6", color="orange")
241 #plt.text(-2, 11, s="7", color="gray")
243 handles, labels = ax.get_legend_handles_labels()
245 if not "edge" in t.keys():
253 ax = fig.add_subplot(122)
254 ax.set_aspect("equal")
255 ax.set_title("All edges")
256 ax.set_xlabel("x [m]")
257 ax.set_ylabel("y [m]")
261 plt.plot(*plot_nodes(o["segment"]), color="black")
265 ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
267 color="black", fill=False, hatch="//"))
272 plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
273 marker=".", linestyle = "None")
278 for edges in t["edge"]:
280 plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
281 color=COLOR["edge"], lw=1)
288 plt.plot(*car_frame(i), color=COLOR["sample"])
290 plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
291 marker=".", linestyle = "None")
293 print("No RRTSample")
296 for path in range(len(t["path"])):
297 plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
302 for traj in range(len(t["traj"])):
304 for i in t["traj"][traj]:
305 plt.plot(*car_frame(i), color=COLOR["log"][traj],
306 label=t["cost"][traj])
310 *plot_nodes(t["traj"][traj]),
311 color=COLOR["log"][traj],
312 label=t["cost"][traj])
315 *plot_nodes(t["traj"][traj]),
317 label=t["cost"][traj])
319 print("No trajectory")
321 plt.plot(*car_frame(s["init"]), color="red", lw=2)
322 plt.plot(*car_frame(s["goal"]), color="red", lw=2)
323 plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
324 plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
327 [-2.7, 3.25, 3.25, -2.7],
329 [26, 26, 26.5, 26.5],
330 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
332 [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
333 [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
334 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
336 [3.25, 3.25, 2.75, 2.75, 3.25],
337 [-0.5, 26.5, 26.5, -0.5, -0.5],
338 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
340 handles, labels = ax.get_legend_handles_labels()