gen_steer_scenario.py

   1 #!/usr/bin/env python3
   2 from math import pi, cos, sin, atan
   3 import json
   4
   5 WHEELBASE = 2.895  # JUPITER (Porsche Cayenne)
   6
   7
   8 def sgn(x):
   9     return (0 < x) - (x < 0)
  10
  11
  12 def dtor(a):
  13     return a * pi / 180
  14
  15
  16 def rtod(a):
  17     return a * 180 / pi
  18
  19
  20 def pose_on_circle_given_by(angle, mtr, wa, x0, y0, angle0):
  21     """Return pose on circle based on parameters:
  22
  23     :param angle: Angle from circle center in degrees, probably -90 to +90.
  24     :param mtr: Minimum turning radius.
  25     :param wa: Wheel angle to be in resulting pose in degrees.
  26     :param x0: How much shift resulting x coordinate.
  27     :param y0: How much shift resulting y coordinate.
  28     :param angle0: How much shift resulting heading from requested angle.
  29     """
  30     x = mtr * cos(dtor(angle)) + x0
  31     y = mtr * sin(dtor(angle)) + y0
  32     h = dtor(angle + angle0)
  33     wa = atan(WHEELBASE / mtr) * sgn(angle0)
  34     return [x, y, h, 1, rtod(wa), False]
  35
  36
  37 def poses_on_circle(*, from_angle, to_angle, by_angle, **kwargs):
  38     def should_continue(a, to_a):
  39         if from_angle < to_angle:
  40             return a <= to_a
  41         else:
  42             return a >= to_a
  43     poses = []
  44     angle = from_angle
  45     while should_continue(angle, to_angle):
  46         poses.append(pose_on_circle_given_by(angle, **kwargs))
  47         angle += by_angle
  48     return poses
  49
  50
  51 def swap_to_second_half_of(oval):
  52     oval["from_angle"] += 180
  53     oval["to_angle"] += 180
  54     oval["x0"] *= -1
  55     return oval
  56
  57
  58 def gen_path_of(oval):
  59     path = []
  60     path.append([0, 0, 0, 1, 0, True])
  61     path.append([oval["x0"], 0, 0, 1, 0, False])
  62     path += poses_on_circle(**oval)
  63     swap_to_second_half_of(oval)
  64     path.append([oval["x0"], 2 * oval["y0"], pi, 1, 0, False])
  65     path += poses_on_circle(**oval)
  66     path.append([0, 0, 0, 1, 0, False])
  67     return path
  68
  69
  70 OVAL = {
  71     "mtr_5_right": {
  72         "from_angle": 90,
  73         "to_angle": -90,
  74         "by_angle": -10,
  75         "mtr": 5,
  76         "wa": -36,
  77         "x0": 5,
  78         "y0": -5,
  79         "angle0": -90},
  80     "mtr_5_left": {
  81         "from_angle": -90,
  82         "to_angle": 90,
  83         "by_angle": 10,
  84         "mtr": 5,
  85         "wa": 36,
  86         "x0": 5,
  87         "y0": 5,
  88         "angle0": 90},
  89     "mtr_min_right": {
  90         "from_angle": 90,
  91         "to_angle": -90,
  92         "by_angle": -10,
  93         "mtr": 3.984628,
  94         "wa": -36,
  95         "x0": 5,
  96         "y0": -3.984628,  # eq. mtr
  97         "angle0": -90},
  98     "mtr_min_left": {
  99         "from_angle": -90,
 100         "to_angle": 90,
 101         "by_angle": 10,
 102         "mtr": 3.984628,
 103         "wa": 36,
 104         "x0": 5,
 105         "y0": 3.984628,  # eq. mtr
 106         "angle0": 90}
 107 }
 108
 109
 110 if __name__ == "__main__":
 111     with open("oval-mtr-min-to-left.json", "w") as f:
 112         json.dump({
 113             "init": [0, 0, 0],
 114             "path": gen_path_of(OVAL["mtr_min_left"]),
 115             "ispath": [[0, 0, 0, 0, False]]}, f, indent="\t")
 116     with open("oval-mtr-min-to-right.json", "w") as f:
 117         json.dump({
 118             "init": [0, 0, 0],
 119             "path": gen_path_of(OVAL["mtr_min_right"]),
 120             "ispath": [[0, 0, 0, 0, False]]}, f, indent="\t")
 121     with open("oval-mtr-5-to-left.json", "w") as f:
 122         json.dump({
 123             "init": [0, 0, 0],
 124             "path": gen_path_of(OVAL["mtr_5_left"]),
 125             "ispath": [[0, 0, 0, 0, False]]}, f, indent="\t")
 126     with open("oval-mtr-5-to-right.json", "w") as f:
 127         json.dump({
 128             "init": [0, 0, 0],
 129             "path": gen_path_of(OVAL["mtr_5_right"]),
 130             "ispath": [[0, 0, 0, 0, False]]}, f, indent="\t")