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__":
133 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"])
225 plt.plot(*car_frame(t["init"]), color="red", lw=2)
226 plt.plot(*plot_nodes([t["init"]]), color="red", marker="+", ms=12)
228 plt.plot(*car_frame(s["init"]), color="red", lw=2)
229 plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
231 plt.plot(*plot_nodes([t["goal"]]), color="red", marker="+", ms=12)
232 plt.plot(*car_frame(t["goal"]), color="red", lw=2)
234 plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
235 plt.plot(*car_frame(s["goal"]), color="red", lw=2)
237 plt.plot(*car_frame(t["midd"]), color="red", lw=2)
238 plt.plot(*plot_nodes([t["midd"]]), color="red", marker="+", ms=12)
243 # [-2.7, 3.25, 3.25, -2.7],
244 # [-0.5, -0.5, 0, 0],
245 # [26, 26, 26.5, 26.5],
246 # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
248 # [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
249 # [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
250 # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
252 # [3.25, 3.25, 2.75, 2.75, 3.25],
253 # [-0.5, 26.5, 26.5, -0.5, -0.5],
254 # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
256 #plt.text(1, 0.2, s="1", color="red")
257 #plt.text(2, 12.2, s="2", color="red")
258 #plt.text(3.6, 10, s="3", color="black")
259 #plt.text(-0.5, 14.3, s="4", color="black")
260 #plt.text(3, 18, s="5", color="blue")
261 #plt.text(-4.7, 0.8, s="6", color="orange")
262 #plt.text(-2, 11, s="7", color="gray")
264 handles, labels = ax.get_legend_handles_labels()
266 if not "edge" in t.keys():
274 ax = fig.add_subplot(122)
275 ax.set_aspect("equal")
276 ax.set_title("All edges")
277 ax.set_xlabel("x [m]")
278 ax.set_ylabel("y [m]")
282 plt.plot(*plot_nodes(o["segment"]), color="black")
286 ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
288 color="black", fill=False, hatch="//"))
293 plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
294 marker=".", linestyle = "None")
299 for edges in t["edge"]:
301 plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
302 color=COLOR["edge"], lw=1)
309 plt.plot(*car_frame(i), color=COLOR["sample"])
311 plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
312 marker=".", linestyle = "None")
314 print("No RRTSample")
317 for path in range(len(t["path"])):
318 plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
323 for traj in range(len(t["traj"])):
325 for i in t["traj"][traj]:
326 plt.plot(*car_frame(i), color=COLOR["log"][traj],
327 label=t["cost"][traj])
331 *plot_nodes(t["traj"][traj]),
332 color=COLOR["log"][traj],
333 label=t["cost"][traj])
336 *plot_nodes(t["traj"][traj]),
338 label=t["cost"][traj])
340 print("No trajectory")
344 plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
348 plt.plot(*car_frame(s["init"]), color="red", lw=2)
349 plt.plot(*car_frame(s["goal"]), color="red", lw=2)
350 plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
351 plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
354 # [-2.7, 3.25, 3.25, -2.7],
355 # [-0.5, -0.5, 0, 0],
356 # [26, 26, 26.5, 26.5],
357 # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
359 # [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
360 # [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
361 # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
363 # [3.25, 3.25, 2.75, 2.75, 3.25],
364 # [-0.5, 26.5, 26.5, -0.5, -0.5],
365 # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
367 handles, labels = ax.get_legend_handles_labels()