scripts/plot_json_objects_scenario.py

   1 #!/usr/bin/env python3
   2 """Plot JSON formatted scenario."""
   3 from json import loads
   4 from math import cos, pi, sin
   5 from math import inf
   6 from matplotlib import pyplot as plt
   7 from sys import argv, exit
   8
   9 BCAR_MTR = 10.820
  10 BCAR_WB = 2.450
  11 BCAR_W = 1.625
  12 BCAR_L = 3.760
  13 BCAR_SD = 0
  14 BCAR_DF = 3.105
  15 BCAR_DR = 0.655
  16
  17 MINX = inf
  18 MINY = inf
  19
  20 def get_scenario(fname):
  21     """Load scenario from file."""
  22     if fname is None:
  23         raise ValueError("File name as argument needed")
  24     with open(fname, "r") as f:
  25         scenario = loads(f.read())
  26     return scenario
  27
  28 def plot_nodes(nodes=[]):
  29     """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
  30
  31     Keyword arguments:
  32     nodes -- The list of nodes to plot.
  33     """
  34     xcoords = []
  35     ycoords = []
  36     for n in nodes:
  37         xcoords.append(n[0] - MINX)
  38         ycoords.append(n[1] - MINY)
  39     return (xcoords, ycoords)
  40
  41 def plot_car(pose):
  42     """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
  43
  44     Keyword arguments:
  45     pose -- The pose of a car.
  46     """
  47     lfx = pose[0]
  48     lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  49     lfx += BCAR_DF * cos(pose[2])
  50     lfx += BCAR_SD * cos(pose[2])
  51
  52     lf3x = pose[0]
  53     lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  54     lf3x += 2/3 * BCAR_DF * cos(pose[2])
  55     lf3x += BCAR_SD * cos(pose[2])
  56
  57     lrx = pose[0]
  58     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  59     lrx += -BCAR_DR * cos(pose[2])
  60     lrx += -BCAR_SD * cos(pose[2])
  61
  62     rrx = pose[0]
  63     rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  64     rrx += -BCAR_DR * cos(pose[2])
  65     rrx += -BCAR_SD * cos(pose[2])
  66
  67     rfx = pose[0]
  68     rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  69     rfx += BCAR_DF * cos(pose[2])
  70     rfx += BCAR_SD * cos(pose[2])
  71
  72     rf3x = pose[0]
  73     rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  74     rf3x += 2/3 * BCAR_DF * cos(pose[2])
  75     rf3x += BCAR_SD * cos(pose[2])
  76
  77     lfy = pose[1]
  78     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  79     lfy += BCAR_DF * sin(pose[2])
  80     lfy += BCAR_SD * sin(pose[2])
  81
  82     lf3y = pose[1]
  83     lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  84     lf3y += 2/3 * BCAR_DF * sin(pose[2])
  85     lf3y += BCAR_SD * sin(pose[2])
  86
  87     lry = pose[1]
  88     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  89     lry += -BCAR_DR * sin(pose[2])
  90     lry += -BCAR_SD * sin(pose[2])
  91
  92     rry = pose[1]
  93     rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
  94     rry += -BCAR_DR * sin(pose[2])
  95     rry += -BCAR_SD * sin(pose[2])
  96
  97     rfy = pose[1]
  98     rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
  99     rfy += BCAR_DF * sin(pose[2])
 100     rfy += BCAR_SD * sin(pose[2])
 101
 102     rf3y = pose[1]
 103     rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 104     rf3y += 2/3 * BCAR_DF * sin(pose[2])
 105     rf3y += BCAR_SD * sin(pose[2])
 106
 107     cfx = pose[0]
 108     cfx += BCAR_DF * cos(pose[2])
 109     cfx += BCAR_SD * cos(pose[2])
 110
 111     cfy = pose[1]
 112     cfy += BCAR_DF * sin(pose[2])
 113     cfy += BCAR_SD * sin(pose[2])
 114
 115     xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
 116     ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
 117     return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
 118
 119 def plot_car_corners(pose):
 120     """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
 121
 122     Keyword arguments:
 123     pose -- The pose of a car.
 124     """
 125     lfx = pose[0]
 126     lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
 127     lfx += BCAR_DF * cos(pose[2])
 128     lfx += BCAR_SD * cos(pose[2])
 129
 130     lrx = pose[0]
 131     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
 132     lrx += -BCAR_DR * cos(pose[2])
 133     lrx += -BCAR_SD * cos(pose[2])
 134
 135     rrx = pose[0]
 136     rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
 137     rrx += -BCAR_DR * cos(pose[2])
 138     rrx += -BCAR_SD * cos(pose[2])
 139
 140     rfx = pose[0]
 141     rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
 142     rfx += BCAR_DF * cos(pose[2])
 143     rfx += BCAR_SD * cos(pose[2])
 144
 145     lfy = pose[1]
 146     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
 147     lfy += BCAR_DF * sin(pose[2])
 148     lfy += BCAR_SD * sin(pose[2])
 149
 150     lry = pose[1]
 151     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
 152     lry += -BCAR_DR * sin(pose[2])
 153     lry += -BCAR_SD * sin(pose[2])
 154
 155     rry = pose[1]
 156     rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 157     rry += -BCAR_DR * sin(pose[2])
 158     rry += -BCAR_SD * sin(pose[2])
 159
 160     rfy = pose[1]
 161     rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 162     rfy += BCAR_DF * sin(pose[2])
 163     rfy += BCAR_SD * sin(pose[2])
 164
 165     xcoords = (lfx, lrx, rrx, rfx)
 166     ycoords = (lfy, lry, rry, rfy)
 167     return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
 168
 169 if __name__ == "__main__":
 170     sc2 = None
 171     if (len(argv) == 2):
 172         SCEN_FILE = argv[1]
 173     elif (len(argv) == 3):
 174         SCEN_FILE = argv[1]
 175         SCEN_FILE2 = argv[2]
 176         sc2 = get_scenario(SCEN_FILE2)
 177     else:
 178         SCEN_FILE = "sc.json"
 179
 180     scenario = get_scenario(SCEN_FILE)
 181
 182     # Font size to be approximately the same in the paper:
 183     #   - sc1-0: 16
 184     #   - sc3-2, Intro: 12
 185     #   - sc4-0+: 22
 186     plt.rc('axes', unicode_minus=False)
 187     plt.rcParams["figure.figsize"] = [14, 7]
 188     plt.rcParams["font.family"] = "cmr10"
 189     plt.rcParams["font.size"] = 22
 190     plt.rcParams['hatch.linewidth'] = 0.5
 191     plt.rcParams['lines.linewidth'] = 1.0
 192     fig = plt.figure()
 193
 194     # here subplot starts
 195     ax = fig.add_subplot(111)
 196     ax.set_aspect("equal")
 197     ax.set_title("Real-world parking scenario")
 198     ax.set_title("Simple parking scenario")
 199     ax.set_xlabel("x [m]")
 200     ax.set_ylabel("y [m]")
 201     # For stage, comment upper, uncomment following:
 202     #plt.xticks([])
 203     #plt.yticks([])
 204
 205     # sps 99
 206     #ax.set_xlim([4, 32]) # 28
 207     #ax.set_ylim([7, 29]) # 22
 208     # sps 551
 209     #ax.set_xlim([-1, 27]) # 28
 210     #ax.set_ylim([-1, 21]) # 22
 211     # sps 591
 212     #ax.set_xlim([-9.9, 18.1]) # 28
 213     #ax.set_ylim([-4.9, 17.1]) # 22
 214     # sps 783
 215     #ax.set_xlim([4, 32]) # 28
 216     #ax.set_ylim([-4, 18]) # 22
 217     # sps 510
 218     #ax.set_xlim([-7, 21]) # 28
 219     #ax.set_ylim([7, 29]) # 22
 220     # sps 501
 221     #ax.set_xlim([-7, 21]) # 28
 222     #ax.set_ylim([2, 24]) # 22
 223     # sps 167
 224     #ax.set_xlim([-1, 27]) # 28
 225     #ax.set_ylim([-1, 21]) # 22
 226
 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")
 232
 233     # For Last Maneuver use:
 234     #ax.set_xlim([38, 44])
 235     #ax.set_ylim([3.1, 6.9])
 236
 237     # For Scenario 3-2 detail use:
 238     # - font size 22
 239     #ax.set_title("Scenario 1")
 240     #ax.set_xlim([1, 16]) # w=15
 241     #ax.set_ylim([35.1, 49.9]) # h=15
 242
 243     # For Scenario 4-0 detail use:
 244     # - font size 22
 245     #ax.set_title("Scenario 2")
 246     #ax.set_xlim([32, 53]) # w=19
 247     #ax.set_ylim([0.5, 19.5]) # h=18
 248
 249     # For Scenario 4-1 detail use:
 250     # - font size 22
 251     #ax.set_title("Scenario 3")
 252     #ax.set_xlim([32.5, 48.5]) # w=16
 253     #ax.set_ylim([1, 16]) # h=15
 254
 255     # For Scenario 5-1 detail use:
 256     # - font size 22
 257     #ax.set_title("Scenario 4")
 258     #ax.set_xlim([10.1, 27])
 259     #ax.set_ylim([10.1, 26])
 260
 261     # For Scenario 4-1-{0,14} detail use:
 262     # - font size 22
 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
 266
 267     # For Scenario 5-3-34 detail use:
 268     # - font size 22
 269     #ax.set_title("Scenario 7") # Scenario 8
 270     #ax.set_xlim([5.5, 27.5])
 271     #ax.set_ylim([15.1, 35])
 272
 273     # For Scenario 5-3-29 detail use:
 274     # - font size 22
 275     #ax.set_title("Scenario 8")
 276     #ax.set_xlim([5.5, 27.5])
 277     #ax.set_ylim([10.1, 30])
 278
 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")
 288
 289     # For scenario 5-3
 290     #ax.set_title("Computed goal")
 291     #ax.set_xlim([6.8, 16.2])
 292     #ax.set_ylim([15, 20])
 293
 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
 299
 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"]:
 307             if not o:
 308                 continue
 309             for n in o:
 310                 if n[0] < MINX:
 311                     MINX = n[0]
 312                 if n[1] < MINY:
 313                     MINY = n[1]
 314                 if n[0] > MAXX:
 315                     MAXX = n[0]
 316                 if n[1] > MAXY:
 317                     MAXY = n[1]
 318     print("w: {}, h: {}".format(abs(MAXX - MINX), abs(MAXY - MINY)))
 319     #MINY = -25
 320     #MINX = -10
 321     c1 = plt.Circle(
 322         (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
 323         4.8677125017335845,
 324         color='red',
 325         fill=False,
 326     )
 327     #ax.add_patch(c1)
 328     c2 = plt.Circle(
 329         (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
 330         3.1984427539075178,
 331         color='red',
 332         fill=False,
 333     )
 334     #ax.add_patch(c2)
 335     c3 = plt.Circle(
 336         (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
 337         5.736429638720212,
 338         color='red',
 339         fill=False,
 340     )
 341     #ax.add_patch(c3)
 342     c4 = plt.Circle(
 343         (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
 344         3.1984427539075178,
 345         color='red',
 346         fill=False,
 347     )
 348     #ax.add_patch(c4)
 349
 350     # Plot all the nodes (if exists.)
 351     if "nodes_x" in scenario and "nodes_y" in scenario:
 352         plt.plot(
 353             [x - MINX for x in scenario["nodes_x"]],
 354             [y - MINY for y in scenario["nodes_y"]],
 355             color="lightgray",
 356             marker="o",
 357             ms=2,
 358             lw=0,
 359         )
 360     # Plot all the steered2 nodes (if exists.)
 361     if "steered2_x" in scenario and "steered2_y" in scenario:
 362         plt.plot(
 363             [x - MINX for x in scenario["steered2_x"]],
 364             [y - MINY for y in scenario["steered2_y"]],
 365             color="orange",
 366             marker="o",
 367             ms=2,
 368             lw=0,
 369         )
 370     # Plot all the steered1 nodes (if exists.)
 371     if "steered1_x" in scenario and "steered1_y" in scenario:
 372         plt.plot(
 373             [x - MINX for x in scenario["steered1_x"]],
 374             [y - MINY for y in scenario["steered1_y"]],
 375             color="blue",
 376             marker="o",
 377             ms=2,
 378             lw=0,
 379         )
 380     # Plot obstacles, slot.
 381     if "obst" in scenario and len(scenario["obst"]) > 0:
 382         for o in scenario["obst"]:
 383             if not o:
 384                 continue
 385             ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//") #fill=True for stage
 386     if "slot" in scenario and len(scenario["slot"]) > 0:
 387         plt.plot(*plot_nodes(scenario["slot"]), color="blue", linewidth=1)
 388         #for s in scenario["slot"]:
 389         #    plt.plot(*plot_nodes(s), color="black")
 390
 391     # For the Possible Entry Configurations from the paper, use:
 392     #ax.set_title("Computed configurations")
 393     inits = "insides"
 394     inits_c = "green"
 395     if False and inits in scenario:
 396         max_i = len(scenario[inits]) - 1
 397         ii = 0
 398         i = scenario[inits][ii]
 399         plt.plot(*plot_car(i), color=inits_c)
 400         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 401         ii = int(max_i / 4)
 402         i = scenario[inits][ii]
 403         plt.plot(*plot_car(i), color=inits_c)
 404         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 405         ii = int(max_i / 2)
 406         i = scenario[inits][ii]
 407         plt.plot(*plot_car(i), color=inits_c)
 408         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 409         ii = int(max_i * 3/4)
 410         i = scenario[inits][ii]
 411         plt.plot(*plot_car(i), color=inits_c)
 412         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 413         ii = max_i
 414         i = scenario[inits][ii]
 415         plt.plot(*plot_car(i), color=inits_c)
 416         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 417     inits = "inits"
 418     inits_c = "orange"
 419     if True and inits in scenario:
 420         max_i = len(scenario[inits]) - 1
 421         ii = 0
 422         i = scenario[inits][ii]
 423         plt.plot(*plot_car(i), color=inits_c)
 424         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 425         ii = int(max_i / 4)
 426         i = scenario[inits][ii]
 427         plt.plot(*plot_car(i), color=inits_c)
 428         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 429         ii = int(max_i / 2)
 430         i = scenario[inits][ii]
 431         plt.plot(*plot_car(i), color=inits_c)
 432         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 433         ii = int(max_i * 3/4)
 434         i = scenario[inits][ii]
 435         plt.plot(*plot_car(i), color=inits_c)
 436         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 437         ii = max_i
 438         i = scenario[inits][ii]
 439         plt.plot(*plot_car(i), color=inits_c)
 440         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 441     # Possible/Candidate entries
 442     #ax.text(
 443     #    44.95,
 444     #    5.5,
 445     #    "p",
 446     #    color="blue",
 447     #    fontfamily="serif",
 448     #    fontstyle="italic",
 449     #)
 450     #ax.arrow(
 451     #    scenario["slot"][-1][0] - MINX,
 452     #    scenario["slot"][-1][1] - MINY,
 453     #    -2,
 454     #    0,
 455     #    width=0.05,
 456     #    color="blue",
 457     #    zorder=2,
 458     #)
 459     #ax.text(
 460     #    scenario["slot"][-1][0] - MINX - 2,
 461     #    5.5,
 462     #    "δ",
 463     #    color="blue",
 464     #    fontfamily="serif",
 465     #    fontstyle="italic",
 466     #)
 467
 468     # Plot `init`, `entry`, and `goal` configurations.
 469     if "init" in scenario and len(scenario["init"]) == 3:
 470         plt.plot(*plot_car(scenario["init"]), color="red")
 471         plt.plot(
 472             scenario["init"][0] - MINX,
 473             scenario["init"][1] - MINY,
 474             color="red",
 475             marker="+",
 476             ms=12
 477         )
 478     #if "init" in scenario and len(scenario["init"]) == 4:
 479     #    plt.plot(*plot_car(scenario["init"]), color="red")
 480     #    scenario["init"][2] = scenario["init"][3]
 481     #    plt.plot(*plot_car(scenario["init"]), color="red")
 482     #    plt.plot(
 483     #        scenario["init"][0] - MINX,
 484     #        scenario["init"][1] - MINY,
 485     #        color="red",
 486     #        marker="+",
 487     #        ms=12
 488     #    )
 489     #if "entries" in scenario:
 490     #    for e in scenario["entries"]:
 491     #        plt.plot(*plot_car(e), color="orange")
 492     #        plt.plot(
 493     #            e[0] - MINX,
 494     #            e[1] - MINY,
 495     #            color="orange",
 496     #            marker="+",
 497     #            ms=12
 498     #        )
 499     #if "entry" in scenario and len(scenario["entry"]) == 3:
 500     #    plt.plot(*plot_car(scenario["entry"]), color="magenta")
 501     #    plt.plot(
 502     #        scenario["entry"][0] - MINX,
 503     #        scenario["entry"][1] - MINY,
 504     #        color="magenta",
 505     #        marker="+",
 506     #        ms=12
 507     #    )
 508     #if "entry" in scenario and len(scenario["entry"]) == 4:
 509     #    esc = scenario["entry"]
 510     #    plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
 511     #    plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
 512     #    plt.plot(
 513     #        scenario["entry"][0] - MINX,
 514     #        scenario["entry"][1] - MINY,
 515     #        color="magenta",
 516     #        marker="+",
 517     #        ms=12
 518     #    )
 519     if "goal" in scenario:
 520         if len(scenario["goal"]) == 3:
 521             plt.plot(*plot_car(scenario["goal"]), color="green")
 522             plt.plot(
 523                 scenario["goal"][0] - MINX,
 524                 scenario["goal"][1] - MINY,
 525                 color="green",
 526                 marker="+",
 527                 ms=12
 528             )
 529     #    elif len(scenario["goal"]) == 4:
 530     #        ctp = scenario["goal"]
 531     #        plt.plot(*plot_car(scenario["goal"]), color="green")
 532     #        ctp[2] = ctp[3]
 533     #        plt.plot(*plot_car(scenario["goal"]), color="green")
 534     #        plt.plot(
 535     #            scenario["goal"][0] - MINX,
 536     #            scenario["goal"][1] - MINY,
 537     #            color="green",
 538     #            marker="+",
 539     #            ms=12
 540     #        )
 541
 542     # Plot `path` and `max_path`.
 543     if (sc2 and "opath" in sc2 and isinstance(sc2["opath"], list)
 544             and len(sc2["opath"]) > 0):
 545         plt.plot(*plot_nodes(sc2["opath"]), color="orange", linestyle="dotted")
 546     if (sc2 and "path" in sc2 and isinstance(sc2["path"], list)
 547             and len(sc2["path"]) > 0):
 548         plt.plot(*plot_nodes(sc2["path"]), color="orange")
 549     if ("opath" in scenario and isinstance(scenario["opath"], list)
 550             and len(scenario["opath"]) > 0):
 551         plt.plot(
 552             *plot_nodes(scenario["opath"]),
 553             color="blue",
 554             linewidth=1,
 555             linestyle="dotted",
 556         )
 557     if ("path" in scenario and isinstance(scenario["path"], list)
 558             and len(scenario["path"]) > 0):
 559         plt.plot(*plot_nodes(scenario["path"]), color="blue")
 560         for p in scenario["path"]:
 561             #plt.plot(*plot_car(p), color="blue")
 562             pass
 563             #cc = plot_car_corners(p)
 564             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 565             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 566             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 567             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 568     if "ispath" in scenario and len(scenario["ispath"]) > 0:
 569         plt.plot(*plot_nodes(scenario["ispath"]), color="green")
 570         for p in scenario["ispath"]:
 571             #plt.plot(*plot_car(p), color="green")
 572             pass
 573             #cc = plot_car_corners(p)
 574             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 575             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 576             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 577             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 578
 579     # If there are possible starts specified, you may print and plot them.
 580     #if "starts" in scenario and len(scenario["starts"]) > 0:
 581     #    print("possible starts:")
 582     #    for p in scenario["starts"]:
 583     #        plt.plot(*p, color="red", marker="+", ms=12)
 584     #        print(" {}".format(p))
 585
 586     # For the Last Maneuver figure from the paper, use:
 587     #   - `init2` -- orange
 588     #plt.plot(*plot_car(scenario["init2"]), color="orange")
 589     #plt.plot(
 590     #    scenario["init2"][0] - MINX,
 591     #    scenario["init2"][1] - MINY,
 592     #    color="orange",
 593     #    #marker="+",
 594     #    ms=12
 595     #)
 596     #   - `goal2` -- orange
 597     #plt.plot(*plot_car(scenario["goal2"]), color="orange")
 598     #plt.plot(
 599     #    scenario["goal2"][0] - MINX,
 600     #    scenario["goal2"][1] - MINY,
 601     #    color="orange",
 602     #    #marker="+",
 603     #    ms=12
 604     #)
 605     #   - `goal2` -- middle (orange)
 606     #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
 607     #plt.plot(
 608     #    scenario["goal2"][0] - MINX,
 609     #    scenario["goal2"][1] - MINY,
 610     #    color="orange",
 611     #    #marker="+",
 612     #    ms=12
 613     #)
 614     #   - `init1` -- green
 615     #plt.plot(*plot_car(scenario["init1"]), color="green")
 616     #plt.plot(
 617     #    scenario["init1"][0] - MINX,
 618     #    scenario["init1"][1] - MINY,
 619     #    color="green",
 620     #    #marker="+",
 621     #    ms=12
 622     #)
 623     #   - `goal1` -- green
 624     #plt.plot(*plot_car(scenario["goal1"]), color="green")
 625     #plt.plot(
 626     #    scenario["goal1"][0] - MINX,
 627     #    scenario["goal1"][1] - MINY,
 628     #    color="green",
 629     #    #marker="+",
 630     #    ms=12
 631     #)
 632
 633     # The `scenario` may also include:
 634     #   - `last` -- not sure what this is, see the source code. Maybe overlaps
 635     #     with the `goal`.
 636     #   - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
 637     #     Planner (PSP.))
 638     #   - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
 639     #     Planner (PSP.))
 640     #   - `max_orig_path` -- maximum original path. I used this when comparing
 641     #     original paths but I had to copy the `max_orig_path` by hand from
 642     #     different scenario result.
 643     #   - `orig_path` -- the path before the optimization.
 644     #   - `max_path` -- the maximum path after optimization. Must be copied by
 645     #     hand.
 646     #   - `path` -- optimized path of the scenario.
 647
 648     handles, labels = ax.get_legend_handles_labels()
 649
 650     # Uncommnent the following line and comment the plt.show() to store to the
 651     # file.
 652     plt.savefig("out.pdf", bbox_inches="tight")
 653     plt.close(fig)