Add obstacle generator to simple scenario

diff --git a/scripts/generate_simple_json_scenario.py b/scripts/generate_simple_json_scenario.py
index 0e7f629dac0449e00e58136fa4318a3537e1541f..c72b72dfd83399fc144494dacb9ee90f2aedc8a1 100644 (file)
--- a/scripts/generate_simple_json_scenario.py
+++ b/scripts/generate_simple_json_scenario.py
@@ -13,10 +13,16 @@ from math import cos, pi, sin
 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 = 4
+OBST_W = 2
+OBST_COUNT = 8
+
 def gen_init():
     """Generate car init position."""
     # TODO if changed, change ``gen_slot`` accordingly
@@ -32,7 +38,7 @@ def gen_slot(l=5.3, w=2.4):
         ol = l
         l = w
         w = ol
-    r = 20
+    r = SLOT_RADI
     angl = uniform(0, 2 * pi)
     x = r * cos(angl)
     y = r * sin(angl)
@@ -78,7 +84,39 @@ def gen_slot(l=5.3, w=2.4):
 
 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()