Add gen obst at procedure

[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 c34d010b82ca4738f04f65ff7794321d76c15ab7..da1a4833b61c539aa5395ebc1aeefc5194945763 100644 (file)
--- a/scripts/generate_simple_json_scenario.py
+++ b/scripts/generate_simple_json_scenario.py
@@ -8,7 +8,7 @@ The scenario contains at least:
 
 - `obst` -- the list of (convex polygon) obstacles.
 """
-from json import dumps, loads
+from json import dumps
 from math import cos, pi, sin
 from random import random, uniform
 
@@ -19,15 +19,65 @@ CTC = 10.820
 MTR = ((CTC / 2)**2 - WB**2)**0.5 - W / 2
 
 SLOT_RADI = 20
-OBST_L = 4
+OBST_L = 2
 OBST_W = 2
 OBST_COUNT = 8
 
+
 def gen_init():
     """Generate car init position."""
     # TODO if changed, change ``gen_slot`` accordingly
     return (0, 0, 0)
 
+
+def gen_slot_at(x, y, h, parallel=True):
+    """Generate slot at specified coordinates."""
+    length = 5.3
+    width = 2.4
+    if parallel:
+        length = 6.5
+        width = 2.2
+    right = 1
+    if parallel:
+        return [
+            [
+                x + length/2 * cos(h - pi/2 * right),
+                y + length/2 * sin(h - pi/2 * right),
+            ],
+            [
+                x + width * cos(h) + length/2 * cos(h - pi/2 * right),
+                y + width * sin(h) + length/2 * sin(h - pi/2 * right),
+            ],
+            [
+                x + width * cos(h) + length/2 * cos(h + pi/2 * right),
+                y + width * sin(h) + length/2 * sin(h + pi/2 * right),
+            ],
+            [
+                x + length/2 * cos(h + pi/2 * right),
+                y + length/2 * sin(h + pi/2 * right),
+            ],
+        ]
+    else:
+        return [
+            [
+                x + width/2 * cos(h - pi/2 * right),
+                y + width/2 * sin(h - pi/2 * right),
+            ],
+            [
+                x + length * cos(h) + width/2 * cos(h - pi/2 * right),
+                y + length * sin(h) + width/2 * sin(h - pi/2 * right),
+            ],
+            [
+                x + length * cos(h) + width/2 * cos(h + pi/2 * right),
+                y + length * sin(h) + width/2 * sin(h + pi/2 * right),
+            ],
+            [
+                x + width/2 * cos(h + pi/2 * right),
+                y + width/2 * sin(h + pi/2 * right),
+            ],
+        ]
+
+
 def gen_slot(l=5.3, w=2.4):
     """Generate parking slot."""
     parallel = True if random() < 0.5 else False
@@ -45,7 +95,7 @@ def gen_slot(l=5.3, w=2.4):
     y = r * sin(angl)
     h = uniform(0, 2 * pi)
     if parallel:
-        return [[
+        return [
             [
                 x + l/2 * cos(h - pi/2 * right),
                 y + l/2 * sin(h - pi/2 * right),
@@ -62,9 +112,9 @@ def gen_slot(l=5.3, w=2.4):
                 x + l/2 * cos(h + pi/2 * right),
                 y + l/2 * sin(h + pi/2 * right),
             ],
-        ]]
+        ]
     else:
-        return [[
+        return [
             [
                 x + w/2 * cos(h - pi/2 * right),
                 y + w/2 * sin(h - pi/2 * right),
@@ -81,8 +131,36 @@ def gen_slot(l=5.3, w=2.4):
                 x + w/2 * cos(h + pi/2 * right),
                 y + w/2 * sin(h + pi/2 * right),
             ],
+        ]
+
+
+def gen_obst_at(x, y, h):
+    """Generate obstacle at specific coordinates."""
+    length = 0.5
+    width = 0.5
+    return [
+        [
+            x + width/2 * cos(h - pi/2) + length/2 * cos(h),
+            y + width/2 * sin(h - pi/2) + length/2 * sin(h),
+        ],
+        [
+            x + width/2 * cos(h - pi/2) - length/2 * cos(h),
+            y + width/2 * sin(h - pi/2) - length/2 * sin(h),
+        ],
+        [
+            x + width/2 * cos(h + pi/2) - length/2 * cos(h),
+            y + width/2 * sin(h + pi/2) - length/2 * sin(h),
+        ],
+        [
+            x + width/2 * cos(h + pi/2) + length/2 * cos(h),
+            y + width/2 * sin(h + pi/2) + length/2 * sin(h),
+        ],
+        [
+            x + width/2 * cos(h - pi/2) + length/2 * cos(h),
+            y + width/2 * sin(h - pi/2) + length/2 * sin(h),
         ]]
 
+
 def gen_obst():
     """Generate obstacles array."""
     obstacles = []
@@ -119,6 +197,7 @@ def gen_obst():
         ])
     return obstacles
 
+
 if __name__ == "__main__":
     init = gen_init()
     slot = gen_slot()