1 """Plot JSON formatted scenario."""
3 from math import cos, pi, sin
5 from matplotlib import pyplot as plt
6 from sys import argv, exit
19 def get_scenario(fname):
20 """Load scenario from file."""
22 raise ValueError("File name as argument needed")
23 with open(fname, "r") as f:
24 scenario = loads(f.read())
27 def plot_nodes(nodes=[]):
28 """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
31 nodes -- The list of nodes to plot.
36 xcoords.append(n[0] - MINX)
37 ycoords.append(n[1] - MINY)
38 return (xcoords, ycoords)
41 """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
44 pose -- The pose of a car.
47 lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
48 lfx += BCAR_DF * cos(pose[2])
49 lfx += BCAR_SD * cos(pose[2])
52 lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
53 lf3x += 2/3 * BCAR_DF * cos(pose[2])
54 lf3x += BCAR_SD * cos(pose[2])
57 lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
58 lrx += -BCAR_DR * cos(pose[2])
59 lrx += -BCAR_SD * cos(pose[2])
62 rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
63 rrx += -BCAR_DR * cos(pose[2])
64 rrx += -BCAR_SD * cos(pose[2])
67 rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
68 rfx += BCAR_DF * cos(pose[2])
69 rfx += BCAR_SD * cos(pose[2])
72 rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
73 rf3x += 2/3 * BCAR_DF * cos(pose[2])
74 rf3x += BCAR_SD * cos(pose[2])
77 lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
78 lfy += BCAR_DF * sin(pose[2])
79 lfy += BCAR_SD * sin(pose[2])
82 lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
83 lf3y += 2/3 * BCAR_DF * sin(pose[2])
84 lf3y += BCAR_SD * sin(pose[2])
87 lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
88 lry += -BCAR_DR * sin(pose[2])
89 lry += -BCAR_SD * sin(pose[2])
92 rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
93 rry += -BCAR_DR * sin(pose[2])
94 rry += -BCAR_SD * sin(pose[2])
97 rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
98 rfy += BCAR_DF * sin(pose[2])
99 rfy += BCAR_SD * sin(pose[2])
102 rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
103 rf3y += 2/3 * BCAR_DF * sin(pose[2])
104 rf3y += BCAR_SD * sin(pose[2])
107 cfx += BCAR_DF * cos(pose[2])
108 cfx += BCAR_SD * cos(pose[2])
111 cfy += BCAR_DF * sin(pose[2])
112 cfy += BCAR_SD * sin(pose[2])
114 xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
115 ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
116 return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
118 def plot_car_corners(pose):
119 """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
122 pose -- The pose of a car.
125 lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
126 lfx += BCAR_DF * cos(pose[2])
127 lfx += BCAR_SD * cos(pose[2])
130 lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
131 lrx += -BCAR_DR * cos(pose[2])
132 lrx += -BCAR_SD * cos(pose[2])
135 rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
136 rrx += -BCAR_DR * cos(pose[2])
137 rrx += -BCAR_SD * cos(pose[2])
140 rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
141 rfx += BCAR_DF * cos(pose[2])
142 rfx += BCAR_SD * cos(pose[2])
145 lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
146 lfy += BCAR_DF * sin(pose[2])
147 lfy += BCAR_SD * sin(pose[2])
150 lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
151 lry += -BCAR_DR * sin(pose[2])
152 lry += -BCAR_SD * sin(pose[2])
155 rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
156 rry += -BCAR_DR * sin(pose[2])
157 rry += -BCAR_SD * sin(pose[2])
160 rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
161 rfy += BCAR_DF * sin(pose[2])
162 rfy += BCAR_SD * sin(pose[2])
164 xcoords = (lfx, lrx, rrx, rfx)
165 ycoords = (lfy, lry, rry, rfy)
166 return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
168 if __name__ == "__main__":
172 elif (len(argv) == 3):
175 sc2 = get_scenario(SCEN_FILE2)
177 SCEN_FILE = "sc.json"
179 scenario = get_scenario(SCEN_FILE)
181 # Font size to be approximately the same in the paper:
185 plt.rcParams["font.family"] = "cmr10"
186 plt.rcParams["font.size"] = 12
187 plt.rcParams['hatch.linewidth'] = 0.5
188 plt.rcParams['lines.linewidth'] = 1.0
191 # here subplot starts
192 ax = fig.add_subplot(111)
193 ax.set_aspect("equal")
194 ax.set_title("Real-world parking scenario")
195 ax.set_xlabel("x [m]")
196 ax.set_ylabel("y [m]")
198 # For Possible Entry Points (Possible Entry Configurations) use:
199 #ax.set_xlim([35.6, 46.4])
200 #ax.set_ylim([2.9, 2.9 + 6.84])
202 # For Last Maneuver use:
203 #ax.set_xlim([38, 44])
204 #ax.set_ylim([3.1, 6.9])
206 # For Scenario 3-2 detail use:
207 #ax.set_xlim([0, 15])
208 #ax.set_ylim([35, 50])
210 # For Real-world parking scenario in Introduction section use:
211 #ax.set_xlim([32, 47.5])
212 #ax.set_ylim([2.4, 9.9])
214 # Set min and max to center the plot.
215 MINX = scenario["init"][0]
216 MINY = scenario["init"][1]
217 if "obst" in scenario and len(scenario["obst"]) > 0:
218 for o in scenario["obst"]:
227 # Plot all the nodes (if exists.)
228 if "nodes_x" in scenario and "nodes_y" in scenario:
230 [x - MINX for x in scenario["nodes_x"]],
231 [y - MINY for y in scenario["nodes_y"]],
237 # Plot all the steered2 nodes (if exists.)
238 if "steered2_x" in scenario and "steered2_y" in scenario:
240 [x - MINX for x in scenario["steered2_x"]],
241 [y - MINY for y in scenario["steered2_y"]],
247 # Plot all the steered1 nodes (if exists.)
248 if "steered1_x" in scenario and "steered1_y" in scenario:
250 [x - MINX for x in scenario["steered1_x"]],
251 [y - MINY for y in scenario["steered1_y"]],
257 # Plot obstacles, slot.
258 if "obst" in scenario and len(scenario["obst"]) > 0:
259 for o in scenario["obst"]:
262 ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//")
263 if "slot" in scenario and len(scenario["slot"]) > 0:
264 plt.plot(*plot_nodes(scenario["slot"][0]), color="blue", linewidth=2)
265 #for s in scenario["slot"]:
266 # plt.plot(*plot_nodes(s), color="black")
268 # Plot `init`, `entry`, and `goal` configurations.
269 if "init" in scenario and len(scenario["init"]) == 3:
270 plt.plot(*plot_car(scenario["init"]), color="red")
272 scenario["init"][0] - MINX,
273 scenario["init"][1] - MINY,
278 if "entries" in scenario:
279 for e in scenario["entries"]:
280 plt.plot(*plot_car(e), color="orange")
288 if "entry" in scenario and len(scenario["entry"]) == 3:
289 plt.plot(*plot_car(scenario["entry"]), color="magenta")
291 scenario["entry"][0] - MINX,
292 scenario["entry"][1] - MINY,
297 if "entry" in scenario and len(scenario["entry"]) == 4:
298 esc = scenario["entry"]
299 plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
300 plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
302 scenario["entry"][0] - MINX,
303 scenario["entry"][1] - MINY,
308 if "goal" in scenario and len(scenario["goal"]) == 3:
309 plt.plot(*plot_car(scenario["goal"]), color="green")
311 scenario["goal"][0] - MINX,
312 scenario["goal"][1] - MINY,
318 # If there are possible starts specified, you may print and plot them.
319 #if "starts" in scenario and len(scenario["starts"]) > 0:
320 # print("possible starts:")
321 # for p in scenario["starts"]:
322 # plt.plot(*p, color="red", marker="+", ms=12)
323 # print(" {}".format(p))
325 # For the Last Maneuver figure from the paper, use:
326 # - `init2` -- orange
327 #plt.plot(*plot_car(scenario["init2"]), color="orange")
329 # scenario["init2"][0] - MINX,
330 # scenario["init2"][1] - MINY,
335 # - `goal2` -- orange
336 #plt.plot(*plot_car(scenario["goal2"]), color="orange")
338 # scenario["goal2"][0] - MINX,
339 # scenario["goal2"][1] - MINY,
344 # - `goal2` -- middle (orange)
345 #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
347 # scenario["goal2"][0] - MINX,
348 # scenario["goal2"][1] - MINY,
354 #plt.plot(*plot_car(scenario["init1"]), color="green")
356 # scenario["init1"][0] - MINX,
357 # scenario["init1"][1] - MINY,
363 #plt.plot(*plot_car(scenario["goal1"]), color="green")
365 # scenario["goal1"][0] - MINX,
366 # scenario["goal1"][1] - MINY,
372 # For the Possible Entry Configurations from the paper, use:
373 #if "inits" in scenario:
374 # #for i in scenario["inits"]:
375 # for j in range(0, len(scenario["inits"]), 110):
376 # i = scenario["inits"][j]
379 # plt.plot(*plot_car(i), color="gray")
387 # #plt.plot(*plot_car(scenario["inits"][0]), color="magenta")
389 # # scenario["inits"][0][0] - MINX,
390 # # scenario["inits"][0][1] - MINY,
396 # Plot `path` and `max_path`.
397 if sc2 and "path" in sc2 and len(sc2["path"]) > 0:
398 plt.plot(*plot_nodes(sc2["path"]), color="orange")
399 if "path" in scenario and len(scenario["path"]) > 0:
400 plt.plot(*plot_nodes(scenario["path"]), color="green")
402 # The `scenario` may also include:
403 # - `last` -- not sure what this is, see the source code. Maybe overlaps
405 # - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
407 # - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
409 # - `max_orig_path` -- maximum original path. I used this when comparing
410 # original paths but I had to copy the `max_orig_path` by hand from
411 # different scenario result.
412 # - `orig_path` -- the path before the optimization.
413 # - `max_path` -- the maximum path after optimization. Must be copied by
415 # - `path` -- optimized path of the scenario.
417 handles, labels = ax.get_legend_handles_labels()
419 # Uncommnent the following line and comment the plt.show() to store to the
421 plt.savefig("out.pdf", bbox_inches="tight")