From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 14 Mar 2023 14:38:15 +0000 (+0100)
Subject: Merge branch 'gen-simple-scenario'
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/commitdiff_plain/d5cabb5d71ebe7d03a6c6cf3d1fb5ce97156f5d8?hp=f564460e3a28eed62c9a9e655fd0ce4f526841c5

Merge branch 'gen-simple-scenario'
---

diff --git a/scripts/generate_simple_json_scenario.py b/scripts/generate_simple_json_scenario.py
old mode 100644
new mode 100755
index ce084d7..39fc505
--- a/scripts/generate_simple_json_scenario.py
+++ b/scripts/generate_simple_json_scenario.py
@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 """Generate simple JSON scenario.
 
 The scenario contains at least:
@@ -8,9 +9,10 @@ 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
+import sys
 
 W = 1.625
 L = 3.760
@@ -23,11 +25,61 @@ 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
@@ -83,6 +135,34 @@ def gen_slot(l=5.3, w=2.4):
             ],
         ]
 
+
+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,12 +199,21 @@ def gen_obst():
         ])
     return obstacles
 
+
+SCENARIOS = {
+    "pa01": {
+        "init": gen_init(),
+        "slot": gen_slot_at(15, 0, 0, True),
+        "obst": [gen_obst_at(p) for p in [[], [], []]]}}
+
 if __name__ == "__main__":
-    init = gen_init()
-    slot = gen_slot()
-    obst = gen_obst()
-    print(dumps({
-        "init": init,
-        "slot": slot,
-        "obst": obst,
-    }))
+    sc = ""
+    if len(sys.argv) == 2:
+        sc = sys.argv[1]
+    if sc in SCENARIOS:
+        print(dumps(SCENARIOS()[sc]))
+    else:
+        print(dumps({
+            "init": gen_init(),
+            "slot": gen_slot(),
+            "obst": gen_obst()}))