Add gen obst at procedure

diff --git a/scripts/generate_simple_json_scenario.py b/scripts/generate_simple_json_scenario.py
index d6d3359b528cd59a09d0434f3c3bf7b0d917e7d4..da1a4833b61c539aa5395ebc1aeefc5194945763 100644 (file)
--- a/scripts/generate_simple_json_scenario.py
+++ b/scripts/generate_simple_json_scenario.py
@@ -134,6 +134,33 @@ 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 = []