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, 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_MTR = 10.820
  11 BCAR_WB = 2.450
  12 BCAR_W = 1.625
  13 BCAR_L = 3.760
  14 BCAR_SD = 0
  15 BCAR_DF = 3.105
  16 BCAR_DR = 0.655
  17
  18 MINX = inf
  19 MINY = inf
  20
  21 def get_scenario(fname):
  22     """Load scenario from file."""
  23     if fname is None:
  24         raise ValueError("File name as argument needed")
  25     with open(fname, "r") as f:
  26         scenario = loads(f.read())
  27     return scenario
  28
  29 def plot_nodes(nodes=[]):
  30     """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
  31
  32     Keyword arguments:
  33     nodes -- The list of nodes to plot.
  34     """
  35     xcoords = []
  36     ycoords = []
  37     for n in nodes:
  38         xcoords.append(n[0] - MINX)
  39         ycoords.append(n[1] - MINY)
  40     return (xcoords, ycoords)
  41
  42 def plot_car(pose):
  43     """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
  44
  45     Keyword arguments:
  46     pose -- The pose of a car.
  47     """
  48     lfx = pose[0]
  49     lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  50     lfx += BCAR_DF * cos(pose[2])
  51     lfx += BCAR_SD * cos(pose[2])
  52
  53     lf3x = pose[0]
  54     lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  55     lf3x += 2/3 * BCAR_DF * cos(pose[2])
  56     lf3x += BCAR_SD * cos(pose[2])
  57
  58     lrx = pose[0]
  59     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  60     lrx += -BCAR_DR * cos(pose[2])
  61     lrx += -BCAR_SD * cos(pose[2])
  62
  63     rrx = pose[0]
  64     rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  65     rrx += -BCAR_DR * cos(pose[2])
  66     rrx += -BCAR_SD * cos(pose[2])
  67
  68     rfx = pose[0]
  69     rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  70     rfx += BCAR_DF * cos(pose[2])
  71     rfx += BCAR_SD * cos(pose[2])
  72
  73     rf3x = pose[0]
  74     rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  75     rf3x += 2/3 * BCAR_DF * cos(pose[2])
  76     rf3x += BCAR_SD * cos(pose[2])
  77
  78     lfy = pose[1]
  79     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  80     lfy += BCAR_DF * sin(pose[2])
  81     lfy += BCAR_SD * sin(pose[2])
  82
  83     lf3y = pose[1]
  84     lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  85     lf3y += 2/3 * BCAR_DF * sin(pose[2])
  86     lf3y += BCAR_SD * sin(pose[2])
  87
  88     lry = pose[1]
  89     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  90     lry += -BCAR_DR * sin(pose[2])
  91     lry += -BCAR_SD * sin(pose[2])
  92
  93     rry = pose[1]
  94     rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
  95     rry += -BCAR_DR * sin(pose[2])
  96     rry += -BCAR_SD * sin(pose[2])
  97
  98     rfy = pose[1]
  99     rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 100     rfy += BCAR_DF * sin(pose[2])
 101     rfy += BCAR_SD * sin(pose[2])
 102
 103     rf3y = pose[1]
 104     rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 105     rf3y += 2/3 * BCAR_DF * sin(pose[2])
 106     rf3y += BCAR_SD * sin(pose[2])
 107
 108     cfx = pose[0]
 109     cfx += BCAR_DF * cos(pose[2])
 110     cfx += BCAR_SD * cos(pose[2])
 111
 112     cfy = pose[1]
 113     cfy += BCAR_DF * sin(pose[2])
 114     cfy += BCAR_SD * sin(pose[2])
 115
 116     xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
 117     ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
 118     return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
 119
 120 def plot_car_corners(pose):
 121     """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
 122
 123     Keyword arguments:
 124     pose -- The pose of a car.
 125     """
 126     lfx = pose[0]
 127     lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
 128     lfx += BCAR_DF * cos(pose[2])
 129     lfx += BCAR_SD * cos(pose[2])
 130
 131     lrx = pose[0]
 132     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
 133     lrx += -BCAR_DR * cos(pose[2])
 134     lrx += -BCAR_SD * cos(pose[2])
 135
 136     rrx = pose[0]
 137     rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
 138     rrx += -BCAR_DR * cos(pose[2])
 139     rrx += -BCAR_SD * cos(pose[2])
 140
 141     rfx = pose[0]
 142     rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
 143     rfx += BCAR_DF * cos(pose[2])
 144     rfx += BCAR_SD * cos(pose[2])
 145
 146     lfy = pose[1]
 147     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
 148     lfy += BCAR_DF * sin(pose[2])
 149     lfy += BCAR_SD * sin(pose[2])
 150
 151     lry = pose[1]
 152     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
 153     lry += -BCAR_DR * sin(pose[2])
 154     lry += -BCAR_SD * sin(pose[2])
 155
 156     rry = pose[1]
 157     rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 158     rry += -BCAR_DR * sin(pose[2])
 159     rry += -BCAR_SD * sin(pose[2])
 160
 161     rfy = pose[1]
 162     rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 163     rfy += BCAR_DF * sin(pose[2])
 164     rfy += BCAR_SD * sin(pose[2])
 165
 166     xcoords = (lfx, lrx, rrx, rfx)
 167     ycoords = (lfy, lry, rry, rfy)
 168     return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
 169
 170 if __name__ == "__main__":
 171     sc2 = None
 172     if (len(argv) == 2):
 173         SCEN_FILE = argv[1]
 174     elif (len(argv) == 3):
 175         SCEN_FILE = argv[1]
 176         SCEN_FILE2 = argv[2]
 177         sc2 = get_scenario(SCEN_FILE2)
 178     else:
 179         SCEN_FILE = "sc.json"
 180
 181     scenario = get_scenario(SCEN_FILE)
 182
 183     # Font size to be approximately the same in the paper:
 184     #   - sc1-0: 16
 185     #   - sc3-2, Intro: 12
 186     #   - sc4-0+: 22
 187     plt.rc('axes', unicode_minus=False)
 188     plt.rcParams["figure.figsize"] = [14, 7]
 189     plt.rcParams["font.family"] = "cmr10"
 190     plt.rcParams["font.size"] = 24
 191     plt.rcParams['hatch.linewidth'] = 6.0
 192     plt.rcParams['lines.linewidth'] = 2.0
 193     fig = plt.figure()
 194
 195     # here subplot starts
 196     ax = fig.add_subplot(111)
 197     ax.set_aspect("equal")
 198     #ax.set_title("Real-world parking scenario")
 199     ax.set_xlabel("x [m]")
 200     ax.set_ylabel("y [m]")
 201     # For stage, comment upper, uncomment following:
 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     # For Goal Zone figure, "Goal zone" file name in j1/figs/
 351     def r2d(w):
 352         return w*180.0/pi
 353     ax.set_ylim([-4.8, 2.8])
 354     ax.set_xlim([-13, 5])
 355     gz_ccr = matplotlib.patches.Arc(
 356         (-744206.185356 - MINX, -1044330.294266 - MINY),
 357         5.207071 * 2, 5.207071 * 2,
 358         theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
 359         theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
 360         color="magenta",
 361         fill=False,
 362         lw=2,
 363     )
 364     ax.add_patch(gz_ccr)
 365     gz_ccr = matplotlib.patches.Arc(
 366         (-744206.185356 - MINX + 3.99, -1044330.294266 - MINY + 2.05),
 367         5.207071 * 2, 5.207071 * 2,
 368         theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
 369         theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
 370         color="magenta",
 371         fill=False,
 372         lw=2, ls="dotted",
 373     )
 374     ax.add_patch(gz_ccr)
 375     gz_gh = 0.47424360277825361
 376     gz_ih = -0.27424360277825361
 377     def li(x, y, h, le=10.0):
 378         return (x, x + le * cos(h)), (y, y + le * sin(h))
 379     # gz border
 380     plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_gh),
 381         color="orange", ls="dotted")
 382     plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_ih),
 383         color="red", ls="dotted")
 384     # path
 385     plt.plot(
 386         *li(-744208.5632466434 - MINX, -1044325.6618554679 - MINY, gz_gh, 4.47),
 387         color="orange", ls="solid")
 388     plt.plot(
 389         *li(-744199.2632466434 - MINX, -1044323.6618554679 - MINY, gz_ih, -1.55),
 390         color="red", ls="solid")
 391     ax.text(
 392         -744208.5632466434 - MINX,
 393         -1044325.6618554679 - MINY - 1.5,
 394         "C",
 395         color="orange",
 396         fontfamily="serif",
 397         fontstyle="italic",
 398     )
 399     ax.text(
 400         -744208.5632466434 - MINX + 0.35,
 401         -1044325.6618554679 - MINY - 1.7,
 402         "E",
 403         color="orange",
 404         fontfamily="serif",
 405         fontstyle="italic",
 406         fontsize=16,
 407     )
 408     ax.text(
 409         -744199.2632466434 - MINX,
 410         -1044323.6618554679 - MINY - 1.5,
 411         "C",
 412         color="red",
 413         fontfamily="serif",
 414         fontstyle="italic",
 415     )
 416     ax.text(
 417         -744199.2632466434 - MINX + 0.35,
 418         -1044323.6618554679 - MINY - 1.7,
 419         "g",
 420         color="red",
 421         fontfamily="serif",
 422         fontstyle="italic",
 423         fontsize=16,
 424     )
 425     ax.text(
 426         -744199.2632466434 - MINX,
 427         -1044323.6618554679 - MINY - 3.9,
 428         "θ",
 429         color="red",
 430         fontfamily="serif",
 431         fontstyle="italic",
 432     )
 433     ax.text(
 434         -744199.2632466434 - MINX + 0.35,
 435         -1044323.6618554679 - MINY - 4.1,
 436         "G",
 437         color="red",
 438         fontfamily="serif",
 439         fontstyle="italic",
 440         fontsize=16,
 441     )
 442     ax.arrow(
 443         -744199.2632466434 - MINX,
 444         -1044323.6618554679 - MINY - 3.18,
 445         cos(gz_ih),
 446         sin(gz_ih),
 447         width=0.05,
 448         color="red",
 449         zorder=2,
 450     )
 451     ax.text(
 452         -744199.2632466434 - MINX,
 453         -1044323.6618554679 - MINY + 1.9,
 454         "θ",
 455         color="orange",
 456         fontfamily="serif",
 457         fontstyle="italic",
 458     )
 459     ax.text(
 460         -744199.2632466434 - MINX + 0.35,
 461         -1044323.6618554679 - MINY + 1.7,
 462         "E",
 463         color="orange",
 464         fontfamily="serif",
 465         fontstyle="italic",
 466         fontsize=16,
 467     )
 468     ax.arrow(
 469         -744199.2632466434 - MINX,
 470         -1044323.6618554679 - MINY + 1.22,
 471         cos(gz_gh),
 472         sin(gz_gh),
 473         width=0.05,
 474         color="orange",
 475         zorder=2,
 476     )
 477     ax.text(
 478         -744199.2632466434 - MINX + 2,
 479         -1044323.6618554679 - MINY + -3,
 480         "G",
 481         color="dimgray",
 482         fontfamily="serif",
 483         fontstyle="normal",
 484         fontweight="bold",
 485         backgroundcolor="white",
 486     )
 487     ax.fill((
 488             -MINX -744204.775115,
 489             -MINX -744204.775115 + 15 * cos(0.47424360277825361),
 490             -MINX -744204.775115 + 15 * cos(0.27424360277825361),
 491             -MINX -744204.775115,
 492         ), (
 493             -MINY -1044325.281765,
 494             -MINY -1044325.281765 + 15 * sin(0.47424360277825361),
 495             -MINY -1044325.281765 - 15 * sin(0.27424360277825361),
 496             -MINY -1044325.281765,
 497         ), color="gainsboro", fill=False, hatch="x")
 498     # --- End of Goal Zone figure ---
 499
 500     # Plot all the nodes (if exists.)
 501     if "nodes_x" in scenario and "nodes_y" in scenario:
 502         plt.plot(
 503             [x - MINX for x in scenario["nodes_x"]],
 504             [y - MINY for y in scenario["nodes_y"]],
 505             color="lightgray",
 506             marker="o",
 507             ms=2,
 508             lw=0,
 509         )
 510     # Plot all the steered2 nodes (if exists.)
 511     if "steered2_x" in scenario and "steered2_y" in scenario:
 512         plt.plot(
 513             [x - MINX for x in scenario["steered2_x"]],
 514             [y - MINY for y in scenario["steered2_y"]],
 515             color="orange",
 516             marker="o",
 517             ms=2,
 518             lw=0,
 519         )
 520     # Plot all the steered1 nodes (if exists.)
 521     if "steered1_x" in scenario and "steered1_y" in scenario:
 522         plt.plot(
 523             [x - MINX for x in scenario["steered1_x"]],
 524             [y - MINY for y in scenario["steered1_y"]],
 525             color="blue",
 526             marker="o",
 527             ms=2,
 528             lw=0,
 529         )
 530     # Plot obstacles, slot.
 531     if "obst" in scenario and len(scenario["obst"]) > 0:
 532         for o in scenario["obst"]:
 533             if not o:
 534                 continue
 535             ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//") #fill=True for stage
 536     if "slot" in scenario and len(scenario["slot"]) > 0:
 537         plt.plot(*plot_nodes(scenario["slot"]), color="blue", linewidth=1)
 538         #for s in scenario["slot"]:
 539         #    plt.plot(*plot_nodes(s), color="black")
 540
 541     # For the Possible Entry Configurations from the paper, use:
 542     #ax.set_title("Computed configurations")
 543     inits = "insides"
 544     inits_c = "green"
 545     if False and inits in scenario:
 546         max_i = len(scenario[inits]) - 1
 547         ii = 0
 548         i = scenario[inits][ii]
 549         plt.plot(*plot_car(i), color=inits_c)
 550         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 551         ii = int(max_i / 4)
 552         i = scenario[inits][ii]
 553         plt.plot(*plot_car(i), color=inits_c)
 554         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 555         ii = int(max_i / 2)
 556         i = scenario[inits][ii]
 557         plt.plot(*plot_car(i), color=inits_c)
 558         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 559         ii = int(max_i * 3/4)
 560         i = scenario[inits][ii]
 561         plt.plot(*plot_car(i), color=inits_c)
 562         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 563         ii = max_i
 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     inits = "inits"
 568     inits_c = "orange"
 569     if True and inits in scenario:
 570         max_i = len(scenario[inits]) - 1
 571         ii = 0
 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 / 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 = int(max_i / 2)
 580         i = scenario[inits][ii]
 581         plt.plot(*plot_car(i), color=inits_c)
 582         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 583         ii = int(max_i * 3/4)
 584         i = scenario[inits][ii]
 585         plt.plot(*plot_car(i), color=inits_c)
 586         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 587         ii = max_i
 588         i = scenario[inits][ii]
 589         plt.plot(*plot_car(i), color=inits_c)
 590         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
 591     # Possible/Candidate entries
 592     #ax.text(
 593     #    44.95,
 594     #    5.5,
 595     #    "p",
 596     #    color="blue",
 597     #    fontfamily="serif",
 598     #    fontstyle="italic",
 599     #)
 600     #ax.arrow(
 601     #    scenario["slot"][-1][0] - MINX,
 602     #    scenario["slot"][-1][1] - MINY,
 603     #    -2,
 604     #    0,
 605     #    width=0.05,
 606     #    color="blue",
 607     #    zorder=2,
 608     #)
 609     #ax.text(
 610     #    scenario["slot"][-1][0] - MINX - 2,
 611     #    5.5,
 612     #    "δ",
 613     #    color="blue",
 614     #    fontfamily="serif",
 615     #    fontstyle="italic",
 616     #)
 617
 618     # Plot `init`, `entry`, and `goal` configurations.
 619     if "init" in scenario and len(scenario["init"]) == 3:
 620         plt.plot(*plot_car(scenario["init"]), color="red")
 621         plt.plot(
 622             scenario["init"][0] - MINX,
 623             scenario["init"][1] - MINY,
 624             color="red",
 625             marker="+",
 626             markeredgewidth=2,
 627             ms=24
 628         )
 629     #if "init" in scenario and len(scenario["init"]) == 4:
 630     #    plt.plot(*plot_car(scenario["init"]), color="red")
 631     #    scenario["init"][2] = scenario["init"][3]
 632     #    plt.plot(*plot_car(scenario["init"]), color="red")
 633     #    plt.plot(
 634     #        scenario["init"][0] - MINX,
 635     #        scenario["init"][1] - MINY,
 636     #        color="red",
 637     #        marker="+",
 638     #        ms=12
 639     #    )
 640     #if "entries" in scenario:
 641     #    for e in scenario["entries"]:
 642     #        plt.plot(*plot_car(e), color="orange")
 643     #        plt.plot(
 644     #            e[0] - MINX,
 645     #            e[1] - MINY,
 646     #            color="orange",
 647     #            marker="+",
 648     #            ms=12
 649     #        )
 650     if "entry" in scenario and len(scenario["entry"]) == 3:
 651         plt.plot(*plot_car(scenario["entry"]), color="magenta")
 652         plt.plot(
 653             scenario["entry"][0] - MINX,
 654             scenario["entry"][1] - MINY,
 655             color="magenta",
 656             marker="+",
 657             markeredgewidth=2,
 658             ms=24
 659         )
 660     #if "entry" in scenario and len(scenario["entry"]) == 4:
 661     #    esc = scenario["entry"]
 662     #    plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
 663     #    plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
 664     #    plt.plot(
 665     #        scenario["entry"][0] - MINX,
 666     #        scenario["entry"][1] - MINY,
 667     #        color="magenta",
 668     #        marker="+",
 669     #        ms=12
 670     #    )
 671     if "goal" in scenario:
 672         if len(scenario["goal"]) == 3:
 673             #plt.plot(*plot_car(scenario["goal"]), color="green")
 674             plt.plot(*plot_car(scenario["goal"]), color="orange")
 675             plt.plot(
 676                 scenario["goal"][0] - MINX,
 677                 scenario["goal"][1] - MINY,
 678                 #color="green",
 679                 color="orange",
 680                 marker="+",
 681                 markeredgewidth=2,
 682                 ms=24
 683             )
 684     #    elif len(scenario["goal"]) == 4:
 685     #        ctp = scenario["goal"]
 686     #        plt.plot(*plot_car(scenario["goal"]), color="green")
 687     #        ctp[2] = ctp[3]
 688     #        plt.plot(*plot_car(scenario["goal"]), color="green")
 689     #        plt.plot(
 690     #            scenario["goal"][0] - MINX,
 691     #            scenario["goal"][1] - MINY,
 692     #            color="green",
 693     #            marker="+",
 694     #            ms=12
 695     #        )
 696
 697     # Plot `path` and `max_path`.
 698     if (sc2 and "opath" in sc2 and isinstance(sc2["opath"], list)
 699             and len(sc2["opath"]) > 0):
 700         plt.plot(*plot_nodes(sc2["opath"]), color="orange", linestyle="dotted")
 701     if (sc2 and "path" in sc2 and isinstance(sc2["path"], list)
 702             and len(sc2["path"]) > 0):
 703         plt.plot(*plot_nodes(sc2["path"]), color="orange")
 704     if ("opath" in scenario and isinstance(scenario["opath"], list)
 705             and len(scenario["opath"]) > 0):
 706         plt.plot(
 707             *plot_nodes(scenario["opath"]),
 708             color="blue",
 709             linewidth=1,
 710             linestyle="dotted",
 711         )
 712     if ("path" in scenario and isinstance(scenario["path"], list)
 713             and len(scenario["path"]) > 0):
 714         plt.plot(*plot_nodes(scenario["path"]), color="blue")
 715         for p in scenario["path"]:
 716             #plt.plot(*plot_car(p), color="blue")
 717             pass
 718             #cc = plot_car_corners(p)
 719             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 720             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 721             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 722             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 723     if "ispath" in scenario and len(scenario["ispath"]) > 0:
 724         plt.plot(*plot_nodes(scenario["ispath"]), color="green")
 725         for p in scenario["ispath"]:
 726             #plt.plot(*plot_car(p), color="green")
 727             pass
 728             #cc = plot_car_corners(p)
 729             #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
 730             #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
 731             #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
 732             #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
 733
 734     # If there are possible starts specified, you may print and plot them.
 735     #if "starts" in scenario and len(scenario["starts"]) > 0:
 736     #    print("possible starts:")
 737     #    for p in scenario["starts"]:
 738     #        plt.plot(*p, color="red", marker="+", ms=12)
 739     #        print(" {}".format(p))
 740
 741     # For the Last Maneuver figure from the paper, use:
 742     #   - `init2` -- orange
 743     #plt.plot(*plot_car(scenario["init2"]), color="orange")
 744     #plt.plot(
 745     #    scenario["init2"][0] - MINX,
 746     #    scenario["init2"][1] - MINY,
 747     #    color="orange",
 748     #    #marker="+",
 749     #    ms=12
 750     #)
 751     #   - `goal2` -- orange
 752     #plt.plot(*plot_car(scenario["goal2"]), color="orange")
 753     #plt.plot(
 754     #    scenario["goal2"][0] - MINX,
 755     #    scenario["goal2"][1] - MINY,
 756     #    color="orange",
 757     #    #marker="+",
 758     #    ms=12
 759     #)
 760     #   - `goal2` -- middle (orange)
 761     #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
 762     #plt.plot(
 763     #    scenario["goal2"][0] - MINX,
 764     #    scenario["goal2"][1] - MINY,
 765     #    color="orange",
 766     #    #marker="+",
 767     #    ms=12
 768     #)
 769     #   - `init1` -- green
 770     #plt.plot(*plot_car(scenario["init1"]), color="green")
 771     #plt.plot(
 772     #    scenario["init1"][0] - MINX,
 773     #    scenario["init1"][1] - MINY,
 774     #    color="green",
 775     #    #marker="+",
 776     #    ms=12
 777     #)
 778     #   - `goal1` -- green
 779     #plt.plot(*plot_car(scenario["goal1"]), color="green")
 780     #plt.plot(
 781     #    scenario["goal1"][0] - MINX,
 782     #    scenario["goal1"][1] - MINY,
 783     #    color="green",
 784     #    #marker="+",
 785     #    ms=12
 786     #)
 787
 788     # The `scenario` may also include:
 789     #   - `last` -- not sure what this is, see the source code. Maybe overlaps
 790     #     with the `goal`.
 791     #   - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
 792     #     Planner (PSP.))
 793     #   - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
 794     #     Planner (PSP.))
 795     #   - `max_orig_path` -- maximum original path. I used this when comparing
 796     #     original paths but I had to copy the `max_orig_path` by hand from
 797     #     different scenario result.
 798     #   - `orig_path` -- the path before the optimization.
 799     #   - `max_path` -- the maximum path after optimization. Must be copied by
 800     #     hand.
 801     #   - `path` -- optimized path of the scenario.
 802
 803     handles, labels = ax.get_legend_handles_labels()
 804
 805     # Uncommnent the following line and comment the plt.show() to store to the
 806     # file.
 807     plt.savefig("out.pdf", bbox_inches="tight")
 808     plt.close(fig)