Add gen steer scenario script

diff --git a/gen_steer_scenario.py b/gen_steer_scenario.py
new file mode 100755 (executable)
index 0000000..91c0739
--- /dev/null
+++ b/gen_steer_scenario.py
@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+from math import pi, cos, sin
+import json
+
+MTR = 3.984628  # minimum turning radius
+TURN_TO_RIGHT = -1  # -1 for turn to left, 1 for turn to right
+DIST_BEFORE_TURN = 5
+
+
+def dtor(a):
+    return a * pi / 180
+
+
+def continue_while(a, first=True):
+    if first:
+        if TURN_TO_RIGHT > 0:
+            return a >= -90 * TURN_TO_RIGHT
+        else:
+            return a <= -90 * TURN_TO_RIGHT
+    else:
+        if TURN_TO_RIGHT > 0:
+            return a >= -270 * TURN_TO_RIGHT
+        else:
+            return a <= -270 * TURN_TO_RIGHT
+
+
+def pose_given_by(a, first=1):
+    x = MTR * cos(dtor(a)) + DIST_BEFORE_TURN * first
+    y = MTR * sin(dtor(a)) - MTR * TURN_TO_RIGHT
+    h = dtor(a - 90 * TURN_TO_RIGHT)
+    return [x, y, h, 1, -36 * TURN_TO_RIGHT, False]
+
+
+if __name__ == "__main__":
+    path = [[0, 0, 0, 1, -36 * TURN_TO_RIGHT, True]]
+    a = 90 * TURN_TO_RIGHT
+    while continue_while(a):
+        path.append(pose_given_by(a))
+        a -= 10 * TURN_TO_RIGHT
+    while continue_while(a, False):
+        path.append(pose_given_by(a, -1))
+        a -= 10 * TURN_TO_RIGHT
+    with open("s.json", "w") as f:
+        json.dump({
+            "init": [0, 0, 0],
+            "path": path,
+            "ispath": [[0, 0, 0, 0, False]]}, f, indent="\t")