2 """Generate simple JSON scenario.
4 The scenario contains at least:
6 - `init` -- the init car position,
8 - `slot` -- the parking slot,
10 - `obst` -- the list of (convex polygon) obstacles.
12 from json import dumps
13 from math import cos, pi, sin
14 from random import random, uniform
21 MTR = ((CTC / 2)**2 - WB**2)**0.5 - W / 2
29 def gen_slot_at(x, y, h, parallel=True):
30 """Generate slot at specified coordinates."""
40 x + length/2 * cos(h - pi/2 * right),
41 y + length/2 * sin(h - pi/2 * right),
44 x + width * cos(h) + length/2 * cos(h - pi/2 * right),
45 y + width * sin(h) + length/2 * sin(h - pi/2 * right),
48 x + width * cos(h) + length/2 * cos(h + pi/2 * right),
49 y + width * sin(h) + length/2 * sin(h + pi/2 * right),
52 x + length/2 * cos(h + pi/2 * right),
53 y + length/2 * sin(h + pi/2 * right),
59 x + width/2 * cos(h - pi/2 * right),
60 y + width/2 * sin(h - pi/2 * right),
63 x + length * cos(h) + width/2 * cos(h - pi/2 * right),
64 y + length * sin(h) + width/2 * sin(h - pi/2 * right),
67 x + length * cos(h) + width/2 * cos(h + pi/2 * right),
68 y + length * sin(h) + width/2 * sin(h + pi/2 * right),
71 x + width/2 * cos(h + pi/2 * right),
72 y + width/2 * sin(h + pi/2 * right),
77 def gen_obst_at(x, y, h, length=0.5, width=0.5):
78 """Generate obstacle at specific coordinates."""
81 x + width/2 * cos(h - pi/2) + length/2 * cos(h),
82 y + width/2 * sin(h - pi/2) + length/2 * sin(h),
85 x + width/2 * cos(h - pi/2) - length/2 * cos(h),
86 y + width/2 * sin(h - pi/2) - length/2 * sin(h),
89 x + width/2 * cos(h + pi/2) - length/2 * cos(h),
90 y + width/2 * sin(h + pi/2) - length/2 * sin(h),
93 x + width/2 * cos(h + pi/2) + length/2 * cos(h),
94 y + width/2 * sin(h + pi/2) + length/2 * sin(h),
97 x + width/2 * cos(h - pi/2) + length/2 * cos(h),
98 y + width/2 * sin(h - pi/2) + length/2 * sin(h),
103 """Generate car init pose.
105 It should be random, but now it is just [0, 0, 0].
111 """Generate random parking slot."""
112 parallel = True if random() < 0.5 else False
114 angl = uniform(0, 2 * pi)
117 h = uniform(0, 2 * pi)
118 return gen_slot_at(x, y, h, parallel)
121 def random_obstacles():
122 """Generate a list of random 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
130 h = uniform(0, 2 * pi)
131 obstacles.append(gen_obst_at(x, y, h, OBST_L, OBST_W))
138 "slot": gen_slot_at(10, 0 - 5, 0, False),
139 "obst": [gen_obst_at(*p) for p in [
140 [8, 2.5 - 5, 0], [6, 2.5 - 5, 0],
141 [10.25, 1.5 - 5, 0], [10.25, -1.5 - 5, 0]]]},
144 "slot": gen_slot_at(10, 0 + 5, 0, False),
145 "obst": [gen_obst_at(*p) for p in [
146 [8, 2.5 + 5, 0], [6, 2.5 + 5, 0],
147 [10.25, 1.5 + 5, 0], [10.25, -1.5 + 5, 0]]]},
150 "slot": gen_slot_at(10, 0, 0, False),
151 "obst": [gen_obst_at(*p) for p in [
152 [8, 2.5, 0], [6, 2.5, 0],
153 [10.25, 1.5, 0], [10.25, -1.5, 0]]]},
156 "slot": gen_slot_at(10, 0, 0, False),
157 "obst": [gen_obst_at(*p) for p in [
158 [10.25, 1.5, 0], [10.25, -1.5, 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 [12.5, -2.5, 0], [17.5, -2.5, 0]]]},
167 "slot": gen_slot_at(15, 0, pi/2, False),
168 "obst": [gen_obst_at(*p) for p in [
169 [13.5, 0.25, 0], [16.5, 0.25, 0],
170 [13.5, -5, 0], [16.5, -5, 0]]]},
173 "slot": gen_slot_at(15, 0, pi/2, False),
174 "obst": [gen_obst_at(*p) for p in [
175 [13.5, 0.25, 0], [16.5, 0.25, 0],
179 "slot": gen_slot_at(12, 2, -pi/2, True),
180 "obst": [gen_obst_at(*p) for p in [
181 [7, 0, 0], [7, 2, 0], [7, 4, 0], [7, -2, 0],
182 [9, -2, 0], [11, -2, 0]]]},
185 "slot": gen_slot_at(12, -2, -pi/2, True),
186 "obst": [gen_obst_at(*p) for p in [
187 [7, -2, 0], [7, -4, 0], [7, -6, 0], [9, -6, 0], [11, -6, 0],
188 [7, -2, 0], [11, 0, 0], [15, 0, 0], [19, 0, 0],
189 [7, 3, 0], [11, 5, 0], [15, 3, 0], [19, 3, 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 [7.5, 2.5, 0], [10, -2.5, 0], [12.5, 0, 0]]]},
198 "slot": gen_slot_at(15, 0, 0, True),
199 "obst": [gen_obst_at(*p) for p in [
200 [15.25, -3.5, 0], [15.25, 3.5, 0],
201 [10, 5, 0], [10, 0, 0], [12.5, 0, 0]]]},
204 "slot": gen_slot_at(15, 0, 0, True),
205 "obst": [gen_obst_at(*p) for p in [
206 [15.25, -3.5, 0], [15.25, 3.5, 0],
207 [12.5, -2.5, 0], [10, 0, 0], [7.5, 2.5, 0]]]}}
209 if __name__ == "__main__":
211 if len(sys.argv) == 2:
214 print(dumps(SCENARIOS[sc]))
217 "init": random_init(),
218 "slot": random_slot(),
219 "obst": random_obstacles()}))