scripts/plot_json_objects_scenario.py

   1 """Plot JSON formatted scenario."""
   2 from json import loads
   3 from math import cos, pi, sin
   4 from math import inf
   5 from matplotlib import pyplot as plt
   6 from sys import argv, exit
   7
   8 BCAR_MTR = 10.820
   9 BCAR_WB = 2.450
  10 BCAR_W = 1.625
  11 BCAR_L = 3.760
  12 BCAR_SD = 0
  13 BCAR_DF = 3.105
  14 BCAR_DR = 0.655
  15
  16 MINX = inf
  17 MINY = inf
  18
  19 def get_scenario(fname):
  20     """Load scenario from file."""
  21     if fname is None:
  22         raise ValueError("File name as argument needed")
  23     with open(fname, "r") as f:
  24         scenario = loads(f.read())
  25     return scenario
  26
  27 def plot_nodes(nodes=[]):
  28     """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
  29
  30     Keyword arguments:
  31     nodes -- The list of nodes to plot.
  32     """
  33     xcoords = []
  34     ycoords = []
  35     for n in nodes:
  36         xcoords.append(n[0] - MINX)
  37         ycoords.append(n[1] - MINY)
  38     return (xcoords, ycoords)
  39
  40 def plot_car(pose):
  41     """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
  42
  43     Keyword arguments:
  44     pose -- The pose of a car.
  45     """
  46     lfx = pose[0]
  47     lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  48     lfx += BCAR_DF * cos(pose[2])
  49     lfx += BCAR_SD * cos(pose[2])
  50
  51     lf3x = pose[0]
  52     lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  53     lf3x += 2/3 * BCAR_DF * cos(pose[2])
  54     lf3x += BCAR_SD * cos(pose[2])
  55
  56     lrx = pose[0]
  57     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  58     lrx += -BCAR_DR * cos(pose[2])
  59     lrx += -BCAR_SD * cos(pose[2])
  60
  61     rrx = pose[0]
  62     rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  63     rrx += -BCAR_DR * cos(pose[2])
  64     rrx += -BCAR_SD * cos(pose[2])
  65
  66     rfx = pose[0]
  67     rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  68     rfx += BCAR_DF * cos(pose[2])
  69     rfx += BCAR_SD * cos(pose[2])
  70
  71     rf3x = pose[0]
  72     rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  73     rf3x += 2/3 * BCAR_DF * cos(pose[2])
  74     rf3x += BCAR_SD * cos(pose[2])
  75
  76     lfy = pose[1]
  77     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  78     lfy += BCAR_DF * sin(pose[2])
  79     lfy += BCAR_SD * sin(pose[2])
  80
  81     lf3y = pose[1]
  82     lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  83     lf3y += 2/3 * BCAR_DF * sin(pose[2])
  84     lf3y += BCAR_SD * sin(pose[2])
  85
  86     lry = pose[1]
  87     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  88     lry += -BCAR_DR * sin(pose[2])
  89     lry += -BCAR_SD * sin(pose[2])
  90
  91     rry = pose[1]
  92     rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
  93     rry += -BCAR_DR * sin(pose[2])
  94     rry += -BCAR_SD * sin(pose[2])
  95
  96     rfy = pose[1]
  97     rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
  98     rfy += BCAR_DF * sin(pose[2])
  99     rfy += BCAR_SD * sin(pose[2])
 100
 101     rf3y = pose[1]
 102     rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
 103     rf3y += 2/3 * BCAR_DF * sin(pose[2])
 104     rf3y += BCAR_SD * sin(pose[2])
 105
 106     cfx = pose[0]
 107     cfx += BCAR_DF * cos(pose[2])
 108     cfx += BCAR_SD * cos(pose[2])
 109
 110     cfy = pose[1]
 111     cfy += BCAR_DF * sin(pose[2])
 112     cfy += BCAR_SD * sin(pose[2])
 113
 114     xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
 115     ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
 116     return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
 117
 118 if __name__ == "__main__":
 119     sc2 = None
 120     if (len(argv) == 2):
 121         SCEN_FILE = argv[1]
 122     elif (len(argv) == 3):
 123         SCEN_FILE = argv[1]
 124         SCEN_FILE2 = argv[2]
 125         sc2 = get_scenario(SCEN_FILE2)
 126     else:
 127         SCEN_FILE = "sc.json"
 128
 129     scenario = get_scenario(SCEN_FILE)
 130
 131     # Font size to be approximately the same in the paper:
 132     #   - sc1-0: 16
 133     #   - sc3-2, Intro: 12
 134     #   - sc4-0+: 22
 135     plt.rcParams["font.family"] = "cmr10"
 136     plt.rcParams["font.size"] = 12
 137     plt.rcParams['hatch.linewidth'] = 1.0
 138     plt.rcParams['lines.linewidth'] = 1.0
 139     fig = plt.figure()
 140
 141     # here subplot starts
 142     ax = fig.add_subplot(111)
 143     ax.set_aspect("equal")
 144     ax.set_title("Real-world parking scenario")
 145     ax.set_xlabel("x [m]")
 146     ax.set_ylabel("y [m]")
 147
 148     # For Possible Entry Points (Possible Entry Configurations) use:
 149     #ax.set_xlim([35.6, 46.4])
 150     #ax.set_ylim([2.9, 2.9 + 6.84])
 151
 152     # For Last Maneuver use:
 153     #ax.set_xlim([38, 44])
 154     #ax.set_ylim([3.1, 6.9])
 155
 156     # For Scenario 3-2 detail use:
 157     #ax.set_xlim([0, 15])
 158     #ax.set_ylim([35, 50])
 159
 160     # For Real-world parking scenario in Introduction section use:
 161     #ax.set_xlim([32, 47.5])
 162     #ax.set_ylim([2.4, 9.9])
 163
 164     # Set min and max to center the plot.
 165     MINX = scenario["init"][0]
 166     MINY = scenario["init"][1]
 167     if "obst" in scenario and  len(scenario["obst"]) > 0:
 168         for o in scenario["obst"]:
 169             if not o:
 170                 continue
 171             for n in o:
 172                 if n[0] < MINX:
 173                     MINX = n[0]
 174                 if n[1] < MINY:
 175                     MINY = n[1]
 176
 177     # Plot all the nodes (if exists.)
 178     if "nodes_x" in scenario and "nodes_y" in scenario:
 179         plt.plot(
 180             [x - MINX for x in scenario["nodes_x"]],
 181             [y - MINY for y in scenario["nodes_y"]],
 182             color="lightgray",
 183             marker="o",
 184             ms=2,
 185             lw=0,
 186         )
 187     # Plot all the steered2 nodes (if exists.)
 188     if "steered2_x" in scenario and "steered2_y" in scenario:
 189         plt.plot(
 190             [x - MINX for x in scenario["steered2_x"]],
 191             [y - MINY for y in scenario["steered2_y"]],
 192             color="orange",
 193             marker="o",
 194             ms=2,
 195             lw=0,
 196         )
 197     # Plot all the steered1 nodes (if exists.)
 198     if "steered1_x" in scenario and "steered1_y" in scenario:
 199         plt.plot(
 200             [x - MINX for x in scenario["steered1_x"]],
 201             [y - MINY for y in scenario["steered1_y"]],
 202             color="blue",
 203             marker="o",
 204             ms=2,
 205             lw=0,
 206         )
 207     # Plot obstacles, slot.
 208     if "obst" in scenario and len(scenario["obst"]) > 0:
 209         for o in scenario["obst"]:
 210             if not o:
 211                 continue
 212             plt.plot(*plot_nodes(o), color="blue", linestyle=":")
 213     if "slot" in scenario and len(scenario["slot"]) > 0:
 214         plt.plot(*plot_nodes(scenario["slot"][0]), color="black")
 215         #for s in scenario["slot"]:
 216         #    plt.plot(*plot_nodes(s), color="black")
 217
 218     # Plot `init`, `entry`, and `goal` configurations.
 219     if "init" in scenario and len(scenario["init"]) == 3:
 220         plt.plot(*plot_car(scenario["init"]), color="red")
 221         plt.plot(
 222             scenario["init"][0] - MINX,
 223             scenario["init"][1] - MINY,
 224             color="red",
 225             marker="+",
 226             ms=12
 227         )
 228     if "entry" in scenario and len(scenario["entry"]) == 3:
 229         plt.plot(*plot_car(scenario["entry"]), color="magenta")
 230         plt.plot(
 231             scenario["entry"][0] - MINX,
 232             scenario["entry"][1] - MINY,
 233             color="magenta",
 234             marker="+",
 235             ms=12
 236         )
 237     if "goal" in scenario and len(scenario["goal"]) == 3:
 238         plt.plot(*plot_car(scenario["goal"]), color="green")
 239         plt.plot(
 240             scenario["goal"][0] - MINX,
 241             scenario["goal"][1] - MINY,
 242             color="green",
 243             marker="+",
 244             ms=12
 245         )
 246
 247     # Plot `path` and `max_path`.
 248     if sc2 and "path" in sc2 and  len(sc2["path"]) > 0:
 249         plt.plot(*plot_nodes(sc2["path"]), color="orange")
 250     if "path" in scenario and  len(scenario["path"]) > 0:
 251         plt.plot(*plot_nodes(scenario["path"]), color="green")
 252
 253     # If there are possible starts specified, you may print and plot them.
 254     #if "starts" in scenario and len(scenario["starts"]) > 0:
 255     #    print("possible starts:")
 256     #    for p in scenario["starts"]:
 257     #        plt.plot(*p, color="red", marker="+", ms=12)
 258     #        print(" {}".format(p))
 259
 260     # For the Last Maneuver figure from the paper, use:
 261     #   - `init2` -- orange
 262     #plt.plot(*plot_car(scenario["init2"]), color="orange")
 263     #plt.plot(
 264     #    scenario["init2"][0] - MINX,
 265     #    scenario["init2"][1] - MINY,
 266     #    color="orange",
 267     #    #marker="+",
 268     #    ms=12
 269     #)
 270     #   - `goal2` -- orange
 271     #plt.plot(*plot_car(scenario["goal2"]), color="orange")
 272     #plt.plot(
 273     #    scenario["goal2"][0] - MINX,
 274     #    scenario["goal2"][1] - MINY,
 275     #    color="orange",
 276     #    #marker="+",
 277     #    ms=12
 278     #)
 279     #   - `goal2` -- middle (orange)
 280     #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
 281     #plt.plot(
 282     #    scenario["goal2"][0] - MINX,
 283     #    scenario["goal2"][1] - MINY,
 284     #    color="orange",
 285     #    #marker="+",
 286     #    ms=12
 287     #)
 288     #   - `init1` -- green
 289     #plt.plot(*plot_car(scenario["init1"]), color="green")
 290     #plt.plot(
 291     #    scenario["init1"][0] - MINX,
 292     #    scenario["init1"][1] - MINY,
 293     #    color="green",
 294     #    #marker="+",
 295     #    ms=12
 296     #)
 297     #   - `goal1` -- green
 298     #plt.plot(*plot_car(scenario["goal1"]), color="green")
 299     #plt.plot(
 300     #    scenario["goal1"][0] - MINX,
 301     #    scenario["goal1"][1] - MINY,
 302     #    color="green",
 303     #    #marker="+",
 304     #    ms=12
 305     #)
 306
 307     # For the Possible Entry Configurations from the paper, use:
 308     #if "inits" in scenario:
 309     #    for i in scenario["inits"]:
 310     #        print(i)
 311     #        if len(i) == 3:
 312     #            plt.plot(*plot_car(i), color="gray")
 313     #            plt.plot(
 314     #                i[0] - MINX,
 315     #                i[1] - MINY,
 316     #                color="gray",
 317     #                marker="+",
 318     #                ms=12,
 319     #            )
 320     #    plt.plot(*plot_car(scenario["inits"][0]), color="magenta")
 321     #    plt.plot(
 322     #        scenario["inits"][0][0] - MINX,
 323     #        scenario["inits"][0][1] - MINY,
 324     #        color="magenta",
 325     #        marker="+",
 326     #        ms=12,
 327     #    )
 328
 329     # The `scenario` may also include:
 330     #   - `last` -- not sure what this is, see the source code. Maybe overlaps
 331     #     with the `goal`.
 332     #   - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
 333     #     Planner (PSP.))
 334     #   - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
 335     #     Planner (PSP.))
 336     #   - `max_orig_path` -- maximum original path. I used this when comparing
 337     #     original paths but I had to copy the `max_orig_path` by hand from
 338     #     different scenario result.
 339     #   - `orig_path` -- the path before the optimization.
 340     #   - `max_path` -- the maximum path after optimization. Must be copied by
 341     #     hand.
 342     #   - `path` -- optimized path of the scenario.
 343
 344     handles, labels = ax.get_legend_handles_labels()
 345
 346     # Uncommnent the following line and comment the plt.show() to store to the
 347     # file.
 348     plt.savefig("out.pdf", bbox_inches="tight")
 349     plt.close(fig)