Do not use unicode minus

[hubacji1/iamcar2.git] / scripts / generate_simple_json_scenario.py

diff --git a/scripts/generate_simple_json_scenario.py b/scripts/generate_simple_json_scenario.py
index 4dc8af6e291d8ef04bfa7d59f73e32ecd47d641f..13e654d749aed20218a22bc851fa3240360baebf 100644 (file)
--- a/scripts/generate_simple_json_scenario.py
+++ b/scripts/generate_simple_json_scenario.py
@@ -10,13 +10,19 @@ The scenario contains at least:
 """
 from json import dumps, loads
 from math import cos, pi, sin
-from random import random
+from random import random, uniform
 
 W = 1.625
+L = 3.760
 WB = 2.450
 CTC = 10.820
 MTR = ((CTC / 2)**2 - WB**2)**0.5 - W / 2
 
+SLOT_RADI = 20
+OBST_L = 2
+OBST_W = 2
+OBST_COUNT = 8
+
 def gen_init():
     """Generate car init position."""
     # TODO if changed, change ``gen_slot`` accordingly
@@ -24,36 +30,94 @@ def gen_init():
 
 def gen_slot(l=5.3, w=2.4):
     """Generate parking slot."""
-    coord_min = 2 * MTR
-    coord_max = 20
-    coord_dif = coord_max - coord_min
-    x = random() * coord_dif + coord_min
-    x *= -1 if random() < 0.5 else 1
-    y = random() * coord_dif + coord_min
-    y *= -1 if random() < 0.5 else 1
-    h = random() * 2 * pi
-    return [[
-        [
-            x + w/2 * cos(h - pi/2),
-            y + w/2 * sin(h - pi/2),
-        ],
-        [
-            x + l * cos(h) + w/2 * cos(h - pi/2),
-            y + l * sin(h) + w/2 * sin(h - pi/2),
-        ],
-        [
-            x + l * cos(h) + w/2 * cos(h + pi/2),
-            y + l * sin(h) + w/2 * sin(h + pi/2),
-        ],
-        [
-            x + w/2 * cos(h + pi/2),
-            y + w/2 * sin(h + pi/2),
-        ],
-    ]]
+    parallel = True if random() < 0.5 else False
+    if parallel and l == 5.3 and w == 2.4:
+        l = 6.5
+        w = 2.2
+    elif parallel:
+        ol = l
+        l = w
+        w = ol
+    right = 1.0 if random() < 0.5 else -1.0
+    r = SLOT_RADI
+    angl = uniform(0, 2 * pi)
+    x = r * cos(angl)
+    y = r * sin(angl)
+    h = uniform(0, 2 * pi)
+    if parallel:
+        return [[
+            [
+                x + l/2 * cos(h - pi/2 * right),
+                y + l/2 * sin(h - pi/2 * right),
+            ],
+            [
+                x + w * cos(h) + l/2 * cos(h - pi/2 * right),
+                y + w * sin(h) + l/2 * sin(h - pi/2 * right),
+            ],
+            [
+                x + w * cos(h) + l/2 * cos(h + pi/2 * right),
+                y + w * sin(h) + l/2 * sin(h + pi/2 * right),
+            ],
+            [
+                x + l/2 * cos(h + pi/2 * right),
+                y + l/2 * sin(h + pi/2 * right),
+            ],
+        ]]
+    else:
+        return [[
+            [
+                x + w/2 * cos(h - pi/2 * right),
+                y + w/2 * sin(h - pi/2 * right),
+            ],
+            [
+                x + l * cos(h) + w/2 * cos(h - pi/2 * right),
+                y + l * sin(h) + w/2 * sin(h - pi/2 * right),
+            ],
+            [
+                x + l * cos(h) + w/2 * cos(h + pi/2 * right),
+                y + l * sin(h) + w/2 * sin(h + pi/2 * right),
+            ],
+            [
+                x + w/2 * cos(h + pi/2 * right),
+                y + w/2 * sin(h + pi/2 * right),
+            ],
+        ]]
 
 def gen_obst():
     """Generate obstacles array."""
-    return []
+    obstacles = []
+    min_r = ((W/2)**2 + ((WB+L)/2)**2)**0.5 + (OBST_W**2 + OBST_L**2)**0.5 / 2
+    for i in range(OBST_COUNT):
+        l = OBST_L
+        w = OBST_W
+        angl = uniform(0, 2 * pi)
+        r = uniform(min_r**2, (SLOT_RADI - 5)**2)**0.5
+        x = r * cos(angl)
+        y = r * sin(angl)
+        h = uniform(0, 2 * pi)
+        obstacles.append([
+            [
+                x + w/2 * cos(h - pi/2) + l/2 * cos(h),
+                y + w/2 * sin(h - pi/2) + l/2 * sin(h),
+            ],
+            [
+                x + w/2 * cos(h - pi/2) - l/2 * cos(h),
+                y + w/2 * sin(h - pi/2) - l/2 * sin(h),
+            ],
+            [
+                x + w/2 * cos(h + pi/2) - l/2 * cos(h),
+                y + w/2 * sin(h + pi/2) - l/2 * sin(h),
+            ],
+            [
+                x + w/2 * cos(h + pi/2) + l/2 * cos(h),
+                y + w/2 * sin(h + pi/2) + l/2 * sin(h),
+            ],
+            [
+                x + w/2 * cos(h - pi/2) + l/2 * cos(h),
+                y + w/2 * sin(h - pi/2) + l/2 * sin(h),
+            ],
+        ])
+    return obstacles
 
 if __name__ == "__main__":
     init = gen_init()