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, loads
  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 def gen_init():
  27     """Generate car init position."""
  28     # TODO if changed, change ``gen_slot`` accordingly
  29     return (0, 0, 0)
  30
  31 def gen_slot(l=5.3, w=2.4):
  32     """Generate parking slot."""
  33     parallel = True if random() < 0.5 else False
  34     if parallel and l == 5.3 and w == 2.4:
  35         l = 6.5
  36         w = 2.2
  37     elif parallel:
  38         ol = l
  39         l = w
  40         w = ol
  41     right = 1.0 if random() < 0.5 else -1.0
  42     r = SLOT_RADI
  43     angl = uniform(0, 2 * pi)
  44     x = r * cos(angl)
  45     y = r * sin(angl)
  46     h = uniform(0, 2 * pi)
  47     if parallel:
  48         return [
  49             [
  50                 x + l/2 * cos(h - pi/2 * right),
  51                 y + l/2 * sin(h - pi/2 * right),
  52             ],
  53             [
  54                 x + w * cos(h) + l/2 * cos(h - pi/2 * right),
  55                 y + w * sin(h) + l/2 * sin(h - pi/2 * right),
  56             ],
  57             [
  58                 x + w * cos(h) + l/2 * cos(h + pi/2 * right),
  59                 y + w * sin(h) + l/2 * sin(h + pi/2 * right),
  60             ],
  61             [
  62                 x + l/2 * cos(h + pi/2 * right),
  63                 y + l/2 * sin(h + pi/2 * right),
  64             ],
  65         ]
  66     else:
  67         return [
  68             [
  69                 x + w/2 * cos(h - pi/2 * right),
  70                 y + w/2 * sin(h - pi/2 * right),
  71             ],
  72             [
  73                 x + l * cos(h) + w/2 * cos(h - pi/2 * right),
  74                 y + l * sin(h) + w/2 * sin(h - pi/2 * right),
  75             ],
  76             [
  77                 x + l * cos(h) + w/2 * cos(h + pi/2 * right),
  78                 y + l * sin(h) + w/2 * sin(h + pi/2 * right),
  79             ],
  80             [
  81                 x + w/2 * cos(h + pi/2 * right),
  82                 y + w/2 * sin(h + pi/2 * right),
  83             ],
  84         ]
  85
  86 def gen_obst():
  87     """Generate obstacles array."""
  88     obstacles = []
  89     min_r = ((W/2)**2 + ((WB+L)/2)**2)**0.5 + (OBST_W**2 + OBST_L**2)**0.5 / 2
  90     for i in range(OBST_COUNT):
  91         l = OBST_L
  92         w = OBST_W
  93         angl = uniform(0, 2 * pi)
  94         r = uniform(min_r**2, (SLOT_RADI - 5)**2)**0.5
  95         x = r * cos(angl)
  96         y = r * sin(angl)
  97         h = uniform(0, 2 * pi)
  98         obstacles.append([
  99             [
 100                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 101                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 102             ],
 103             [
 104                 x + w/2 * cos(h - pi/2) - l/2 * cos(h),
 105                 y + w/2 * sin(h - pi/2) - l/2 * sin(h),
 106             ],
 107             [
 108                 x + w/2 * cos(h + pi/2) - l/2 * cos(h),
 109                 y + w/2 * sin(h + pi/2) - l/2 * sin(h),
 110             ],
 111             [
 112                 x + w/2 * cos(h + pi/2) + l/2 * cos(h),
 113                 y + w/2 * sin(h + pi/2) + l/2 * sin(h),
 114             ],
 115             [
 116                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 117                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 118             ],
 119         ])
 120     return obstacles
 121
 122 if __name__ == "__main__":
 123     init = gen_init()
 124     slot = gen_slot()
 125     obst = gen_obst()
 126     print(dumps({
 127         "init": init,
 128         "slot": slot,
 129         "obst": obst,
 130     }))