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 matplotlib import pyplot as plt
   5 from sys import argv, exit
   6
   7 BCAR_MTR = 10.820
   8 BCAR_WB = 2.450
   9 BCAR_W = 1.625
  10 BCAR_L = 3.760
  11 BCAR_SD = 0
  12 BCAR_DF = 3.105
  13 BCAR_DR = 0.655
  14
  15 def get_scenario(fname):
  16     """Load scenario from file."""
  17     if fname is None:
  18         raise ValueError("File name as argument needed")
  19     with open(fname, "r") as f:
  20         scenario = loads(f.read())
  21     return scenario
  22
  23 def plot_nodes(nodes=[]):
  24     """Return ``xcoords``, ``ycoords`` arrays of nodes to plot.
  25
  26     Keyword arguments:
  27     nodes -- The list of nodes to plot.
  28     """
  29     xcoords = []
  30     ycoords = []
  31     for n in nodes:
  32         xcoords.append(n[0])
  33         ycoords.append(n[1])
  34     return (xcoords, ycoords)
  35
  36 def plot_car(pose):
  37     """Return ``xcoords``, ``ycoords`` arrays of car frame to plot.
  38
  39     Keyword arguments:
  40     pose -- The pose of a car.
  41     """
  42     lfx = pose[0]
  43     lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  44     lfx += BCAR_DF * cos(pose[2])
  45     lfx += BCAR_SD * cos(pose[2])
  46
  47     lrx = pose[0]
  48     lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
  49     lrx += -BCAR_DR * cos(pose[2])
  50     lrx += -BCAR_SD * cos(pose[2])
  51
  52     rrx = pose[0]
  53     rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  54     rrx += -BCAR_DR * cos(pose[2])
  55     rrx += -BCAR_SD * cos(pose[2])
  56
  57     rfx = pose[0]
  58     rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
  59     rfx += BCAR_DF * cos(pose[2])
  60     rfx += BCAR_SD * cos(pose[2])
  61
  62     lfy = pose[1]
  63     lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  64     lfy += BCAR_DF * sin(pose[2])
  65     lfy += BCAR_SD * sin(pose[2])
  66
  67     lry = pose[1]
  68     lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
  69     lry += -BCAR_DR * sin(pose[2])
  70     lry += -BCAR_SD * sin(pose[2])
  71
  72     rry = pose[1]
  73     rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
  74     rry += -BCAR_DR * sin(pose[2])
  75     rry += -BCAR_SD * sin(pose[2])
  76
  77     rfy = pose[1]
  78     rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
  79     rfy += BCAR_DF * sin(pose[2])
  80     rfy += BCAR_SD * sin(pose[2])
  81
  82     xcoords = (lfx, lrx, rrx, rfx)
  83     ycoords = (lfy, lry, rry, rfy)
  84     return (xcoords, ycoords)
  85
  86 if __name__ == "__main__":
  87     if (len(argv) == 2):
  88         SCEN_FILE = argv[1]
  89     else:
  90         SCEN_FILE = "sc.json"
  91
  92     scenario = get_scenario(SCEN_FILE)
  93
  94     plt.rcParams["font.size"] = 24
  95     plt.rcParams['hatch.linewidth'] = 1.0
  96     fig = plt.figure()
  97
  98     # here subplot starts
  99     ax = fig.add_subplot(111)
 100     ax.set_aspect("equal")
 101     ax.set_title("{}".format(SCEN_FILE))
 102     ax.set_xlabel("x [m]")
 103     ax.set_ylabel("y [m]")
 104
 105     if "obst" in scenario and  len(scenario["obst"]) > 0:
 106         for o in scenario["obst"]:
 107             plt.plot(*plot_nodes(o), color="blue")
 108     if "path" in scenario and  len(scenario["path"]) > 0:
 109         for n in scenario["path"]:
 110             plt.plot(*plot_car(n), color="orange")
 111     if "slot" in scenario and len(scenario["slot"]) > 0:
 112         for s in scenario["slot"]:
 113             plt.plot(*plot_nodes(s), color="gray")
 114     if "init" in scenario and len(scenario["init"]) == 3:
 115         plt.plot(*plot_car(scenario["init"]), color="red")
 116         plt.plot(
 117             scenario["init"][0],
 118             scenario["init"][1],
 119             color="red",
 120             marker="+",
 121             ms=12
 122         )
 123     if "goals" in scenario:
 124         for i in scenario["goals"]:
 125             if len(i) == 3:
 126                 plt.plot(*plot_car(i), color="lightcoral")
 127                 plt.plot(i[0], i[1], color="lightcoral", marker="+", ms=12)
 128     if "goal" in scenario and len(scenario["goal"]) == 3:
 129         plt.plot(*plot_car(scenario["goal"]), color="green")
 130         plt.plot(
 131             scenario["goal"][0],
 132             scenario["goal"][1],
 133             color="green",
 134             marker="+",
 135             ms=12
 136         )
 137     if "starts" in scenario and len(scenario["starts"]) > 0:
 138         print("possible starts:")
 139         for p in scenario["starts"]:
 140             plt.plot(*p, color="red", marker="+", ms=12)
 141             print(" {}".format(p))
 142
 143     handles, labels = ax.get_legend_handles_labels()
 144
 145     plt.show()
 146     plt.close(fig)