scripts/gen_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_slot_at(x, y, h, parallel=True):
  30     """Generate slot at specified coordinates."""
  31     length = 5.3
  32     width = 2.4
  33     if parallel:
  34         length = 6.5
  35         width = 2.2
  36     right = 1
  37     if parallel:
  38         return [
  39             [
  40                 x + length/2 * cos(h - pi/2 * right),
  41                 y + length/2 * sin(h - pi/2 * right),
  42             ],
  43             [
  44                 x + width * cos(h) + length/2 * cos(h - pi/2 * right),
  45                 y + width * sin(h) + 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 + length/2 * cos(h + pi/2 * right),
  53                 y + length/2 * sin(h + pi/2 * right),
  54             ],
  55         ]
  56     else:
  57         return [
  58             [
  59                 x + width/2 * cos(h - pi/2 * right),
  60                 y + width/2 * sin(h - pi/2 * right),
  61             ],
  62             [
  63                 x + length * cos(h) + width/2 * cos(h - pi/2 * right),
  64                 y + length * sin(h) + 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 + width/2 * cos(h + pi/2 * right),
  72                 y + width/2 * sin(h + pi/2 * right),
  73             ],
  74         ]
  75
  76
  77 def gen_obst_at(x, y, h, length=0.5, width=0.5):
  78     """Generate obstacle at specific coordinates."""
  79     return [
  80         [
  81             x + width/2 * cos(h - pi/2) + length/2 * cos(h),
  82             y + width/2 * sin(h - pi/2) + length/2 * sin(h),
  83         ],
  84         [
  85             x + width/2 * cos(h - pi/2) - length/2 * cos(h),
  86             y + width/2 * sin(h - pi/2) - length/2 * sin(h),
  87         ],
  88         [
  89             x + width/2 * cos(h + pi/2) - length/2 * cos(h),
  90             y + width/2 * sin(h + pi/2) - length/2 * sin(h),
  91         ],
  92         [
  93             x + width/2 * cos(h + pi/2) + length/2 * cos(h),
  94             y + width/2 * sin(h + pi/2) + length/2 * sin(h),
  95         ],
  96         [
  97             x + width/2 * cos(h - pi/2) + length/2 * cos(h),
  98             y + width/2 * sin(h - pi/2) + length/2 * sin(h),
  99         ]]
 100
 101
 102 def random_init():
 103     """Generate car init pose.
 104
 105     It should be random, but now it is just [0, 0, 0].
 106     """
 107     return [0, 0, 0]
 108
 109
 110 def random_slot():
 111     """Generate random parking slot."""
 112     parallel = True if random() < 0.5 else False
 113     r = SLOT_RADI
 114     angl = uniform(0, 2 * pi)
 115     x = r * cos(angl)
 116     y = r * sin(angl)
 117     h = uniform(0, 2 * pi)
 118     return gen_slot_at(x, y, h, parallel)
 119
 120
 121 def random_obstacles():
 122     """Generate a list of random obstacles."""
 123     obstacles = []
 124     min_r = ((W/2)**2 + ((WB+L)/2)**2)**0.5 + (OBST_W**2 + OBST_L**2)**0.5 / 2
 125     for i in range(OBST_COUNT):
 126         angl = uniform(0, 2 * pi)
 127         r = uniform(min_r**2, (SLOT_RADI - 5)**2)**0.5
 128         x = r * cos(angl)
 129         y = r * sin(angl)
 130         h = uniform(0, 2 * pi)
 131         obstacles.append(gen_obst_at(x, y, h, OBST_L, OBST_W))
 132     return obstacles
 133
 134
 135 SCENARIOS = {
 136     "pe05": {
 137         "init": [0, 0, 0],
 138         "slot": gen_slot_at(10, 0, 0, False),
 139         "obst": [gen_obst_at(*p) for p in [
 140             [8, 2.5, 0], [6, 2.5, 0],
 141             [10.25, 1.5, 0], [10.25, -1.5, 0]]]},
 142     "pe04": {
 143         "init": [0, 0, 0],
 144         "slot": gen_slot_at(10, 0, 0, False),
 145         "obst": [gen_obst_at(*p) for p in [
 146             [10.25, 1.5, 0], [10.25, -1.5, 0]]]},
 147     "pe03": {
 148         "init": [0, 0, 0],
 149         "slot": gen_slot_at(15, 0, pi/2, False),
 150         "obst": [gen_obst_at(*p) for p in [
 151             [13.5, 0.25, 0], [16.5, 0.25, 0],
 152             [12.5, -2.5, 0], [17.5, -2.5, 0]]]},
 153     "pe02": {
 154         "init": [0, 0, 0],
 155         "slot": gen_slot_at(15, 0, pi/2, False),
 156         "obst": [gen_obst_at(*p) for p in [
 157             [13.5, 0.25, 0], [16.5, 0.25, 0],
 158             [13.5, -5, 0], [16.5, -5, 0]]]},
 159     "pe01": {
 160         "init": [0, 0, 0],
 161         "slot": gen_slot_at(15, 0, pi/2, False),
 162         "obst": [gen_obst_at(*p) for p in [
 163             [13.5, 0.25, 0], [16.5, 0.25, 0],
 164             [15, -5, 0]]]},
 165     "pa05": {
 166         "init": [0, 0, 0],
 167         "slot": gen_slot_at(12, 2, -pi/2, True),
 168         "obst": [gen_obst_at(*p) for p in [
 169                 [7, 0, 0], [7, 2, 0], [7, 4, 0], [7, -2, 0],
 170                 [9, -2, 0], [11, -2, 0]]]},
 171     "pa04": {
 172         "init": [0, 0, 0],
 173         "slot": gen_slot_at(12, -2, -pi/2, True),
 174         "obst": [gen_obst_at(*p) for p in [
 175                 [7, -2, 0], [7, -4, 0], [7, -6, 0], [9, -6, 0], [11, -6, 0],
 176                 [7, -2, 0], [11, 0, 0], [15, 0, 0], [19, 0, 0],
 177                 [7, 3, 0], [11, 5, 0], [15, 3, 0], [19, 3, 0]]]},
 178     "pa03": {
 179         "init": [0, 0, 0],
 180         "slot": gen_slot_at(15, 0, 0, True),
 181         "obst": [gen_obst_at(*p) for p in [
 182             [15.25, -3.5, 0], [15.25, 3.5, 0],
 183             [7.5, 2.5, 0], [10, -2.5, 0], [12.5, 0, 0]]]},
 184     "pa02": {
 185         "init": [0, 0, 0],
 186         "slot": gen_slot_at(15, 0, 0, True),
 187         "obst": [gen_obst_at(*p) for p in [
 188             [15.25, -3.5, 0], [15.25, 3.5, 0],
 189             [10, 5, 0], [10, 0, 0], [12.5, 0, 0]]]},
 190     "pa01": {
 191         "init": [0, 0, 0],
 192         "slot": gen_slot_at(15, 0, 0, True),
 193         "obst": [gen_obst_at(*p) for p in [
 194             [15.25, -3.5, 0], [15.25, 3.5, 0],
 195             [12.5, -2.5, 0], [10, 0, 0], [7.5, 2.5, 0]]]}}
 196
 197 if __name__ == "__main__":
 198     sc = ""
 199     if len(sys.argv) == 2:
 200         sc = sys.argv[1]
 201     if sc in SCENARIOS:
 202         print(dumps(SCENARIOS[sc]))
 203     else:
 204         print(dumps({
 205             "init": random_init(),
 206             "slot": random_slot(),
 207             "obst": random_obstacles()}))