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)
37 "trajectory-frame": "lightblue",
39 "log": ("gold", "orange", "blueviolet", "blue", "navy", "black"),
44 """Return `xcoords`, `ycoords` arrays of car frame.
47 pose -- The pose of a car.
49 dr = (LENGTH - WHEEL_BASE) / 2
53 lfx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
54 lfx += df * cos(pose[2])
55 lfx += SAFETY_DIST * cos(pose[2])
58 lrx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
59 lrx += -dr * cos(pose[2])
60 lrx += -SAFETY_DIST * cos(pose[2])
63 rrx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
64 rrx += -dr * cos(pose[2])
65 rrx += -SAFETY_DIST * cos(pose[2])
68 rfx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
69 rfx += df * cos(pose[2])
70 rfx += SAFETY_DIST * cos(pose[2])
73 lfy += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
74 lfy += df * sin(pose[2])
75 lfy += SAFETY_DIST * sin(pose[2])
78 lry += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
79 lry += -dr * sin(pose[2])
80 lry += -SAFETY_DIST * sin(pose[2])
83 rry += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
84 rry += -dr * sin(pose[2])
85 rry += -SAFETY_DIST * sin(pose[2])
88 rfy += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
89 rfy += df * sin(pose[2])
90 rfy += SAFETY_DIST * sin(pose[2])
92 xcoords = (lfx, lrx, rrx, rfx)
93 ycoords = (lfy, lry, rry, rfy)
94 return (xcoords, ycoords)
96 def load_scenario(fname):
97 """Load scenario from file."""
99 raise ValueError("File name as argument needed")
100 with open(fname, "r") as f:
101 scenario = loads(f.read())
104 def load_trajectory(fname):
105 """Load trajectory from file."""
107 raise ValueError("File name as argument needed")
108 with open(fname, "r") as f:
109 trajectory = loads(f.read())
112 def plot_nodes(nodes=[]):
113 """Return xcoords, ycoords of nodes to plot.
116 nodes -- The list of nodes to plot.
123 return (xcoords, ycoords)
125 def plot_segments(segments=[]):
126 """Return xcoords, ycoords of segments to plot.
129 segments -- The list of segments to plot.
133 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["slot"])
224 plt.plot(*car_frame(s["init"]), color="red", lw=2)
225 plt.plot(*car_frame(s["goal"]), color="red", lw=2)
226 plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
227 plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
230 [-2.7, 3.25, 3.25, -2.7],
232 [26, 26, 26.5, 26.5],
233 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
235 [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
236 [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
237 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
239 [3.25, 3.25, 2.75, 2.75, 3.25],
240 [-0.5, 26.5, 26.5, -0.5, -0.5],
241 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
243 #plt.text(1, 0.2, s="1", color="red")
244 #plt.text(2, 12.2, s="2", color="red")
245 #plt.text(3.6, 10, s="3", color="black")
246 #plt.text(-0.5, 14.3, s="4", color="black")
247 #plt.text(3, 18, s="5", color="blue")
248 #plt.text(-4.7, 0.8, s="6", color="orange")
249 #plt.text(-2, 11, s="7", color="gray")
251 handles, labels = ax.get_legend_handles_labels()
253 if not "edge" in t.keys():
261 ax = fig.add_subplot(122)
262 ax.set_aspect("equal")
263 ax.set_title("All edges")
264 ax.set_xlabel("x [m]")
265 ax.set_ylabel("y [m]")
269 plt.plot(*plot_nodes(o["segment"]), color="black")
273 ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
275 color="black", fill=False, hatch="//"))
280 plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
281 marker=".", linestyle = "None")
286 for edges in t["edge"]:
288 plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
289 color=COLOR["edge"], lw=1)
296 plt.plot(*car_frame(i), color=COLOR["sample"])
298 plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
299 marker=".", linestyle = "None")
301 print("No RRTSample")
304 for path in range(len(t["path"])):
305 plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
310 for traj in range(len(t["traj"])):
312 for i in t["traj"][traj]:
313 plt.plot(*car_frame(i), color=COLOR["log"][traj],
314 label=t["cost"][traj])
318 *plot_nodes(t["traj"][traj]),
319 color=COLOR["log"][traj],
320 label=t["cost"][traj])
323 *plot_nodes(t["traj"][traj]),
325 label=t["cost"][traj])
327 print("No trajectory")
331 plt.plot(*plot_nodes(s["slot"]["polygon"]), COLOR["slot"])
335 plt.plot(*car_frame(s["init"]), color="red", lw=2)
336 plt.plot(*car_frame(s["goal"]), color="red", lw=2)
337 plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
338 plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
341 [-2.7, 3.25, 3.25, -2.7],
343 [26, 26, 26.5, 26.5],
344 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
346 [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
347 [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
348 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
350 [3.25, 3.25, 2.75, 2.75, 3.25],
351 [-0.5, 26.5, 26.5, -0.5, -0.5],
352 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
354 handles, labels = ax.get_legend_handles_labels()