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
  50     p1 = ax.plot(t, wa, 'b-', label="Wheel Angle [deg]")
  51     p2 = ax2.plot(t, sp, 'r-', label="Speed [kmph]")
  52
  53     lns = p1 + p2
  54     labs = [l.get_label() for l in lns]
  55     ax.legend(lns, labs, loc=0)
  56     ax.grid(True, "both", linestyle=":", color="tab:gray")
  57     plt.show()
  58     plt.close(fig)