Add wheel angle and speed plot script

diff --git a/wa_and_sp_plot.py b/wa_and_sp_plot.py
new file mode 100755 (executable)
index 0000000..77a453f
--- /dev/null
+++ b/wa_and_sp_plot.py
@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+"""Plot wheel angle and speed of the scenario.
+
+The expected use is:
+
+    ./sp_and_wa_plot.py some-generated-file.traj.json
+"""
+from math import pi
+from matplotlib import pyplot as plt
+import json
+import sys
+
+
+def ticks_to_seconds(t):
+    return t * 0.02  # one tick is 20 ms
+
+
+def rad_to_deg(x):
+    return x * 180 / pi
+
+
+def mps_to_kmph(x):
+    return x * 3.6
+
+
+if __name__ == "__main__":
+    if len(sys.argv) != 2:
+        print(__doc__)
+        exit(1)
+    plan = None
+    with open(f"{sys.argv[1]}", "r") as f:
+        plan = json.load(f)
+    assert "traj" in plan
+
+    plt.rc('axes', unicode_minus=False)
+    fig = plt.figure()
+
+    ax = fig.add_subplot(111)
+    ax.set_aspect("equal")
+    ax.set_title("Wheel Angle and Speed in time")
+    ax.set_xlabel("Time [s]")
+    ax.set_ylabel("Wheel Angle [deg] / Speed [kmph]")
+
+    t = [ticks_to_seconds(i) for i in range(len(plan["traj"]))]
+    wa = [rad_to_deg(t[3]) for t in plan["traj"]]
+    sp = [mps_to_kmph(t[4]) for t in plan["traj"]]
+
+    plt.plot(t, wa, label="Wheel Angle [deg]")
+    plt.plot(t, sp, label="Speed [kmph]")
+
+    ax.legend()
+    ax.grid(True, "both", linestyle=":", color="tab:gray")
+    plt.show()
+    plt.close(fig)