scripts/plot_json_objects_scenario.py

   1 """Plot JSON formatted scenario."""
   2 from json import loads
   3 from math import cos, pi, sin
   4 from math import inf
   5 from matplotlib import pyplot as plt
   6 from sys import argv, exit
   7
   8 BCAR_MTR = 10.820
   9 BCAR_WB = 2.450
  10 BCAR_W = 1.625
  11 BCAR_L = 3.760
  12 BCAR_SD = 0
  13 BCAR_DF = 3.105
  14 BCAR_DR = 0.655
  15
  16 MINX = inf
  17 MINY = inf
  18
  19 def get_scenario(fname):
  20     """Load scenario from file."""
  21     if fname is None:
  22         raise ValueError("File name as argument needed")
  23     with open(fname, "r") as f:
  24         scenario = loads(f.read())
  25     return scenario
  26
  27 def plot_nodes(nodes=[]):
  28     """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
  29
  30     Keyword arguments:
  31     nodes -- The list of nodes to plot.
  32     """
  33     xcoords = []
  34     ycoords = []
  35     for n in nodes:
  36         xcoords.append(n[0] - MINX)
  37         ycoords.append(n[1] - MINY)
  38     return (xcoords, ycoords)
  39
  40 def plot_car(pose):
  41     """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
  42
  43     Keyword arguments:
  44     pose -- The pose of a car.
  45     """
  46     lfx = pose[0]
  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])
  50
  51     lf3x = pose[0]
  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])
  55
  56     lrx = pose[0]
  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])
  60
  61     rrx = pose[0]
  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])
  65
  66     rfx = pose[0]
  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])
  70
  71     rf3x = pose[0]
  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])
  75
  76     lfy = pose[1]
  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])
  80
  81     lf3y = pose[1]
  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])
  85
  86     lry = pose[1]
  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])
  90
  91     rry = pose[1]
  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])
  95
  96     rfy = pose[1]
  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])
 100
 101     rf3y = pose[1]
 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])
 105
 106     cfx = pose[0]
 107     cfx += BCAR_DF * cos(pose[2])
 108     cfx += BCAR_SD * cos(pose[2])
 109
 110     cfy = pose[1]
 111     cfy += BCAR_DF * sin(pose[2])
 112     cfy += BCAR_SD * sin(pose[2])
 113
 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])
 117
 118 def plot_car_corners(pose):
 119     """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
 120
 121     Keyword arguments:
 122     pose -- The pose of a car.
 123     """
 124     lfx = pose[0]
 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])
 128
 129     lrx = pose[0]
 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])
 133
 134     rrx = pose[0]
 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])
 138
 139     rfx = pose[0]
 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])
 143
 144     lfy = pose[1]
 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])
 148
 149     lry = pose[1]
 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])
 153
 154     rry = pose[1]
 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])
 158
 159     rfy = pose[1]
 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])
 163
 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])
 167
 168 if __name__ == "__main__":
 169     sc2 = None
 170     if (len(argv) == 2):
 171         SCEN_FILE = argv[1]
 172     elif (len(argv) == 3):
 173         SCEN_FILE = argv[1]
 174         SCEN_FILE2 = argv[2]
 175         sc2 = get_scenario(SCEN_FILE2)
 176     else:
 177         SCEN_FILE = "sc.json"
 178
 179     scenario = get_scenario(SCEN_FILE)
 180
 181     # Font size to be approximately the same in the paper:
 182     #   - sc1-0: 16
 183     #   - sc3-2, Intro: 12
 184     #   - sc4-0+: 22
 185     plt.rc('axes', unicode_minus=False)
 186     plt.rcParams["font.family"] = "cmr10"
 187     plt.rcParams["font.size"] = 12
 188     plt.rcParams['hatch.linewidth'] = 0.5
 189     plt.rcParams['lines.linewidth'] = 1.0
 190     fig = plt.figure()
 191
 192     # here subplot starts
 193     ax = fig.add_subplot(111)
 194     ax.set_aspect("equal")
 195     ax.set_title("Real-world parking scenario")
 196     ax.set_xlabel("x [m]")
 197     ax.set_ylabel("y [m]")
 198
 199     # For Possible Entry Points (Possible Entry Configurations) use:
 200     #ax.set_xlim([37.6, 45.6])
 201     #ax.set_ylim([2.4, 8.5])
 202     #ax.set_title("Possible configurations")
 203
 204     # For Last Maneuver use:
 205     #ax.set_xlim([38, 44])
 206     #ax.set_ylim([3.1, 6.9])
 207
 208     # For Scenario 3-2 detail use:
 209     # - font size 22
 210     #ax.set_title("Scenario 1")
 211     #ax.set_xlim([1, 16]) # w=15
 212     #ax.set_ylim([35.1, 49.9]) # h=15
 213
 214     # For Scenario 4-0 detail use:
 215     # - font size 22
 216     #ax.set_title("Scenario 2")
 217     #ax.set_xlim([32, 51]) # w=19
 218     #ax.set_ylim([1, 19]) # h=18
 219
 220     # For Scenario 4-1 detail use:
 221     # - font size 22
 222     #ax.set_title("Scenario 3")
 223     #ax.set_xlim([32, 47]) # w=15
 224     #ax.set_ylim([1, 16]) # h=15
 225
 226     # For Scenario 5-1 detail use:
 227     # - font size 22
 228     #ax.set_title("Scenario 4")
 229     #ax.set_xlim([10.1, 27])
 230     #ax.set_ylim([10.1, 26])
 231
 232     # For Scenario 4-1-{0,14} detail use:
 233     # - font size 22
 234     #ax.set_title("Scenario 5") # Scenario 6
 235     #ax.set_xlim([31, 47.5]) # w=16.5
 236     #ax.set_ylim([1, 16.3]) # h=15.3
 237
 238     # For Scenario 5-3-{35,} detail use:
 239     # - font size 22
 240     #ax.set_title("Scenario 7") # Scenario 8
 241     #ax.set_xlim([5.5, 27.5])
 242     #ax.set_ylim([8.1, 28])
 243
 244     # For Real-world parking scenario in Introduction section use:
 245     #ax.set_title("Real-world parking scenario with artificial obstacle")
 246     #ax.set_xlim([23, 58.5])
 247     #ax.set_ylim([-8.4, 20.9])
 248     #ax.text(34, 9.2, "Initial configuration", color="red")
 249     #ax.text(43.5, 8.5, "Final path", color="blue")
 250     #ax.text(48.25, 5.5, "Entry\nconfigurations", color="orange", ha="right")
 251     #ax.text(38, 3.8, "Parking\nslot", color="blue", ha="right", backgroundcolor="white")
 252     #ax.text(35.2, 5.5, "Goal configuration", color="green")
 253
 254     # For scenario 5-3
 255     #ax.set_title("Guessed goal")
 256     #ax.set_xlim([6.8, 16.2])
 257     #ax.set_ylim([15, 20])
 258
 259     # For simple scenarios 92 and 96 (simple-1k-test49)
 260     # - Use MINY=-25, MINX=-10 for 96.
 261     #ax.set_title("Simple parking scenario")
 262     #ax.set_xlim([-11, 24]) # w=35
 263     #ax.set_ylim([-2.5, 37.5]) # h=40
 264
 265     # Set min and max to center the plot.
 266     MINX = scenario["init"][0]
 267     MINY = scenario["init"][1]
 268     if "obst" in scenario and  len(scenario["obst"]) > 0:
 269         for o in scenario["obst"]:
 270             if not o:
 271                 continue
 272             for n in o:
 273                 if n[0] < MINX:
 274                     MINX = n[0]
 275                 if n[1] < MINY:
 276                     MINY = n[1]
 277     #MINY = -25
 278     #MINX = -10
 279     c1 = plt.Circle(
 280         (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
 281         4.8677125017335845,
 282         color='red',
 283         fill=False,
 284     )
 285     #ax.add_patch(c1)
 286     c2 = plt.Circle(
 287         (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
 288         3.1984427539075178,
 289         color='red',
 290         fill=False,
 291     )
 292     #ax.add_patch(c2)
 293     c3 = plt.Circle(
 294         (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
 295         5.736429638720212,
 296         color='red',
 297         fill=False,
 298     )
 299     #ax.add_patch(c3)
 300     c4 = plt.Circle(
 301         (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
 302         3.1984427539075178,
 303         color='red',
 304         fill=False,
 305     )
 306     #ax.add_patch(c4)
 307
 308     # Plot all the nodes (if exists.)
 309     if "nodes_x" in scenario and "nodes_y" in scenario:
 310         plt.plot(
 311             [x - MINX for x in scenario["nodes_x"]],
 312             [y - MINY for y in scenario["nodes_y"]],
 313             color="lightgray",
 314             marker="o",
 315             ms=2,
 316             lw=0,
 317         )
 318     # Plot all the steered2 nodes (if exists.)
 319     if "steered2_x" in scenario and "steered2_y" in scenario:
 320         plt.plot(
 321             [x - MINX for x in scenario["steered2_x"]],
 322             [y - MINY for y in scenario["steered2_y"]],
 323             color="orange",
 324             marker="o",
 325             ms=2,
 326             lw=0,
 327         )
 328     # Plot all the steered1 nodes (if exists.)
 329     if "steered1_x" in scenario and "steered1_y" in scenario:
 330         plt.plot(
 331             [x - MINX for x in scenario["steered1_x"]],
 332             [y - MINY for y in scenario["steered1_y"]],
 333             color="blue",
 334             marker="o",
 335             ms=2,
 336             lw=0,
 337         )
 338     # Plot obstacles, slot.
 339     if "obst" in scenario and len(scenario["obst"]) > 0:
 340         for o in scenario["obst"]:
 341             if not o:
 342                 continue
 343             ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//")
 344     if "slot" in scenario and len(scenario["slot"]) > 0:
 345         plt.plot(*plot_nodes(scenario["slot"]), color="blue", linewidth=1)
 346         #for s in scenario["slot"]:
 347         #    plt.plot(*plot_nodes(s), color="black")
 348
 349     # For the Possible Entry Configurations from the paper, use:
 350     #ax.set_title("Computed configurations")
 351     inits = "insides"
 352     inits_c = "green"
 353     if False and inits in scenario:
 354         max_i = len(scenario[inits]) - 1
 355         ii = 0
 356         i = scenario[inits][ii]
 357         plt.plot(*plot_car(i), color=inits_c)
 358         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 359         ii = int(max_i / 4)
 360         i = scenario[inits][ii]
 361         plt.plot(*plot_car(i), color=inits_c)
 362         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 363         ii = int(max_i / 2)
 364         i = scenario[inits][ii]
 365         plt.plot(*plot_car(i), color=inits_c)
 366         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 367         ii = int(max_i * 3/4)
 368         i = scenario[inits][ii]
 369         plt.plot(*plot_car(i), color=inits_c)
 370         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 371         ii = max_i
 372         i = scenario[inits][ii]
 373         plt.plot(*plot_car(i), color=inits_c)
 374         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 375     inits = "inits"
 376     inits_c = "orange"
 377     if False and inits in scenario:
 378         max_i = len(scenario[inits]) - 1
 379         ii = 0
 380         i = scenario[inits][ii]
 381         plt.plot(*plot_car(i), color=inits_c)
 382         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 383         ii = int(max_i / 4)
 384         i = scenario[inits][ii]
 385         plt.plot(*plot_car(i), color=inits_c)
 386         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 387         ii = int(max_i / 2)
 388         i = scenario[inits][ii]
 389         plt.plot(*plot_car(i), color=inits_c)
 390         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 391         ii = int(max_i * 3/4)
 392         i = scenario[inits][ii]
 393         plt.plot(*plot_car(i), color=inits_c)
 394         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 395         ii = max_i
 396         i = scenario[inits][ii]
 397         plt.plot(*plot_car(i), color=inits_c)
 398         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 399
 400     # Plot `init`, `entry`, and `goal` configurations.
 401     if "init" in scenario and len(scenario["init"]) == 3:
 402         plt.plot(*plot_car(scenario["init"]), color="red")
 403         plt.plot(
 404             scenario["init"][0] - MINX,
 405             scenario["init"][1] - MINY,
 406             color="red",
 407             marker="+",
 408             ms=12
 409         )
 410     if "init" in scenario and len(scenario["init"]) == 4:
 411         plt.plot(*plot_car(scenario["init"]), color="red")
 412         scenario["init"][2] = scenario["init"][3]
 413         plt.plot(*plot_car(scenario["init"]), color="red")
 414         plt.plot(
 415             scenario["init"][0] - MINX,
 416             scenario["init"][1] - MINY,
 417             color="red",
 418             marker="+",
 419             ms=12
 420         )
 421     if "entries" in scenario:
 422         for e in scenario["entries"]:
 423             plt.plot(*plot_car(e), color="orange")
 424             plt.plot(
 425                 e[0] - MINX,
 426                 e[1] - MINY,
 427                 color="orange",
 428                 marker="+",
 429                 ms=12
 430             )
 431     if "entry" in scenario and len(scenario["entry"]) == 3:
 432         plt.plot(*plot_car(scenario["entry"]), color="magenta")
 433         plt.plot(
 434             scenario["entry"][0] - MINX,
 435             scenario["entry"][1] - MINY,
 436             color="magenta",
 437             marker="+",
 438             ms=12
 439         )
 440     if "entry" in scenario and len(scenario["entry"]) == 4:
 441         esc = scenario["entry"]
 442         plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
 443         plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
 444         plt.plot(
 445             scenario["entry"][0] - MINX,
 446             scenario["entry"][1] - MINY,
 447             color="magenta",
 448             marker="+",
 449             ms=12
 450         )
 451     if "goal" in scenario:
 452         if len(scenario["goal"]) == 3:
 453             plt.plot(*plot_car(scenario["goal"]), color="green")
 454             plt.plot(
 455                 scenario["goal"][0] - MINX,
 456                 scenario["goal"][1] - MINY,
 457                 color="green",
 458                 marker="+",
 459                 ms=12
 460             )
 461         elif len(scenario["goal"]) == 4:
 462             ctp = scenario["goal"]
 463             plt.plot(*plot_car(scenario["goal"]), color="green")
 464             ctp[2] = ctp[3]
 465             plt.plot(*plot_car(scenario["goal"]), color="green")
 466             plt.plot(
 467                 scenario["goal"][0] - MINX,
 468                 scenario["goal"][1] - MINY,
 469                 color="green",
 470                 marker="+",
 471                 ms=12
 472             )
 473
 474     # Plot `path` and `max_path`.
 475     if (sc2 and "opath" in sc2 and isinstance(sc2["opath"], list)
 476             and len(sc2["opath"]) > 0):
 477         plt.plot(*plot_nodes(sc2["opath"]), color="orange", linestyle="dotted")
 478     if (sc2 and "path" in sc2 and isinstance(sc2["path"], list)
 479             and len(sc2["path"]) > 0):
 480         plt.plot(*plot_nodes(sc2["path"]), color="orange")
 481     if ("opath" in scenario and isinstance(scenario["opath"], list)
 482             and len(scenario["opath"]) > 0):
 483         plt.plot(
 484             *plot_nodes(scenario["opath"]),
 485             color="blue",
 486             linewidth=1,
 487             linestyle="dotted",
 488         )
 489     if ("path" in scenario and isinstance(scenario["path"], list)
 490             and len(scenario["path"]) > 0):
 491         plt.plot(*plot_nodes(scenario["path"]), color="blue")
 492         for p in scenario["path"]:
 493             #plt.plot(*plot_car(p), color="blue")
 494             pass
 495             #cc = plot_car_corners(p)
 496             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 497             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 498             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 499             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 500     if "ispath" in scenario and len(scenario["ispath"]) > 0:
 501         plt.plot(*plot_nodes(scenario["ispath"]), color="green")
 502         for p in scenario["ispath"]:
 503             #plt.plot(*plot_car(p), color="green")
 504             pass
 505             #cc = plot_car_corners(p)
 506             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 507             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 508             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 509             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 510
 511     # If there are possible starts specified, you may print and plot them.
 512     #if "starts" in scenario and len(scenario["starts"]) > 0:
 513     #    print("possible starts:")
 514     #    for p in scenario["starts"]:
 515     #        plt.plot(*p, color="red", marker="+", ms=12)
 516     #        print(" {}".format(p))
 517
 518     # For the Last Maneuver figure from the paper, use:
 519     #   - `init2` -- orange
 520     #plt.plot(*plot_car(scenario["init2"]), color="orange")
 521     #plt.plot(
 522     #    scenario["init2"][0] - MINX,
 523     #    scenario["init2"][1] - MINY,
 524     #    color="orange",
 525     #    #marker="+",
 526     #    ms=12
 527     #)
 528     #   - `goal2` -- orange
 529     #plt.plot(*plot_car(scenario["goal2"]), color="orange")
 530     #plt.plot(
 531     #    scenario["goal2"][0] - MINX,
 532     #    scenario["goal2"][1] - MINY,
 533     #    color="orange",
 534     #    #marker="+",
 535     #    ms=12
 536     #)
 537     #   - `goal2` -- middle (orange)
 538     #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
 539     #plt.plot(
 540     #    scenario["goal2"][0] - MINX,
 541     #    scenario["goal2"][1] - MINY,
 542     #    color="orange",
 543     #    #marker="+",
 544     #    ms=12
 545     #)
 546     #   - `init1` -- green
 547     #plt.plot(*plot_car(scenario["init1"]), color="green")
 548     #plt.plot(
 549     #    scenario["init1"][0] - MINX,
 550     #    scenario["init1"][1] - MINY,
 551     #    color="green",
 552     #    #marker="+",
 553     #    ms=12
 554     #)
 555     #   - `goal1` -- green
 556     #plt.plot(*plot_car(scenario["goal1"]), color="green")
 557     #plt.plot(
 558     #    scenario["goal1"][0] - MINX,
 559     #    scenario["goal1"][1] - MINY,
 560     #    color="green",
 561     #    #marker="+",
 562     #    ms=12
 563     #)
 564
 565     # The `scenario` may also include:
 566     #   - `last` -- not sure what this is, see the source code. Maybe overlaps
 567     #     with the `goal`.
 568     #   - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
 569     #     Planner (PSP.))
 570     #   - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
 571     #     Planner (PSP.))
 572     #   - `max_orig_path` -- maximum original path. I used this when comparing
 573     #     original paths but I had to copy the `max_orig_path` by hand from
 574     #     different scenario result.
 575     #   - `orig_path` -- the path before the optimization.
 576     #   - `max_path` -- the maximum path after optimization. Must be copied by
 577     #     hand.
 578     #   - `path` -- optimized path of the scenario.
 579
 580     handles, labels = ax.get_legend_handles_labels()
 581
 582     # Uncommnent the following line and comment the plt.show() to store to the
 583     # file.
 584     plt.savefig("out.pdf", bbox_inches="tight")
 585     plt.close(fig)