plot.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 import importlib
  10 from path2traj import BicycleCar
  11
  12 BCAR_SD = 0
  13 # j1
  14 #BCAR_W = 1.771
  15 #BCAR_DF = 3.427
  16 #BCAR_DR = 0.657
  17 # wang 2017
  18 #BCAR_W = 1.81
  19 #BCAR_DF = 3.7
  20 #BCAR_DR = 4.85 - 3.7
  21 # jhang 2020
  22 #BCAR_W = 2.022
  23 #BCAR_DF = 4.236
  24 #BCAR_DR = 5.171 - 4.236
  25 # Opel Corsa
  26 #BCAR_W = 1.532
  27 #BCAR_DF = 3.212
  28 #BCAR_DR = 3.622 - BCAR_DF
  29 # Porsche Cayenne
  30 BCAR_W = 1.983
  31 BCAR_DF = 2.895 + 0.9
  32 BCAR_DR = 1.123
  33
  34 MINX = inf
  35 MINY = inf
  36
  37 def get_scenario(fname):
  38     """Load scenario from file."""
  39     if fname is None:
  40         raise ValueError("File name as argument needed")
  41     with open(fname, "r") as f:
  42         scenario = loads(f.read())
  43     return scenario
  44
  45 def plot_nodes(nodes=[]):
  46     """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
  47
  48     Keyword arguments:
  49     nodes -- The list of nodes to plot.
  50     """
  51     xcoords = []
  52     ycoords = []
  53     for n in nodes:
  54         xcoords.append(n[0] - MINX)
  55         ycoords.append(n[1] - MINY)
  56     return (xcoords, ycoords)
  57
  58 def plot_car(pose):
  59     """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
  60
  61     Keyword arguments:
  62     pose -- The pose of a car.
  63     """
  64     lfx = pose[0]
  65     lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  66     lfx += BCAR_DF * cos(pose[2])
  67     lfx += BCAR_SD * cos(pose[2])
  68
  69     lf3x = pose[0]
  70     lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  71     lf3x += 2/3 * BCAR_DF * cos(pose[2])
  72     lf3x += BCAR_SD * cos(pose[2])
  73
  74     lrx = pose[0]
  75     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  76     lrx += -BCAR_DR * cos(pose[2])
  77     lrx += -BCAR_SD * cos(pose[2])
  78
  79     rrx = pose[0]
  80     rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  81     rrx += -BCAR_DR * cos(pose[2])
  82     rrx += -BCAR_SD * cos(pose[2])
  83
  84     rfx = pose[0]
  85     rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  86     rfx += BCAR_DF * cos(pose[2])
  87     rfx += BCAR_SD * cos(pose[2])
  88
  89     rf3x = pose[0]
  90     rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  91     rf3x += 2/3 * BCAR_DF * cos(pose[2])
  92     rf3x += BCAR_SD * cos(pose[2])
  93
  94     lfy = pose[1]
  95     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  96     lfy += BCAR_DF * sin(pose[2])
  97     lfy += BCAR_SD * sin(pose[2])
  98
  99     lf3y = pose[1]
 100     lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
 101     lf3y += 2/3 * BCAR_DF * sin(pose[2])
 102     lf3y += BCAR_SD * sin(pose[2])
 103
 104     lry = pose[1]
 105     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
 106     lry += -BCAR_DR * sin(pose[2])
 107     lry += -BCAR_SD * sin(pose[2])
 108
 109     rry = pose[1]
 110     rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 111     rry += -BCAR_DR * sin(pose[2])
 112     rry += -BCAR_SD * sin(pose[2])
 113
 114     rfy = pose[1]
 115     rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 116     rfy += BCAR_DF * sin(pose[2])
 117     rfy += BCAR_SD * sin(pose[2])
 118
 119     rf3y = pose[1]
 120     rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 121     rf3y += 2/3 * BCAR_DF * sin(pose[2])
 122     rf3y += BCAR_SD * sin(pose[2])
 123
 124     cfx = pose[0]
 125     cfx += BCAR_DF * cos(pose[2])
 126     cfx += BCAR_SD * cos(pose[2])
 127
 128     cfy = pose[1]
 129     cfy += BCAR_DF * sin(pose[2])
 130     cfy += BCAR_SD * sin(pose[2])
 131
 132     xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
 133     ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
 134     return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
 135
 136 def plot_car_corners(pose):
 137     """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
 138
 139     Keyword arguments:
 140     pose -- The pose of a car.
 141     """
 142     lfx = pose[0]
 143     lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
 144     lfx += BCAR_DF * cos(pose[2])
 145     lfx += BCAR_SD * cos(pose[2])
 146
 147     lrx = pose[0]
 148     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
 149     lrx += -BCAR_DR * cos(pose[2])
 150     lrx += -BCAR_SD * cos(pose[2])
 151
 152     rrx = pose[0]
 153     rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
 154     rrx += -BCAR_DR * cos(pose[2])
 155     rrx += -BCAR_SD * cos(pose[2])
 156
 157     rfx = pose[0]
 158     rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
 159     rfx += BCAR_DF * cos(pose[2])
 160     rfx += BCAR_SD * cos(pose[2])
 161
 162     lfy = pose[1]
 163     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
 164     lfy += BCAR_DF * sin(pose[2])
 165     lfy += BCAR_SD * sin(pose[2])
 166
 167     lry = pose[1]
 168     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
 169     lry += -BCAR_DR * sin(pose[2])
 170     lry += -BCAR_SD * sin(pose[2])
 171
 172     rry = pose[1]
 173     rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 174     rry += -BCAR_DR * sin(pose[2])
 175     rry += -BCAR_SD * sin(pose[2])
 176
 177     rfy = pose[1]
 178     rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 179     rfy += BCAR_DF * sin(pose[2])
 180     rfy += BCAR_SD * sin(pose[2])
 181
 182     xcoords = (lfx, lrx, rrx, rfx)
 183     ycoords = (lfy, lry, rry, rfy)
 184     return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
 185
 186 if __name__ == "__main__":
 187     sc2 = None
 188     if (len(argv) == 2):
 189         SCEN_FILE = argv[1]
 190     elif (len(argv) == 3):
 191         SCEN_FILE = argv[1]
 192         SCEN_FILE2 = argv[2]
 193         sc2 = get_scenario(SCEN_FILE2)
 194     else:
 195         SCEN_FILE = "sc.json"
 196
 197     scenario = get_scenario(SCEN_FILE)
 198
 199     # Font size to be approximately the same in the paper:
 200     #   - sc1-0: 16
 201     #   - sc3-2, Intro: 12
 202     #   - sc4-0+: 22
 203     plt.rc('axes', unicode_minus=False)
 204     plt.rcParams["figure.figsize"] = [14, 7]
 205     plt.rcParams["font.family"] = "cmr10"
 206     plt.rcParams["font.size"] = 24
 207     plt.rcParams['hatch.linewidth'] = 0.5  # 6.0
 208     plt.rcParams['lines.linewidth'] = 1.0  # 2.0
 209     fig = plt.figure()
 210
 211     # here subplot starts
 212     ax = fig.add_subplot(111)
 213     ax.set_aspect("equal")
 214     ax.set_title(SCEN_FILE.split(".")[0])
 215     ax.set_xlabel("x [m]")
 216     ax.set_ylabel("y [m]")
 217     # For stage, comment upper, uncomment following:
 218     #plt.xticks([])
 219     #plt.yticks([])
 220
 221     # sps 99
 222     #ax.set_xlim([4, 32]) # 28
 223     #ax.set_ylim([7, 29]) # 22
 224     # sps 551
 225     #ax.set_xlim([-1, 27]) # 28
 226     #ax.set_ylim([-1, 21]) # 22
 227     # sps 591
 228     #ax.set_xlim([-9.9, 18.1]) # 28
 229     #ax.set_ylim([-4.9, 17.1]) # 22
 230     # sps 783
 231     #ax.set_xlim([4, 32]) # 28
 232     #ax.set_ylim([-4, 18]) # 22
 233     # sps 510
 234     #ax.set_xlim([-7, 21]) # 28
 235     #ax.set_ylim([7, 29]) # 22
 236     # sps 501
 237     #ax.set_xlim([-7, 21]) # 28
 238     #ax.set_ylim([2, 24]) # 22
 239     # sps 167
 240     #ax.set_xlim([-1, 27]) # 28
 241     #ax.set_ylim([-1, 21]) # 22
 242
 243     # For Possible Entry Points (Possible Entry Configurations) use:
 244     #plt.rcParams["font.size"] = 26
 245     #ax.set_xlim([37.6, 45.6])
 246     #ax.set_ylim([2.4, 8.5])
 247     #ax.set_title("Possible configurations")
 248
 249     # For Last Maneuver use:
 250     #ax.set_xlim([38, 44])
 251     #ax.set_ylim([3.1, 6.9])
 252
 253     # For Scenario 3-2 detail use:
 254     # - font size 22
 255     #ax.set_title("Scenario 1")
 256     #ax.set_xlim([1, 16]) # w=15
 257     #ax.set_ylim([35.1, 49.9]) # h=15
 258
 259     # For Scenario 4-0 detail use:
 260     # - font size 22
 261     #ax.set_title("Scenario 2")
 262     #ax.set_xlim([32, 53]) # w=19
 263     #ax.set_ylim([0.5, 19.5]) # h=18
 264
 265     # For Scenario 4-1 detail use:
 266     # - font size 22
 267     #ax.set_title("Scenario 3")
 268     #ax.set_xlim([32.5, 48.5]) # w=16
 269     #ax.set_ylim([1, 16]) # h=15
 270
 271     # For Scenario 5-1 detail use:
 272     # - font size 22
 273     #ax.set_title("Scenario 4")
 274     #ax.set_xlim([10.1, 27])
 275     #ax.set_ylim([10.1, 26])
 276
 277     # For Scenario 4-1-{0,14} detail use:
 278     # - font size 22
 279     #ax.set_title("Scenario 5") # Scenario 6
 280     #ax.set_xlim([32.5, 48.5]) # w=16
 281     #ax.set_ylim([1, 16.3]) # h=15.3
 282
 283     # For Scenario 5-3-34 detail use:
 284     # - font size 22
 285     #ax.set_title("Scenario 7") # Scenario 8
 286     #ax.set_xlim([5.5, 27.5])
 287     #ax.set_ylim([15.1, 35])
 288
 289     # For Scenario 5-3-29 detail use:
 290     # - font size 22
 291     #ax.set_title("Scenario 8")
 292     #ax.set_xlim([5.5, 27.5])
 293     #ax.set_ylim([10.1, 30])
 294
 295     # For Real-world parking scenario in Introduction section use:
 296     #ax.set_title("Real-world parking scenario with artificial obstacle")
 297     #ax.set_xlim([23, 58.5])
 298     #ax.set_ylim([-8.4, 20.9])
 299     #ax.text(34, 9.2, "Initial configuration", color="red")
 300     #ax.text(43.5, 8.5, "Final path", color="blue")
 301     #ax.text(48.25, 5.5, "Entry\nconfigurations", color="orange", ha="right")
 302     #ax.text(38, 3.8, "Parking\nslot", color="blue", ha="right", backgroundcolor="white")
 303     #ax.text(35.2, 5.5, "Goal configuration", color="green")
 304
 305     # For scenario 5-3
 306     #ax.set_title("Computed goal")
 307     #ax.set_xlim([6.8, 16.2])
 308     #ax.set_ylim([15, 20])
 309
 310     # For simple scenarios 92 and 96 (simple-1k-test49)
 311     # - Use MINY=-25, MINX=-10 for 96.
 312     #ax.set_title("Simple parking scenario")
 313     #ax.set_xlim([-11, 24]) # w=35
 314     #ax.set_ylim([-2.5, 37.5]) # h=40
 315
 316     # Set min and max to center the plot.
 317     MINX = scenario["init"][0]
 318     MINY = scenario["init"][1]
 319     MAXX = scenario["init"][0]
 320     MAXY = scenario["init"][1]
 321     if ("obst" in scenario and isinstance(scenario["obst"], list)
 322             and len(scenario["obst"]) > 0):
 323         for o in scenario["obst"]:
 324             if not o:
 325                 continue
 326             for n in o:
 327                 if n[0] < MINX:
 328                     MINX = n[0]
 329                 if n[1] < MINY:
 330                     MINY = n[1]
 331                 if n[0] > MAXX:
 332                     MAXX = n[0]
 333                 if n[1] > MAXY:
 334                     MAXY = n[1]
 335     print("w: {}, h: {}".format(abs(MAXX - MINX), abs(MAXY - MINY)))
 336     MINY = 0
 337     MINX = 0
 338     c1 = plt.Circle(
 339         (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
 340         4.8677125017335845,
 341         color='red',
 342         fill=False,
 343     )
 344     #ax.add_patch(c1)
 345     c2 = plt.Circle(
 346         (-744239.7727016528 - MINX, -1044308.987006895 - MINY),
 347         3.1984427539075178,
 348         color='red',
 349         fill=False,
 350     )
 351     #ax.add_patch(c2)
 352     c3 = plt.Circle(
 353         (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
 354         5.736429638720212,
 355         color='red',
 356         fill=False,
 357     )
 358     #ax.add_patch(c3)
 359     c4 = plt.Circle(
 360         (-744238.8067612824 - MINX, -1044309.1038891475 - MINY),
 361         3.1984427539075178,
 362         color='red',
 363         fill=False,
 364     )
 365     #ax.add_patch(c4)
 366
 367     # For Goal Zone figure, "Goal zone" file name in j1/figs/
 368     def r2d(w):
 369         return w*180.0/pi
 370     # ax.set_ylim([-4.8, 2.8])
 371     # ax.set_xlim([-13, 5])
 372     gz_ccr = matplotlib.patches.Arc(
 373         (-744206.185356 - MINX, -1044330.294266 - MINY),
 374         5.207071 * 2, 5.207071 * 2,
 375         theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
 376         theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
 377         color="magenta",
 378         fill=False,
 379         lw=2,
 380     )
 381     # ax.add_patch(gz_ccr)
 382     gz_ccr = matplotlib.patches.Arc(
 383         (-744206.185356 - MINX + 3.99, -1044330.294266 - MINY + 2.05),
 384         5.207071 * 2, 5.207071 * 2,
 385         theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
 386         theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
 387         color="magenta",
 388         fill=False,
 389         lw=2, ls="dotted",
 390     )
 391     # ax.add_patch(gz_ccr)
 392     gz_gh = 0.47424360277825361
 393     gz_ih = -0.27424360277825361
 394     def li(x, y, h, le=10.0):
 395         return (x, x + le * cos(h)), (y, y + le * sin(h))
 396     # gz border
 397     # plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_gh),
 398     #     color="orange", ls="dotted")
 399     # plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_ih),
 400     #     color="red", ls="dotted")
 401     # path
 402     # plt.plot(
 403     #     *li(-744208.5632466434 - MINX, -1044325.6618554679 - MINY, gz_gh, 4.47),
 404     #     color="orange", ls="solid")
 405     # plt.plot(
 406     #     *li(-744199.2632466434 - MINX, -1044323.6618554679 - MINY, gz_ih, -1.55),
 407     #     color="red", ls="solid")
 408     # ax.text(
 409     #     -744208.5632466434 - MINX,
 410     #     -1044325.6618554679 - MINY - 1.5,
 411     #     "C",
 412     #     color="orange",
 413     #     fontfamily="serif",
 414     #     fontstyle="italic",
 415     # )
 416     # ax.text(
 417     #     -744208.5632466434 - MINX + 0.35,
 418     #     -1044325.6618554679 - MINY - 1.7,
 419     #     "E",
 420     #     color="orange",
 421     #     fontfamily="serif",
 422     #     fontstyle="italic",
 423     #     fontsize=16,
 424     # )
 425     # ax.text(
 426     #     -744199.2632466434 - MINX,
 427     #     -1044323.6618554679 - MINY - 1.5,
 428     #     "C",
 429     #     color="red",
 430     #     fontfamily="serif",
 431     #     fontstyle="italic",
 432     # )
 433     # ax.text(
 434     #     -744199.2632466434 - MINX + 0.35,
 435     #     -1044323.6618554679 - MINY - 1.7,
 436     #     "g",
 437     #     color="red",
 438     #     fontfamily="serif",
 439     #     fontstyle="italic",
 440     #     fontsize=16,
 441     # )
 442     # ax.text(
 443     #     -744199.2632466434 - MINX,
 444     #     -1044323.6618554679 - MINY - 3.9,
 445     #     "θ",
 446     #     color="red",
 447     #     fontfamily="serif",
 448     #     fontstyle="italic",
 449     # )
 450     # ax.text(
 451     #     -744199.2632466434 - MINX + 0.35,
 452     #     -1044323.6618554679 - MINY - 4.1,
 453     #     "G",
 454     #     color="red",
 455     #     fontfamily="serif",
 456     #     fontstyle="italic",
 457     #     fontsize=16,
 458     # )
 459     # ax.arrow(
 460     #     -744199.2632466434 - MINX,
 461     #     -1044323.6618554679 - MINY - 3.18,
 462     #     cos(gz_ih),
 463     #     sin(gz_ih),
 464     #     width=0.05,
 465     #     color="red",
 466     #     zorder=2,
 467     # )
 468     # ax.text(
 469     #     -744199.2632466434 - MINX,
 470     #     -1044323.6618554679 - MINY + 1.9,
 471     #     "θ",
 472     #     color="orange",
 473     #     fontfamily="serif",
 474     #     fontstyle="italic",
 475     # )
 476     # ax.text(
 477     #     -744199.2632466434 - MINX + 0.35,
 478     #     -1044323.6618554679 - MINY + 1.7,
 479     #     "E",
 480     #     color="orange",
 481     #     fontfamily="serif",
 482     #     fontstyle="italic",
 483     #     fontsize=16,
 484     # )
 485     # ax.arrow(
 486     #     -744199.2632466434 - MINX,
 487     #     -1044323.6618554679 - MINY + 1.22,
 488     #     cos(gz_gh),
 489     #     sin(gz_gh),
 490     #     width=0.05,
 491     #     color="orange",
 492     #     zorder=2,
 493     # )
 494     # ax.text(
 495     #     -744199.2632466434 - MINX + 2,
 496     #     -1044323.6618554679 - MINY + -3,
 497     #     "G",
 498     #     color="dimgray",
 499     #     fontfamily="serif",
 500     #     fontstyle="normal",
 501     #     fontweight="bold",
 502     #     backgroundcolor="white",
 503     # )
 504     # ax.fill((
 505     #         -MINX -744204.775115,
 506     #         -MINX -744204.775115 + 15 * cos(0.47424360277825361),
 507     #         -MINX -744204.775115 + 15 * cos(0.27424360277825361),
 508     #         -MINX -744204.775115,
 509     #     ), (
 510     #         -MINY -1044325.281765,
 511     #         -MINY -1044325.281765 + 15 * sin(0.47424360277825361),
 512     #         -MINY -1044325.281765 - 15 * sin(0.27424360277825361),
 513     #         -MINY -1044325.281765,
 514     #     ), color="gainsboro", fill=False, hatch="x")
 515     # # --- End of Goal Zone figure ---
 516
 517     # Plot all the nodes (if exists.)
 518     if "nodes_x" in scenario and "nodes_y" in scenario:
 519         plt.plot(
 520             [x - MINX for x in scenario["nodes_x"]],
 521             [y - MINY for y in scenario["nodes_y"]],
 522             color="lightgray",
 523             marker="o",
 524             ms=2,
 525             lw=0,
 526         )
 527     # Plot all the steered2 nodes (if exists.)
 528     if "steered2_x" in scenario and "steered2_y" in scenario:
 529         plt.plot(
 530             [x - MINX for x in scenario["steered2_x"]],
 531             [y - MINY for y in scenario["steered2_y"]],
 532             color="orange",
 533             marker="o",
 534             ms=2,
 535             lw=0,
 536         )
 537     # Plot all the steered1 nodes (if exists.)
 538     if "steered1_x" in scenario and "steered1_y" in scenario:
 539         plt.plot(
 540             [x - MINX for x in scenario["steered1_x"]],
 541             [y - MINY for y in scenario["steered1_y"]],
 542             color="blue",
 543             marker="o",
 544             ms=2,
 545             lw=0,
 546         )
 547     # Plot obstacles, slot.
 548     if ("obst" in scenario and isinstance(scenario["obst"], list)
 549             and len(scenario["obst"]) > 0):
 550         for o in scenario["obst"]:
 551             if not o:
 552                 continue
 553             ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//") #fill=True for stage
 554     if "slot" in scenario and len(scenario["slot"]) > 0:
 555         plt.plot(*plot_nodes(scenario["slot"]), color="blue", linewidth=1)
 556         #for s in scenario["slot"]:
 557         #    plt.plot(*plot_nodes(s), color="black")
 558
 559     # For the Possible Entry Configurations from the paper, use:
 560     #ax.set_title("Computed configurations")
 561     inits = "insides"
 562     inits_c = "green"
 563     if False and inits in scenario:
 564         max_i = len(scenario[inits]) - 1
 565         ii = 0
 566         i = scenario[inits][ii]
 567         plt.plot(*plot_car(i), color=inits_c)
 568         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 569         ii = int(max_i / 4)
 570         i = scenario[inits][ii]
 571         plt.plot(*plot_car(i), color=inits_c)
 572         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 573         ii = int(max_i / 2)
 574         i = scenario[inits][ii]
 575         plt.plot(*plot_car(i), color=inits_c)
 576         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 577         ii = int(max_i * 3/4)
 578         i = scenario[inits][ii]
 579         plt.plot(*plot_car(i), color=inits_c)
 580         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 581         ii = max_i
 582         i = scenario[inits][ii]
 583         plt.plot(*plot_car(i), color=inits_c)
 584         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 585     inits = "inits"
 586     inits_c = "orange"
 587     if True and inits in scenario:
 588         max_i = len(scenario[inits]) - 1
 589         ii = 0
 590         i = scenario[inits][ii]
 591         plt.plot(*plot_car(i), color=inits_c)
 592         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 593         ii = int(max_i / 4)
 594         i = scenario[inits][ii]
 595         plt.plot(*plot_car(i), color=inits_c)
 596         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 597         ii = int(max_i / 2)
 598         i = scenario[inits][ii]
 599         plt.plot(*plot_car(i), color=inits_c)
 600         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 601         ii = int(max_i * 3/4)
 602         i = scenario[inits][ii]
 603         plt.plot(*plot_car(i), color=inits_c)
 604         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 605         ii = max_i
 606         i = scenario[inits][ii]
 607         plt.plot(*plot_car(i), color=inits_c)
 608         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 609     # Possible/Candidate entries
 610     #ax.text(
 611     #    44.95,
 612     #    5.5,
 613     #    "p",
 614     #    color="blue",
 615     #    fontfamily="serif",
 616     #    fontstyle="italic",
 617     #)
 618     #ax.arrow(
 619     #    scenario["slot"][-1][0] - MINX,
 620     #    scenario["slot"][-1][1] - MINY,
 621     #    -2,
 622     #    0,
 623     #    width=0.05,
 624     #    color="blue",
 625     #    zorder=2,
 626     #)
 627     #ax.text(
 628     #    scenario["slot"][-1][0] - MINX - 2,
 629     #    5.5,
 630     #    "δ",
 631     #    color="blue",
 632     #    fontfamily="serif",
 633     #    fontstyle="italic",
 634     #)
 635
 636     # Plot `init`, `entry`, and `goal` configurations.
 637     if "init" in scenario and len(scenario["init"]) == 3:
 638         plt.plot(*plot_car(scenario["init"]), color="red")
 639         plt.plot(
 640             scenario["init"][0] - MINX,
 641             scenario["init"][1] - MINY,
 642             color="red",
 643             marker="+",
 644             markeredgewidth=2,
 645             ms=24
 646         )
 647     #if "init" in scenario and len(scenario["init"]) == 4:
 648     #    plt.plot(*plot_car(scenario["init"]), color="red")
 649     #    scenario["init"][2] = scenario["init"][3]
 650     #    plt.plot(*plot_car(scenario["init"]), color="red")
 651     #    plt.plot(
 652     #        scenario["init"][0] - MINX,
 653     #        scenario["init"][1] - MINY,
 654     #        color="red",
 655     #        marker="+",
 656     #        ms=12
 657     #    )
 658     #if "entries" in scenario:
 659     #    for e in scenario["entries"]:
 660     #        plt.plot(*plot_car(e), color="orange")
 661     #        plt.plot(
 662     #            e[0] - MINX,
 663     #            e[1] - MINY,
 664     #            color="orange",
 665     #            marker="+",
 666     #            ms=12
 667     #        )
 668     if False and "entry" in scenario and len(scenario["entry"]) == 3:
 669         plt.plot(*plot_car(scenario["entry"]), color="magenta")
 670         plt.plot(
 671             scenario["entry"][0] - MINX,
 672             scenario["entry"][1] - MINY,
 673             color="magenta",
 674             marker="+",
 675             markeredgewidth=2,
 676             ms=24
 677         )
 678     if False and "entry" in scenario and len(scenario["entry"]) == 4:
 679         esc = scenario["entry"]
 680         plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
 681         plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
 682         plt.plot(
 683             scenario["entry"][0] - MINX,
 684             scenario["entry"][1] - MINY,
 685             color="magenta",
 686             marker="+",
 687             ms=12
 688         )
 689     if "goal" in scenario:
 690         if len(scenario["goal"]) == 3:
 691             #plt.plot(*plot_car(scenario["goal"]), color="green")
 692             plt.plot(*plot_car(scenario["goal"]), color="orange")
 693             plt.plot(
 694                 scenario["goal"][0] - MINX,
 695                 scenario["goal"][1] - MINY,
 696                 #color="green",
 697                 color="orange",
 698                 marker="+",
 699                 markeredgewidth=2,
 700                 ms=24
 701             )
 702     #    elif len(scenario["goal"]) == 4:
 703     #        ctp = scenario["goal"]
 704     #        plt.plot(*plot_car(scenario["goal"]), color="green")
 705     #        ctp[2] = ctp[3]
 706     #        plt.plot(*plot_car(scenario["goal"]), color="green")
 707     #        plt.plot(
 708     #            scenario["goal"][0] - MINX,
 709     #            scenario["goal"][1] - MINY,
 710     #            color="green",
 711     #            marker="+",
 712     #            ms=12
 713     #        )
 714
 715     # Plot `path` and `max_path`.
 716     if False and (sc2 and "opath" in sc2 and isinstance(sc2["opath"], list)
 717             and len(sc2["opath"]) > 0):
 718         plt.plot(*plot_nodes(sc2["opath"]), color="orange", linestyle="dotted")
 719     if (sc2 and "path" in sc2 and isinstance(sc2["path"], list)
 720             and len(sc2["path"]) > 0):
 721         plt.plot(*plot_nodes(sc2["path"]), color="orange")
 722     if False and ("opath" in scenario and isinstance(scenario["opath"], list)
 723             and len(scenario["opath"]) > 0):
 724         plt.plot(
 725             *plot_nodes(scenario["opath"]),
 726             color="blue",
 727             linewidth=1,
 728             linestyle="dotted",
 729         )
 730     if ("path" in scenario and isinstance(scenario["path"], list)
 731             and len(scenario["path"]) > 0):
 732         plt.plot(*plot_nodes(scenario["path"]), color="blue")
 733         i = 0
 734         for p in scenario["path"]:
 735             if len(p) > 5:
 736                 pc = {36: "red", 0: "blue", -36: "green"}
 737                 pm = "o" if p[5] else "+"
 738                 plt.plot(p[0] - MINX, p[1] - MINY, color=pc[p[4]], marker=pm)
 739                 #plt.plot(*plot_car(p), color="blue")
 740                 pass
 741                 #cc = plot_car_corners(p)
 742                 #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 743                 #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 744                 #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 745                 #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 746             else:
 747                 plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker="+")
 748     if ("traj" in scenario and isinstance(scenario["traj"], list)
 749             and len(scenario["traj"]) > 0):
 750         print(len(scenario["traj"]))
 751         # print(scenario["traj"])
 752         print(len(plot_nodes(scenario["traj"])[0]))
 753         # print(plot_nodes(scenario["traj"]))
 754         plt.plot(*plot_nodes(scenario["traj"]), color="red")
 755         plt.plot(*plot_car(scenario["traj"][-1]), color="red")
 756         for p in scenario["traj"]:
 757             if len(p) > 4:
 758                 if p[3] > 0:
 759                     color = "red" if p[3] == BicycleCar.max_wa else "darkred"
 760                     plt.plot(p[0] - MINX, p[1] - MINY, color=color, marker="+", markersize=2)
 761                 elif p[3] < 0:
 762                     color = "green" if -p[3] == BicycleCar.max_wa else "darkgreen"
 763                     plt.plot(p[0] - MINX, p[1] - MINY, color=color, marker="x", markersize=2)
 764                 else:
 765                     plt.plot(p[0] - MINX, p[1] - MINY, color="blue", marker=".", markersize=2)
 766             else:
 767                 plt.plot(p[0] - MINX, p[1] - MINY, color="red", marker=".")
 768             #plt.plot(*plot_car(p), color="red")
 769             pass
 770             #cc = plot_car_corners(p)
 771             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 772             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 773             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 774             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 775     if False and "ispath" in scenario and len(scenario["ispath"]) > 0:
 776         plt.plot(*plot_nodes(scenario["ispath"]), color="green")
 777         for p in scenario["ispath"]:
 778             #plt.plot(*plot_car(p), color="green")
 779             pass
 780             #cc = plot_car_corners(p)
 781             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 782             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 783             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 784             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 785
 786     # If there are possible starts specified, you may print and plot them.
 787     #if "starts" in scenario and len(scenario["starts"]) > 0:
 788     #    print("possible starts:")
 789     #    for p in scenario["starts"]:
 790     #        plt.plot(*p, color="red", marker="+", ms=12)
 791     #        print(" {}".format(p))
 792
 793     # For the Last Maneuver figure from the paper, use:
 794     #   - `init2` -- orange
 795     #plt.plot(*plot_car(scenario["init2"]), color="orange")
 796     #plt.plot(
 797     #    scenario["init2"][0] - MINX,
 798     #    scenario["init2"][1] - MINY,
 799     #    color="orange",
 800     #    #marker="+",
 801     #    ms=12
 802     #)
 803     #   - `goal2` -- orange
 804     #plt.plot(*plot_car(scenario["goal2"]), color="orange")
 805     #plt.plot(
 806     #    scenario["goal2"][0] - MINX,
 807     #    scenario["goal2"][1] - MINY,
 808     #    color="orange",
 809     #    #marker="+",
 810     #    ms=12
 811     #)
 812     #   - `goal2` -- middle (orange)
 813     #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
 814     #plt.plot(
 815     #    scenario["goal2"][0] - MINX,
 816     #    scenario["goal2"][1] - MINY,
 817     #    color="orange",
 818     #    #marker="+",
 819     #    ms=12
 820     #)
 821     #   - `init1` -- green
 822     #plt.plot(*plot_car(scenario["init1"]), color="green")
 823     #plt.plot(
 824     #    scenario["init1"][0] - MINX,
 825     #    scenario["init1"][1] - MINY,
 826     #    color="green",
 827     #    #marker="+",
 828     #    ms=12
 829     #)
 830     #   - `goal1` -- green
 831     #plt.plot(*plot_car(scenario["goal1"]), color="green")
 832     #plt.plot(
 833     #    scenario["goal1"][0] - MINX,
 834     #    scenario["goal1"][1] - MINY,
 835     #    color="green",
 836     #    #marker="+",
 837     #    ms=12
 838     #)
 839
 840     # The `scenario` may also include:
 841     #   - `last` -- not sure what this is, see the source code. Maybe overlaps
 842     #     with the `goal`.
 843     #   - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
 844     #     Planner (PSP.))
 845     #   - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
 846     #     Planner (PSP.))
 847     #   - `max_orig_path` -- maximum original path. I used this when comparing
 848     #     original paths but I had to copy the `max_orig_path` by hand from
 849     #     different scenario result.
 850     #   - `orig_path` -- the path before the optimization.
 851     #   - `max_path` -- the maximum path after optimization. Must be copied by
 852     #     hand.
 853     #   - `path` -- optimized path of the scenario.
 854
 855     handles, labels = ax.get_legend_handles_labels()
 856
 857     # Uncommnent the following line and comment the plt.show() to store to the
 858     # file.
 859     #plt.savefig("out.pdf", bbox_inches="tight")
 860     plt.savefig(argv[1] + ".pdf", bbox_inches="tight")
 861     plt.savefig(argv[1] + ".png", bbox_inches="tight")
 862     #plt.show()
 863     plt.close(fig)