cplot.py

   1 # -*- coding: utf-8 -*-
   2 """This scipt loads result JSON and print cost dependent on time.
   3
   4 Keyword arguments:
   5 argv[1] -- 1st file/folder with json trajectory results.
   6 argv[2] -- 2nd file/folder with json trajectory results.
   7 """
   8 from json import loads
   9 from math import cos, pi, sin
  10 from matplotlib import pyplot as plt
  11 from os import listdir
  12 from sys import argv, exit
  13
  14 if len(argv) != 3:
  15     print("Exactly 2 arguments needed.")
  16     exit(1)
  17
  18 TRAJ_FILE = argv[1]
  19 TRAJ_FILE2 = argv[2]
  20
  21 def load_trajectory(fname):
  22     """Load trajectory from file."""
  23     if fname is None:
  24         raise ValueError("File name as argument needed")
  25     with open(fname, "r") as f:
  26         trajectory = loads(f.read())
  27         return trajectory
  28
  29 def plot_nodes(nodes=[]):
  30     """Return xcoords, ycoords 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])
  39         ycoords.append(n[1])
  40     return (xcoords, ycoords)
  41
  42 if __name__ == "__main__":
  43     nodes = []
  44     podes = []
  45     # blue nodes (not parallel)
  46     try:
  47         for f in listdir(TRAJ_FILE):
  48             t = load_trajectory("{}/{}".format(TRAJ_FILE, f))
  49             for p in zip(t["secs"], t["cost"]):
  50                 nodes.append(p)
  51     except:
  52         try:
  53             t = load_trajectory(TRAJ_FILE) # fixed to trajectories
  54             for p in zip(t["secs"], t["cost"]):
  55                 nodes.append(p)
  56         except:
  57             print("Failed loading 1st")
  58             exit(1)
  59     # red nodes (parallel)
  60     try:
  61         for f in listdir(TRAJ_FILE2):
  62             t = load_trajectory("{}/{}".format(TRAJ_FILE2, f))
  63             for p in zip(t["secs"], t["cost"]):
  64                 podes.append(p)
  65     except:
  66         try:
  67             t = load_trajectory(TRAJ_FILE2) # fixed to trajectories
  68             for p in zip(t["secs"], t["cost"]):
  69                 podes.append(p)
  70         except:
  71             print("Failed loading 2nd")
  72             exit(1)
  73
  74     fig = plt.figure()
  75     ax = fig.add_subplot(111)
  76     ax.set_aspect("equal")
  77     ax.set_title("SCENARIO")
  78     ax.set_xlabel("Time [s]")
  79     ax.set_ylabel("Cost (Euclidean length) [m]")
  80     #ax.set_xlim(0, 60)
  81     #ax.set_ylim(0, 100)
  82
  83     # plot here
  84     nodes.sort()
  85     podes.sort()
  86     try:
  87         plt.plot(*plot_nodes(nodes), color="blue")
  88         plt.plot(*plot_nodes(podes), color="red")
  89     except:
  90         pass
  91     # end plot here
  92
  93     plt.show()
  94     #plt.savefig("{}_{}.png".format(argv[1], argv[2]))
  95     plt.close(fig)