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     lrx = pose[0]
  52     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  53     lrx += -BCAR_DR * cos(pose[2])
  54     lrx += -BCAR_SD * cos(pose[2])
  55
  56     rrx = pose[0]
  57     rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  58     rrx += -BCAR_DR * cos(pose[2])
  59     rrx += -BCAR_SD * cos(pose[2])
  60
  61     rfx = pose[0]
  62     rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  63     rfx += BCAR_DF * cos(pose[2])
  64     rfx += BCAR_SD * cos(pose[2])
  65
  66     lfy = pose[1]
  67     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  68     lfy += BCAR_DF * sin(pose[2])
  69     lfy += BCAR_SD * sin(pose[2])
  70
  71     lry = pose[1]
  72     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  73     lry += -BCAR_DR * sin(pose[2])
  74     lry += -BCAR_SD * sin(pose[2])
  75
  76     rry = pose[1]
  77     rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
  78     rry += -BCAR_DR * sin(pose[2])
  79     rry += -BCAR_SD * sin(pose[2])
  80
  81     rfy = pose[1]
  82     rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
  83     rfy += BCAR_DF * sin(pose[2])
  84     rfy += BCAR_SD * sin(pose[2])
  85
  86     xcoords = (lfx - MINX, lrx - MINX, rrx - MINX, rfx - MINX)
  87     ycoords = (lfy - MINY, lry - MINY, rry - MINY, rfy - MINY)
  88     return (xcoords, ycoords)
  89
  90 if __name__ == "__main__":
  91     if (len(argv) == 2):
  92         SCEN_FILE = argv[1]
  93     else:
  94         SCEN_FILE = "sc.json"
  95
  96     scenario = get_scenario(SCEN_FILE)
  97
  98     plt.rcParams["font.size"] = 24
  99     plt.rcParams['hatch.linewidth'] = 1.0
 100     fig = plt.figure()
 101
 102     # here subplot starts
 103     ax = fig.add_subplot(111)
 104     ax.set_aspect("equal")
 105     ax.set_title("{}".format(SCEN_FILE))
 106     ax.set_xlabel("x [m]")
 107     ax.set_ylabel("y [m]")
 108     #ax.set_xlim([34, 53]) # 19
 109     #ax.set_ylim([-1, 16]) # 17
 110     #ax.set_xlim([28, 53]) # 25
 111     #ax.set_ylim([-1, 21]) # 22
 112
 113     MINX = scenario["init"][0]
 114     MINY = scenario["init"][1]
 115     if "obst" in scenario and  len(scenario["obst"]) > 0:
 116         for o in scenario["obst"]:
 117             for n in o:
 118                 if n[0] < MINX:
 119                     MINX = n[0]
 120                 if n[1] < MINY:
 121                     MINY = n[1]
 122
 123     if "nodes_x" in scenario and "nodes_y" in scenario:
 124         plt.plot(
 125             scenario["nodes_x"] - MINX,
 126             scenario["nodes_y"] - MINY,
 127             color="lightgray",
 128             marker="o",
 129             ms=2,
 130             lw=0,
 131         )
 132     if "obst" in scenario and  len(scenario["obst"]) > 0:
 133         for o in scenario["obst"]:
 134             plt.plot(*plot_nodes(o), color="blue")
 135     if "last" in scenario and  len(scenario["last"]) > 0:
 136         for n in scenario["last"]:
 137             plt.plot(*plot_car(n), color="peachpuff")
 138     if "path" in scenario and  len(scenario["path"]) > 0:
 139         for n in scenario["path"]:
 140             plt.plot(*plot_car(n), color="peachpuff")
 141         plt.plot(*plot_nodes(scenario["path"]), color="orange")
 142     if "last" in scenario and  len(scenario["last"]) > 0:
 143         plt.plot(*plot_nodes(scenario["last"]), color="orange")
 144     if "slot" in scenario and len(scenario["slot"]) > 0:
 145         plt.plot(*plot_nodes(scenario["slot"][0]), color="gray")
 146         for s in scenario["slot"]:
 147             pass#plt.plot(*plot_nodes(s), color="gray")
 148     if "init" in scenario and len(scenario["init"]) == 3:
 149         plt.plot(*plot_car(scenario["init"]), color="red")
 150         plt.plot(
 151             scenario["init"][0] - MINX,
 152             scenario["init"][1] - MINY,
 153             color="red",
 154             marker="+",
 155             ms=12
 156         )
 157     if "goals" in scenario:
 158         for i in scenario["goals"]:
 159             if len(i) == 3:
 160                 plt.plot(*plot_car(i), color="darkseagreen")
 161                 plt.plot(i[0] - MINX, i[1] - MINY, color="darkseagreen", marker="+", ms=12)
 162     if "goal" in scenario and len(scenario["goal"]) == 3:
 163         # plot entry point
 164         plt.plot(*plot_car(scenario["goal"]), color="magenta")
 165         plt.plot(
 166             scenario["goal"][0] - MINX,
 167             scenario["goal"][1] - MINY,
 168             color="magenta",
 169             marker="+",
 170             ms=12
 171         )
 172     # plot gc
 173     if "last" in scenario:
 174         plt.plot(*plot_car(scenario["last"][0]), color="green")
 175         plt.plot(
 176             scenario["last"][0][0] - MINX,
 177             scenario["last"][0][1] - MINY,
 178             color="green",
 179             marker="+",
 180             ms=12
 181         )
 182     if "starts" in scenario and len(scenario["starts"]) > 0:
 183         print("possible starts:")
 184         for p in scenario["starts"]:
 185             plt.plot(*p, color="red", marker="+", ms=12)
 186             print(" {}".format(p))
 187
 188     handles, labels = ax.get_legend_handles_labels()
 189
 190     plt.show()
 191     plt.close(fig)