wa_and_sp_plot.py

   1 #!/usr/bin/env python3
   2 """Plot wheel angle and speed of the scenario.
   3
   4 The expected use is:
   5
   6     ./sp_and_wa_plot.py some-generated-file.traj.json
   7 """
   8 from math import pi
   9 from matplotlib import pyplot as plt
  10 import json
  11 import sys
  12
  13
  14 def ticks_to_seconds(t):
  15     return t * 0.02  # one tick is 20 ms
  16
  17
  18 def rad_to_deg(x):
  19     return x * 180 / pi
  20
  21
  22 def mps_to_kmph(x):
  23     return x * 3.6
  24
  25
  26 if __name__ == "__main__":
  27     if len(sys.argv) != 2:
  28         print(__doc__)
  29         exit(1)
  30     plan = None
  31     with open(f"{sys.argv[1]}", "r") as f:
  32         plan = json.load(f)
  33     assert "traj" in plan
  34
  35     plt.rc('axes', unicode_minus=False)
  36     fig = plt.figure()
  37
  38     ax = fig.add_subplot(111)
  39     ax.set_title("Wheel Angle and Speed in time")
  40     ax.set_xlabel("Time [s]")
  41     ax.set_ylabel("Wheel Angle [deg]")
  42
  43     ax2 = ax.twinx()
  44     ax2.set_ylabel("Speed [kmph]")
  45
  46     t = [ticks_to_seconds(i) for i in range(len(plan["traj"]))]
  47     wa = [rad_to_deg(t[3]) for t in plan["traj"]]
  48     sp = [mps_to_kmph(t[4]) for t in plan["traj"]]
  49     fwd = [t[6]*0.2-0.1 for t in plan["traj"]]
  50
  51     p1 = ax.plot(t, wa, 'b-', label="Wheel Angle [deg]")
  52     p2 = ax2.plot(t, sp, 'r-', label="Speed [kmph]")
  53     p3 = ax2.plot(t, fwd, 'g-', label="Gear")
  54
  55     # Center both Y-axes at zero
  56     yabs_max = abs(max(ax.get_ylim(), key=abs))
  57     ax.set_ylim(ymin=-yabs_max, ymax=yabs_max)
  58     yabs_max = abs(max(ax2.get_ylim(), key=abs))
  59     ax2.set_ylim(ymin=-yabs_max, ymax=yabs_max)
  60
  61     lns = p1 + p2 + p3
  62     labs = [l.get_label() for l in lns]
  63     ax.legend(lns, labs, loc=0)
  64     ax.grid(True, "both", linestyle=":", color="tab:gray")
  65     #plt.show()
  66     plt.savefig(sys.argv[1] + "-vals.pdf", bbox_inches="tight")
  67     plt.savefig(sys.argv[1] + "-vals.png", bbox_inches="tight")
  68     plt.close(fig)