2 """Plot JSON formatted scenario."""
4 from math import cos, pi, sin, atan, atan2
6 from matplotlib import pyplot as plt
7 from sys import argv, exit
22 #BCAR_DR = 5.171 - 4.236
26 #BCAR_DR = 3.622 - BCAR_DF
35 def get_scenario(fname):
36 """Load scenario from file."""
38 raise ValueError("File name as argument needed")
39 with open(fname, "r") as f:
40 scenario = loads(f.read())
43 def plot_nodes(nodes=[]):
44 """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
47 nodes -- The list of nodes to plot.
52 xcoords.append(n[0] - MINX)
53 ycoords.append(n[1] - MINY)
54 return (xcoords, ycoords)
57 """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
60 pose -- The pose of a car.
63 lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
64 lfx += BCAR_DF * cos(pose[2])
65 lfx += BCAR_SD * cos(pose[2])
68 lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
69 lf3x += 2/3 * BCAR_DF * cos(pose[2])
70 lf3x += BCAR_SD * cos(pose[2])
73 lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
74 lrx += -BCAR_DR * cos(pose[2])
75 lrx += -BCAR_SD * cos(pose[2])
78 rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
79 rrx += -BCAR_DR * cos(pose[2])
80 rrx += -BCAR_SD * cos(pose[2])
83 rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
84 rfx += BCAR_DF * cos(pose[2])
85 rfx += BCAR_SD * cos(pose[2])
88 rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
89 rf3x += 2/3 * BCAR_DF * cos(pose[2])
90 rf3x += BCAR_SD * cos(pose[2])
93 lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
94 lfy += BCAR_DF * sin(pose[2])
95 lfy += BCAR_SD * sin(pose[2])
98 lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
99 lf3y += 2/3 * BCAR_DF * sin(pose[2])
100 lf3y += BCAR_SD * sin(pose[2])
103 lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
104 lry += -BCAR_DR * sin(pose[2])
105 lry += -BCAR_SD * sin(pose[2])
108 rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
109 rry += -BCAR_DR * sin(pose[2])
110 rry += -BCAR_SD * sin(pose[2])
113 rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
114 rfy += BCAR_DF * sin(pose[2])
115 rfy += BCAR_SD * sin(pose[2])
118 rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
119 rf3y += 2/3 * BCAR_DF * sin(pose[2])
120 rf3y += BCAR_SD * sin(pose[2])
123 cfx += BCAR_DF * cos(pose[2])
124 cfx += BCAR_SD * cos(pose[2])
127 cfy += BCAR_DF * sin(pose[2])
128 cfy += BCAR_SD * sin(pose[2])
130 xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
131 ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
132 return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
134 def plot_car_corners(pose):
135 """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
138 pose -- The pose of a car.
141 lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
142 lfx += BCAR_DF * cos(pose[2])
143 lfx += BCAR_SD * cos(pose[2])
146 lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
147 lrx += -BCAR_DR * cos(pose[2])
148 lrx += -BCAR_SD * cos(pose[2])
151 rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
152 rrx += -BCAR_DR * cos(pose[2])
153 rrx += -BCAR_SD * cos(pose[2])
156 rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
157 rfx += BCAR_DF * cos(pose[2])
158 rfx += BCAR_SD * cos(pose[2])
161 lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
162 lfy += BCAR_DF * sin(pose[2])
163 lfy += BCAR_SD * sin(pose[2])
166 lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
167 lry += -BCAR_DR * sin(pose[2])
168 lry += -BCAR_SD * sin(pose[2])
171 rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
172 rry += -BCAR_DR * sin(pose[2])
173 rry += -BCAR_SD * sin(pose[2])
176 rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
177 rfy += BCAR_DF * sin(pose[2])
178 rfy += BCAR_SD * sin(pose[2])
180 xcoords = (lfx, lrx, rrx, rfx)
181 ycoords = (lfy, lry, rry, rfy)
182 return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
184 if __name__ == "__main__":
188 elif (len(argv) == 3):
191 sc2 = get_scenario(SCEN_FILE2)
193 SCEN_FILE = "sc.json"
195 scenario = get_scenario(SCEN_FILE)
197 # Font size to be approximately the same in the paper:
201 plt.rc('axes', unicode_minus=False)
202 plt.rcParams["figure.figsize"] = [14, 7]
203 plt.rcParams["font.family"] = "cmr10"
204 plt.rcParams["font.size"] = 24
205 plt.rcParams['hatch.linewidth'] = 6.0
206 plt.rcParams['lines.linewidth'] = 2.0
209 # here subplot starts
210 ax = fig.add_subplot(111)
211 ax.set_aspect("equal")
212 #ax.set_title("Real-world parking scenario")
213 ax.set_xlabel("x [m]")
214 ax.set_ylabel("y [m]")
215 # For stage, comment upper, uncomment following:
220 #ax.set_xlim([4, 32]) # 28
221 #ax.set_ylim([7, 29]) # 22
223 #ax.set_xlim([-1, 27]) # 28
224 #ax.set_ylim([-1, 21]) # 22
226 #ax.set_xlim([-9.9, 18.1]) # 28
227 #ax.set_ylim([-4.9, 17.1]) # 22
229 #ax.set_xlim([4, 32]) # 28
230 #ax.set_ylim([-4, 18]) # 22
232 #ax.set_xlim([-7, 21]) # 28
233 #ax.set_ylim([7, 29]) # 22
235 #ax.set_xlim([-7, 21]) # 28
236 #ax.set_ylim([2, 24]) # 22
238 #ax.set_xlim([-1, 27]) # 28
239 #ax.set_ylim([-1, 21]) # 22
241 # For Possible Entry Points (Possible Entry Configurations) use:
242 #plt.rcParams["font.size"] = 26
243 #ax.set_xlim([37.6, 45.6])
244 #ax.set_ylim([2.4, 8.5])
245 #ax.set_title("Possible configurations")
247 # For Last Maneuver use:
248 #ax.set_xlim([38, 44])
249 #ax.set_ylim([3.1, 6.9])
251 # For Scenario 3-2 detail use:
253 #ax.set_title("Scenario 1")
254 #ax.set_xlim([1, 16]) # w=15
255 #ax.set_ylim([35.1, 49.9]) # h=15
257 # For Scenario 4-0 detail use:
259 #ax.set_title("Scenario 2")
260 #ax.set_xlim([32, 53]) # w=19
261 #ax.set_ylim([0.5, 19.5]) # h=18
263 # For Scenario 4-1 detail use:
265 #ax.set_title("Scenario 3")
266 #ax.set_xlim([32.5, 48.5]) # w=16
267 #ax.set_ylim([1, 16]) # h=15
269 # For Scenario 5-1 detail use:
271 #ax.set_title("Scenario 4")
272 #ax.set_xlim([10.1, 27])
273 #ax.set_ylim([10.1, 26])
275 # For Scenario 4-1-{0,14} detail use:
277 #ax.set_title("Scenario 5") # Scenario 6
278 #ax.set_xlim([32.5, 48.5]) # w=16
279 #ax.set_ylim([1, 16.3]) # h=15.3
281 # For Scenario 5-3-34 detail use:
283 #ax.set_title("Scenario 7") # Scenario 8
284 #ax.set_xlim([5.5, 27.5])
285 #ax.set_ylim([15.1, 35])
287 # For Scenario 5-3-29 detail use:
289 #ax.set_title("Scenario 8")
290 #ax.set_xlim([5.5, 27.5])
291 #ax.set_ylim([10.1, 30])
293 # For Real-world parking scenario in Introduction section use:
294 #ax.set_title("Real-world parking scenario with artificial obstacle")
295 #ax.set_xlim([23, 58.5])
296 #ax.set_ylim([-8.4, 20.9])
297 #ax.text(34, 9.2, "Initial configuration", color="red")
298 #ax.text(43.5, 8.5, "Final path", color="blue")
299 #ax.text(48.25, 5.5, "Entry\nconfigurations", color="orange", ha="right")
300 #ax.text(38, 3.8, "Parking\nslot", color="blue", ha="right", backgroundcolor="white")
301 #ax.text(35.2, 5.5, "Goal configuration", color="green")
304 #ax.set_title("Computed goal")
305 #ax.set_xlim([6.8, 16.2])
306 #ax.set_ylim([15, 20])
308 # For simple scenarios 92 and 96 (simple-1k-test49)
309 # - Use MINY=-25, MINX=-10 for 96.
310 #ax.set_title("Simple parking scenario")
311 #ax.set_xlim([-11, 24]) # w=35
312 #ax.set_ylim([-2.5, 37.5]) # h=40
314 # Set min and max to center the plot.
315 MINX = scenario["init"][0]
316 MINY = scenario["init"][1]
317 MAXX = scenario["init"][0]
318 MAXY = scenario["init"][1]
319 if ("obst" in scenario and isinstance(scenario["obst"], list)
320 and len(scenario["obst"]) > 0):
321 for o in scenario["obst"]:
333 print("w: {}, h: {}".format(abs(MAXX - MINX), abs(MAXY - MINY)))
337 (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
344 (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
351 (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
358 (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
365 # For Goal Zone figure, "Goal zone" file name in j1/figs/
368 ax.set_ylim([-4.8, 2.8])
369 ax.set_xlim([-13, 5])
370 gz_ccr = matplotlib.patches.Arc(
371 (-744206.185356 - MINX, -1044330.294266 - MINY),
372 5.207071 * 2, 5.207071 * 2,
373 theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
374 theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
380 gz_ccr = matplotlib.patches.Arc(
381 (-744206.185356 - MINX + 3.99, -1044330.294266 - MINY + 2.05),
382 5.207071 * 2, 5.207071 * 2,
383 theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
384 theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
390 gz_gh = 0.47424360277825361
391 gz_ih = -0.27424360277825361
392 def li(x, y, h, le=10.0):
393 return (x, x + le * cos(h)), (y, y + le * sin(h))
395 plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_gh),
396 color="orange", ls="dotted")
397 plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_ih),
398 color="red", ls="dotted")
401 *li(-744208.5632466434 - MINX, -1044325.6618554679 - MINY, gz_gh, 4.47),
402 color="orange", ls="solid")
404 *li(-744199.2632466434 - MINX, -1044323.6618554679 - MINY, gz_ih, -1.55),
405 color="red", ls="solid")
407 -744208.5632466434 - MINX,
408 -1044325.6618554679 - MINY - 1.5,
415 -744208.5632466434 - MINX + 0.35,
416 -1044325.6618554679 - MINY - 1.7,
424 -744199.2632466434 - MINX,
425 -1044323.6618554679 - MINY - 1.5,
432 -744199.2632466434 - MINX + 0.35,
433 -1044323.6618554679 - MINY - 1.7,
441 -744199.2632466434 - MINX,
442 -1044323.6618554679 - MINY - 3.9,
449 -744199.2632466434 - MINX + 0.35,
450 -1044323.6618554679 - MINY - 4.1,
458 -744199.2632466434 - MINX,
459 -1044323.6618554679 - MINY - 3.18,
467 -744199.2632466434 - MINX,
468 -1044323.6618554679 - MINY + 1.9,
475 -744199.2632466434 - MINX + 0.35,
476 -1044323.6618554679 - MINY + 1.7,
484 -744199.2632466434 - MINX,
485 -1044323.6618554679 - MINY + 1.22,
493 -744199.2632466434 - MINX + 2,
494 -1044323.6618554679 - MINY + -3,
500 backgroundcolor="white",
503 -MINX -744204.775115,
504 -MINX -744204.775115 + 15 * cos(0.47424360277825361),
505 -MINX -744204.775115 + 15 * cos(0.27424360277825361),
506 -MINX -744204.775115,
508 -MINY -1044325.281765,
509 -MINY -1044325.281765 + 15 * sin(0.47424360277825361),
510 -MINY -1044325.281765 - 15 * sin(0.27424360277825361),
511 -MINY -1044325.281765,
512 ), color="gainsboro", fill=False, hatch="x")
513 # --- End of Goal Zone figure ---
515 # Plot all the nodes (if exists.)
516 if "nodes_x" in scenario and "nodes_y" in scenario:
518 [x - MINX for x in scenario["nodes_x"]],
519 [y - MINY for y in scenario["nodes_y"]],
525 # Plot all the steered2 nodes (if exists.)
526 if "steered2_x" in scenario and "steered2_y" in scenario:
528 [x - MINX for x in scenario["steered2_x"]],
529 [y - MINY for y in scenario["steered2_y"]],
535 # Plot all the steered1 nodes (if exists.)
536 if "steered1_x" in scenario and "steered1_y" in scenario:
538 [x - MINX for x in scenario["steered1_x"]],
539 [y - MINY for y in scenario["steered1_y"]],
545 # Plot obstacles, slot.
546 if ("obst" in scenario and isinstance(scenario["obst"], list)
547 and len(scenario["obst"]) > 0):
548 for o in scenario["obst"]:
551 ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//") #fill=True for stage
552 if "slot" in scenario and len(scenario["slot"]) > 0:
553 plt.plot(*plot_nodes(scenario["slot"]), color="blue", linewidth=1)
554 #for s in scenario["slot"]:
555 # plt.plot(*plot_nodes(s), color="black")
557 # For the Possible Entry Configurations from the paper, use:
558 #ax.set_title("Computed configurations")
561 if False and inits in scenario:
562 max_i = len(scenario[inits]) - 1
564 i = scenario[inits][ii]
565 plt.plot(*plot_car(i), color=inits_c)
566 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
568 i = scenario[inits][ii]
569 plt.plot(*plot_car(i), color=inits_c)
570 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
572 i = scenario[inits][ii]
573 plt.plot(*plot_car(i), color=inits_c)
574 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
575 ii = int(max_i * 3/4)
576 i = scenario[inits][ii]
577 plt.plot(*plot_car(i), color=inits_c)
578 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
580 i = scenario[inits][ii]
581 plt.plot(*plot_car(i), color=inits_c)
582 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
585 if True and inits in scenario:
586 max_i = len(scenario[inits]) - 1
588 i = scenario[inits][ii]
589 plt.plot(*plot_car(i), color=inits_c)
590 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
592 i = scenario[inits][ii]
593 plt.plot(*plot_car(i), color=inits_c)
594 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
596 i = scenario[inits][ii]
597 plt.plot(*plot_car(i), color=inits_c)
598 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
599 ii = int(max_i * 3/4)
600 i = scenario[inits][ii]
601 plt.plot(*plot_car(i), color=inits_c)
602 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
604 i = scenario[inits][ii]
605 plt.plot(*plot_car(i), color=inits_c)
606 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
607 # Possible/Candidate entries
613 # fontfamily="serif",
614 # fontstyle="italic",
617 # scenario["slot"][-1][0] - MINX,
618 # scenario["slot"][-1][1] - MINY,
626 # scenario["slot"][-1][0] - MINX - 2,
630 # fontfamily="serif",
631 # fontstyle="italic",
634 # Plot `init`, `entry`, and `goal` configurations.
635 if "init" in scenario and len(scenario["init"]) == 3:
636 plt.plot(*plot_car(scenario["init"]), color="red")
638 scenario["init"][0] - MINX,
639 scenario["init"][1] - MINY,
645 #if "init" in scenario and len(scenario["init"]) == 4:
646 # plt.plot(*plot_car(scenario["init"]), color="red")
647 # scenario["init"][2] = scenario["init"][3]
648 # plt.plot(*plot_car(scenario["init"]), color="red")
650 # scenario["init"][0] - MINX,
651 # scenario["init"][1] - MINY,
656 #if "entries" in scenario:
657 # for e in scenario["entries"]:
658 # plt.plot(*plot_car(e), color="orange")
666 if "entry" in scenario and len(scenario["entry"]) == 3:
667 plt.plot(*plot_car(scenario["entry"]), color="magenta")
669 scenario["entry"][0] - MINX,
670 scenario["entry"][1] - MINY,
676 #if "entry" in scenario and len(scenario["entry"]) == 4:
677 # esc = scenario["entry"]
678 # plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
679 # plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
681 # scenario["entry"][0] - MINX,
682 # scenario["entry"][1] - MINY,
687 if "goal" in scenario:
688 if len(scenario["goal"]) == 3:
689 #plt.plot(*plot_car(scenario["goal"]), color="green")
690 plt.plot(*plot_car(scenario["goal"]), color="orange")
692 scenario["goal"][0] - MINX,
693 scenario["goal"][1] - MINY,
700 # elif len(scenario["goal"]) == 4:
701 # ctp = scenario["goal"]
702 # plt.plot(*plot_car(scenario["goal"]), color="green")
704 # plt.plot(*plot_car(scenario["goal"]), color="green")
706 # scenario["goal"][0] - MINX,
707 # scenario["goal"][1] - MINY,
713 # Plot `path` and `max_path`.
714 if (sc2 and "opath" in sc2 and isinstance(sc2["opath"], list)
715 and len(sc2["opath"]) > 0):
716 plt.plot(*plot_nodes(sc2["opath"]), color="orange", linestyle="dotted")
717 if (sc2 and "path" in sc2 and isinstance(sc2["path"], list)
718 and len(sc2["path"]) > 0):
719 plt.plot(*plot_nodes(sc2["path"]), color="orange")
720 if ("opath" in scenario and isinstance(scenario["opath"], list)
721 and len(scenario["opath"]) > 0):
723 *plot_nodes(scenario["opath"]),
728 if ("path" in scenario and isinstance(scenario["path"], list)
729 and len(scenario["path"]) > 0):
730 plt.plot(*plot_nodes(scenario["path"]), color="blue")
732 for p in scenario["path"]:
733 if False and len(p) > 4:
735 plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker="+")
737 plt.plot(p[0] - MINX, p[1] - MINY, color="green", marker="x")
739 plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker=".")
741 plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="+")
742 #plt.plot(*plot_car(p), color="blue")
744 #cc = plot_car_corners(p)
745 #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
746 #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
747 #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
748 #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
749 if ("traj" in scenario and isinstance(scenario["traj"], list)
750 and len(scenario["traj"]) > 0):
751 print(len(scenario["traj"]))
752 # print(scenario["traj"])
753 print(len(plot_nodes(scenario["traj"])[0]))
754 # print(plot_nodes(scenario["traj"]))
755 plt.plot(*plot_nodes(scenario["traj"]), color="red")
756 plt.plot(*plot_car(scenario["traj"][-1]), color="red")
757 for p in scenario["traj"]:
760 plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker="+")
762 plt.plot(p[0] - MINX, p[1] - MINY, color="green", marker="x")
764 plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker=".")
766 plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker=".")
767 #plt.plot(*plot_car(p), color="red")
769 #cc = plot_car_corners(p)
770 #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
771 #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
772 #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
773 #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
774 if "ispath" in scenario and len(scenario["ispath"]) > 0:
775 plt.plot(*plot_nodes(scenario["ispath"]), color="green")
776 for p in scenario["ispath"]:
777 #plt.plot(*plot_car(p), color="green")
779 #cc = plot_car_corners(p)
780 #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
781 #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
782 #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
783 #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
785 # If there are possible starts specified, you may print and plot them.
786 #if "starts" in scenario and len(scenario["starts"]) > 0:
787 # print("possible starts:")
788 # for p in scenario["starts"]:
789 # plt.plot(*p, color="red", marker="+", ms=12)
790 # print(" {}".format(p))
792 # For the Last Maneuver figure from the paper, use:
793 # - `init2` -- orange
794 #plt.plot(*plot_car(scenario["init2"]), color="orange")
796 # scenario["init2"][0] - MINX,
797 # scenario["init2"][1] - MINY,
802 # - `goal2` -- orange
803 #plt.plot(*plot_car(scenario["goal2"]), color="orange")
805 # scenario["goal2"][0] - MINX,
806 # scenario["goal2"][1] - MINY,
811 # - `goal2` -- middle (orange)
812 #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
814 # scenario["goal2"][0] - MINX,
815 # scenario["goal2"][1] - MINY,
821 #plt.plot(*plot_car(scenario["init1"]), color="green")
823 # scenario["init1"][0] - MINX,
824 # scenario["init1"][1] - MINY,
830 #plt.plot(*plot_car(scenario["goal1"]), color="green")
832 # scenario["goal1"][0] - MINX,
833 # scenario["goal1"][1] - MINY,
839 # The `scenario` may also include:
840 # - `last` -- not sure what this is, see the source code. Maybe overlaps
842 # - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
844 # - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
846 # - `max_orig_path` -- maximum original path. I used this when comparing
847 # original paths but I had to copy the `max_orig_path` by hand from
848 # different scenario result.
849 # - `orig_path` -- the path before the optimization.
850 # - `max_path` -- the maximum path after optimization. Must be copied by
852 # - `path` -- optimized path of the scenario.
854 handles, labels = ax.get_legend_handles_labels()
856 # Uncommnent the following line and comment the plt.show() to store to the
858 plt.savefig("out.pdf", bbox_inches="tight")