scripts/generate_simple_json_scenario.py

   1 """Generate simple JSON scenario.
   2
   3 The scenario contains at least:
   4
   5 - `init` -- the init car position,
   6
   7 - `slot` -- the parking slot,
   8
   9 - `obst` -- the list of (convex polygon) obstacles.
  10 """
  11 from json import dumps
  12 from math import cos, pi, sin
  13 from random import random, uniform
  14
  15 W = 1.625
  16 L = 3.760
  17 WB = 2.450
  18 CTC = 10.820
  19 MTR = ((CTC / 2)**2 - WB**2)**0.5 - W / 2
  20
  21 SLOT_RADI = 20
  22 OBST_L = 2
  23 OBST_W = 2
  24 OBST_COUNT = 8
  25
  26
  27 def gen_init():
  28     """Generate car init position."""
  29     # TODO if changed, change ``gen_slot`` accordingly
  30     return (0, 0, 0)
  31
  32
  33 def gen_slot(l=5.3, w=2.4):
  34     """Generate parking slot."""
  35     parallel = True if random() < 0.5 else False
  36     if parallel and l == 5.3 and w == 2.4:
  37         l = 6.5
  38         w = 2.2
  39     elif parallel:
  40         ol = l
  41         l = w
  42         w = ol
  43     right = 1.0 if random() < 0.5 else -1.0
  44     r = SLOT_RADI
  45     angl = uniform(0, 2 * pi)
  46     x = r * cos(angl)
  47     y = r * sin(angl)
  48     h = uniform(0, 2 * pi)
  49     if parallel:
  50         return [
  51             [
  52                 x + l/2 * cos(h - pi/2 * right),
  53                 y + l/2 * sin(h - pi/2 * right),
  54             ],
  55             [
  56                 x + w * cos(h) + l/2 * cos(h - pi/2 * right),
  57                 y + w * sin(h) + l/2 * sin(h - pi/2 * right),
  58             ],
  59             [
  60                 x + w * cos(h) + l/2 * cos(h + pi/2 * right),
  61                 y + w * sin(h) + l/2 * sin(h + pi/2 * right),
  62             ],
  63             [
  64                 x + l/2 * cos(h + pi/2 * right),
  65                 y + l/2 * sin(h + pi/2 * right),
  66             ],
  67         ]
  68     else:
  69         return [
  70             [
  71                 x + w/2 * cos(h - pi/2 * right),
  72                 y + w/2 * sin(h - pi/2 * right),
  73             ],
  74             [
  75                 x + l * cos(h) + w/2 * cos(h - pi/2 * right),
  76                 y + l * sin(h) + w/2 * sin(h - pi/2 * right),
  77             ],
  78             [
  79                 x + l * cos(h) + w/2 * cos(h + pi/2 * right),
  80                 y + l * sin(h) + w/2 * sin(h + pi/2 * right),
  81             ],
  82             [
  83                 x + w/2 * cos(h + pi/2 * right),
  84                 y + w/2 * sin(h + pi/2 * right),
  85             ],
  86         ]
  87
  88
  89 def gen_obst():
  90     """Generate obstacles array."""
  91     obstacles = []
  92     min_r = ((W/2)**2 + ((WB+L)/2)**2)**0.5 + (OBST_W**2 + OBST_L**2)**0.5 / 2
  93     for i in range(OBST_COUNT):
  94         l = OBST_L
  95         w = OBST_W
  96         angl = uniform(0, 2 * pi)
  97         r = uniform(min_r**2, (SLOT_RADI - 5)**2)**0.5
  98         x = r * cos(angl)
  99         y = r * sin(angl)
 100         h = uniform(0, 2 * pi)
 101         obstacles.append([
 102             [
 103                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 104                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 105             ],
 106             [
 107                 x + w/2 * cos(h - pi/2) - l/2 * cos(h),
 108                 y + w/2 * sin(h - pi/2) - l/2 * sin(h),
 109             ],
 110             [
 111                 x + w/2 * cos(h + pi/2) - l/2 * cos(h),
 112                 y + w/2 * sin(h + pi/2) - l/2 * sin(h),
 113             ],
 114             [
 115                 x + w/2 * cos(h + pi/2) + l/2 * cos(h),
 116                 y + w/2 * sin(h + pi/2) + l/2 * sin(h),
 117             ],
 118             [
 119                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 120                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 121             ],
 122         ])
 123     return obstacles
 124
 125
 126 if __name__ == "__main__":
 127     init = gen_init()
 128     slot = gen_slot()
 129     obst = gen_obst()
 130     print(dumps({
 131         "init": init,
 132         "slot": slot,
 133         "obst": obst,
 134     }))