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 import sys
  15
  16 W = 1.625
  17 L = 3.760
  18 WB = 2.450
  19 CTC = 10.820
  20 MTR = ((CTC / 2)**2 - WB**2)**0.5 - W / 2
  21
  22 SLOT_RADI = 20
  23 OBST_L = 2
  24 OBST_W = 2
  25 OBST_COUNT = 8
  26
  27
  28 def gen_init():
  29     """Generate car init position."""
  30     # TODO if changed, change ``gen_slot`` accordingly
  31     return (0, 0, 0)
  32
  33
  34 def gen_slot_at(x, y, h, parallel=True):
  35     """Generate slot at specified coordinates."""
  36     length = 5.3
  37     width = 2.4
  38     if parallel:
  39         length = 6.5
  40         width = 2.2
  41     right = 1
  42     if parallel:
  43         return [
  44             [
  45                 x + length/2 * cos(h - pi/2 * right),
  46                 y + length/2 * sin(h - pi/2 * right),
  47             ],
  48             [
  49                 x + width * cos(h) + length/2 * cos(h - pi/2 * right),
  50                 y + width * sin(h) + length/2 * sin(h - pi/2 * right),
  51             ],
  52             [
  53                 x + width * cos(h) + length/2 * cos(h + pi/2 * right),
  54                 y + width * sin(h) + length/2 * sin(h + pi/2 * right),
  55             ],
  56             [
  57                 x + length/2 * cos(h + pi/2 * right),
  58                 y + length/2 * sin(h + pi/2 * right),
  59             ],
  60         ]
  61     else:
  62         return [
  63             [
  64                 x + width/2 * cos(h - pi/2 * right),
  65                 y + width/2 * sin(h - pi/2 * right),
  66             ],
  67             [
  68                 x + length * cos(h) + width/2 * cos(h - pi/2 * right),
  69                 y + length * sin(h) + width/2 * sin(h - pi/2 * right),
  70             ],
  71             [
  72                 x + length * cos(h) + width/2 * cos(h + pi/2 * right),
  73                 y + length * sin(h) + width/2 * sin(h + pi/2 * right),
  74             ],
  75             [
  76                 x + width/2 * cos(h + pi/2 * right),
  77                 y + width/2 * sin(h + pi/2 * right),
  78             ],
  79         ]
  80
  81
  82 def gen_slot(l=5.3, w=2.4):
  83     """Generate parking slot."""
  84     parallel = True if random() < 0.5 else False
  85     if parallel and l == 5.3 and w == 2.4:
  86         l = 6.5
  87         w = 2.2
  88     elif parallel:
  89         ol = l
  90         l = w
  91         w = ol
  92     right = 1.0 if random() < 0.5 else -1.0
  93     r = SLOT_RADI
  94     angl = uniform(0, 2 * pi)
  95     x = r * cos(angl)
  96     y = r * sin(angl)
  97     h = uniform(0, 2 * pi)
  98     if parallel:
  99         return [
 100             [
 101                 x + l/2 * cos(h - pi/2 * right),
 102                 y + l/2 * sin(h - pi/2 * right),
 103             ],
 104             [
 105                 x + w * cos(h) + l/2 * cos(h - pi/2 * right),
 106                 y + w * sin(h) + l/2 * sin(h - pi/2 * right),
 107             ],
 108             [
 109                 x + w * cos(h) + l/2 * cos(h + pi/2 * right),
 110                 y + w * sin(h) + l/2 * sin(h + pi/2 * right),
 111             ],
 112             [
 113                 x + l/2 * cos(h + pi/2 * right),
 114                 y + l/2 * sin(h + pi/2 * right),
 115             ],
 116         ]
 117     else:
 118         return [
 119             [
 120                 x + w/2 * cos(h - pi/2 * right),
 121                 y + w/2 * sin(h - pi/2 * right),
 122             ],
 123             [
 124                 x + l * cos(h) + w/2 * cos(h - pi/2 * right),
 125                 y + l * sin(h) + w/2 * sin(h - pi/2 * right),
 126             ],
 127             [
 128                 x + l * cos(h) + w/2 * cos(h + pi/2 * right),
 129                 y + l * sin(h) + w/2 * sin(h + pi/2 * right),
 130             ],
 131             [
 132                 x + w/2 * cos(h + pi/2 * right),
 133                 y + w/2 * sin(h + pi/2 * right),
 134             ],
 135         ]
 136
 137
 138 def gen_obst_at(x, y, h):
 139     """Generate obstacle at specific coordinates."""
 140     length = 0.5
 141     width = 0.5
 142     return [
 143         [
 144             x + width/2 * cos(h - pi/2) + length/2 * cos(h),
 145             y + width/2 * sin(h - pi/2) + length/2 * sin(h),
 146         ],
 147         [
 148             x + width/2 * cos(h - pi/2) - length/2 * cos(h),
 149             y + width/2 * sin(h - pi/2) - length/2 * sin(h),
 150         ],
 151         [
 152             x + width/2 * cos(h + pi/2) - length/2 * cos(h),
 153             y + width/2 * sin(h + pi/2) - length/2 * sin(h),
 154         ],
 155         [
 156             x + width/2 * cos(h + pi/2) + length/2 * cos(h),
 157             y + width/2 * sin(h + pi/2) + length/2 * sin(h),
 158         ],
 159         [
 160             x + width/2 * cos(h - pi/2) + length/2 * cos(h),
 161             y + width/2 * sin(h - pi/2) + length/2 * sin(h),
 162         ]]
 163
 164
 165 def gen_obst():
 166     """Generate obstacles array."""
 167     obstacles = []
 168     min_r = ((W/2)**2 + ((WB+L)/2)**2)**0.5 + (OBST_W**2 + OBST_L**2)**0.5 / 2
 169     for i in range(OBST_COUNT):
 170         l = OBST_L
 171         w = OBST_W
 172         angl = uniform(0, 2 * pi)
 173         r = uniform(min_r**2, (SLOT_RADI - 5)**2)**0.5
 174         x = r * cos(angl)
 175         y = r * sin(angl)
 176         h = uniform(0, 2 * pi)
 177         obstacles.append([
 178             [
 179                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 180                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 181             ],
 182             [
 183                 x + w/2 * cos(h - pi/2) - l/2 * cos(h),
 184                 y + w/2 * sin(h - pi/2) - l/2 * sin(h),
 185             ],
 186             [
 187                 x + w/2 * cos(h + pi/2) - l/2 * cos(h),
 188                 y + w/2 * sin(h + pi/2) - l/2 * sin(h),
 189             ],
 190             [
 191                 x + w/2 * cos(h + pi/2) + l/2 * cos(h),
 192                 y + w/2 * sin(h + pi/2) + l/2 * sin(h),
 193             ],
 194             [
 195                 x + w/2 * cos(h - pi/2) + l/2 * cos(h),
 196                 y + w/2 * sin(h - pi/2) + l/2 * sin(h),
 197             ],
 198         ])
 199     return obstacles
 200
 201
 202 SCENARIOS = {
 203     "pa01": {
 204         "init": gen_init(),
 205         "slot": gen_slot_at(15, 0, 0, True),
 206         "obst": [gen_obst_at(p) for p in [[], [], []]]}}
 207
 208 if __name__ == "__main__":
 209     sc = ""
 210     if len(sys.argv) == 2:
 211         sc = sys.argv[1]
 212     if sc in SCENARIOS:
 213         print(dumps(SCENARIOS()[sc]))
 214     else:
 215         print(dumps({
 216             "init": gen_init(),
 217             "slot": gen_slot(),
 218             "obst": gen_obst()}))