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_at(x, y, h, parallel=True):
  34     """Generate slot at specified coordinates."""
  35     length = 5.3
  36     width = 2.4
  37     if parallel:
  38         length = 6.5
  39         width = 2.2
  40     right = 1
  41     if parallel:
  42         return [
  43             [
  44                 x + length/2 * cos(h - pi/2 * right),
  45                 y + length/2 * sin(h - pi/2 * right),
  46             ],
  47             [
  48                 x + width * cos(h) + length/2 * cos(h - pi/2 * right),
  49                 y + width * sin(h) + length/2 * sin(h - pi/2 * right),
  50             ],
  51             [
  52                 x + width * cos(h) + length/2 * cos(h + pi/2 * right),
  53                 y + width * sin(h) + length/2 * sin(h + pi/2 * right),
  54             ],
  55             [
  56                 x + length/2 * cos(h + pi/2 * right),
  57                 y + length/2 * sin(h + pi/2 * right),
  58             ],
  59         ]
  60     else:
  61         return [
  62             [
  63                 x + width/2 * cos(h - pi/2 * right),
  64                 y + width/2 * sin(h - pi/2 * right),
  65             ],
  66             [
  67                 x + length * cos(h) + width/2 * cos(h - pi/2 * right),
  68                 y + length * sin(h) + width/2 * sin(h - pi/2 * right),
  69             ],
  70             [
  71                 x + length * cos(h) + width/2 * cos(h + pi/2 * right),
  72                 y + length * sin(h) + width/2 * sin(h + pi/2 * right),
  73             ],
  74             [
  75                 x + width/2 * cos(h + pi/2 * right),
  76                 y + width/2 * sin(h + pi/2 * right),
  77             ],
  78         ]
  79
  80
  81 def gen_slot(l=5.3, w=2.4):
  82     """Generate parking slot."""
  83     parallel = True if random() < 0.5 else False
  84     if parallel and l == 5.3 and w == 2.4:
  85         l = 6.5
  86         w = 2.2
  87     elif parallel:
  88         ol = l
  89         l = w
  90         w = ol
  91     right = 1.0 if random() < 0.5 else -1.0
  92     r = SLOT_RADI
  93     angl = uniform(0, 2 * pi)
  94     x = r * cos(angl)
  95     y = r * sin(angl)
  96     h = uniform(0, 2 * pi)
  97     if parallel:
  98         return [
  99             [
 100                 x + l/2 * cos(h - pi/2 * right),
 101                 y + l/2 * sin(h - pi/2 * right),
 102             ],
 103             [
 104                 x + w * cos(h) + l/2 * cos(h - pi/2 * right),
 105                 y + w * sin(h) + l/2 * sin(h - pi/2 * right),
 106             ],
 107             [
 108                 x + w * cos(h) + l/2 * cos(h + pi/2 * right),
 109                 y + w * sin(h) + l/2 * sin(h + pi/2 * right),
 110             ],
 111             [
 112                 x + l/2 * cos(h + pi/2 * right),
 113                 y + l/2 * sin(h + pi/2 * right),
 114             ],
 115         ]
 116     else:
 117         return [
 118             [
 119                 x + w/2 * cos(h - pi/2 * right),
 120                 y + w/2 * sin(h - pi/2 * right),
 121             ],
 122             [
 123                 x + l * cos(h) + w/2 * cos(h - pi/2 * right),
 124                 y + l * sin(h) + w/2 * sin(h - pi/2 * right),
 125             ],
 126             [
 127                 x + l * cos(h) + w/2 * cos(h + pi/2 * right),
 128                 y + l * sin(h) + w/2 * sin(h + pi/2 * right),
 129             ],
 130             [
 131                 x + w/2 * cos(h + pi/2 * right),
 132                 y + w/2 * sin(h + pi/2 * right),
 133             ],
 134         ]
 135
 136
 137 def gen_obst():
 138     """Generate obstacles array."""
 139     obstacles = []
 140     min_r = ((W/2)**2 + ((WB+L)/2)**2)**0.5 + (OBST_W**2 + OBST_L**2)**0.5 / 2
 141     for i in range(OBST_COUNT):
 142         l = OBST_L
 143         w = OBST_W
 144         angl = uniform(0, 2 * pi)
 145         r = uniform(min_r**2, (SLOT_RADI - 5)**2)**0.5
 146         x = r * cos(angl)
 147         y = r * sin(angl)
 148         h = uniform(0, 2 * pi)
 149         obstacles.append([
 150             [
 151                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 152                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 153             ],
 154             [
 155                 x + w/2 * cos(h - pi/2) - l/2 * cos(h),
 156                 y + w/2 * sin(h - pi/2) - l/2 * sin(h),
 157             ],
 158             [
 159                 x + w/2 * cos(h + pi/2) - l/2 * cos(h),
 160                 y + w/2 * sin(h + pi/2) - l/2 * sin(h),
 161             ],
 162             [
 163                 x + w/2 * cos(h + pi/2) + l/2 * cos(h),
 164                 y + w/2 * sin(h + pi/2) + l/2 * sin(h),
 165             ],
 166             [
 167                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 168                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 169             ],
 170         ])
 171     return obstacles
 172
 173
 174 if __name__ == "__main__":
 175     init = gen_init()
 176     slot = gen_slot()
 177     obst = gen_obst()
 178     print(dumps({
 179         "init": init,
 180         "slot": slot,
 181         "obst": obst,
 182     }))