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)
34 "trajectory-frame": "lightblue",
36 "log": ("gold", "orange", "blueviolet", "blue", "navy", "black"),
41 """Return `xcoords`, `ycoords` arrays of car frame.
44 pose -- The pose of a car.
46 dr = (LENGTH - WHEEL_BASE) / 2
50 lfx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
51 lfx += df * cos(pose[2])
52 lfx += SAFETY_DIST * cos(pose[2])
55 lrx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
56 lrx += -dr * cos(pose[2])
57 lrx += -SAFETY_DIST * cos(pose[2])
60 rrx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
61 rrx += -dr * cos(pose[2])
62 rrx += -SAFETY_DIST * cos(pose[2])
65 rfx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
66 rfx += df * cos(pose[2])
67 rfx += SAFETY_DIST * cos(pose[2])
70 lfy += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
71 lfy += df * sin(pose[2])
72 lfy += SAFETY_DIST * sin(pose[2])
75 lry += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
76 lry += -dr * sin(pose[2])
77 lry += -SAFETY_DIST * sin(pose[2])
80 rry += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
81 rry += -dr * sin(pose[2])
82 rry += -SAFETY_DIST * sin(pose[2])
85 rfy += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
86 rfy += df * sin(pose[2])
87 rfy += SAFETY_DIST * sin(pose[2])
89 xcoords = (lfx, lrx, rrx, rfx)
90 ycoords = (lfy, lry, rry, rfy)
91 return (xcoords, ycoords)
93 def load_scenario(fname):
94 """Load scenario from file."""
96 raise ValueError("File name as argument needed")
97 with open(fname, "r") as f:
98 scenario = loads(f.read())
101 def load_trajectory(fname):
102 """Load trajectory from file."""
104 raise ValueError("File name as argument needed")
105 with open(fname, "r") as f:
106 trajectory = loads(f.read())
109 def plot_nodes(nodes=[]):
110 """Return xcoords, ycoords of nodes to plot.
113 nodes -- The list of nodes to plot.
120 return (xcoords, ycoords)
122 def plot_segments(segments=[]):
123 """Return xcoords, ycoords of segments to plot.
126 segments -- The list of segments to plot.
130 if __name__ == "__main__":
134 s = load_scenario(SCEN_FILE)
136 t = load_trajectory(TRAJ_FILE) # fixed to trajectories
140 plt.rcParams["font.size"] = 24
141 plt.rcParams['hatch.linewidth'] = 1.0
147 if "edge" in t.keys():
148 ax = fig.add_subplot(121)
150 ax = fig.add_subplot(111)
151 ax.set_aspect("equal")
152 ax.set_title("Final path")
153 ax.set_xlabel("x [m]")
154 ax.set_ylabel("y [m]")
158 plt.plot(*plot_nodes(o["segment"]), color="black")
162 ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
164 color="black", fill=False, hatch="//"))
169 plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
170 marker=".", linestyle = "None")
175 for edges in t["edge"]:
177 plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
185 plt.plot(*car_frame(i), color=COLOR["sample"])
187 plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
188 marker=".", linestyle = "None")
190 print("No RRTSample")
193 for path in range(len(t["path"])):
194 plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
199 for traj in range(len(t["traj"])):
201 for i in t["traj"][traj]:
202 plt.plot(*car_frame(i), color=COLOR["log"][traj],
203 label=t["cost"][traj], lw=1)
207 *plot_nodes(t["traj"][traj]),
208 color=COLOR["log"][traj],
209 label=t["cost"][traj])
212 *plot_nodes(t["traj"][traj]),
214 label=t["cost"][traj])
216 print("No trajectory")
220 plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
224 plt.plot(*car_frame(s["init"]), color="red", lw=2)
225 plt.plot(*car_frame(t["goal"]), color="red", lw=2)
226 plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
227 plt.plot(*plot_nodes([t["goal"]]), color="red", marker="+", ms=12)
229 plt.plot(*car_frame(t["midd"]), color="red", lw=2)
230 plt.plot(*plot_nodes([t["midd"]]), color="red", marker="+", ms=12)
235 [-2.7, 3.25, 3.25, -2.7],
237 [26, 26, 26.5, 26.5],
238 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
240 [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
241 [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
242 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
244 [3.25, 3.25, 2.75, 2.75, 3.25],
245 [-0.5, 26.5, 26.5, -0.5, -0.5],
246 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
248 #plt.text(1, 0.2, s="1", color="red")
249 #plt.text(2, 12.2, s="2", color="red")
250 #plt.text(3.6, 10, s="3", color="black")
251 #plt.text(-0.5, 14.3, s="4", color="black")
252 #plt.text(3, 18, s="5", color="blue")
253 #plt.text(-4.7, 0.8, s="6", color="orange")
254 #plt.text(-2, 11, s="7", color="gray")
256 handles, labels = ax.get_legend_handles_labels()
258 if not "edge" in t.keys():
266 ax = fig.add_subplot(122)
267 ax.set_aspect("equal")
268 ax.set_title("All edges")
269 ax.set_xlabel("x [m]")
270 ax.set_ylabel("y [m]")
274 plt.plot(*plot_nodes(o["segment"]), color="black")
278 ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
280 color="black", fill=False, hatch="//"))
285 plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
286 marker=".", linestyle = "None")
291 for edges in t["edge"]:
293 plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
294 color=COLOR["edge"], lw=1)
301 plt.plot(*car_frame(i), color=COLOR["sample"])
303 plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
304 marker=".", linestyle = "None")
306 print("No RRTSample")
309 for path in range(len(t["path"])):
310 plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
315 for traj in range(len(t["traj"])):
317 for i in t["traj"][traj]:
318 plt.plot(*car_frame(i), color=COLOR["log"][traj],
319 label=t["cost"][traj])
323 *plot_nodes(t["traj"][traj]),
324 color=COLOR["log"][traj],
325 label=t["cost"][traj])
328 *plot_nodes(t["traj"][traj]),
330 label=t["cost"][traj])
332 print("No trajectory")
336 plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
340 plt.plot(*car_frame(s["init"]), color="red", lw=2)
341 plt.plot(*car_frame(s["goal"]), color="red", lw=2)
342 plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
343 plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
346 [-2.7, 3.25, 3.25, -2.7],
348 [26, 26, 26.5, 26.5],
349 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
351 [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
352 [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
353 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
355 [3.25, 3.25, 2.75, 2.75, 3.25],
356 [-0.5, 26.5, 26.5, -0.5, -0.5],
357 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
359 handles, labels = ax.get_legend_handles_labels()