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
21 def get_scenario(fname):
22 """Load scenario from file."""
24 raise ValueError("File name as argument needed")
25 with open(fname, "r") as f:
26 scenario = loads(f.read())
29 def plot_nodes(nodes=[]):
30 """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
33 nodes -- The list of nodes to plot.
38 xcoords.append(n[0] - MINX)
39 ycoords.append(n[1] - MINY)
40 return (xcoords, ycoords)
43 """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
46 pose -- The pose of a car.
49 lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
50 lfx += BCAR_DF * cos(pose[2])
51 lfx += BCAR_SD * cos(pose[2])
54 lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
55 lf3x += 2/3 * BCAR_DF * cos(pose[2])
56 lf3x += BCAR_SD * cos(pose[2])
59 lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
60 lrx += -BCAR_DR * cos(pose[2])
61 lrx += -BCAR_SD * cos(pose[2])
64 rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
65 rrx += -BCAR_DR * cos(pose[2])
66 rrx += -BCAR_SD * cos(pose[2])
69 rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
70 rfx += BCAR_DF * cos(pose[2])
71 rfx += BCAR_SD * cos(pose[2])
74 rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
75 rf3x += 2/3 * BCAR_DF * cos(pose[2])
76 rf3x += BCAR_SD * cos(pose[2])
79 lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
80 lfy += BCAR_DF * sin(pose[2])
81 lfy += BCAR_SD * sin(pose[2])
84 lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
85 lf3y += 2/3 * BCAR_DF * sin(pose[2])
86 lf3y += BCAR_SD * sin(pose[2])
89 lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
90 lry += -BCAR_DR * sin(pose[2])
91 lry += -BCAR_SD * sin(pose[2])
94 rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
95 rry += -BCAR_DR * sin(pose[2])
96 rry += -BCAR_SD * sin(pose[2])
99 rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
100 rfy += BCAR_DF * sin(pose[2])
101 rfy += BCAR_SD * sin(pose[2])
104 rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
105 rf3y += 2/3 * BCAR_DF * sin(pose[2])
106 rf3y += BCAR_SD * sin(pose[2])
109 cfx += BCAR_DF * cos(pose[2])
110 cfx += BCAR_SD * cos(pose[2])
113 cfy += BCAR_DF * sin(pose[2])
114 cfy += BCAR_SD * sin(pose[2])
116 xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
117 ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
118 return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
120 def plot_car_corners(pose):
121 """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
124 pose -- The pose of a car.
127 lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
128 lfx += BCAR_DF * cos(pose[2])
129 lfx += BCAR_SD * cos(pose[2])
132 lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
133 lrx += -BCAR_DR * cos(pose[2])
134 lrx += -BCAR_SD * cos(pose[2])
137 rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
138 rrx += -BCAR_DR * cos(pose[2])
139 rrx += -BCAR_SD * cos(pose[2])
142 rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
143 rfx += BCAR_DF * cos(pose[2])
144 rfx += BCAR_SD * cos(pose[2])
147 lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
148 lfy += BCAR_DF * sin(pose[2])
149 lfy += BCAR_SD * sin(pose[2])
152 lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
153 lry += -BCAR_DR * sin(pose[2])
154 lry += -BCAR_SD * sin(pose[2])
157 rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
158 rry += -BCAR_DR * sin(pose[2])
159 rry += -BCAR_SD * sin(pose[2])
162 rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
163 rfy += BCAR_DF * sin(pose[2])
164 rfy += BCAR_SD * sin(pose[2])
166 xcoords = (lfx, lrx, rrx, rfx)
167 ycoords = (lfy, lry, rry, rfy)
168 return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
170 if __name__ == "__main__":
174 elif (len(argv) == 3):
177 sc2 = get_scenario(SCEN_FILE2)
179 SCEN_FILE = "sc.json"
181 scenario = get_scenario(SCEN_FILE)
183 # Font size to be approximately the same in the paper:
187 plt.rc('axes', unicode_minus=False)
188 plt.rcParams["figure.figsize"] = [14, 7]
189 plt.rcParams["font.family"] = "cmr10"
190 plt.rcParams["font.size"] = 24
191 plt.rcParams['hatch.linewidth'] = 6.0
192 plt.rcParams['lines.linewidth'] = 2.0
195 # here subplot starts
196 ax = fig.add_subplot(111)
197 ax.set_aspect("equal")
198 #ax.set_title("Real-world parking scenario")
199 ax.set_xlabel("x [m]")
200 ax.set_ylabel("y [m]")
201 # For stage, comment upper, uncomment following:
206 #ax.set_xlim([4, 32]) # 28
207 #ax.set_ylim([7, 29]) # 22
209 #ax.set_xlim([-1, 27]) # 28
210 #ax.set_ylim([-1, 21]) # 22
212 #ax.set_xlim([-9.9, 18.1]) # 28
213 #ax.set_ylim([-4.9, 17.1]) # 22
215 #ax.set_xlim([4, 32]) # 28
216 #ax.set_ylim([-4, 18]) # 22
218 #ax.set_xlim([-7, 21]) # 28
219 #ax.set_ylim([7, 29]) # 22
221 #ax.set_xlim([-7, 21]) # 28
222 #ax.set_ylim([2, 24]) # 22
224 #ax.set_xlim([-1, 27]) # 28
225 #ax.set_ylim([-1, 21]) # 22
227 # For Possible Entry Points (Possible Entry Configurations) use:
228 #plt.rcParams["font.size"] = 26
229 #ax.set_xlim([37.6, 45.6])
230 #ax.set_ylim([2.4, 8.5])
231 #ax.set_title("Possible configurations")
233 # For Last Maneuver use:
234 #ax.set_xlim([38, 44])
235 #ax.set_ylim([3.1, 6.9])
237 # For Scenario 3-2 detail use:
239 #ax.set_title("Scenario 1")
240 #ax.set_xlim([1, 16]) # w=15
241 #ax.set_ylim([35.1, 49.9]) # h=15
243 # For Scenario 4-0 detail use:
245 #ax.set_title("Scenario 2")
246 #ax.set_xlim([32, 53]) # w=19
247 #ax.set_ylim([0.5, 19.5]) # h=18
249 # For Scenario 4-1 detail use:
251 #ax.set_title("Scenario 3")
252 #ax.set_xlim([32.5, 48.5]) # w=16
253 #ax.set_ylim([1, 16]) # h=15
255 # For Scenario 5-1 detail use:
257 #ax.set_title("Scenario 4")
258 #ax.set_xlim([10.1, 27])
259 #ax.set_ylim([10.1, 26])
261 # For Scenario 4-1-{0,14} detail use:
263 #ax.set_title("Scenario 5") # Scenario 6
264 #ax.set_xlim([32.5, 48.5]) # w=16
265 #ax.set_ylim([1, 16.3]) # h=15.3
267 # For Scenario 5-3-34 detail use:
269 #ax.set_title("Scenario 7") # Scenario 8
270 #ax.set_xlim([5.5, 27.5])
271 #ax.set_ylim([15.1, 35])
273 # For Scenario 5-3-29 detail use:
275 #ax.set_title("Scenario 8")
276 #ax.set_xlim([5.5, 27.5])
277 #ax.set_ylim([10.1, 30])
279 # For Real-world parking scenario in Introduction section use:
280 #ax.set_title("Real-world parking scenario with artificial obstacle")
281 #ax.set_xlim([23, 58.5])
282 #ax.set_ylim([-8.4, 20.9])
283 #ax.text(34, 9.2, "Initial configuration", color="red")
284 #ax.text(43.5, 8.5, "Final path", color="blue")
285 #ax.text(48.25, 5.5, "Entry\nconfigurations", color="orange", ha="right")
286 #ax.text(38, 3.8, "Parking\nslot", color="blue", ha="right", backgroundcolor="white")
287 #ax.text(35.2, 5.5, "Goal configuration", color="green")
290 #ax.set_title("Computed goal")
291 #ax.set_xlim([6.8, 16.2])
292 #ax.set_ylim([15, 20])
294 # For simple scenarios 92 and 96 (simple-1k-test49)
295 # - Use MINY=-25, MINX=-10 for 96.
296 #ax.set_title("Simple parking scenario")
297 #ax.set_xlim([-11, 24]) # w=35
298 #ax.set_ylim([-2.5, 37.5]) # h=40
300 # Set min and max to center the plot.
301 MINX = scenario["init"][0]
302 MINY = scenario["init"][1]
303 MAXX = scenario["init"][0]
304 MAXY = scenario["init"][1]
305 if "obst" in scenario and len(scenario["obst"]) > 0:
306 for o in scenario["obst"]:
318 print("w: {}, h: {}".format(abs(MAXX - MINX), abs(MAXY - MINY)))
322 (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
329 (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
336 (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
343 (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
350 # For Goal Zone figure, "Goal zone" file name in j1/figs/
353 ax.set_ylim([-4.8, 2.8])
354 ax.set_xlim([-13, 5])
355 gz_ccr = matplotlib.patches.Arc(
356 (-744206.185356 - MINX, -1044330.294266 - MINY),
357 5.207071 * 2, 5.207071 * 2,
358 theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
359 theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
365 gz_ccr = matplotlib.patches.Arc(
366 (-744206.185356 - MINX + 3.99, -1044330.294266 - MINY + 2.05),
367 5.207071 * 2, 5.207071 * 2,
368 theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
369 theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
375 gz_gh = 0.47424360277825361
376 gz_ih = -0.27424360277825361
377 def li(x, y, h, le=10.0):
378 return (x, x + le * cos(h)), (y, y + le * sin(h))
380 plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_gh),
381 color="orange", ls="dotted")
382 plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_ih),
383 color="red", ls="dotted")
386 *li(-744208.5632466434 - MINX, -1044325.6618554679 - MINY, gz_gh, 4.47),
387 color="orange", ls="solid")
389 *li(-744199.2632466434 - MINX, -1044323.6618554679 - MINY, gz_ih, -1.55),
390 color="red", ls="solid")
392 -744208.5632466434 - MINX,
393 -1044325.6618554679 - MINY - 1.5,
400 -744208.5632466434 - MINX + 0.35,
401 -1044325.6618554679 - MINY - 1.7,
409 -744199.2632466434 - MINX,
410 -1044323.6618554679 - MINY - 1.5,
417 -744199.2632466434 - MINX + 0.35,
418 -1044323.6618554679 - MINY - 1.7,
426 -744199.2632466434 - MINX,
427 -1044323.6618554679 - MINY - 3.9,
434 -744199.2632466434 - MINX + 0.35,
435 -1044323.6618554679 - MINY - 4.1,
443 -744199.2632466434 - MINX,
444 -1044323.6618554679 - MINY - 3.18,
452 -744199.2632466434 - MINX,
453 -1044323.6618554679 - MINY + 1.9,
460 -744199.2632466434 - MINX + 0.35,
461 -1044323.6618554679 - MINY + 1.7,
469 -744199.2632466434 - MINX,
470 -1044323.6618554679 - MINY + 1.22,
478 -744199.2632466434 - MINX + 2,
479 -1044323.6618554679 - MINY + -3,
485 backgroundcolor="white",
488 -MINX -744204.775115,
489 -MINX -744204.775115 + 15 * cos(0.47424360277825361),
490 -MINX -744204.775115 + 15 * cos(0.27424360277825361),
491 -MINX -744204.775115,
493 -MINY -1044325.281765,
494 -MINY -1044325.281765 + 15 * sin(0.47424360277825361),
495 -MINY -1044325.281765 - 15 * sin(0.27424360277825361),
496 -MINY -1044325.281765,
497 ), color="gainsboro", fill=False, hatch="x")
498 # --- End of Goal Zone figure ---
500 # Plot all the nodes (if exists.)
501 if "nodes_x" in scenario and "nodes_y" in scenario:
503 [x - MINX for x in scenario["nodes_x"]],
504 [y - MINY for y in scenario["nodes_y"]],
510 # Plot all the steered2 nodes (if exists.)
511 if "steered2_x" in scenario and "steered2_y" in scenario:
513 [x - MINX for x in scenario["steered2_x"]],
514 [y - MINY for y in scenario["steered2_y"]],
520 # Plot all the steered1 nodes (if exists.)
521 if "steered1_x" in scenario and "steered1_y" in scenario:
523 [x - MINX for x in scenario["steered1_x"]],
524 [y - MINY for y in scenario["steered1_y"]],
530 # Plot obstacles, slot.
531 if "obst" in scenario and len(scenario["obst"]) > 0:
532 for o in scenario["obst"]:
535 ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//") #fill=True for stage
536 if "slot" in scenario and len(scenario["slot"]) > 0:
537 plt.plot(*plot_nodes(scenario["slot"]), color="blue", linewidth=1)
538 #for s in scenario["slot"]:
539 # plt.plot(*plot_nodes(s), color="black")
541 # For the Possible Entry Configurations from the paper, use:
542 #ax.set_title("Computed configurations")
545 if False and inits in scenario:
546 max_i = len(scenario[inits]) - 1
548 i = scenario[inits][ii]
549 plt.plot(*plot_car(i), color=inits_c)
550 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
552 i = scenario[inits][ii]
553 plt.plot(*plot_car(i), color=inits_c)
554 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
556 i = scenario[inits][ii]
557 plt.plot(*plot_car(i), color=inits_c)
558 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
559 ii = int(max_i * 3/4)
560 i = scenario[inits][ii]
561 plt.plot(*plot_car(i), color=inits_c)
562 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
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)
569 if True and inits in scenario:
570 max_i = len(scenario[inits]) - 1
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)
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)
583 ii = int(max_i * 3/4)
584 i = scenario[inits][ii]
585 plt.plot(*plot_car(i), color=inits_c)
586 plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
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)
591 # Possible/Candidate entries
597 # fontfamily="serif",
598 # fontstyle="italic",
601 # scenario["slot"][-1][0] - MINX,
602 # scenario["slot"][-1][1] - MINY,
610 # scenario["slot"][-1][0] - MINX - 2,
614 # fontfamily="serif",
615 # fontstyle="italic",
618 # Plot `init`, `entry`, and `goal` configurations.
619 if "init" in scenario and len(scenario["init"]) == 3:
620 plt.plot(*plot_car(scenario["init"]), color="red")
622 scenario["init"][0] - MINX,
623 scenario["init"][1] - MINY,
629 #if "init" in scenario and len(scenario["init"]) == 4:
630 # plt.plot(*plot_car(scenario["init"]), color="red")
631 # scenario["init"][2] = scenario["init"][3]
632 # plt.plot(*plot_car(scenario["init"]), color="red")
634 # scenario["init"][0] - MINX,
635 # scenario["init"][1] - MINY,
640 #if "entries" in scenario:
641 # for e in scenario["entries"]:
642 # plt.plot(*plot_car(e), color="orange")
650 if "entry" in scenario and len(scenario["entry"]) == 3:
651 plt.plot(*plot_car(scenario["entry"]), color="magenta")
653 scenario["entry"][0] - MINX,
654 scenario["entry"][1] - MINY,
660 #if "entry" in scenario and len(scenario["entry"]) == 4:
661 # esc = scenario["entry"]
662 # plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
663 # plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
665 # scenario["entry"][0] - MINX,
666 # scenario["entry"][1] - MINY,
671 if "goal" in scenario:
672 if len(scenario["goal"]) == 3:
673 #plt.plot(*plot_car(scenario["goal"]), color="green")
674 plt.plot(*plot_car(scenario["goal"]), color="orange")
676 scenario["goal"][0] - MINX,
677 scenario["goal"][1] - MINY,
684 # elif len(scenario["goal"]) == 4:
685 # ctp = scenario["goal"]
686 # plt.plot(*plot_car(scenario["goal"]), color="green")
688 # plt.plot(*plot_car(scenario["goal"]), color="green")
690 # scenario["goal"][0] - MINX,
691 # scenario["goal"][1] - MINY,
697 # Plot `path` and `max_path`.
698 if (sc2 and "opath" in sc2 and isinstance(sc2["opath"], list)
699 and len(sc2["opath"]) > 0):
700 plt.plot(*plot_nodes(sc2["opath"]), color="orange", linestyle="dotted")
701 if (sc2 and "path" in sc2 and isinstance(sc2["path"], list)
702 and len(sc2["path"]) > 0):
703 plt.plot(*plot_nodes(sc2["path"]), color="orange")
704 if ("opath" in scenario and isinstance(scenario["opath"], list)
705 and len(scenario["opath"]) > 0):
707 *plot_nodes(scenario["opath"]),
712 if ("path" in scenario and isinstance(scenario["path"], list)
713 and len(scenario["path"]) > 0):
714 plt.plot(*plot_nodes(scenario["path"]), color="blue")
715 for p in scenario["path"]:
716 #plt.plot(*plot_car(p), color="blue")
718 #cc = plot_car_corners(p)
719 #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
720 #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
721 #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
722 #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
723 if "ispath" in scenario and len(scenario["ispath"]) > 0:
724 plt.plot(*plot_nodes(scenario["ispath"]), color="green")
725 for p in scenario["ispath"]:
726 #plt.plot(*plot_car(p), color="green")
728 #cc = plot_car_corners(p)
729 #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
730 #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
731 #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
732 #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
734 # If there are possible starts specified, you may print and plot them.
735 #if "starts" in scenario and len(scenario["starts"]) > 0:
736 # print("possible starts:")
737 # for p in scenario["starts"]:
738 # plt.plot(*p, color="red", marker="+", ms=12)
739 # print(" {}".format(p))
741 # For the Last Maneuver figure from the paper, use:
742 # - `init2` -- orange
743 #plt.plot(*plot_car(scenario["init2"]), color="orange")
745 # scenario["init2"][0] - MINX,
746 # scenario["init2"][1] - MINY,
751 # - `goal2` -- orange
752 #plt.plot(*plot_car(scenario["goal2"]), color="orange")
754 # scenario["goal2"][0] - MINX,
755 # scenario["goal2"][1] - MINY,
760 # - `goal2` -- middle (orange)
761 #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
763 # scenario["goal2"][0] - MINX,
764 # scenario["goal2"][1] - MINY,
770 #plt.plot(*plot_car(scenario["init1"]), color="green")
772 # scenario["init1"][0] - MINX,
773 # scenario["init1"][1] - MINY,
779 #plt.plot(*plot_car(scenario["goal1"]), color="green")
781 # scenario["goal1"][0] - MINX,
782 # scenario["goal1"][1] - MINY,
788 # The `scenario` may also include:
789 # - `last` -- not sure what this is, see the source code. Maybe overlaps
791 # - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
793 # - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
795 # - `max_orig_path` -- maximum original path. I used this when comparing
796 # original paths but I had to copy the `max_orig_path` by hand from
797 # different scenario result.
798 # - `orig_path` -- the path before the optimization.
799 # - `max_path` -- the maximum path after optimization. Must be copied by
801 # - `path` -- optimized path of the scenario.
803 handles, labels = ax.get_legend_handles_labels()
805 # Uncommnent the following line and comment the plt.show() to store to the
807 plt.savefig("out.pdf", bbox_inches="tight")