scripts/plot_scenario.py

   1 #!/usr/bin/env python3
   2 """Plot JSON formatted scenario."""
   3 from json import loads
   4 from math import cos, pi, sin, atan, atan2
   5 from math import inf
   6 from matplotlib import pyplot as plt
   7 from sys import argv, exit
   8 import matplotlib
   9
  10 BCAR_SD = 0
  11 # j1
  12 #BCAR_W = 1.771
  13 #BCAR_DF = 3.427
  14 #BCAR_DR = 0.657
  15 # wang 2017
  16 #BCAR_W = 1.81
  17 #BCAR_DF = 3.7
  18 #BCAR_DR = 4.85 - 3.7
  19 # jhang 2020
  20 #BCAR_W = 2.022
  21 #BCAR_DF = 4.236
  22 #BCAR_DR = 5.171 - 4.236
  23 # Opel Corsa
  24 #BCAR_W = 1.532
  25 #BCAR_DF = 3.212
  26 #BCAR_DR = 3.622 - BCAR_DF
  27 # Porsche Cayenne
  28 BCAR_W = 1.983
  29 BCAR_DF = 2.895 + 0.9
  30 BCAR_DR = 1.123
  31
  32 MINX = inf
  33 MINY = inf
  34
  35 def get_scenario(fname):
  36     """Load scenario from file."""
  37     if fname is None:
  38         raise ValueError("File name as argument needed")
  39     with open(fname, "r") as f:
  40         scenario = loads(f.read())
  41     return scenario
  42
  43 def plot_nodes(nodes=[]):
  44     """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
  45
  46     Keyword arguments:
  47     nodes -- The list of nodes to plot.
  48     """
  49     xcoords = []
  50     ycoords = []
  51     for n in nodes:
  52         xcoords.append(n[0] - MINX)
  53         ycoords.append(n[1] - MINY)
  54     return (xcoords, ycoords)
  55
  56 def plot_car(pose):
  57     """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
  58
  59     Keyword arguments:
  60     pose -- The pose of a car.
  61     """
  62     lfx = pose[0]
  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])
  66
  67     lf3x = pose[0]
  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])
  71
  72     lrx = pose[0]
  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])
  76
  77     rrx = pose[0]
  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])
  81
  82     rfx = pose[0]
  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])
  86
  87     rf3x = pose[0]
  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])
  91
  92     lfy = pose[1]
  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])
  96
  97     lf3y = pose[1]
  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])
 101
 102     lry = pose[1]
 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])
 106
 107     rry = pose[1]
 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])
 111
 112     rfy = pose[1]
 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])
 116
 117     rf3y = pose[1]
 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])
 121
 122     cfx = pose[0]
 123     cfx += BCAR_DF * cos(pose[2])
 124     cfx += BCAR_SD * cos(pose[2])
 125
 126     cfy = pose[1]
 127     cfy += BCAR_DF * sin(pose[2])
 128     cfy += BCAR_SD * sin(pose[2])
 129
 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])
 133
 134 def plot_car_corners(pose):
 135     """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
 136
 137     Keyword arguments:
 138     pose -- The pose of a car.
 139     """
 140     lfx = pose[0]
 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])
 144
 145     lrx = pose[0]
 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])
 149
 150     rrx = pose[0]
 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])
 154
 155     rfx = pose[0]
 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])
 159
 160     lfy = pose[1]
 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])
 164
 165     lry = pose[1]
 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])
 169
 170     rry = pose[1]
 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])
 174
 175     rfy = pose[1]
 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])
 179
 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])
 183
 184 if __name__ == "__main__":
 185     sc2 = None
 186     if (len(argv) == 2):
 187         SCEN_FILE = argv[1]
 188     elif (len(argv) == 3):
 189         SCEN_FILE = argv[1]
 190         SCEN_FILE2 = argv[2]
 191         sc2 = get_scenario(SCEN_FILE2)
 192     else:
 193         SCEN_FILE = "sc.json"
 194
 195     scenario = get_scenario(SCEN_FILE)
 196
 197     # Font size to be approximately the same in the paper:
 198     #   - sc1-0: 16
 199     #   - sc3-2, Intro: 12
 200     #   - sc4-0+: 22
 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'] = 1  # 6.0
 206     plt.rcParams['lines.linewidth'] = 1  # 2.0
 207     fig = plt.figure()
 208
 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:
 216     #plt.xticks([])
 217     #plt.yticks([])
 218
 219     # sps 99
 220     #ax.set_xlim([4, 32]) # 28
 221     #ax.set_ylim([7, 29]) # 22
 222     # sps 551
 223     #ax.set_xlim([-1, 27]) # 28
 224     #ax.set_ylim([-1, 21]) # 22
 225     # sps 591
 226     #ax.set_xlim([-9.9, 18.1]) # 28
 227     #ax.set_ylim([-4.9, 17.1]) # 22
 228     # sps 783
 229     #ax.set_xlim([4, 32]) # 28
 230     #ax.set_ylim([-4, 18]) # 22
 231     # sps 510
 232     #ax.set_xlim([-7, 21]) # 28
 233     #ax.set_ylim([7, 29]) # 22
 234     # sps 501
 235     #ax.set_xlim([-7, 21]) # 28
 236     #ax.set_ylim([2, 24]) # 22
 237     # sps 167
 238     #ax.set_xlim([-1, 27]) # 28
 239     #ax.set_ylim([-1, 21]) # 22
 240
 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")
 246
 247     # For Last Maneuver use:
 248     #ax.set_xlim([38, 44])
 249     #ax.set_ylim([3.1, 6.9])
 250
 251     # For Scenario 3-2 detail use:
 252     # - font size 22
 253     #ax.set_title("Scenario 1")
 254     #ax.set_xlim([1, 16]) # w=15
 255     #ax.set_ylim([35.1, 49.9]) # h=15
 256
 257     # For Scenario 4-0 detail use:
 258     # - font size 22
 259     #ax.set_title("Scenario 2")
 260     #ax.set_xlim([32, 53]) # w=19
 261     #ax.set_ylim([0.5, 19.5]) # h=18
 262
 263     # For Scenario 4-1 detail use:
 264     # - font size 22
 265     #ax.set_title("Scenario 3")
 266     #ax.set_xlim([32.5, 48.5]) # w=16
 267     #ax.set_ylim([1, 16]) # h=15
 268
 269     # For Scenario 5-1 detail use:
 270     # - font size 22
 271     #ax.set_title("Scenario 4")
 272     #ax.set_xlim([10.1, 27])
 273     #ax.set_ylim([10.1, 26])
 274
 275     # For Scenario 4-1-{0,14} detail use:
 276     # - font size 22
 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
 280
 281     # For Scenario 5-3-34 detail use:
 282     # - font size 22
 283     #ax.set_title("Scenario 7") # Scenario 8
 284     #ax.set_xlim([5.5, 27.5])
 285     #ax.set_ylim([15.1, 35])
 286
 287     # For Scenario 5-3-29 detail use:
 288     # - font size 22
 289     #ax.set_title("Scenario 8")
 290     #ax.set_xlim([5.5, 27.5])
 291     #ax.set_ylim([10.1, 30])
 292
 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")
 302
 303     # For scenario 5-3
 304     #ax.set_title("Computed goal")
 305     #ax.set_xlim([6.8, 16.2])
 306     #ax.set_ylim([15, 20])
 307
 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
 313
 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"]:
 322             if not o:
 323                 continue
 324             for n in o:
 325                 if n[0] < MINX:
 326                     MINX = n[0]
 327                 if n[1] < MINY:
 328                     MINY = n[1]
 329                 if n[0] > MAXX:
 330                     MAXX = n[0]
 331                 if n[1] > MAXY:
 332                     MAXY = n[1]
 333     print("w: {}, h: {}".format(abs(MAXX - MINX), abs(MAXY - MINY)))
 334     MINY = 0
 335     MINX = 0
 336     c1 = plt.Circle(
 337         (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
 338         4.8677125017335845,
 339         color='red',
 340         fill=False,
 341     )
 342     #ax.add_patch(c1)
 343     c2 = plt.Circle(
 344         (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
 345         3.1984427539075178,
 346         color='red',
 347         fill=False,
 348     )
 349     #ax.add_patch(c2)
 350     c3 = plt.Circle(
 351         (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
 352         5.736429638720212,
 353         color='red',
 354         fill=False,
 355     )
 356     #ax.add_patch(c3)
 357     c4 = plt.Circle(
 358         (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
 359         3.1984427539075178,
 360         color='red',
 361         fill=False,
 362     )
 363     #ax.add_patch(c4)
 364
 365     # For Goal Zone figure, "Goal zone" file name in j1/figs/
 366     # def r2d(w):
 367     #     return w*180.0/pi
 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)),
 375     #     color="magenta",
 376     #     fill=False,
 377     #     lw=2,
 378     # )
 379     # ax.add_patch(gz_ccr)
 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)),
 385     #     color="magenta",
 386     #     fill=False,
 387     #     lw=2, ls="dotted",
 388     # )
 389     # ax.add_patch(gz_ccr)
 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))
 394     # # gz border
 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")
 399     # # path
 400     # plt.plot(
 401     #     *li(-744208.5632466434 - MINX, -1044325.6618554679 - MINY, gz_gh, 4.47),
 402     #     color="orange", ls="solid")
 403     # plt.plot(
 404     #     *li(-744199.2632466434 - MINX, -1044323.6618554679 - MINY, gz_ih, -1.55),
 405     #     color="red", ls="solid")
 406     # ax.text(
 407     #     -744208.5632466434 - MINX,
 408     #     -1044325.6618554679 - MINY - 1.5,
 409     #     "C",
 410     #     color="orange",
 411     #     fontfamily="serif",
 412     #     fontstyle="italic",
 413     # )
 414     # ax.text(
 415     #     -744208.5632466434 - MINX + 0.35,
 416     #     -1044325.6618554679 - MINY - 1.7,
 417     #     "E",
 418     #     color="orange",
 419     #     fontfamily="serif",
 420     #     fontstyle="italic",
 421     #     fontsize=16,
 422     # )
 423     # ax.text(
 424     #     -744199.2632466434 - MINX,
 425     #     -1044323.6618554679 - MINY - 1.5,
 426     #     "C",
 427     #     color="red",
 428     #     fontfamily="serif",
 429     #     fontstyle="italic",
 430     # )
 431     # ax.text(
 432     #     -744199.2632466434 - MINX + 0.35,
 433     #     -1044323.6618554679 - MINY - 1.7,
 434     #     "g",
 435     #     color="red",
 436     #     fontfamily="serif",
 437     #     fontstyle="italic",
 438     #     fontsize=16,
 439     # )
 440     # ax.text(
 441     #     -744199.2632466434 - MINX,
 442     #     -1044323.6618554679 - MINY - 3.9,
 443     #     "θ",
 444     #     color="red",
 445     #     fontfamily="serif",
 446     #     fontstyle="italic",
 447     # )
 448     # ax.text(
 449     #     -744199.2632466434 - MINX + 0.35,
 450     #     -1044323.6618554679 - MINY - 4.1,
 451     #     "G",
 452     #     color="red",
 453     #     fontfamily="serif",
 454     #     fontstyle="italic",
 455     #     fontsize=16,
 456     # )
 457     # ax.arrow(
 458     #     -744199.2632466434 - MINX,
 459     #     -1044323.6618554679 - MINY - 3.18,
 460     #     cos(gz_ih),
 461     #     sin(gz_ih),
 462     #     width=0.05,
 463     #     color="red",
 464     #     zorder=2,
 465     # )
 466     # ax.text(
 467     #     -744199.2632466434 - MINX,
 468     #     -1044323.6618554679 - MINY + 1.9,
 469     #     "θ",
 470     #     color="orange",
 471     #     fontfamily="serif",
 472     #     fontstyle="italic",
 473     # )
 474     # ax.text(
 475     #     -744199.2632466434 - MINX + 0.35,
 476     #     -1044323.6618554679 - MINY + 1.7,
 477     #     "E",
 478     #     color="orange",
 479     #     fontfamily="serif",
 480     #     fontstyle="italic",
 481     #     fontsize=16,
 482     # )
 483     # ax.arrow(
 484     #     -744199.2632466434 - MINX,
 485     #     -1044323.6618554679 - MINY + 1.22,
 486     #     cos(gz_gh),
 487     #     sin(gz_gh),
 488     #     width=0.05,
 489     #     color="orange",
 490     #     zorder=2,
 491     # )
 492     # ax.text(
 493     #     -744199.2632466434 - MINX + 2,
 494     #     -1044323.6618554679 - MINY + -3,
 495     #     "G",
 496     #     color="dimgray",
 497     #     fontfamily="serif",
 498     #     fontstyle="normal",
 499     #     fontweight="bold",
 500     #     backgroundcolor="white",
 501     # )
 502     # ax.fill((
 503     #         -MINX -744204.775115,
 504     #         -MINX -744204.775115 + 15 * cos(0.47424360277825361),
 505     #         -MINX -744204.775115 + 15 * cos(0.27424360277825361),
 506     #         -MINX -744204.775115,
 507     #     ), (
 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 ---
 514
 515     # Plot all the nodes (if exists.)
 516     if "nodes_x" in scenario and "nodes_y" in scenario:
 517         plt.plot(
 518             [x - MINX for x in scenario["nodes_x"]],
 519             [y - MINY for y in scenario["nodes_y"]],
 520             color="lightgray",
 521             marker="o",
 522             ms=2,
 523             lw=0,
 524         )
 525     # Plot all the steered2 nodes (if exists.)
 526     if "steered2_x" in scenario and "steered2_y" in scenario:
 527         plt.plot(
 528             [x - MINX for x in scenario["steered2_x"]],
 529             [y - MINY for y in scenario["steered2_y"]],
 530             color="orange",
 531             marker="o",
 532             ms=2,
 533             lw=0,
 534         )
 535     # Plot all the steered1 nodes (if exists.)
 536     if "steered1_x" in scenario and "steered1_y" in scenario:
 537         plt.plot(
 538             [x - MINX for x in scenario["steered1_x"]],
 539             [y - MINY for y in scenario["steered1_y"]],
 540             color="blue",
 541             marker="o",
 542             ms=2,
 543             lw=0,
 544         )
 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"]:
 549             if not o:
 550                 continue
 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")
 556
 557     # For the Possible Entry Configurations from the paper, use:
 558     #ax.set_title("Computed configurations")
 559     inits = "insides"
 560     inits_c = "green"
 561     if False and inits in scenario:
 562         max_i = len(scenario[inits]) - 1
 563         ii = 0
 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)
 567         ii = int(max_i / 4)
 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)
 571         ii = int(max_i / 2)
 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)
 579         ii = max_i
 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     inits = "inits"
 584     inits_c = "orange"
 585     if True and inits in scenario:
 586         max_i = len(scenario[inits]) - 1
 587         ii = 0
 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         ii = int(max_i / 4)
 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)
 595         ii = int(max_i / 2)
 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)
 603         ii = max_i
 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
 608     #ax.text(
 609     #    44.95,
 610     #    5.5,
 611     #    "p",
 612     #    color="blue",
 613     #    fontfamily="serif",
 614     #    fontstyle="italic",
 615     #)
 616     #ax.arrow(
 617     #    scenario["slot"][-1][0] - MINX,
 618     #    scenario["slot"][-1][1] - MINY,
 619     #    -2,
 620     #    0,
 621     #    width=0.05,
 622     #    color="blue",
 623     #    zorder=2,
 624     #)
 625     #ax.text(
 626     #    scenario["slot"][-1][0] - MINX - 2,
 627     #    5.5,
 628     #    "δ",
 629     #    color="blue",
 630     #    fontfamily="serif",
 631     #    fontstyle="italic",
 632     #)
 633
 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")
 637         plt.plot(
 638             scenario["init"][0] - MINX,
 639             scenario["init"][1] - MINY,
 640             color="red",
 641             marker="+",
 642             markeredgewidth=2,
 643             ms=24
 644         )
 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")
 649     #    plt.plot(
 650     #        scenario["init"][0] - MINX,
 651     #        scenario["init"][1] - MINY,
 652     #        color="red",
 653     #        marker="+",
 654     #        ms=12
 655     #    )
 656     #if "entries" in scenario:
 657     #    for e in scenario["entries"]:
 658     #        plt.plot(*plot_car(e), color="orange")
 659     #        plt.plot(
 660     #            e[0] - MINX,
 661     #            e[1] - MINY,
 662     #            color="orange",
 663     #            marker="+",
 664     #            ms=12
 665     #        )
 666     if "entry" in scenario and len(scenario["entry"]) == 3:
 667         plt.plot(*plot_car(scenario["entry"]), color="magenta")
 668         plt.plot(
 669             scenario["entry"][0] - MINX,
 670             scenario["entry"][1] - MINY,
 671             color="magenta",
 672             marker="+",
 673             markeredgewidth=2,
 674             ms=24
 675         )
 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")
 680     #    plt.plot(
 681     #        scenario["entry"][0] - MINX,
 682     #        scenario["entry"][1] - MINY,
 683     #        color="magenta",
 684     #        marker="+",
 685     #        ms=12
 686     #    )
 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")
 691             plt.plot(
 692                 scenario["goal"][0] - MINX,
 693                 scenario["goal"][1] - MINY,
 694                 #color="green",
 695                 color="orange",
 696                 marker="+",
 697                 markeredgewidth=2,
 698                 ms=24
 699             )
 700     #    elif len(scenario["goal"]) == 4:
 701     #        ctp = scenario["goal"]
 702     #        plt.plot(*plot_car(scenario["goal"]), color="green")
 703     #        ctp[2] = ctp[3]
 704     #        plt.plot(*plot_car(scenario["goal"]), color="green")
 705     #        plt.plot(
 706     #            scenario["goal"][0] - MINX,
 707     #            scenario["goal"][1] - MINY,
 708     #            color="green",
 709     #            marker="+",
 710     #            ms=12
 711     #        )
 712
 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):
 722         plt.plot(
 723             *plot_nodes(scenario["opath"]),
 724             color="blue",
 725             linewidth=1,
 726             linestyle="dotted",
 727         )
 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")
 731         i = 0
 732         for p in scenario["path"]:
 733             if False and len(p) > 4:
 734                 if p[4] > 0:
 735                     plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker="+")
 736                 elif p[4] < 0:
 737                     plt.plot(p[0] - MINX, p[1] - MINY, color="green", marker="x")
 738                 else:
 739                     plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker=".")
 740             else:
 741                 if len(p) > 5 and p[5]:  # parent is cusp
 742                     plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="o")
 743                 else:
 744                     plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="+")
 745             #plt.plot(*plot_car(p), color="blue")
 746             pass
 747             #cc = plot_car_corners(p)
 748             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 749             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 750             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 751             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 752     if ("traj" in scenario and isinstance(scenario["traj"], list)
 753             and len(scenario["traj"]) > 0):
 754         print(len(scenario["traj"]))
 755         # print(scenario["traj"])
 756         print(len(plot_nodes(scenario["traj"])[0]))
 757         # print(plot_nodes(scenario["traj"]))
 758         plt.plot(*plot_nodes(scenario["traj"]), color="red")
 759         plt.plot(*plot_car(scenario["traj"][-1]), color="red")
 760         for p in scenario["traj"]:
 761             if len(p) > 4:
 762                 if p[3] > 0:
 763                     plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker="+")
 764                 elif p[3] < 0:
 765                     plt.plot(p[0] - MINX, p[1] - MINY, color="green", marker="x")
 766                 else:
 767                     plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker=".")
 768             else:
 769                 plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker=".")
 770             #plt.plot(*plot_car(p), color="red")
 771             pass
 772             #cc = plot_car_corners(p)
 773             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 774             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 775             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 776             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 777     if "ispath" in scenario and len(scenario["ispath"]) > 0:
 778         plt.plot(*plot_nodes(scenario["ispath"]), color="green")
 779         for p in scenario["ispath"]:
 780             #plt.plot(*plot_car(p), color="green")
 781             pass
 782             #cc = plot_car_corners(p)
 783             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 784             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 785             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 786             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 787
 788     # If there are possible starts specified, you may print and plot them.
 789     #if "starts" in scenario and len(scenario["starts"]) > 0:
 790     #    print("possible starts:")
 791     #    for p in scenario["starts"]:
 792     #        plt.plot(*p, color="red", marker="+", ms=12)
 793     #        print(" {}".format(p))
 794
 795     # For the Last Maneuver figure from the paper, use:
 796     #   - `init2` -- orange
 797     #plt.plot(*plot_car(scenario["init2"]), color="orange")
 798     #plt.plot(
 799     #    scenario["init2"][0] - MINX,
 800     #    scenario["init2"][1] - MINY,
 801     #    color="orange",
 802     #    #marker="+",
 803     #    ms=12
 804     #)
 805     #   - `goal2` -- orange
 806     #plt.plot(*plot_car(scenario["goal2"]), color="orange")
 807     #plt.plot(
 808     #    scenario["goal2"][0] - MINX,
 809     #    scenario["goal2"][1] - MINY,
 810     #    color="orange",
 811     #    #marker="+",
 812     #    ms=12
 813     #)
 814     #   - `goal2` -- middle (orange)
 815     #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
 816     #plt.plot(
 817     #    scenario["goal2"][0] - MINX,
 818     #    scenario["goal2"][1] - MINY,
 819     #    color="orange",
 820     #    #marker="+",
 821     #    ms=12
 822     #)
 823     #   - `init1` -- green
 824     #plt.plot(*plot_car(scenario["init1"]), color="green")
 825     #plt.plot(
 826     #    scenario["init1"][0] - MINX,
 827     #    scenario["init1"][1] - MINY,
 828     #    color="green",
 829     #    #marker="+",
 830     #    ms=12
 831     #)
 832     #   - `goal1` -- green
 833     #plt.plot(*plot_car(scenario["goal1"]), color="green")
 834     #plt.plot(
 835     #    scenario["goal1"][0] - MINX,
 836     #    scenario["goal1"][1] - MINY,
 837     #    color="green",
 838     #    #marker="+",
 839     #    ms=12
 840     #)
 841
 842     # The `scenario` may also include:
 843     #   - `last` -- not sure what this is, see the source code. Maybe overlaps
 844     #     with the `goal`.
 845     #   - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
 846     #     Planner (PSP.))
 847     #   - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
 848     #     Planner (PSP.))
 849     #   - `max_orig_path` -- maximum original path. I used this when comparing
 850     #     original paths but I had to copy the `max_orig_path` by hand from
 851     #     different scenario result.
 852     #   - `orig_path` -- the path before the optimization.
 853     #   - `max_path` -- the maximum path after optimization. Must be copied by
 854     #     hand.
 855     #   - `path` -- optimized path of the scenario.
 856
 857     handles, labels = ax.get_legend_handles_labels()
 858
 859     # Uncommnent the following line and comment the plt.show() to store to the
 860     # file.
 861     #plt.savefig("out.pdf", bbox_inches="tight")
 862     plt.show()
 863     plt.close(fig)