scripts/generate_simple_json_scenario.py

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