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_at(x, y, h):
 138     """Generate obstacle at specific coordinates."""
 139     length = 0.5
 140     width = 0.5
 141     return [
 142         [
 143             x + width/2 * cos(h - pi/2) + length/2 * cos(h),
 144             y + width/2 * sin(h - pi/2) + length/2 * sin(h),
 145         ],
 146         [
 147             x + width/2 * cos(h - pi/2) - length/2 * cos(h),
 148             y + width/2 * sin(h - pi/2) - length/2 * sin(h),
 149         ],
 150         [
 151             x + width/2 * cos(h + pi/2) - length/2 * cos(h),
 152             y + width/2 * sin(h + pi/2) - length/2 * sin(h),
 153         ],
 154         [
 155             x + width/2 * cos(h + pi/2) + length/2 * cos(h),
 156             y + width/2 * sin(h + pi/2) + length/2 * sin(h),
 157         ],
 158         [
 159             x + width/2 * cos(h - pi/2) + length/2 * cos(h),
 160             y + width/2 * sin(h - pi/2) + length/2 * sin(h),
 161         ]]
 162
 163
 164 def gen_obst():
 165     """Generate obstacles array."""
 166     obstacles = []
 167     min_r = ((W/2)**2 + ((WB+L)/2)**2)**0.5 + (OBST_W**2 + OBST_L**2)**0.5 / 2
 168     for i in range(OBST_COUNT):
 169         l = OBST_L
 170         w = OBST_W
 171         angl = uniform(0, 2 * pi)
 172         r = uniform(min_r**2, (SLOT_RADI - 5)**2)**0.5
 173         x = r * cos(angl)
 174         y = r * sin(angl)
 175         h = uniform(0, 2 * pi)
 176         obstacles.append([
 177             [
 178                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 179                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 180             ],
 181             [
 182                 x + w/2 * cos(h - pi/2) - l/2 * cos(h),
 183                 y + w/2 * sin(h - pi/2) - l/2 * sin(h),
 184             ],
 185             [
 186                 x + w/2 * cos(h + pi/2) - l/2 * cos(h),
 187                 y + w/2 * sin(h + pi/2) - l/2 * sin(h),
 188             ],
 189             [
 190                 x + w/2 * cos(h + pi/2) + l/2 * cos(h),
 191                 y + w/2 * sin(h + pi/2) + l/2 * sin(h),
 192             ],
 193             [
 194                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 195                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 196             ],
 197         ])
 198     return obstacles
 199
 200
 201 if __name__ == "__main__":
 202     init = gen_init()
 203     slot = gen_slot()
 204     obst = gen_obst()
 205     print(dumps({
 206         "init": init,
 207         "slot": slot,
 208         "obst": obst,
 209     }))