tplot.py

   1 # -*- coding: utf-8 -*-
   2 """This scipt loads scenario and result trajectory from files and plots it."""
   3 from json import loads
   4 from math import copysign, cos, pi, sin
   5 from matplotlib import pyplot as plt
   6 from sys import argv, exit
   7
   8 sign = lambda x: copysign(1, x)
   9
  10 HEIGHT = 1.450
  11 LENGTH = 3.760
  12 SAFETY_DIST = 0
  13 WHEEL_BASE = 2.450
  14 WIDTH = 1.625
  15
  16 PLOT = {
  17         "enable" : True,
  18         "node" : False,
  19         "edge" : True,
  20         "sample" : True,
  21         "frame" : False,
  22         "path" : False,
  23         "traj" : True,
  24         "slot" : True,
  25         }
  26 COLOR = {
  27         "node": "lightgrey",
  28         "edge": "lightgrey",
  29         "start": "violet",
  30         "goal": "red",
  31         "sample": "magenta",
  32         "path": "grey",
  33         "trajectory": "blue",
  34         "trajectory-frame": "lightblue",
  35         "obstacle": "black",
  36         "log": ("gold", "orange", "blueviolet", "blue", "navy", "black"),
  37         "slot": "red"
  38         }
  39
  40 def car_frame(pose):
  41     """Return `xcoords`, `ycoords` arrays of car frame.
  42
  43     Keyword arguments:
  44     pose -- The pose of a car.
  45     """
  46     dr = (LENGTH - WHEEL_BASE) / 2
  47     df = LENGTH - dr
  48
  49     lfx = pose[0]
  50     lfx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
  51     lfx += df * cos(pose[2])
  52     lfx += SAFETY_DIST * cos(pose[2])
  53
  54     lrx = pose[0]
  55     lrx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
  56     lrx += -dr * cos(pose[2])
  57     lrx += -SAFETY_DIST * cos(pose[2])
  58
  59     rrx = pose[0]
  60     rrx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
  61     rrx += -dr * cos(pose[2])
  62     rrx += -SAFETY_DIST * cos(pose[2])
  63
  64     rfx = pose[0]
  65     rfx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
  66     rfx += df * cos(pose[2])
  67     rfx += SAFETY_DIST * cos(pose[2])
  68
  69     lfy = pose[1]
  70     lfy += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
  71     lfy += df * sin(pose[2])
  72     lfy += SAFETY_DIST * sin(pose[2])
  73
  74     lry = pose[1]
  75     lry += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
  76     lry += -dr * sin(pose[2])
  77     lry += -SAFETY_DIST * sin(pose[2])
  78
  79     rry = pose[1]
  80     rry += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
  81     rry += -dr * sin(pose[2])
  82     rry += -SAFETY_DIST * sin(pose[2])
  83
  84     rfy = pose[1]
  85     rfy += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
  86     rfy += df * sin(pose[2])
  87     rfy += SAFETY_DIST * sin(pose[2])
  88
  89     xcoords = (lfx, lrx, rrx, rfx)
  90     ycoords = (lfy, lry, rry, rfy)
  91     return (xcoords, ycoords)
  92
  93 def load_scenario(fname):
  94     """Load scenario from file."""
  95     if fname is None:
  96         raise ValueError("File name as argument needed")
  97     with open(fname, "r") as f:
  98         scenario = loads(f.read())
  99     return scenario
 100
 101 def load_trajectory(fname):
 102     """Load trajectory from file."""
 103     if fname is None:
 104         raise ValueError("File name as argument needed")
 105     with open(fname, "r") as f:
 106         trajectory = loads(f.read())
 107         return trajectory
 108
 109 def plot_nodes(nodes=[]):
 110     """Return xcoords, ycoords of nodes to plot.
 111
 112     Keyword arguments:
 113     nodes -- The list of nodes to plot.
 114     """
 115     xcoords = []
 116     ycoords = []
 117     for n in nodes:
 118         xcoords.append(n[0])
 119         ycoords.append(n[1])
 120     return (xcoords, ycoords)
 121
 122 def plot_segments(segments=[]):
 123     """Return xcoords, ycoords of segments to plot.
 124
 125     Keyword arguments:
 126     segments -- The list of segments to plot.
 127     """
 128     pass
 129
 130 if __name__ == "__main__":
 131     SCEN_FILE = argv[1]
 132     TRAJ_FILE = argv[2]
 133
 134     s = load_scenario(SCEN_FILE)
 135     try:
 136         t = load_trajectory(TRAJ_FILE) # fixed to trajectories
 137     except:
 138         pass
 139
 140     plt.rcParams["font.size"] = 24
 141     plt.rcParams['hatch.linewidth'] = 1.0
 142     fig = plt.figure()
 143
 144     ################
 145     ## 1st subplot
 146
 147     if "edge" in t.keys():
 148         ax = fig.add_subplot(121)
 149     else:
 150         ax = fig.add_subplot(111)
 151     ax.set_aspect("equal")
 152     ax.set_title("Final path")
 153     ax.set_xlabel("x [m]")
 154     ax.set_ylabel("y [m]")
 155
 156     for o in s["obst"]:
 157         try:
 158             plt.plot(*plot_nodes(o["segment"]), color="black")
 159         except:
 160             pass
 161         try:
 162             ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
 163                     o["circle"][2],
 164                     color="black", fill=False, hatch="//"))
 165         except:
 166             pass
 167     if PLOT["node"]:
 168         try:
 169             plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
 170                     marker=".", linestyle = "None")
 171         except:
 172             print("No RRTNode")
 173     if False:
 174         try:
 175             for edges in t["edge"]:
 176                 for e in edges:
 177                     plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
 178                             color=COLOR["edge"])
 179         except:
 180             print("No edges")
 181     if PLOT["sample"]:
 182         try:
 183             if PLOT["frame"]:
 184                 for i in t["samp"]:
 185                     plt.plot(*car_frame(i), color=COLOR["sample"])
 186             else:
 187                 plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
 188                         marker=".", linestyle = "None")
 189         except:
 190             print("No RRTSample")
 191     if PLOT["path"]:
 192         try:
 193             for path in range(len(t["path"])):
 194                 plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
 195         except:
 196             print("No path")
 197     if PLOT["traj"]:
 198         try:
 199             for traj in range(len(t["traj"])):
 200                 if PLOT["frame"]:
 201                     for i in t["traj"][traj]:
 202                         plt.plot(*car_frame(i), color=COLOR["log"][traj],
 203                                 label=t["cost"][traj], lw=1)
 204                 else:
 205                     try:
 206                         plt.plot(
 207                                 *plot_nodes(t["traj"][traj]),
 208                                 color=COLOR["log"][traj],
 209                                 label=t["cost"][traj])
 210                     except:
 211                         plt.plot(
 212                                 *plot_nodes(t["traj"][traj]),
 213                                 color="black",
 214                                 label=t["cost"][traj])
 215         except:
 216             print("No trajectory")
 217
 218     if PLOT["slot"]:
 219         try:
 220             plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
 221         except:
 222             print("No slot")
 223
 224     plt.plot(*car_frame(s["init"]), color="red", lw=2)
 225     plt.plot(*car_frame(t["goal"]), color="red", lw=2)
 226     plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
 227     plt.plot(*plot_nodes([t["goal"]]), color="red", marker="+", ms=12)
 228     try: # middle
 229         plt.plot(*car_frame(t["midd"]), color="red", lw=2)
 230         plt.plot(*plot_nodes([t["midd"]]), color="red", marker="+", ms=12)
 231     except:
 232         pass
 233
 234     plt.fill_between(
 235                 [-2.7, 3.25, 3.25, -2.7],
 236                 [-0.5, -0.5, 0, 0],
 237                 [26, 26, 26.5, 26.5],
 238                 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
 239     plt.fill_between(
 240                 [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
 241                 [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
 242                 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
 243     plt.fill_between(
 244                 [3.25, 3.25, 2.75, 2.75, 3.25],
 245                 [-0.5, 26.5, 26.5, -0.5, -0.5],
 246                 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
 247
 248     #plt.text(1, 0.2, s="1", color="red")
 249     #plt.text(2, 12.2, s="2", color="red")
 250     #plt.text(3.6, 10, s="3", color="black")
 251     #plt.text(-0.5, 14.3, s="4", color="black")
 252     #plt.text(3, 18, s="5", color="blue")
 253     #plt.text(-4.7, 0.8, s="6", color="orange")
 254     #plt.text(-2, 11, s="7", color="gray")
 255
 256     handles, labels = ax.get_legend_handles_labels()
 257
 258     if not "edge" in t.keys():
 259         plt.show()
 260         plt.close(fig)
 261         exit(0)
 262
 263     ################
 264     ## 2nd subplot
 265
 266     ax = fig.add_subplot(122)
 267     ax.set_aspect("equal")
 268     ax.set_title("All edges")
 269     ax.set_xlabel("x [m]")
 270     ax.set_ylabel("y [m]")
 271
 272     for o in s["obst"]:
 273         try:
 274             plt.plot(*plot_nodes(o["segment"]), color="black")
 275         except:
 276             pass
 277         try:
 278             ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
 279                     o["circle"][2],
 280                     color="black", fill=False, hatch="//"))
 281         except:
 282             pass
 283     if PLOT["node"]:
 284         try:
 285             plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
 286                     marker=".", linestyle = "None")
 287         except:
 288             print("No RRTNode")
 289     if True:
 290         try:
 291             for edges in t["edge"]:
 292                 for e in edges:
 293                     plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
 294                             color=COLOR["edge"], lw=1)
 295         except:
 296             print("No edges")
 297     if PLOT["sample"]:
 298         try:
 299             if PLOT["frame"]:
 300                 for i in t["samp"]:
 301                     plt.plot(*car_frame(i), color=COLOR["sample"])
 302             else:
 303                 plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
 304                         marker=".", linestyle = "None")
 305         except:
 306             print("No RRTSample")
 307     if PLOT["path"]:
 308         try:
 309             for path in range(len(t["path"])):
 310                 plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
 311         except:
 312             print("No path")
 313     if False:
 314         try:
 315             for traj in range(len(t["traj"])):
 316                 if PLOT["frame"]:
 317                     for i in t["traj"][traj]:
 318                         plt.plot(*car_frame(i), color=COLOR["log"][traj],
 319                                 label=t["cost"][traj])
 320                 else:
 321                     try:
 322                         plt.plot(
 323                                 *plot_nodes(t["traj"][traj]),
 324                                 color=COLOR["log"][traj],
 325                                 label=t["cost"][traj])
 326                     except:
 327                         plt.plot(
 328                                 *plot_nodes(t["traj"][traj]),
 329                                 color="black",
 330                                 label=t["cost"][traj])
 331         except:
 332             print("No trajectory")
 333
 334     if PLOT["slot"]:
 335         try:
 336             plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
 337         except:
 338             print("No slot")
 339
 340     plt.plot(*car_frame(s["init"]), color="red", lw=2)
 341     plt.plot(*car_frame(s["goal"]), color="red", lw=2)
 342     plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
 343     plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
 344
 345     plt.fill_between(
 346                 [-2.7, 3.25, 3.25, -2.7],
 347                 [-0.5, -0.5, 0, 0],
 348                 [26, 26, 26.5, 26.5],
 349                 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
 350     plt.fill_between(
 351                 [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
 352                 [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
 353                 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
 354     plt.fill_between(
 355                 [3.25, 3.25, 2.75, 2.75, 3.25],
 356                 [-0.5, 26.5, 26.5, -0.5, -0.5],
 357                 facecolor="none", hatch="//", edgecolor="black", linewidth=0)
 358
 359     handles, labels = ax.get_legend_handles_labels()
 360
 361     # END OF SUBPLOTS
 362     plt.show()
 363     plt.close(fig)