From a56f0befe71c8f4bc18a02872c2a0b3fae89b919 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Fri, 24 Mar 2023 11:55:21 +0100 Subject: [PATCH] Add gen steer scenario script --- gen_steer_scenario.py | 47 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100755 gen_steer_scenario.py diff --git a/gen_steer_scenario.py b/gen_steer_scenario.py new file mode 100755 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") -- 2.39.2